Skip to content

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

Comply to ERP5 JsLint

parent 8ca353c0
Hide whitespace changes
Inline Side-by-side
Showing with 684 additions and 663 deletions
bouncy-flight.js
View file @ 29e7a5d7
...@@ -93,10 +93,11 @@ ...@@ -93,10 +93,11 @@
function altitudeReached(altitude, target_altitude) { function altitudeReached(altitude, target_altitude) {
console.log( console.log(
`[DEMO] Waiting for altitude... (${altitude} , ${target_altitude})` "[DEMO] Waiting for altitude... ("
); + altitude + " , " + target_altitude + ")"
return Math.abs(altitude - target_altitude) < EPSILON_ALTITUDE; );
} return Math.abs(altitude - target_altitude) < EPSILON_ALTITUDE;
}
function distance(lat1, lon1, lat2, lon2) { function distance(lat1, lon1, lat2, lon2) {
var R = 6371e3, // meters var R = 6371e3, // meters
...@@ -109,32 +110,32 @@ ...@@ -109,32 +110,32 @@
h = haversine_phi + Math.cos(la1) * Math.cos(la2) * sin_lon * sin_lon; h = haversine_phi + Math.cos(la1) * Math.cos(la2) * sin_lon * sin_lon;
return 2 * R * Math.asin(Math.sqrt(h)); return 2 * R * Math.asin(Math.sqrt(h));
} }
function exit_on_fail(ret, msg) { function exit_on_fail(ret, msg) {
if (ret) { if (ret) {
console.log(msg); console.log(msg);
me.exit(1); me.exit(1);
} }
} }
function mustWait(timestamp) { function mustWait(timestamp) {
if (me.timestamp === 0) { if (me.timestamp === 0) {
me.timestamp = timestamp; me.timestamp = timestamp;
} }
return timestamp - me.timestamp < me.must_wait; return timestamp - me.timestamp < me.must_wait;
} }
function groundLevel(drone) { function groundLevel(drone) {
return drone.getAltitudeAbs() - drone.getCurrentPosition().z; return drone.getAltitudeAbs() - drone.getCurrentPosition().z;
} }
me.onStart = function () { me.onStart = function () {
me.direction_set = false; me.direction_set = false;
me.landing_alt_reached = false; me.landing_alt_reached = false;
me.next_checkpoint = 0; me.next_checkpoint = 0;
me.parachute_triggered = false; me.parachute_triggered = false;
}; };
me.onUpdate = function (timestamp) { me.onUpdate = function (timestamp) {
if (me.must_wait > 0) { if (me.must_wait > 0) {
if (!mustWait(timestamp)) { if (!mustWait(timestamp)) {
...@@ -154,7 +155,7 @@ ...@@ -154,7 +155,7 @@
), ),
"Failed to set checkpoint coordinates" "Failed to set checkpoint coordinates"
); );
console.log(`[DEMO] Going to Checkpoint ${me.next_checkpoint}\n`); console.log("[DEMO] Going to Checkpoint " + me.next_checkpoint + "\n");
} else { } else {
me.loiter(); me.loiter();
console.log("[DEMO] Going to landing altitude...\n"); console.log("[DEMO] Going to landing altitude...\n");
...@@ -167,7 +168,7 @@ ...@@ -167,7 +168,7 @@
me.direction_set = true; me.direction_set = true;
return; return;
} }
if (me.next_checkpoint < CHECKPOINT_LIST.length) { if (me.next_checkpoint < CHECKPOINT_LIST.length) {
me.current_position = me.getCurrentPosition(); me.current_position = me.getCurrentPosition();
me.distance = distance( me.distance = distance(
...@@ -178,9 +179,10 @@ ...@@ -178,9 +179,10 @@
); );
if (me.distance > EPSILON) { if (me.distance > EPSILON) {
console.log( console.log(
`Waiting for drone to get to destination (${me.distance} m)`); "Waiting for drone to get to destination (" + me.distance + " m)"
);
} else { } else {
console.log(`[DEMO] Reached Checkpoint ${me.next_checkpoint}\n`); console.log("[DEMO] Reached Checkpoint " + me.next_checkpoint + "\n");
me.next_checkpoint += 1; me.next_checkpoint += 1;
me.direction_set = false; me.direction_set = false;
} }
......
cli.js
View file @ 29e7a5d7
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */ /*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, me*/ /*global console, me*/
(function (console, me) { (function (console, me) {
"use strict"; "use strict";
function displayMessage(message) { function displayMessage(message) {
console.log(message); console.log(message);
return 0; return 0;
} }
var help = ["altitude(altitude)", "exit", var help = ["altitude(altitude)", "exit",
"goto(latitude, longitude, altitude)", "help", "land", "loiter", "goto(latitude, longitude, altitude)", "help", "land", "loiter",
"positions", "speed(speed)"].join("\n"), "positions", "speed(speed)"].join("\n"),
wrongParameters = displayMessage.bind(null, "Wrong parameters"); wrongParameters = displayMessage.bind(null, "Wrong parameters");
function checkNumber(value, toExecute) { function checkNumber(value, toExecute) {
return ( return (
Number.isNaN(value) Number.isNaN(value)
? wrongParameters ? wrongParameters
: toExecute.bind(null, value) : toExecute.bind(null, value)
); );
} }
function displayDronePositions() { function displayDronePositions() {
Object.entries(me.droneDict).forEach(function ([id, drone]) { Object.entries(me.droneDict).forEach(function ([id, drone]) {
console.log( console.log(
id, id,
drone.latitude, drone.latitude,
drone.longitude, drone.longitude,
drone.altitudeAbs, drone.altitudeAbs,
drone.altitudeRel drone.altitudeRel
); );
}); });
return 0; return 0;
} }
function exit() { function exit() {
try { try {
me.f.close(); me.f.close();
} catch (error) { } catch (error) {
console.error(error); console.error(error);
} }
me.exit(0); me.exit(0);
} }
me.onStart = function () { me.onStart = function () {
me.f = me.fdopen(me.in, "r"); me.f = me.fdopen(me.in, "r");
console.log(help); console.log(help);
}; };
me.onUpdate = function () { me.onUpdate = function () {
var altitude, var altitude,
latitude, latitude,
longitude, longitude,
speed, speed,
user_input, user_input,
undefined_cmd = false, undefined_cmd = false,
cmd, cmd,
ret; ret;
console.log("> "); console.log("> ");
user_input = me.f.getline(); user_input = me.f.getline();
switch (user_input) { switch (user_input) {
case "altitude": case "altitude":
console.log("Altitude: "); console.log("Altitude: ");
altitude = parseFloat(me.f.getline()); altitude = parseFloat(me.f.getline());
cmd = checkNumber(altitude, me.setAltitude); cmd = checkNumber(altitude, me.setAltitude);
break; break;
case "exit": case "exit":
cmd = exit; cmd = exit;
break; break;
case "goto": case "goto":
console.log("Latitude: "); console.log("Latitude: ");
latitude = parseFloat(me.f.getline()); latitude = parseFloat(me.f.getline());
console.log("Longitude: "); console.log("Longitude: ");
longitude = parseFloat(me.f.getline()); longitude = parseFloat(me.f.getline());
console.log("Altitude: "); console.log("Altitude: ");
altitude = parseFloat(me.f.getline()); altitude = parseFloat(me.f.getline());
cmd = checkNumber( cmd = checkNumber(
altitude, altitude,
checkNumber(longitude, checkNumber(latitude, me.setTargetCoordinates)) checkNumber(longitude, checkNumber(latitude, me.setTargetCoordinates))
); );
break; break;
case "help": case "help":
cmd = displayMessage.bind(null, help); cmd = displayMessage.bind(null, help);
break; break;
case "land": case "land":
cmd = me.triggerParachute; cmd = me.triggerParachute;
break; break;
case "loiter": case "loiter":
cmd = me.loiter; cmd = me.loiter;
break; break;
case "positions": case "positions":
cmd = displayDronePositions; cmd = displayDronePositions;
break; break;
case "speed": case "speed":
console.log("Speed: "); console.log("Speed: ");
speed = parseFloat(me.f.getline()); speed = parseFloat(me.f.getline());
cmd = checkNumber(speed, me.setAirspeed); cmd = checkNumber(speed, me.setAirspeed);
break; break;
default: default:
undefined_cmd = true; undefined_cmd = true;
cmd = displayMessage.bind(null, " Undefined command"); cmd = displayMessage.bind(null, " Undefined command");
} }
ret = cmd(); ret = cmd();
if (ret) { if (ret) {
console.log(" [ERROR] function:\n", cmd, "\nreturn value:", ret); console.log(" [ERROR] function:\n", cmd, "\nreturn value:", ret);
} else if (user_input !== "help" && !undefined_cmd) { } else if (user_input !== "help" && !undefined_cmd) {
console.log(" Command successful"); console.log(" Command successful");
} }
}; };
}(console, me)); }(console, me));
\ No newline at end of file
default.js
View file @ 29e7a5d7
me.onStart = function() { /*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
me.exit(0); /*global me*/
};
(function (me) {
"use strict";
me.onStart = function () {
me.exit(0);
};
}(me));
demo.js
View file @ 29e7a5d7
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */ /*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, me*/ /*global console, me*/
(function (console, me) { (function (console, me) {
"use strict"; "use strict";
var ALTITUDE_DIFF = 30, var ALTITUDE_DIFF = 30,
FLIGH_ALTITUDE = 100, FLIGH_ALTITUDE = 100,
PARACHUTE_ALTITUDE = 35, PARACHUTE_ALTITUDE = 35,
EPSILON = 9, EPSILON = 9,
EPSILON_ALTITUDE = 7, EPSILON_ALTITUDE = 7,
CHECKPOINT_LIST = [ CHECKPOINT_LIST = [
{ {
altitude: 585.1806861589965, altitude: 585.1806861589965,
latitude: 45.64492790560583, latitude: 45.64492790560583,
longitude: 14.25334942966329 longitude: 14.25334942966329
}, },
{ {
altitude: 589.8802607573035, altitude: 589.8802607573035,
latitude: 45.64316335436476, latitude: 45.64316335436476,
longitude: 14.26332880184475 longitude: 14.26332880184475
}, },
{ {
altitude: 608.6648153348965, altitude: 608.6648153348965,
latitude: 45.64911917196595, latitude: 45.64911917196595,
longitude: 14.26214792790128 longitude: 14.26214792790128
}, },
{ {
altitude: 606.1448368129072, altitude: 606.1448368129072,
latitude: 45.64122685351364, latitude: 45.64122685351364,
longitude: 14.26590493128597 longitude: 14.26590493128597
}, },
{ {
altitude: 630.0829598206344, altitude: 630.0829598206344,
latitude: 45.64543355564817, latitude: 45.64543355564817,
longitude: 14.27242391207985 longitude: 14.27242391207985
}, },
{ {
altitude: 616.1839898415284, altitude: 616.1839898415284,
latitude: 45.6372792927328, latitude: 45.6372792927328,
longitude: 14.27533492411138 longitude: 14.27533492411138
}, },
{ {
altitude: 598.0603137354178, altitude: 598.0603137354178,
latitude: 45.64061299543953, latitude: 45.64061299543953,
longitude: 14.26161958465814 longitude: 14.26161958465814
}, },
{ {
altitude: 607.1243119862851, altitude: 607.1243119862851,
latitude: 45.64032340702919, latitude: 45.64032340702919,
longitude: 14.2682896662383 longitude: 14.2682896662383
} }
], ],
LANDING_POINT = { LANDING_POINT = {
latitude: 45.6398451, latitude: 45.6398451,
longitude: 14.2699217 longitude: 14.2699217
}, },
LEADER_ID = 0, LEADER_ID = 0,
IS_LEADER = me.id === LEADER_ID; IS_LEADER = me.id === LEADER_ID;
function altitudeReached(altitude, target_altitude) { function altitudeReached(altitude, target_altitude) {
console.log( console.log(
`[DEMO] Waiting for altitude... (${altitude} , ${target_altitude})`); "[DEMO] Waiting for altitude... ("
return Math.abs(altitude - target_altitude) < EPSILON_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 function distance(lat1, lon1, lat2, lon2) {
la2 = lat2 * Math.PI / 180, var R = 6371e3, // meters
lo1 = lon1 * Math.PI / 180, la1 = lat1 * Math.PI / 180, // la, lo in radians
lo2 = lon2 * Math.PI / 180, la2 = lat2 * Math.PI / 180,
haversine_phi = Math.pow(Math.sin((la2 - la1) / 2), 2), lo1 = lon1 * Math.PI / 180,
sin_lon = Math.sin((lo2 - lo1) / 2), lo2 = lon2 * Math.PI / 180,
h = haversine_phi + Math.cos(la1) * Math.cos(la2) * sin_lon * sin_lon; haversine_phi = Math.pow(Math.sin((la2 - la1) / 2), 2),
return 2 * R * Math.asin(Math.sqrt(h)); 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); function exitOnFail(ret, msg) {
me.exit(1); if (ret) {
} console.log(msg);
} me.exit(1);
}
function mustWait(timestamp) { }
if (me.timestamp === 0) {
me.timestamp = timestamp; function mustWait(timestamp) {
} if (me.timestamp === 0) {
return timestamp - me.timestamp < me.must_wait; me.timestamp = timestamp;
} }
return timestamp - me.timestamp < me.must_wait;
function leaderStartAltitude(drone) { }
return drone.start_altitude - ALTITUDE_DIFF * me.id;
} function leaderStartAltitude(drone) {
return drone.start_altitude - ALTITUDE_DIFF * me.id;
function leaderReachedInitAltitude(drone) { }
return drone.drone_dict[LEADER_ID].altitudeAbs >= leaderStartAltitude(me);
} function leaderReachedInitAltitude(drone) {
return drone.drone_dict[LEADER_ID].altitudeAbs >= leaderStartAltitude(me);
function groundLevel(drone) { }
return drone.getAltitudeAbs() - drone.getCurrentPosition().z;
} function groundLevel(drone) {
return drone.getAltitudeAbs() - drone.getCurrentPosition().z;
me.onStart = function () { }
me.direction_set = false;
me.going_to_landing_point = false; me.onStart = function () {
me.init_alt_reached = false; me.direction_set = false;
me.landing = false; me.going_to_landing_point = false;
me.landing_alt_reached = false; me.init_alt_reached = false;
me.landing_point_reached = false; me.landing = false;
me.last_checkpoint_reached = false; me.landing_alt_reached = false;
me.must_wait = 3000; me.landing_point_reached = false;
me.next_checkpoint = 0; me.last_checkpoint_reached = false;
me.parachute_triggered = false; me.must_wait = 3000;
me.start_altitude = me.getInitialAltitude() + FLIGH_ALTITUDE; me.next_checkpoint = 0;
me.timestamp = 0; me.parachute_triggered = false;
me.start_altitude = me.getInitialAltitude() + FLIGH_ALTITUDE;
if (!IS_LEADER) { me.timestamp = 0;
me.follow_leader = true;
me.leader_init_alt_reached = false; if (!IS_LEADER) {
me.start_altitude += ALTITUDE_DIFF * me.id; me.follow_leader = true;
me.must_wait = 0; me.leader_init_alt_reached = false;
} me.start_altitude += ALTITUDE_DIFF * me.id;
exitOnFail( me.must_wait = 0;
me.setAltitude(me.start_altitude + 1), }
"Failed to set start altitude" 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.onUpdate = function (timestamp) {
me.getAltitudeAbs(), if (!me.init_alt_reached) {
me.start_altitude me.init_alt_reached = altitudeReached(
); me.getAltitudeAbs(),
return; me.start_altitude
} );
return;
if (me.must_wait > 0) { }
if (!mustWait(timestamp)) {
me.must_wait = 0; if (me.must_wait > 0) {
me.timestamp = 0; if (!mustWait(timestamp)) {
} me.must_wait = 0;
return; me.timestamp = 0;
} }
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 (!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( if (!me.leader_init_alt_reached) {
"[DEMO] Waiting for leader to reach altitude " + me.leader_init_alt_reached = leaderReachedInitAltitude(me);
leaderStartAltitude(me) + return console.log(
`(currently ${me.drone_dict[LEADER_ID].altitudeAbs})`, "[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( if (me.drone_dict[LEADER_ID].altitudeRel > PARACHUTE_ALTITUDE) {
me.drone_dict[LEADER_ID].latitude, exitOnFail(
me.drone_dict[LEADER_ID].longitude, me.setTargetCoordinates(
me.drone_dict[LEADER_ID].altitudeAbs + ALTITUDE_DIFF * me.id me.drone_dict[LEADER_ID].latitude,
), me.drone_dict[LEADER_ID].longitude,
"Failed to follow leader" me.drone_dict[LEADER_ID].altitudeAbs + ALTITUDE_DIFF * me.id
); ),
} else { "Failed to follow leader"
me.follow_leader = false; );
console.log("[DEMO] Stop following...\n"); } else {
} me.follow_leader = false;
return; console.log("[DEMO] Stop following...\n");
} }
return;
if (!me.direction_set) { }
if (me.next_checkpoint < CHECKPOINT_LIST.length) {
exitOnFail( if (!me.direction_set) {
me.setTargetCoordinates( if (me.next_checkpoint < CHECKPOINT_LIST.length) {
CHECKPOINT_LIST[me.next_checkpoint].latitude, exitOnFail(
CHECKPOINT_LIST[me.next_checkpoint].longitude, me.setTargetCoordinates(
CHECKPOINT_LIST[me.next_checkpoint].altitude + FLIGH_ALTITUDE CHECKPOINT_LIST[me.next_checkpoint].latitude,
), CHECKPOINT_LIST[me.next_checkpoint].longitude,
"Failed to set checkpoint coordinates" CHECKPOINT_LIST[me.next_checkpoint].altitude + FLIGH_ALTITUDE
); ),
console.log(`[DEMO] Going to Checkpoint ${me.next_checkpoint}\n`); "Failed to set checkpoint coordinates"
} else { );
console.log("[DEMO] Setting last checkpoint coordinates...\n"); console.log("[DEMO] Going to Checkpoint " + me.next_checkpoint + "\n");
me.landing_altitude = groundLevel(me) + PARACHUTE_ALTITUDE; } else {
exitOnFail( console.log("[DEMO] Setting last checkpoint coordinates...\n");
me.setTargetCoordinates( me.landing_altitude = groundLevel(me) + PARACHUTE_ALTITUDE;
CHECKPOINT_LIST[CHECKPOINT_LIST.length - 1].latitude, exitOnFail(
CHECKPOINT_LIST[CHECKPOINT_LIST.length - 1].longitude, me.setTargetCoordinates(
me.landing_altitude CHECKPOINT_LIST[CHECKPOINT_LIST.length - 1].latitude,
), CHECKPOINT_LIST[CHECKPOINT_LIST.length - 1].longitude,
"Failed to set last checkpoint coordinates" me.landing_altitude
); ),
} "Failed to set last checkpoint coordinates"
me.direction_set = true; );
return; }
} me.direction_set = true;
return;
if (me.next_checkpoint < CHECKPOINT_LIST.length) { }
me.current_position = me.getCurrentPosition();
me.distance = distance( if (me.next_checkpoint < CHECKPOINT_LIST.length) {
me.current_position.x, me.current_position = me.getCurrentPosition();
me.current_position.y, me.distance = distance(
CHECKPOINT_LIST[me.next_checkpoint].latitude, me.current_position.x,
CHECKPOINT_LIST[me.next_checkpoint].longitude me.current_position.y,
); CHECKPOINT_LIST[me.next_checkpoint].latitude,
if (me.distance > EPSILON) { CHECKPOINT_LIST[me.next_checkpoint].longitude
console.log( );
"Waiting for drone to get to destination (checkpoint " + if (me.distance > EPSILON) {
`${me.next_checkpoint}: ${me.distance} m)`, console.log(
); "Waiting for drone to get to destination (checkpoint "
} else { + me.next_checkpoint + ": " + me.distance + " m)`"
me.loiter(100); );
console.log(`[DEMO] Reached Checkpoint ${me.next_checkpoint}\n`); } else {
me.next_checkpoint += 1; me.loiter(100);
me.sendMsg(JSON.stringify({next_checkpoint: me.next_checkpoint})); console.log("[DEMO] Reached Checkpoint " + me.next_checkpoint + "\n");
me.direction_set = false; me.next_checkpoint += 1;
me.must_wait = 50000; me.sendMsg(JSON.stringify({next_checkpoint: me.next_checkpoint}));
} me.direction_set = false;
return; me.must_wait = 50000;
} }
return;
if (!me.last_checkpoint_reached) { }
me.current_position = me.getCurrentPosition();
me.distance = distance( if (!me.last_checkpoint_reached) {
me.current_position.x, me.current_position = me.getCurrentPosition();
me.current_position.y, me.distance = distance(
CHECKPOINT_LIST[CHECKPOINT_LIST.length - 1].latitude, me.current_position.x,
CHECKPOINT_LIST[CHECKPOINT_LIST.length - 1].longitude me.current_position.y,
); CHECKPOINT_LIST[CHECKPOINT_LIST.length - 1].latitude,
if (me.distance > EPSILON) { CHECKPOINT_LIST[CHECKPOINT_LIST.length - 1].longitude
console.log( );
"Waiting for drone to get to destination (last checkpoint: " + if (me.distance > EPSILON) {
`${me.distance} m)`, console.log(
); "Waiting for drone to get to destination (last checkpoint: "
} else { + me.distance + " m)"
me.loiter(100); );
me.last_checkpoint_reached = true; } else {
} me.loiter(100);
return; me.last_checkpoint_reached = true;
} }
return;
if (!me.landing_alt_reached) { }
me.landing_alt_reached = altitudeReached(
me.getAltitudeAbs(), if (!me.landing_alt_reached) {
me.landing_altitude me.landing_alt_reached = altitudeReached(
); me.getAltitudeAbs(),
me.setAltitude(me.landing_altitude); me.landing_altitude
return; );
} me.setAltitude(me.landing_altitude);
return;
if (!me.going_to_landing_point) { }
exitOnFail(
me.setTargetCoordinates( if (!me.going_to_landing_point) {
LANDING_POINT.latitude, exitOnFail(
LANDING_POINT.longitude, me.setTargetCoordinates(
me.landing_altitude LANDING_POINT.latitude,
), LANDING_POINT.longitude,
"Failed to set landing point coordinates" me.landing_altitude
); ),
me.going_to_landing_point = true; "Failed to set landing point coordinates"
return; );
} me.going_to_landing_point = true;
return;
if (!me.landing) { }
me.current_position = me.getCurrentPosition();
me.distance = distance( if (!me.landing) {
me.current_position.x, me.current_position = me.getCurrentPosition();
me.current_position.y, me.distance = distance(
LANDING_POINT.latitude, me.current_position.x,
LANDING_POINT.longitude me.current_position.y,
); LANDING_POINT.latitude,
if (me.distance > EPSILON) { LANDING_POINT.longitude
console.log( );
"[DEMO] Waiting to reach landing point (current distance is " + if (me.distance > EPSILON) {
`${me.distance})`, console.log(
); "[DEMO] Waiting to reach landing point (current distance is "
} else { + me.distance + ")"
console.log("[DEMO] Landing...\n"); );
me.landing = true; } else {
} console.log("[DEMO] Landing...\n");
return; me.landing = true;
} }
return;
if (!me.parachute_triggered) { }
console.log("[DEMO] Deploying parachute...");
exitOnFail(me.triggerParachute(), "Failed to deploy parachute"); if (!me.parachute_triggered) {
me.parachute_triggered = true; console.log("[DEMO] Deploying parachute...");
} exitOnFail(me.triggerParachute(), "Failed to deploy parachute");
me.parachute_triggered = true;
if (me.landed()) { }
me.exit(0);
} if (me.landed()) {
}; me.exit(0);
}
me.onGetMsg = function (msg) { };
me.msgDict = JSON.parse(msg);
if (me.follow_leader && me.msgDict.hasOwnProperty("next_checkpoint")) { me.onGetMsg = function (msg) {
me.next_checkpoint = me.msgDict.next_checkpoint; me.msgDict = JSON.parse(msg);
console.log("next checkpoint is", me.next_checkpoint); if (me.follow_leader && me.msgDict.hasOwnProperty("next_checkpoint")) {
} me.next_checkpoint = me.msgDict.next_checkpoint;
}; console.log("next checkpoint is", me.next_checkpoint);
}(console, me)); }
};
\ No newline at end of file }(console, me));
manual-flight.js
View file @ 29e7a5d7
/*jslint indent2 */ /*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console */ /*global console, me, sleep*/
const ALTITUDE_DIFF = 30; (function (console, me, sleep) {
const FLIGH_ALTITUDE = 100; "use strict";
const PARACHUTE_ALTITUDE = 35;
var ALTITUDE_DIFF = 30,
const EPSILON_ALTITUDE = 5; EPSILON_ALTITUDE = 5,
FLIGH_ALTITUDE = 100,
let INITIAL_ALTITUDE; INITIAL_ALTITUDE,
let START_ALTITUDE; LEADER_ID = 0,
IS_LEADER = me.id === LEADER_ID,
var leaderAltitudeAbs; PARACHUTE_ALTITUDE = 35,
var leaderAltitudeRel; START_ALTITUDE,
var leaderLatitude; leaderAltitudeAbs,
var leaderLongitude; leaderAltitudeRel,
leaderLatitude,
function waitForAltitude(target_altitude) { leaderLongitude;
var altitude = getAltitude();
while(Math.abs(altitude - target_altitude) > EPSILON_ALTITUDE) { function exit_on_fail(ret, msg) {
console.log( if (ret) {
`[DEMO] Waiting for altitude... (${altitude} , ${target_altitude})`); console.log(msg);
sleep(1000); me.exit(1);
altitude = getAltitude(); }
}
}
function goToAltitude(target_altitude, wait, go) {
if(go) {
exit_on_fail(
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(droneDict[leaderId].altitudeAbs == 0) {
console.log("[DEMO] Waiting for leader to send its altitude");
sleep(1000);
} }
while(droneDict[leaderId].altitudeAbs < initialAltitude) { function waitForAltitude(target_altitude) {
console.log(`[DEMO] Waiting for leader to reach altitude ${initialAltitude} (currently ${droneDict[leaderId].altitudeAbs})`); var altitude = me.getAltitude();
sleep(1000); while (Math.abs(altitude - target_altitude) > EPSILON_ALTITUDE) {
console.log(
"[DEMO] Waiting for altitude... ("
+ altitude + " , " + target_altitude + ")"
);
sleep(1000);
altitude = me.getAltitude();
}
} }
console.log("[DEMO] Switching to following mode...\n"); function goToAltitude(target_altitude, wait, go) {
do { if (go) {
leaderAltitudeAbs = droneDict[leaderId].altitudeAbs; exit_on_fail(
leaderAltitudeRel = droneDict[leaderId].altitudeRel; me.setAltitude(target_altitude),
leaderLatitude = droneDict[leaderId].latitude; "Failed to go to altitude " + target_altitude + " m"
leaderLongitude = droneDict[leaderId].longitude; );
}
setTargetCoordinates(
leaderLatitude, if (wait) {
leaderLongitude, waitForAltitude(target_altitude);
leaderAltitudeAbs + altitudeDiff, }
30.001
);
sleep(500);
} while(leaderAltitudeRel > PARACHUTE_ALTITUDE);
console.log("[DEMO] Stop following...\n");
}
function waitForAltitude(altitude) {
var curAltitude;
do {
sleep(1000);
curAltitude = getAltitude();
console.log(
`[DEMO] Waiting for altitude... (${curAltitude} , ${altitude})`);
} }
while(curAltitude < altitude);
}
me.onStart = function() {
INITIAL_ALTITUDE = getInitialAltitude();
START_ALTITUDE = INITIAL_ALTITUDE + FLIGH_ALTITUDE;
goToAltitude(altitude, true, true);
waitForAltitude(START_ALTITUDE);
console.log("[DEMO] Setting loiter mode...\n"); function followLeader(leaderId, initialAltitude, altitudeDiff) {
loiter(); goToAltitude(START_ALTITUDE + ALTITUDE_DIFF, false, true);
sleep(3000);
}; while (me.droneDict[leaderId].altitudeAbs === 0) {
console.log("[DEMO] Waiting for leader to send its altitude");
me.onUpdate = function() { sleep(1000);
if(!IS_LEADER) { }
followLeader(LEADER_ID, START_ALTITUDE, ALTITUDE_DIFF);
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");
} }
console.log("[DEMO] Loitering until manual intructions are given\n") me.onStart = function () {
while(!landed()) { INITIAL_ALTITUDE = me.getInitialAltitude();
sleep(1000); 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));
simulator-script.js
View file @ 29e7a5d7
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */ /*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, me*/ /*global console, me*/
(function (console, me) { (function (console, me) {
"use strict"; "use strict";
var ALTITUDE = 100, var ALTITUDE = 100,
EPSILON = 9, EPSILON = 9,
CHECKPOINT_LIST = [ CHECKPOINT_LIST = [
{ {
altitude: 585.1806861589965, altitude: 585.1806861589965,
latitude: 45.64492790560583, latitude: 45.64492790560583,
longitude: 14.25334942966329 longitude: 14.25334942966329
}, },
{ {
altitude: 589.8802607573035, altitude: 589.8802607573035,
latitude: 45.64316335436476, latitude: 45.64316335436476,
longitude: 14.26332880184475 longitude: 14.26332880184475
}, },
{ {
altitude: 608.6648153348965, altitude: 608.6648153348965,
latitude: 45.64911917196595, latitude: 45.64911917196595,
longitude: 14.26214792790128 longitude: 14.26214792790128
}, },
{ {
altitude: 606.1448368129072, altitude: 606.1448368129072,
latitude: 45.64122685351364, latitude: 45.64122685351364,
longitude: 14.26590493128597 longitude: 14.26590493128597
}, },
{ {
altitude: 630.0829598206344, altitude: 630.0829598206344,
latitude: 45.64543355564817, latitude: 45.64543355564817,
longitude: 14.27242391207985 longitude: 14.27242391207985
}, },
{ {
altitude: 616.1839898415284, altitude: 616.1839898415284,
latitude: 45.6372792927328, latitude: 45.6372792927328,
longitude: 14.27533492411138 longitude: 14.27533492411138
}, },
{ {
altitude: 598.0603137354178, altitude: 598.0603137354178,
latitude: 45.64061299543953, latitude: 45.64061299543953,
longitude: 14.26161958465814 longitude: 14.26161958465814
}, },
{ {
altitude: 607.1243119862851, altitude: 607.1243119862851,
latitude: 45.64032340702919, latitude: 45.64032340702919,
longitude: 14.2682896662383 longitude: 14.2682896662383
} }
]; ];
function distance(lat1, lon1, lat2, lon2) { function distance(lat1, lon1, lat2, lon2) {
var R = 6371e3, // meters var R = 6371e3, // meters
la1 = lat1 * Math.PI / 180, // lat, lon in radians la1 = lat1 * Math.PI / 180, // lat, lon in radians
la2 = lat2 * Math.PI / 180, la2 = lat2 * Math.PI / 180,
lo1 = lon1 * Math.PI / 180, lo1 = lon1 * Math.PI / 180,
lo2 = lon2 * Math.PI / 180, lo2 = lon2 * Math.PI / 180,
haversine_phi = Math.pow(Math.sin((la2 - la1) / 2), 2), haversine_phi = Math.pow(Math.sin((la2 - la1) / 2), 2),
sin_lon = Math.sin((lo2 - lo1) / 2), sin_lon = Math.sin((lo2 - lo1) / 2),
h = haversine_phi + Math.cos(la1) * Math.cos(la2) * sin_lon * sin_lon; h = haversine_phi + Math.cos(la1) * Math.cos(la2) * sin_lon * sin_lon;
return 2 * R * Math.asin(Math.sqrt(h)); return 2 * R * Math.asin(Math.sqrt(h));
} }
me.onStart = function () { me.onStart = function () {
me.direction_set = false; me.direction_set = false;
me.next_checkpoint = 0; me.next_checkpoint = 0;
me.parachute_triggered = false; me.parachute_triggered = false;
}; };
me.onUpdate = function (timestamp) { me.onUpdate = function (timestamp) {
if (!me.direction_set) { if (!me.direction_set) {
if (me.next_checkpoint < CHECKPOINT_LIST.length) { if (me.next_checkpoint < CHECKPOINT_LIST.length) {
me.setTargetCoordinates( me.setTargetCoordinates(
CHECKPOINT_LIST[me.next_checkpoint].latitude, CHECKPOINT_LIST[me.next_checkpoint].latitude,
CHECKPOINT_LIST[me.next_checkpoint].longitude, CHECKPOINT_LIST[me.next_checkpoint].longitude,
CHECKPOINT_LIST[me.next_checkpoint].altitude + ALTITUDE + ALTITUDE * me.id CHECKPOINT_LIST[me.next_checkpoint].altitude
); + ALTITUDE + ALTITUDE * me.id
console.log("[DEMO] Going to Checkpoint %d", me.next_checkpoint); );
} console.log("[DEMO] Going to Checkpoint %d", me.next_checkpoint);
me.direction_set = true; }
return; me.direction_set = true;
} return;
}
if (me.next_checkpoint < CHECKPOINT_LIST.length) {
me.current_position = me.getCurrentPosition(); if (me.next_checkpoint < CHECKPOINT_LIST.length) {
me.distance = distance( me.current_position = me.getCurrentPosition();
me.current_position.x, me.distance = distance(
me.current_position.y, me.current_position.x,
CHECKPOINT_LIST[me.next_checkpoint].latitude, me.current_position.y,
CHECKPOINT_LIST[me.next_checkpoint].longitude CHECKPOINT_LIST[me.next_checkpoint].latitude,
); CHECKPOINT_LIST[me.next_checkpoint].longitude
if (me.distance <= EPSILON) { );
console.log("[DEMO] Reached Checkpoint %d", me.next_checkpoint); if (me.distance <= EPSILON) {
me.next_checkpoint += 1; console.log("[DEMO] Reached Checkpoint %d", me.next_checkpoint);
me.direction_set = false; me.next_checkpoint += 1;
} me.direction_set = false;
return; }
} return;
}
if (!me.parachute_triggered) {
console.log("[DEMO] Deploying parachute..."); if (!me.parachute_triggered) {
me.triggerParachute(); console.log("[DEMO] Deploying parachute...");
me.parachute_triggered = true; me.triggerParachute();
} me.parachute_triggered = true;
}
if (me.landed()) {
me.exit(0); if (me.landed()) {
} me.exit(0);
}; }
}(console, me)); };
}(console, me));
\ No newline at end of file
subscribe.js
View file @ 29e7a5d7
me.onStart = function() { /*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
me.f = me.fdopen(me.in, "r"); /*global console, me*/
console.log("Use q to quit");
};
me.onUpdate= function(timestamp) { (function (console, me) {
while(me.f.getline() != "q") { "use strict";
continue;
} me.onStart = function () {
try { me.f = me.fdopen(me.in, "r");
me.f.close();; console.log("Use q to quit");
} catch (error) { };
console.error(error);
} me.onUpdate = function (timestamp) {
me.exit(0); if (me.f.getline() !== "q") {
}; return;
}
try {
me.f.close();
} catch (error) {
console.error(error);
}
me.exit(0);
};
}(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