Skip to content

F
flight-scripts
Commit 8d1d1ecb authored by Léo-Paul Géneau's avatar Léo-Paul Géneau 👾
Browse files
Options

Add swarm flight scripts

parent c55e7090
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 1317 additions and 0 deletions
collision_avoidance.js 0 → 100644
View file @ 8d1d1ecb
/*
* The point of this scenario is to prove that failsafes can be coded. It is
* supposed to be tested with only 2 drones.
* The leader follows a path of checkpoints. The follower tries to follow it at
* a distance of TARGETED_DISTANCE (30) meters. When the distance between the 2
* UAVs is lesser than SAFETY_DISTANCE (50) meters, the follower increases its
* regular flight altitude (535 meters) by OVERRIDE_ALTITUDE (20) meters.
*/
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, me*/
(function (console, me) {
"use strict";
var ALTITUDE_DIFF = 20,
BASE_ALTITUDE = 515,
CHECKPOINT_LIST = [
{
latitude: 45.56328,
longitude: 13.90470
},
{
latitude: 45.55926,
longitude: 13.90522
},
{
latitude: 45.55087,
longitude: 13.91845
},
{
latitude: 45.55487,
longitude: 13.91807
}
],
EPSILON_ALTITUDE = 1,
LOITER_RADIUS = 100,
LOITER_ACCEPTANCE = 105,
OVERRIDE_ALTITUDE = 20,
PARACHUTE_ALTITUDE = 473,
PARACHUTE_EPSILON = 10,
PARACHUTE_POINT = {
altitude: 463,
latitude: 45.55808,
longitude: 13.91086
},
PARACHUTE_YAW = -42,
R = 6371e3,
RALLY_POINT = {
altitude: 435,
latitude: 45.55601,
longitude: 13.91483
},
REGULAR_EPSILON = 50,
SAFETY_DISTANCE = 50,
START_DISTANCE = 200,
TARGETED_DISTANCE = 30,
TAKE_OFF_POINT = {
latitude: 45.55938,
longitude: 13.90846
},
TIMESTAMP_ACCEPTANCE = 1000,
YAW_EPSILON = 5,
MIN_SPEED = 17,
DEFAULT_SPEED = 18,
MAX_SPEED = 20,
SPEED_FACTOR = 0.2;
function exitOnFail(ret, msg) {
if (ret) {
console.log(msg);
me.exit(1);
}
}
function deployParachute(drone) {
console.log("Deploying parachute");
me.sendMsg(JSON.stringify(
{id: me.id, inAir: false, state: "Landed", type: "state"}
));
exitOnFail(drone.triggerParachute(), "Failed to deploy parachute");
me.exit(0);
}
function toRad(angle) {
return Math.PI * angle / 180;
}
function distance(lat1, lon1, lat2, lon2) {
var la1 = toRad(lat1),
la2 = toRad(lat2),
lo1 = toRad(lon1),
lo2 = toRad(lon2),
haversine_phi = Math.pow(Math.sin((la2 - la1) / 2), 2),
sin_lon = Math.sin((lo2 - lo1) / 2),
h = haversine_phi + Math.cos(la1) * Math.cos(la2) * sin_lon * sin_lon;
return 2 * R * Math.asin(Math.sqrt(h));
}
function leaderId(drone) {
return drone.id_list[0];
}
function pointReached(drone, point, acceptance) {
var current_position = drone.getCurrentPosition(),
dist = distance(
current_position.x,
current_position.y,
point.latitude,
point.longitude
);
console.log("Distance to point", dist);
return dist <= acceptance;
}
me.onStart = function () {
me.direction_set = false;
me.flightAltitude = BASE_ALTITUDE + me.id * ALTITUDE_DIFF;
me.following = false;
me.going_to_rally = false;
me.id_list = Object.keys(me.drone_dict).map((x) => Number(x));
me.id_list.sort();
me.landing = false;
me.next_checkpoint = 0;
me.parachute_altitude_reached = false;
me.parachute_yaw_reached = false;
me.started = false;
me.setAirSpeed(DEFAULT_SPEED);
me.sendMsg(JSON.stringify(
{id: me.id, inAir: true, state: "Ready", type: "state"}
));
};
me.onUpdate = function (timestamp) {
var checkpointIndex,
distance2d,
distanceDiff,
distanceToTakeOffPoint,
leader_id = leaderId(me),
me_index = me.id_list.indexOf(me.id),
neighbor_id = me.id_list[(!me.reverse) ? me_index - 1 : me_index + 1],
newSpeed,
position = me.getCurrentPosition();
if (!me.started) {
if (leader_id !== me.id) {
console.log(
"timestamp difference",
position.timestamp - me.drone_dict[neighbor_id].timestamp
);
}
return;
}
if (!me.landing) {
if (leader_id !== me.id) {
distance2d = distance(
position.x,
position.y,
me.drone_dict[neighbor_id].latitude,
me.drone_dict[neighbor_id].longitude
);
if (!me.following) {
distanceToTakeOffPoint = distance(
me.drone_dict[neighbor_id].latitude,
me.drone_dict[neighbor_id].longitude,
TAKE_OFF_POINT.latitude,
TAKE_OFF_POINT.longitude
);
console.log(
"Distance from neighbor to takeoff point",
distanceToTakeOffPoint
);
if (distanceToTakeOffPoint < START_DISTANCE) {
return;
}
me.following = true;
me.sendMsg(JSON.stringify(
{id: me.id, inAir: true, state: "Flying", type: "state"}
));
}
if (Math.abs(
position.timestamp - me.drone_dict[neighbor_id].timestamp
) >= TIMESTAMP_ACCEPTANCE) {
console.log(
"timestamp difference",
position.timestamp - me.drone_dict[neighbor_id].timestamp
);
return;
}
distanceDiff = distance2d - TARGETED_DISTANCE;
newSpeed = Math.max(
Math.min(distanceDiff * SPEED_FACTOR + DEFAULT_SPEED, MAX_SPEED),
MIN_SPEED
);
me.setAirSpeed(newSpeed);
if (distance2d < SAFETY_DISTANCE) {
me.flightAltitude =
BASE_ALTITUDE + me.id * ALTITUDE_DIFF + OVERRIDE_ALTITUDE;
} else {
me.flightAltitude = BASE_ALTITUDE + me.id * ALTITUDE_DIFF;
}
me.setTargetCoordinates(
me.drone_dict[neighbor_id].latitude,
me.drone_dict[neighbor_id].longitude,
me.flightAltitude
);
console.log(
"distance to leader",
distance(
position.x,
position.y,
me.drone_dict[neighbor_id].latitude,
me.drone_dict[neighbor_id].longitude
)
);
return;
}
checkpointIndex = (!me.reverse) ? me.next_checkpoint
: CHECKPOINT_LIST.length - me.next_checkpoint - 1;
if (!me.direction_set) {
console.log("Going to Checkpoint", checkpointIndex);
me.setTargetCoordinates(
CHECKPOINT_LIST[checkpointIndex].latitude,
CHECKPOINT_LIST[checkpointIndex].longitude,
me.flightAltitude
);
me.direction_set = true;
}
if (
pointReached(me, CHECKPOINT_LIST[checkpointIndex], REGULAR_EPSILON)
) {
console.log("Reached Checkpoint", checkpointIndex);
me.next_checkpoint += 1;
me.next_checkpoint %= CHECKPOINT_LIST.length;
me.direction_set = false;
me.sendMsg(JSON.stringify({
type: "checkpoint",
next_checkpoint: me.next_checkpoint
}));
}
return;
}
if (me.going_to_rally) {
if (pointReached(me, RALLY_POINT, LOITER_ACCEPTANCE)) {
console.log("Arrived to rally");
me.going_to_rally = false;
me.loiter(
RALLY_POINT.latitude,
RALLY_POINT.longitude,
PARACHUTE_ALTITUDE,
LOITER_RADIUS
);
}
return;
}
if (!me.parachute_altitude_reached) {
console.log("Altitude", me.getAltitudeAbs());
if (Math.abs(me.getAltitudeAbs() - PARACHUTE_ALTITUDE) <=
EPSILON_ALTITUDE) {
me.parachute_altitude_reached = true;
}
return;
}
if (!me.parachute_yaw_reached) {
console.log("Yaw:", me.getYaw());
if (Math.abs(me.getYaw() - PARACHUTE_YAW) <= YAW_EPSILON) {
me.parachute_yaw_reached = true;
me.setTargetCoordinates(
PARACHUTE_POINT.latitude,
PARACHUTE_POINT.longitude,
PARACHUTE_POINT.altitude
);
}
return;
}
if (pointReached(me, PARACHUTE_POINT, PARACHUTE_EPSILON)) {
deployParachute(me);
}
};
me.onGetMsg = function (msg) {
var msgDict = JSON.parse(msg);
if (msgDict.hasOwnProperty("status")) {
switch (msgDict.status) {
case "running":
me.started = true;
if (me.id === leaderId(me)) {
me.sendMsg(JSON.stringify(
{id: me.id, inAir: true, state: "Flying", type: "state"}
));
}
break;
case "stopped":
if (me.id === me.id_list[0]) {
me.landing = true;
me.going_to_rally = true;
me.loiter(
RALLY_POINT.latitude,
RALLY_POINT.longitude,
me.flightAltitude,
LOITER_RADIUS
);
}
break;
default:
console.log("Unknown status:", me.msgDict.status);
}
return;
}
switch (msgDict.type) {
case "state":
if (msgDict.state === "Landed") {
me.id_list.splice(me.id_list.indexOf(msgDict.id), 1);
if (me.id === me.id_list[0]) {
me.landing = true;
me.going_to_rally = true;
me.loiter(
RALLY_POINT.latitude,
RALLY_POINT.longitude,
me.flightAltitude,
LOITER_RADIUS
);
}
}
break;
default:
console.log("Unknown message type: " + msgDict.type);
}
};
}(console, me));
fixed_wings_swarm.js 0 → 100644
View file @ 8d1d1ecb
This diff is collapsed.
gnss_denial.js 0 → 100644
View file @ 8d1d1ecb
/*
* The point of this scenario is to prove that gps jamming can be detected.
* It is supposed to be tested with only 2 drones.
* The leader follows a path of checkpoints. The follower follows it at
* flying at the altitude of 525 meters. When gps is jammed, the follower
* increases its flight altitude by OVERRIDE_ALTITUDE (20) meters.
*/
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, me*/
(function (console, me) {
"use strict";
var ALTITUDE_DIFF = 20,
BASE_ALTITUDE = 515,
CHECKPOINT_LIST = [
{
latitude: 45.56328,
longitude: 13.90470
},
{
latitude: 45.55926,
longitude: 13.90522
},
{
latitude: 45.55087,
longitude: 13.91845
},
{
latitude: 45.55487,
longitude: 13.91807
}
],
EPSILON_ALTITUDE = 1,
LOITER_RADIUS = 100,
LOITER_ACCEPTANCE = 105,
OVERRIDE_ALTITUDE = 20,
PARACHUTE_ALTITUDE = 473,
PARACHUTE_EPSILON = 10,
PARACHUTE_POINT = {
altitude: 463,
latitude: 45.55808,
longitude: 13.91086
},
PARACHUTE_YAW = -42,
R = 6371e3,
RALLY_POINT = {
altitude: 435,
latitude: 45.55601,
longitude: 13.91483
},
REGULAR_EPSILON = 50,
TARGETED_DISTANCE = 100,
TAKE_OFF_POINT = {
latitude: 45.55938,
longitude: 13.90846
},
TIMESTAMP_ACCEPTANCE = 1000,
YAW_EPSILON = 5,
MIN_SPEED = 17,
DEFAULT_SPEED = 18,
MAX_SPEED = 20,
SPEED_FACTOR = 0.2;
function exitOnFail(ret, msg) {
if (ret) {
console.log(msg);
me.exit(1);
}
}
function deployParachute(drone) {
console.log("Deploying parachute");
me.sendMsg(JSON.stringify(
{id: me.id, inAir: false, state: "Landed", type: "state"}
));
exitOnFail(drone.triggerParachute(), "Failed to deploy parachute");
me.exit(0);
}
function toRad(angle) {
return Math.PI * angle / 180;
}
function distance(lat1, lon1, lat2, lon2) {
var la1 = toRad(lat1),
la2 = toRad(lat2),
lo1 = toRad(lon1),
lo2 = toRad(lon2),
haversine_phi = Math.pow(Math.sin((la2 - la1) / 2), 2),
sin_lon = Math.sin((lo2 - lo1) / 2),
h = haversine_phi + Math.cos(la1) * Math.cos(la2) * sin_lon * sin_lon;
return 2 * R * Math.asin(Math.sqrt(h));
}
function leaderId(drone) {
return drone.id_list[0];
}
function pointReached(drone, point, acceptance) {
var current_position = drone.getCurrentPosition(),
dist = distance(
current_position.x,
current_position.y,
point.latitude,
point.longitude
);
console.log("Distance to point", dist);
return dist <= acceptance;
}
me.onStart = function () {
me.direction_set = false;
me.flightAltitude = BASE_ALTITUDE + me.id * ALTITUDE_DIFF;
me.following = false;
me.going_to_rally = false;
me.id_list = Object.keys(me.drone_dict).map((x) => Number(x));
me.id_list.sort();
me.landing = false;
me.next_checkpoint = 0;
me.parachute_altitude_reached = false;
me.parachute_yaw_reached = false;
me.started = false;
me.setAirSpeed(DEFAULT_SPEED);
me.sendMsg(JSON.stringify(
{id: me.id, inAir: true, state: "Ready", type: "state"}
));
};
me.onUpdate = function (timestamp) {
var checkpointIndex,
distance2d,
distanceDiff,
distanceToTakeOffPoint,
leader_id = leaderId(me),
me_index = me.id_list.indexOf(me.id),
neighbor_id = me.id_list[(!me.reverse) ? me_index - 1 : me_index + 1],
newSpeed,
position = me.getCurrentPosition();
if (!me.started) {
if (leader_id !== me.id) {
console.log(
"timestamp difference",
position.timestamp - me.drone_dict[neighbor_id].timestamp
);
}
return;
}
if (!me.landing) {
if (leader_id !== me.id) {
if (!me.gpsIsOk()) {
me.flightAltitude =
BASE_ALTITUDE + me.id * ALTITUDE_DIFF + OVERRIDE_ALTITUDE;
} else {
me.flightAltitude = BASE_ALTITUDE + me.id * ALTITUDE_DIFF;
}
distance2d = distance(
position.x,
position.y,
me.drone_dict[neighbor_id].latitude,
me.drone_dict[neighbor_id].longitude
);
if (!me.following) {
distanceToTakeOffPoint = distance(
me.drone_dict[neighbor_id].latitude,
me.drone_dict[neighbor_id].longitude,
TAKE_OFF_POINT.latitude,
TAKE_OFF_POINT.longitude
);
console.log(
"Distance from neighbor to takeoff point",
distanceToTakeOffPoint
);
if (distanceToTakeOffPoint < 2 * TARGETED_DISTANCE) {
return;
}
me.following = true;
me.sendMsg(JSON.stringify(
{id: me.id, inAir: true, state: "Flying", type: "state"}
));
}
if (Math.abs(
position.timestamp - me.drone_dict[neighbor_id].timestamp
) >= TIMESTAMP_ACCEPTANCE) {
console.log(
"timestamp difference",
position.timestamp - me.drone_dict[neighbor_id].timestamp
);
return;
}
distanceDiff = distance2d - TARGETED_DISTANCE;
newSpeed = Math.max(
Math.min(distanceDiff * SPEED_FACTOR + DEFAULT_SPEED, MAX_SPEED),
MIN_SPEED
);
me.setAirSpeed(newSpeed);
me.setTargetCoordinates(
me.drone_dict[neighbor_id].latitude,
me.drone_dict[neighbor_id].longitude,
me.flightAltitude
);
console.log(
"distance to leader",
distance(
position.x,
position.y,
me.drone_dict[neighbor_id].latitude,
me.drone_dict[neighbor_id].longitude
)
);
return;
}
checkpointIndex = (!me.reverse) ? me.next_checkpoint
: CHECKPOINT_LIST.length - me.next_checkpoint - 1;
if (!me.direction_set) {
console.log("Going to Checkpoint", checkpointIndex);
me.setTargetCoordinates(
CHECKPOINT_LIST[checkpointIndex].latitude,
CHECKPOINT_LIST[checkpointIndex].longitude,
me.flightAltitude
);
me.direction_set = true;
}
if (
pointReached(me, CHECKPOINT_LIST[checkpointIndex], REGULAR_EPSILON)
) {
console.log("Reached Checkpoint", checkpointIndex);
me.next_checkpoint += 1;
me.next_checkpoint %= CHECKPOINT_LIST.length;
me.direction_set = false;
me.sendMsg(JSON.stringify({
type: "checkpoint",
next_checkpoint: me.next_checkpoint
}));
}
return;
}
if (me.going_to_rally) {
if (pointReached(me, RALLY_POINT, LOITER_ACCEPTANCE)) {
console.log("Arrived to rally");
me.going_to_rally = false;
me.loiter(
RALLY_POINT.latitude,
RALLY_POINT.longitude,
PARACHUTE_ALTITUDE,
LOITER_RADIUS
);
}
return;
}
if (!me.parachute_altitude_reached) {
console.log("Altitude", me.getAltitudeAbs());
if (Math.abs(me.getAltitudeAbs() - PARACHUTE_ALTITUDE) <=
EPSILON_ALTITUDE) {
me.parachute_altitude_reached = true;
}
return;
}
if (!me.parachute_yaw_reached) {
console.log("Yaw:", me.getYaw());
if (Math.abs(me.getYaw() - PARACHUTE_YAW) <= YAW_EPSILON) {
me.parachute_yaw_reached = true;
me.setTargetCoordinates(
PARACHUTE_POINT.latitude,
PARACHUTE_POINT.longitude,
PARACHUTE_POINT.altitude
);
}
return;
}
if (pointReached(me, PARACHUTE_POINT, PARACHUTE_EPSILON)) {
deployParachute(me);
}
};
me.onGetMsg = function (msg) {
var msgDict = JSON.parse(msg);
if (msgDict.hasOwnProperty("status")) {
switch (msgDict.status) {
case "running":
me.started = true;
if (me.id === leaderId(me)) {
me.sendMsg(JSON.stringify(
{id: me.id, inAir: true, state: "Flying", type: "state"}
));
}
break;
case "stopped":
if (me.id === me.id_list[0]) {
me.landing = true;
me.going_to_rally = true;
me.loiter(
RALLY_POINT.latitude,
RALLY_POINT.longitude,
me.flightAltitude,
LOITER_RADIUS
);
}
break;
default:
console.log("Unknown status:", me.msgDict.status);
}
return;
}
switch (msgDict.type) {
case "state":
if (msgDict.state === "Landed") {
me.id_list.splice(me.id_list.indexOf(msgDict.id), 1);
if (me.id === me.id_list[0]) {
me.landing = true;
me.going_to_rally = true;
me.loiter(
RALLY_POINT.latitude,
RALLY_POINT.longitude,
me.flightAltitude,
LOITER_RADIUS
);
}
}
break;
default:
console.log("Unknown message type: " + msgDict.type);
}
};
}(console, me));
v.js 0 → 100644
View file @ 8d1d1ecb
/*
* In the script the swarm follows continuously a list of checkpoint while
* keeping the shape of a "V". This is a demo for the simulator.
*/
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, me*/
(function (console, me) {
"use strict";
var FLIGH_ALTITUDE = 100,
MINIMAL_DISTANCE = 20,
EPSILON = 20,
CHECKPOINT_LIST = [
{
altitude: 585.1806861589965,
latitude: 45.64492790560583,
longitude: 14.25334942966329
},
{
altitude: 589.8802607573035,
latitude: 45.64316335436476,
longitude: 14.26332880184475
},
{
altitude: 608.6648153348965,
latitude: 45.64911917196595,
longitude: 14.26214792790128
},
{
altitude: 606.1448368129072,
latitude: 45.64122685351364,
longitude: 14.26590493128597
},
{
altitude: 630.0829598206344,
latitude: 45.64543355564817,
longitude: 14.27242391207985
},
{
altitude: 616.1839898415284,
latitude: 45.6372792927328,
longitude: 14.27533492411138
},
{
altitude: 598.0603137354178,
latitude: 45.64061299543953,
longitude: 14.26161958465814
},
{
altitude: 607.1243119862851,
latitude: 45.64032340702919,
longitude: 14.2682896662383
}
],
R = 6371e3;
function exitOnFail(ret, msg) {
if (ret) {
console.log(msg);
me.exit(1);
}
}
function deployParachute(drone) {
console.log("[DEMO] Deploying parachute...");
me.sendMsg("landing");
exitOnFail(drone.triggerParachute(), "Failed to deploy parachute");
me.exit(0);
}
function distance(lat1, lon1, lat2, lon2) {
var la1 = lat1 * Math.PI / 180, // la, lo in radians
la2 = lat2 * Math.PI / 180,
lo1 = lon1 * Math.PI / 180,
lo2 = lon2 * Math.PI / 180,
haversine_phi = Math.pow(Math.sin((la2 - la1) / 2), 2),
sin_lon = Math.sin((lo2 - lo1) / 2),
h = haversine_phi + Math.cos(la1) * Math.cos(la2) * sin_lon * sin_lon;
return 2 * R * Math.asin(Math.sqrt(h));
}
function toRad(angle) {
return Math.PI * angle / 180;
}
function toDeg(angle) {
return 180 * angle / Math.PI;
}
function constrain(input, min, max) {
return (input > max) ? max : (input < min) ? min : input;
}
//azimuthal projection
function toLocalCoordinates(lat, lon, ref_sin_lat, ref_cos_lat, ref_lon_rad) {
var lat_rad = toRad(lat),
lon_rad = toRad(lon),
sin_lat = Math.sin(lat_rad),
cos_lat = Math.cos(lat_rad),
cos_d_lon = Math.cos(lon_rad - ref_lon_rad),
c = Math.acos(constrain(
ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon,
-1,
1
)),
k = (Math.abs(c) > 0) ? (c / Math.sin(c)) : 1,
mul = k * R;
return {
"lat": mul * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon),
"lon": mul * cos_lat * Math.sin(lon_rad - ref_lon_rad)
};
}
function toGlobalCoordinates(lat, lon, ref_sin_lat, ref_cos_lat,
ref_lon_rad) {
var x_rad = lat / R,
y_rad = lon / R,
c = Math.sqrt(x_rad * x_rad + y_rad * y_rad),
sin_c = Math.sin(c),
cos_c = Math.cos(c),
lat_rad = Math.asin(
cos_c * ref_sin_lat + (x_rad * sin_c * ref_cos_lat) / c
),
lon_rad = ref_lon_rad + Math.atan2(
y_rad * sin_c,
c * ref_cos_lat * cos_c - x_rad * ref_sin_lat * sin_c
);
return {"lat": toDeg(lat_rad), "lon": toDeg(lon_rad)};
}
me.onStart = function () {
var position = me.getCurrentPosition(),
ref_lat_rad = toRad(position.x);
me.direction_set = false;
me.last_checkpoint_reached = false;
me.next_checkpoint = 0;
me.start_altitude = me.getInitialAltitude() + FLIGH_ALTITUDE;
me.firstRun = true;
me.ref_cos_lat = Math.cos(ref_lat_rad);
me.ref_lon_rad = toRad(position.y);
me.ref_sin_lat = Math.sin(ref_lat_rad);
};
me.onUpdate = function (timestamp) {
var leaderYawRad;
if (me.firstRun) {
me.firstRun = false;
me.secondRun = true;
return;
}
if (me.secondRun) {
me.leader_id = Math.trunc(me.drone_dict.length / 2);
me.is_leader = me.id === me.leader_id;
me.secondRun = false;
me.neighbour_id = (me.id < me.leader_id) ? me.id + 1 : me.id - 1;
me.angleUpdate = ((me.id < me.leader_id) ? 3 : 5) * Math.PI / 4;
}
if (!me.is_leader) {
leaderYawRad = toRad(me.drone_dict[me.neighbour_id].yaw);
me.neighbor_local_coordinates = toLocalCoordinates(
me.drone_dict[me.neighbour_id].latitude,
me.drone_dict[me.neighbour_id].longitude,
me.ref_sin_lat,
me.ref_cos_lat,
me.ref_lon_rad
);
me.next_coordinates = toGlobalCoordinates(
me.neighbor_local_coordinates.lat
+ MINIMAL_DISTANCE * Math.sin(leaderYawRad + me.angleUpdate),
me.neighbor_local_coordinates.lon
+ MINIMAL_DISTANCE * Math.cos(leaderYawRad + me.angleUpdate),
me.ref_sin_lat,
me.ref_cos_lat,
me.ref_lon_rad
);
return exitOnFail(
me.setTargetCoordinates(
me.next_coordinates.lat,
me.next_coordinates.lon,
me.drone_dict[me.neighbour_id].altitudeAbs
),
"Failed to follow leader"
);
}
if (!me.direction_set) {
if (me.next_checkpoint < CHECKPOINT_LIST.length) {
exitOnFail(
me.setTargetCoordinates(
CHECKPOINT_LIST[me.next_checkpoint].latitude,
CHECKPOINT_LIST[me.next_checkpoint].longitude,
CHECKPOINT_LIST[me.next_checkpoint].altitude + FLIGH_ALTITUDE
),
"Failed to set checkpoint coordinates"
);
console.log("[DEMO] Going to Checkpoint " + me.next_checkpoint + "\n");
}
me.direction_set = true;
return;
}
if (me.next_checkpoint < CHECKPOINT_LIST.length) {
me.current_position = me.getCurrentPosition();
me.distance = distance(
me.current_position.x,
me.current_position.y,
CHECKPOINT_LIST[me.next_checkpoint].latitude,
CHECKPOINT_LIST[me.next_checkpoint].longitude
);
if (me.distance > EPSILON) {
console.log(
"Waiting for drone to get to destination (checkpoint " +
me.next_checkpoint + ": " + me.distance + " m)"
);
} else {
console.log("[DEMO] Reached Checkpoint " + me.next_checkpoint + "\n");
me.next_checkpoint += 1;
me.direction_set = false;
}
return;
}
deployParachute(me);
};
me.onGetMsg = function (msg) {
me.msgDict = JSON.parse(msg);
if (msg === "landing") {
deployParachute(me);
} else {
console.log("Unknown message: ", msg);
}
};
}(console, me));
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment
GitLab Nexedi Edition | About GitLab | About Nexedi | 沪ICP备2021021310号-2 | 沪ICP备2021021310号-7