Skip to content

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

Swarm flight scripts 

See merge request !1
parents bf6a0a5b 8de07085
Expand all Show whitespace changes
Inline Side-by-side
Showing with 1720 additions and 519 deletions
bouncy-flight.js
View file @ e566cfb6
/*
* In this script the drone goes to different checkpoints with all kinds of
* angles between. This has mostly a test purpose to compare simulation logs
* and real ones.
*/
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, me*/
(function (console, me) {
"use strict";
var EPSILON = 9,
EPSILON_ALTITUDE = 7,
var EPSILON = 42,
FLIGH_ALTITUDE = 100,
PARACHUTE_ALTITUDE = 35,
CHECKPOINT_LIST = [
{
"altitude": 604,
......@@ -91,14 +95,6 @@
}
];
function altitudeReached(altitude, target_altitude) {
console.log(
"[DEMO] Waiting for altitude... ("
+ altitude + " , " + target_altitude + ")"
);
return Math.abs(altitude - target_altitude) < EPSILON_ALTITUDE;
}
function distance(lat1, lon1, lat2, lon2) {
var R = 6371e3, // meters
la1 = lat1 * Math.PI / 180, // la, lo in radians
......@@ -125,15 +121,9 @@
return timestamp - me.timestamp < me.must_wait;
}
function groundLevel(drone) {
return drone.getAltitudeAbs() - drone.getCurrentPosition().z;
}
me.onStart = function () {
me.direction_set = false;
me.landing_alt_reached = false;
me.next_checkpoint = 0;
me.parachute_triggered = false;
};
me.onUpdate = function (timestamp) {
......@@ -156,14 +146,6 @@
"Failed to set checkpoint coordinates"
);
console.log("[DEMO] Going to Checkpoint " + me.next_checkpoint + "\n");
} else {
me.loiter();
console.log("[DEMO] Going to landing altitude...\n");
me.landing_altitude = groundLevel(me) + PARACHUTE_ALTITUDE;
exit_on_fail(
me.setAltitude(me.landing_altitude),
"Failed to set landing altitude"
);
}
me.direction_set = true;
return;
......@@ -189,22 +171,8 @@
return;
}
if (!me.landing_alt_reached) {
me.landing_alt_reached = altitudeReached(
me.getAltitudeAbs(),
me.landing_altitude
);
return;
}
if (!me.parachute_triggered) {
console.log("[DEMO] Deploying parachute...");
exit_on_fail(me.triggerParachute(), "Failed to deploy parachute");
me.parachute_triggered = true;
}
if (me.landed()) {
me.exit(0);
}
};
}(console, me));
cli.js deleted 100644 → 0
View file @ bf6a0a5b
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, me*/
(function (console, me) {
"use strict";
function displayMessage(message) {
console.log(message);
return 0;
}
var help = ["altitude(altitude)", "exit",
"goto(latitude, longitude, altitude)", "help", "land", "loiter",
"positions", "speed(speed)"].join("\n"),
wrongParameters = displayMessage.bind(null, "Wrong parameters");
function checkNumber(value, toExecute) {
return (
Number.isNaN(value)
? wrongParameters
: toExecute.bind(null, value)
);
}
function displayDronePositions() {
Object.entries(me.droneDict).forEach(function ([id, drone]) {
console.log(
id,
drone.latitude,
drone.longitude,
drone.altitudeAbs,
drone.altitudeRel
);
});
return 0;
}
function exit() {
try {
me.f.close();
} catch (error) {
console.error(error);
}
me.exit(0);
}
me.onStart = function () {
me.f = me.fdopen(me.in, "r");
console.log(help);
};
me.onUpdate = function () {
var altitude,
latitude,
longitude,
speed,
user_input,
undefined_cmd = false,
cmd,
ret;
console.log("> ");
user_input = me.f.getline();
switch (user_input) {
case "altitude":
console.log("Altitude: ");
altitude = parseFloat(me.f.getline());
cmd = checkNumber(altitude, me.setAltitude);
break;
case "exit":
cmd = exit;
break;
case "goto":
console.log("Latitude: ");
latitude = parseFloat(me.f.getline());
console.log("Longitude: ");
longitude = parseFloat(me.f.getline());
console.log("Altitude: ");
altitude = parseFloat(me.f.getline());
cmd = checkNumber(
altitude,
checkNumber(longitude, checkNumber(latitude, me.setTargetCoordinates))
);
break;
case "help":
cmd = displayMessage.bind(null, help);
break;
case "land":
cmd = me.triggerParachute;
break;
case "loiter":
cmd = me.loiter;
break;
case "positions":
cmd = displayDronePositions;
break;
case "speed":
console.log("Speed: ");
speed = parseFloat(me.f.getline());
cmd = checkNumber(speed, me.setAirspeed);
break;
default:
undefined_cmd = true;
cmd = displayMessage.bind(null, " Undefined command");
}
ret = cmd();
if (ret) {
console.log(" [ERROR] function:\n", cmd, "\nreturn value:", ret);
} else if (user_input !== "help" && !undefined_cmd) {
console.log(" Command successful");
}
};
}(console, me));
\ No newline at end of file
collision_avoidance.js 0 → 100644
View file @ e566cfb6
/*
* 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));
default.js
View file @ e566cfb6
/*
* This is the script deployed by default as the user script. It does nothing
* except exiting the program which prevents drones unexpected behavior.
*/
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global me*/
......
fixed_wings_swarm.js 0 → 100644
View file @ e566cfb6
This diff is collapsed.
gnss_denial.js 0 → 100644
View file @ e566cfb6
/*
* 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));
manual-flight.js deleted 100644 → 0
View file @ bf6a0a5b
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, me, sleep*/
(function (console, me, sleep) {
"use strict";
var ALTITUDE_DIFF = 30,
EPSILON_ALTITUDE = 5,
FLIGH_ALTITUDE = 100,
INITIAL_ALTITUDE,
LEADER_ID = 0,
IS_LEADER = me.id === LEADER_ID,
PARACHUTE_ALTITUDE = 35,
START_ALTITUDE,
leaderAltitudeAbs,
leaderAltitudeRel,
leaderLatitude,
leaderLongitude;
function exit_on_fail(ret, msg) {
if (ret) {
console.log(msg);
me.exit(1);
}
}
function waitForAltitude(target_altitude) {
var altitude = me.getAltitude();
while (Math.abs(altitude - target_altitude) > EPSILON_ALTITUDE) {
console.log(
"[DEMO] Waiting for altitude... ("
+ altitude + " , " + target_altitude + ")"
);
sleep(1000);
altitude = me.getAltitude();
}
}
function goToAltitude(target_altitude, wait, go) {
if (go) {
exit_on_fail(
me.setAltitude(target_altitude),
"Failed to go to altitude " + target_altitude + " m"
);
}
if (wait) {
waitForAltitude(target_altitude);
}
}
function followLeader(leaderId, initialAltitude, altitudeDiff) {
goToAltitude(START_ALTITUDE + ALTITUDE_DIFF, false, true);
while (me.droneDict[leaderId].altitudeAbs === 0) {
console.log("[DEMO] Waiting for leader to send its altitude");
sleep(1000);
}
while (me.droneDict[leaderId].altitudeAbs < initialAltitude) {
console.log(
"[DEMO] Waiting for leader to reach altitude " + initialAltitude
+ " (currently " + me.droneDict[leaderId].altitudeAbs + ")"
);
sleep(1000);
}
console.log("[DEMO] Switching to following mode...\n");
do {
leaderAltitudeAbs = me.droneDict[leaderId].altitudeAbs;
leaderAltitudeRel = me.droneDict[leaderId].altitudeRel;
leaderLatitude = me.droneDict[leaderId].latitude;
leaderLongitude = me.droneDict[leaderId].longitude;
me.setTargetCoordinates(
leaderLatitude,
leaderLongitude,
leaderAltitudeAbs + altitudeDiff,
30.001
);
sleep(500);
} while (leaderAltitudeRel > PARACHUTE_ALTITUDE);
console.log("[DEMO] Stop following...\n");
}
me.onStart = function () {
INITIAL_ALTITUDE = me.getInitialAltitude();
START_ALTITUDE = INITIAL_ALTITUDE + FLIGH_ALTITUDE;
goToAltitude(START_ALTITUDE, true, true);
waitForAltitude(START_ALTITUDE);
console.log("[DEMO] Setting loiter mode...\n");
me.loiter();
sleep(3000);
};
me.onUpdate = function () {
if (!IS_LEADER) {
followLeader(LEADER_ID, START_ALTITUDE, ALTITUDE_DIFF);
}
console.log("[DEMO] Loitering until manual intructions are given\n");
while (!me.landed()) {
sleep(1000);
}
};
}(console, me));
multicopter_swarm.js 0 → 100644
View file @ e566cfb6
/*
* In this flight all drone take off when start signal is sent. The leader
* follows continuously a list of checkpoints. The other drones follow each
* other (second drone follows leader, third drone follows second drone, ...).
* When the switch signal is sent, the last drone becomes the leader and follow
* the checkpoints in the reversed order. When stop signal is sent, each drone
* lands one at the time from the last one to the leader.
*/
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, me*/
(function (console, me) {
"use strict";
var ALTITUDE_DIFF = 5,
BASE_ALTITUDE = 25,
CHECKPOINT_LIST = [
{
latitude: 45.133816745111254,
longitude: 5.681756602776511
},
{
latitude: 45.13412766661184,
longitude: 5.681916517286368
},
{
latitude: 45.13399163453284,
longitude: 5.682459947632148
},
{
latitude: 45.133670102874454,
longitude: 5.68237448882777
}
],
EPSILON = 4,
PARACHUTE_POINT_ARRAY = [
{
latitude: 45.13342642944715,
longitude: 5.681879999829597
},
{
latitude: 45.133244783529136,
longitude: 5.681845964441365
},
{
latitude: 45.1333696,
longitude: 5.6817394
}
],
TARGETED_DISTANCE = 10,
TAKE_OFF_POINT = {
latitude: 45.13342642944715,
longitude: 5.681879999829597
},
MIN_CMD_INTERVAL = 500,
MIN_SPEED = 4,
DEFAULT_SPEED = 5,
MAX_SPEED = 6,
R = 6371e3,
SPEED_FACTOR = 0.2;
function exitOnFail(ret, msg) {
if (ret) {
console.log(msg);
me.exit(1);
}
}
function sendLandingMessage(drone) {
console.log("Landing");
drone.sendMsg(JSON.stringify(
{id: drone.id, inAir: false, state: "Landed", type: "state"}
));
}
function land(drone) {
console.log("Landing");
sendLandingMessage(drone);
exitOnFail(drone.land(), "Failed to land");
}
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) {
var id_list = drone.id_list;
return (!drone.reverse) ? id_list[0] : id_list[id_list.length - 1];
}
function pointReached(drone, point, acceptance) {
var current_position = drone.getCurrentPosition(),
dist = distance(
current_position.latitude,
current_position.longitude,
point.latitude,
point.longitude
);
console.log("Distance to point", dist);
return dist <= acceptance;
}
function goTo(drone, latitude, longitude, speed, timestamp) {
if (timestamp - drone.last_cmd_timestamp >= MIN_CMD_INTERVAL) {
drone.setTargetCoordinates(
latitude,
longitude,
drone.flightAltitude,
speed
);
drone.last_cmd_timestamp = timestamp;
}
}
me.onStart = function () {
me.direction_set = false;
me.flightAltitude = BASE_ALTITUDE + me.id * ALTITUDE_DIFF;
me.following = false;
me.going_to_parachute = false;
me.id_list = Object.keys(me.getDroneDict()).map((x) => Number(x));
me.id_list.sort();
me.landing = false;
me.last_cmd_timestamp = 0;
me.next_checkpoint = 0;
me.started = false;
me.stopped = false;
me.reverse = false;
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],
neighbor_position,
newSpeed,
position = me.getCurrentPosition();
if (me.isLanding()) {
return sendLandingMessage(me);
}
if (!me.started) {
if (leader_id !== me.id) {
console.log(
"timestamp difference",
position.timestamp - neighbor_position.timestamp
);
}
return;
}
if (!me.landing) {
if (!me.isReadyToFly()) {
return console.log("Taking off");
}
if (leader_id !== me.id) {
neighbor_position = me.getDroneDict()[neighbor_id];
distance2d = distance(
position.latitude,
position.longitude,
neighbor_position.latitude,
neighbor_position.longitude
);
if (!me.following) {
distanceToTakeOffPoint = distance(
neighbor_position.latitude,
neighbor_position.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 - neighbor_position.timestamp
) >= 1000) {
console.log(
"timestamp difference",
position.timestamp - neighbor_position.timestamp
);
return;
}
distanceDiff = distance2d - TARGETED_DISTANCE;
newSpeed = Math.max(
Math.min(distanceDiff * SPEED_FACTOR + DEFAULT_SPEED, MAX_SPEED),
MIN_SPEED
);
goTo(
me,
neighbor_position.latitude,
neighbor_position.longitude,
newSpeed,
timestamp
);
return console.log("distance to leader", distance2d);
}
checkpointIndex = (!me.reverse) ? me.next_checkpoint
: CHECKPOINT_LIST.length - me.next_checkpoint - 1;
if (!me.direction_set) {
console.log("Going to Checkpoint", checkpointIndex);
me.next_point = {
"latitude": CHECKPOINT_LIST[checkpointIndex].latitude,
"longitude": CHECKPOINT_LIST[checkpointIndex].longitude
};
me.direction_set = true;
}
if (
pointReached(me, CHECKPOINT_LIST[checkpointIndex], 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
}));
}
}
if (me.going_to_parachute &&
pointReached(me, PARACHUTE_POINT_ARRAY[me.id], EPSILON)) {
me.going_to_parachute = false;
return land(me);
}
if (!me.landing || me.going_to_parachute) {
goTo(
me,
me.next_point.latitude,
me.next_point.longitude,
DEFAULT_SPEED,
timestamp
);
}
};
me.onGetMsg = function (msg) {
var msgDict = JSON.parse(msg);
if (msgDict.hasOwnProperty("status")) {
switch (msgDict.status) {
case "running":
me.started = true;
console.log("running");
me.takeOff();
me.sendMsg(JSON.stringify(
{id: me.id, inAir: true, state: "Flying", type: "state"}
));
break;
case "stopped":
me.stopped = true;
if (me.id === me.id_list[0]) {
me.landing = true;
me.going_to_parachute = true;
me.next_point = {
"latitude": PARACHUTE_POINT_ARRAY[me.id].latitude,
"longitude": PARACHUTE_POINT_ARRAY[me.id].longitude
};
console.log("Going to rally");
}
break;
case "switch":
me.reverse = !me.reverse;
me.direction_set = false;
break;
default:
console.log("Unknown status:", me.msgDict.status);
}
return;
}
switch (msgDict.type) {
case "checkpoint":
me.next_checkpoint = msgDict.next_checkpoint;
break;
case "state":
if (msgDict.state === "Landed" && me.id_list.includes(msgDict.id)) {
me.id_list.splice(me.id_list.indexOf(msgDict.id), 1);
if (me.stopped && me.id === me.id_list[0]) {
me.landing = true;
me.going_to_parachute = true;
me.next_point = {
"latitude": PARACHUTE_POINT_ARRAY[me.id].latitude,
"longitude": PARACHUTE_POINT_ARRAY[me.id].longitude
};
}
}
break;
default:
console.log("Unknown message type: " + msgDict.type);
}
};
}(console, me));
restricted_flight.js deleted 100644 → 0
View file @ bf6a0a5b
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, me*/
(function (console, me) {
"use strict";
var ALTITUDE_DIFF = 40,
EPSILON = 9,
EPSILON_ALTITUDE = 7,
FLIGH_ALTITUDE = 120,
LANDING_POINT = {
latitude: 45.6398451,
longitude: 14.2699217
},
LEADER_ID = 0,
IS_LEADER = me.id === LEADER_ID,
PARACHUTE_ALTITUDE = 100,
POINT_A = {
latitude: 45.650833,
longitude: 14.259722,
altitude: 606
},
POINT_B = {
latitude: 45.644167,
longitude: 14.263611,
altitude: 607
},
ROUND_NB = 1;
function altitudeReached(altitude, target_altitude) {
console.log(
"[DEMO] Waiting for altitude... ("
+ altitude + " , " + target_altitude + ")"
);
return Math.abs(altitude - target_altitude) < EPSILON_ALTITUDE;
}
function distance(lat1, lon1, lat2, lon2) {
var R = 6371e3, // meters
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 exitOnFail(ret, msg) {
if (ret) {
console.log(msg);
me.exit(1);
}
}
function leaderStartAltitude(drone) {
return drone.start_altitude - ALTITUDE_DIFF * me.id;
}
function leaderReachedInitAltitude(drone) {
return drone.drone_dict[LEADER_ID].altitudeAbs >= leaderStartAltitude(me);
}
me.onStart = function () {
me.going_to_point_a = false;
me.going_to_landing_point = false;
me.init_alt_reached = false;
me.landing = false;
me.landing_altitude = me.getInitialAltitude() + PARACHUTE_ALTITUDE;
me.parachute_triggered = false;
me.round_count = 0;
me.start_altitude = me.getInitialAltitude() + FLIGH_ALTITUDE;
if (!IS_LEADER) {
me.follow_leader = true;
me.leader_init_alt_reached = false;
me.start_altitude += ALTITUDE_DIFF * me.id;
}
exitOnFail(
me.setAltitude(me.start_altitude + 1),
"Failed to set start altitude"
);
};
me.onUpdate = function (timestamp) {
if (!me.init_alt_reached) {
me.init_alt_reached = altitudeReached(
me.getAltitudeAbs(),
me.start_altitude
);
if (me.init_alt_reached && IS_LEADER) {
exitOnFail(
me.setTargetCoordinates(
POINT_A.latitude,
POINT_A.longitude,
POINT_A.altitude + FLIGH_ALTITUDE
),
"Failed to set point B coordinates"
);
me.going_to_point_a = true;
me.sendMsg(JSON.stringify({going_to_point_a: me.going_to_point_a}));
}
return;
}
if (!IS_LEADER && me.follow_leader) {
if (me.drone_dict[LEADER_ID].altitudeAbs === 0) {
return console.log("[DEMO] Waiting for leader to send its altitude");
}
if (!me.leader_init_alt_reached) {
me.leader_init_alt_reached = leaderReachedInitAltitude(me);
return console.log(
"[DEMO] Waiting for leader to reach altitude "
+ leaderStartAltitude(me)
+ "(currently " + me.drone_dict[LEADER_ID].altitudeAbs + ")"
);
}
if (me.drone_dict[LEADER_ID].altitudeRel > PARACHUTE_ALTITUDE) {
exitOnFail(
me.setTargetCoordinates(
me.drone_dict[LEADER_ID].latitude,
me.drone_dict[LEADER_ID].longitude,
me.drone_dict[LEADER_ID].altitudeAbs + ALTITUDE_DIFF * me.id
),
"Failed to follow leader"
);
} else {
me.follow_leader = false;
console.log("[DEMO] Stop following...\n");
}
return;
}
if (!me.going_to_landing_point) {
me.current_position = me.getCurrentPosition();
me.distance = distance(
me.current_position.x,
me.current_position.y,
(me.going_to_point_a) ? POINT_A.latitude : POINT_B.latitude,
(me.going_to_point_a) ? POINT_A.longitude : POINT_B.longitude
);
if (me.distance > EPSILON) {
console.log(
"Waiting for drone to get to destination (" + me.distance + " m)"
);
} else {
if (me.going_to_point_a) {
exitOnFail(
me.setTargetCoordinates(
POINT_B.latitude,
POINT_B.longitude,
POINT_B.altitude + FLIGH_ALTITUDE
),
"Failed to set point B coordinates"
);
me.going_to_point_a = false;
me.sendMsg(JSON.stringify({going_to_point_a: me.going_to_point_a}));
} else {
me.round_count += 1;
if (me.round_count < ROUND_NB) {
exitOnFail(
me.setTargetCoordinates(
POINT_A.latitude,
POINT_A.longitude,
POINT_A.altitude + FLIGH_ALTITUDE
),
"Failed to set point A coordinates"
);
me.going_to_point_a = true;
me.sendMsg(JSON.stringify({going_to_point_a: me.going_to_point_a}));
} else {
exitOnFail(
me.setTargetCoordinates(
LANDING_POINT.latitude,
LANDING_POINT.longitude,
me.landing_altitude
),
"Failed to set landing point coordinates"
);
me.going_to_landing_point = true;
}
}
}
return;
}
if (!me.landing) {
me.current_position = me.getCurrentPosition();
me.distance = distance(
me.current_position.x,
me.current_position.y,
LANDING_POINT.latitude,
LANDING_POINT.longitude
);
if (me.distance > EPSILON) {
console.log(
"[DEMO] Waiting to reach landing point (current distance is "
+ me.distance + ")"
);
} else {
console.log("[DEMO] Landing...\n");
me.landing = true;
}
return;
}
if (!me.parachute_triggered) {
console.log("[DEMO] Deploying parachute...");
exitOnFail(me.triggerParachute(), "Failed to deploy parachute");
me.parachute_triggered = true;
}
if (me.landed()) {
me.exit(0);
}
};
me.onGetMsg = function (msg) {
me.msgDict = JSON.parse(msg);
if (me.follow_leader && me.msgDict.hasOwnProperty("going_to_point_a")) {
me.going_to_point_a = me.msgDict.going_to_point_a;
}
};
}(console, me));
simulator-script.js
View file @ e566cfb6
/*
* This is the default script in the simulator. All drones follow the same
* path of checkpoints but at different altitudes.
*/
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, me*/
......
subscribe.js
View file @ e566cfb6
/*
* This is the script run by subscribers. It is used has a bridge between the
* swarm and the GUI.
*/
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, me*/
/*global me*/
(function (console, me) {
(function (me) {
"use strict";
me.onStart = function () {
me.f = me.fdopen(me.in, "r");
console.log("Use q to quit");
};
me.onUpdate = function (timestamp) {
if (me.f.getline() !== "q") {
return;
}
try {
me.f.close();
} catch (error) {
console.error(error);
var drone_dict = {};
Object.entries(me.getDroneDict()).forEach(function ([id, drone]) {
drone_dict[id] = {
latitude: drone.latitude.toFixed(6),
longitude: drone.longitude.toFixed(6),
altitude: drone.altitudeAbs.toFixed(2),
yaw: drone.yaw.toFixed(2),
speed: drone.speed.toFixed(2),
climbRate: drone.climbRate.toFixed(2),
timestamp: drone.timestamp,
log: drone.log
};
});
me.writeWebsocketMessage(JSON.stringify({drone_dict: drone_dict}));
}
me.onWebSocketMessage = function (msg) {
switch (msg) {
case "quit":
me.exit(0);
break;
case "start":
me.sendMsg(JSON.stringify({status: "running"}));
break;
case "stop":
me.sendMsg(JSON.stringify({status: "stopped"}));
break;
case "switch":
me.sendMsg(JSON.stringify({status: "switch"}));
break;
default:
me.writeWebsocketMessage("Unknown instruction " + msg);
};
}
me.onGetMsg = function (msg) {
me.writeWebsocketMessage(msg);
};
}(console, me));
}(me));
demo.js → v.js
View file @ e566cfb6
This diff is collapsed.
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