Skip to content

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

Comply to JSLint

parent 5c0b63f0
Show whitespace changes
Inline Side-by-side
Showing with 434 additions and 406 deletions
cli.js
View file @ 5e57abdf
/*jslint indent2 */ /*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, std */ /*global console, me*/
const EPSILON_YAW = 6;
const EPSILON_ALTITUDE = 5;
const TARGET_YAW = 0;
var running = false;
const wrongParameters = displayMessage.bind(null, "Wrong parameters");
function displayDronePositions() {
if(!pubsubRunning)
console.log("You must start pubsub first !");
else {
for (const [id, drone] of Object.entries(droneDict)) {
console.log(id, drone.latitude, drone.longitude, drone.altitudeAbs, drone.altitudeRel);
}
}
return 0;
}
function land() { (function (console, me) {
var yaw; "use strict";
while(true) { function displayMessage(message) {
yaw = getYaw(); console.log(message);
console.log(`[DEMO] Waiting for yaw... (${yaw} , ${TARGET_YAW})`); return 0;
if(Math.abs(yaw - TARGET_YAW) < EPSILON_YAW) {
break;
}
sleep(250);
} }
console.log("[DEMO] Deploying parachute..."); var help = ["altitude(altitude)", "exit",
exit_on_fail(doParachute(2), "Failed to deploy parachute"); "goto(latitude, longitude, altitude)", "help", "land", "loiter",
} "positions", "speed(speed)"].join("\n"),
wrongParameters = displayMessage.bind(null, "Wrong parameters");
function waitForAltitude(target_altitude) { function checkNumber(value, toExecute) {
var altitude = getAltitude(); return (
while(Math.abs(altitude - target_altitude) > EPSILON_ALTITUDE) { Number.isNaN(value)
console.log( ? wrongParameters
`[DEMO] Waiting for altitude... (${altitude} , ${target_altitude})`); : toExecute.bind(null, value)
sleep(1000); );
altitude = getAltitude();
} }
}
function goToAltitude(target_altitude, wait, go) { function displayDronePositions() {
if(go) { Object.entries(me.droneDict).forEach(function ([id, drone]) {
exit_on_fail( console.log(
setAltitude(target_altitude), id,
`Failed to go to altitude ${target_altitude} m` drone.latitude,
drone.longitude,
drone.altitudeAbs,
drone.altitudeRel
); );
});
return 0;
} }
if(wait) { function exit() {
waitForAltitude(target_altitude); try {
me.f.close();
} catch (error) {
console.error(error);
}
me.exit(0);
} }
}
function checkNumber(value, toExecute) { me.onStart = function () {
return ( me.f = me.fdopen(me.in, "r");
Number.isNaN(value) console.log(help);
? wrongParameters };
: toExecute.bind(null, value)
);
}
function displayMessage(message) { me.onUpdate = function () {
console.log(message); var altitude,
return 0; latitude,
} longitude,
speed,
user_input,
undefined_cmd = false,
cmd,
ret;
function exit() { console.log("> ");
running = false; user_input = me.f.getline();
return 0;
} switch (user_input) {
function getInput() {
let undefined_cmd;
let altitude;
let cmd;
let latitude;
let longitude;
let s;
let speed;
const help = `
land
goto(point)
gotoCoord(latitude, longitude)
altitude(altitude)
speed(speed)
positions
reboot
exit
help
`;
const f = std.fdopen(std.in, "r");
running = true;
while (running) {
std.printf("> ");
s = f.getline();
undefined_cmd = false;
switch (s) {
case "altitude": case "altitude":
std.printf("Altitude: "); console.log("Altitude: ");
altitude = parseFloat(f.getline()); altitude = parseFloat(me.f.getline());
cmd = checkNumber(altitude, setAltitude); cmd = checkNumber(altitude, me.setAltitude);
break; break;
case "exit": case "exit":
cmd = exit; cmd = exit;
break; break;
/* case "gotoCoord": case "goto":
std.printf("Latitude: "); console.log("Latitude: ");
latitude = parseFloat(f.getline()); latitude = parseFloat(me.f.getline());
std.printf("Longitude: "); console.log("Longitude: ");
longitude = parseFloat(f.getline()); longitude = parseFloat(me.f.getline());
cmd = checkNumber(longitude, checkNumber(latitude, setTargetLatLong)); console.log("Altitude: ");
break;*/ altitude = parseFloat(me.f.getline());
cmd = checkNumber(
altitude,
checkNumber(longitude, checkNumber(latitude, me.setTargetCoordinates))
);
break;
case "help": case "help":
cmd = displayMessage.bind(null, help); cmd = displayMessage.bind(null, help);
break; break;
case "land": case "land":
cmd = land; cmd = me.triggerParachute;
break; break;
case "loiter": case "loiter":
cmd = loiter; cmd = me.loiter;
break; break;
case "positions": case "positions":
cmd = displayDronePositions; cmd = displayDronePositions;
break; break;
case "reboot":
cmd = reboot;
break;
case "speed": case "speed":
std.printf("Speed: "); console.log("Speed: ");
speed = parseFloat(f.getline()); speed = parseFloat(me.f.getline());
cmd = checkNumber(speed, setAirspeed); cmd = checkNumber(speed, me.setAirspeed);
break; break;
default: default:
...@@ -154,18 +113,12 @@ function getInput() { ...@@ -154,18 +113,12 @@ function getInput() {
cmd = displayMessage.bind(null, " Undefined command"); cmd = displayMessage.bind(null, " Undefined command");
} }
let 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 (s !== "help" && !undefined_cmd) {
console.log(" Command successful"); console.log(" Command successful");
} }
}; };
}(console, me));
\ No newline at end of file
f.close();
}
me.onUpdate = function() {
getInput();
};
demo.js
View file @ 5e57abdf
/*jslint indent2 */ /*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console */ /*global console, me*/
const ALTITUDE_DIFF = 30; (function (console, me) {
const FLIGH_ALTITUDE = 100; "use strict";
const PARACHUTE_ALTITUDE = 35;
var ALTITUDE_DIFF = 30,
const EPSILON = 105; FLIGH_ALTITUDE = 100,
const EPSILON_YAW = 6; PARACHUTE_ALTITUDE = 35,
const EPSILON_ALTITUDE = 5; EPSILON = 9,
const TARGET_YAW = 0; EPSILON_ALTITUDE = 7,
CHECKPOINT_LIST = [
const checkpointList = [
{ {
"latitude": 45.64492790560583, altitude: 585.1806861589965,
"longitude": 14.25334942966329, latitude: 45.64492790560583,
"altitude": 585.1806861589965 longitude: 14.25334942966329
}, },
{ {
"latitude": 45.64316335436476, altitude: 589.8802607573035,
"longitude": 14.26332880184475, latitude: 45.64316335436476,
"altitude": 589.8802607573035 longitude: 14.26332880184475
}, },
{ {
"latitude": 45.64911917196595, altitude: 608.6648153348965,
"longitude": 14.26214792790128, latitude: 45.64911917196595,
"altitude": 608.6648153348965 longitude: 14.26214792790128
}, },
{ {
"latitude": 45.64122685351364, altitude: 606.1448368129072,
"longitude": 14.26590493128597, latitude: 45.64122685351364,
"altitude": 606.1448368129072 longitude: 14.26590493128597
}, },
{ {
"latitude": 45.64543355564817, altitude: 630.0829598206344,
"longitude": 14.27242391207985, latitude: 45.64543355564817,
"altitude": 630.0829598206344 longitude: 14.27242391207985
}, },
{ {
"latitude": 45.6372792927328, altitude: 616.1839898415284,
"longitude": 14.27533492411138, latitude: 45.6372792927328,
"altitude": 616.1839898415284 longitude: 14.27533492411138
}, },
{ {
"latitude": 45.64061299543953, altitude: 598.0603137354178,
"longitude": 14.26161958465814, latitude: 45.64061299543953,
"altitude": 598.0603137354178 longitude: 14.26161958465814
}, },
{ {
"latitude": 45.64032340702919, altitude: 607.1243119862851,
"longitude": 14.2682896662383, latitude: 45.64032340702919,
"altitude": 607.1243119862851 longitude: 14.2682896662383
} }
]; ],
LANDING_POINT = {
latitude: 45.6398451,
longitude: 14.2699217
},
LEADER_ID = 0,
IS_LEADER = me.id === LEADER_ID;
const landingPoint = [ function altitudeReached(altitude, target_altitude) {
{ console.log(
"latitude": 45.6398451, `[DEMO] Waiting for altitude... (${altitude} , ${target_altitude})`);
"longitude": 14.2699217 return Math.abs(altitude - target_altitude) < EPSILON_ALTITUDE;
} }
];
let INITIAL_ALTITUDE; function distance(lat1, lon1, lat2, lon2) {
let START_ALTITUDE; 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));
}
var nextCheckpoint = 0; function exitOnFail(ret, msg) {
if (ret) {
console.log(msg);
me.exit(1);
}
}
var distanceToLandingPoint = 100; function mustWait(timestamp) {
if (me.timestamp === 0) {
me.timestamp = timestamp;
}
return timestamp - me.timestamp < me.must_wait;
}
var leaderAltitudeAbs; function leaderStartAltitude(drone) {
var leaderAltitudeRel; return drone.start_altitude - ALTITUDE_DIFF * me.id;
var leaderLatitude; }
var leaderLongitude;
function distance(lat1, lon1, lat2, lon2) { function leaderReachedInitAltitude(drone) {
const R = 6371e3; // meters return drone.drone_dict[LEADER_ID].altitudeAbs >= leaderStartAltitude(me);
const la1 = lat1 * Math.PI/180; // la, lo in radians }
const la2 = lat2 * Math.PI/180;
const lo1 = lon1 * Math.PI/180;
const lo2 = lon2 * Math.PI/180;
//haversine formula function groundLevel(drone) {
const sinLat = Math.sin((la2 - la1)/2); return drone.getAltitudeAbs() - drone.getCurrentPosition().z;
const sinLon = Math.sin((lo2 - lo1)/2); }
const h = sinLat*sinLat + Math.cos(la1)*Math.cos(la2)*sinLon*sinLon
return 2*R*Math.asin(Math.sqrt(h));
}
function waitForAltitude(target_altitude) { me.onStart = function () {
var altitude = getAltitude(); me.direction_set = false;
while(Math.abs(altitude - target_altitude) > EPSILON_ALTITUDE) { me.going_to_landing_point = false;
console.log( me.init_alt_reached = false;
`[DEMO] Waiting for altitude... (${altitude} , ${target_altitude})`); me.landing = false;
sleep(1000); me.landing_alt_reached = false;
altitude = getAltitude(); me.landing_point_reached = false;
me.last_checkpoint_reached = false;
me.must_wait = 3000;
me.next_checkpoint = 0;
me.parachute_triggered = false;
me.start_altitude = me.getInitialAltitude() + FLIGH_ALTITUDE;
me.timestamp = 0;
if (!IS_LEADER) {
me.follow_leader = true;
me.leader_init_alt_reached = false;
me.start_altitude += ALTITUDE_DIFF * me.id;
me.must_wait = 0;
} }
} exitOnFail(
me.setAltitude(me.start_altitude + 1),
"Failed to set start altitude"
);
};
function goToAltitude(target_altitude, wait, go) { me.onUpdate = function (timestamp) {
if(go) { if (!me.init_alt_reached) {
exit_on_fail( me.init_alt_reached = altitudeReached(
setAltitude(target_altitude), me.getAltitudeAbs(),
`Failed to go to altitude ${target_altitude} m` me.start_altitude
); );
return;
} }
if(wait) { if (me.must_wait > 0) {
waitForAltitude(target_altitude); if (!mustWait(timestamp)) {
me.must_wait = 0;
me.timestamp = 0;
}
return;
} }
}
function land() { if (!IS_LEADER && me.follow_leader) {
var yaw; if (me.drone_dict[LEADER_ID].altitudeAbs === 0) {
return console.log("[DEMO] Waiting for leader to send its altitude");
}
while(true) { if (!me.leader_init_alt_reached) {
yaw = getYaw(); me.leader_init_alt_reached = leaderReachedInitAltitude(me);
console.log(`[DEMO] Waiting for yaw... (${yaw} , ${TARGET_YAW})`); return console.log(
if(Math.abs(yaw - TARGET_YAW) < EPSILON_YAW) { "[DEMO] Waiting for leader to reach altitude " +
break; leaderStartAltitude(me) +
`(currently ${me.drone_dict[LEADER_ID].altitudeAbs})`,
);
} }
sleep(250);
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;
} }
console.log("[DEMO] Deploying parachute..."); if (!me.direction_set) {
exit_on_fail(doParachute(2), "Failed to deploy parachute"); if (me.next_checkpoint < CHECKPOINT_LIST.length) {
} exitOnFail(
me.setTargetCoordinates(
function goTo(latitude, longitude, altitude, radius, sleep_time, epsilon) { CHECKPOINT_LIST[me.next_checkpoint].latitude,
var cur_latitude; CHECKPOINT_LIST[me.next_checkpoint].longitude,
var cur_longitude; CHECKPOINT_LIST[me.next_checkpoint].altitude + FLIGH_ALTITUDE
var d; ),
"Failed to set checkpoint coordinates"
console.log(`Going to (${latitude}, ${longitude}) from (${getLatitude()}, ${getLongitude()})`); );
exit_on_fail( console.log(`[DEMO] Going to Checkpoint ${me.next_checkpoint}\n`);
setTargetCoordinates(latitude, longitude, altitude, radius), } else {
`Failed to go to (${latitude}, ${longitude})` console.log("[DEMO] Setting last checkpoint coordinates...\n");
me.landing_altitude = groundLevel(me) + PARACHUTE_ALTITUDE;
exitOnFail(
me.setTargetCoordinates(
CHECKPOINT_LIST[CHECKPOINT_LIST.length - 1].latitude,
CHECKPOINT_LIST[CHECKPOINT_LIST.length - 1].longitude,
me.landing_altitude
),
"Failed to set last checkpoint coordinates"
); );
}
me.direction_set = true;
return;
}
do { if (me.next_checkpoint < CHECKPOINT_LIST.length) {
sleep(sleep_time); me.current_position = me.getCurrentPosition();
cur_latitude = getLatitude(); me.distance = distance(
cur_longitude = getLongitude(); me.current_position.x,
d = distance(cur_latitude, cur_longitude, latitude, longitude); me.current_position.y,
console.log(`Waiting for drone to get to destination (${d} m), CHECKPOINT_LIST[me.next_checkpoint].latitude,
(${cur_latitude} , ${cur_longitude}), (${latitude}, ${longitude})`); CHECKPOINT_LIST[me.next_checkpoint].longitude
} while(d > epsilon); );
} if (me.distance > EPSILON) {
console.log(
function followLeader(leaderId, initialAltitude, altitudeDiff) { "Waiting for drone to get to destination (checkpoint " +
goToAltitude(START_ALTITUDE + ALTITUDE_DIFF, false, true); `${me.next_checkpoint}: ${me.distance} m)`,
while(droneDict[leaderId].altitudeAbs == 0) {
console.log("[DEMO] Waiting for leader to send its altitude");
sleep(1000);
}
while(droneDict[leaderId].altitudeAbs < initialAltitude) {
console.log(`[DEMO] Waiting for leader to reach altitude ${initialAltitude} (currently ${droneDict[leaderId].altitudeAbs})`);
sleep(1000);
}
console.log("[DEMO] Switching to following mode...\n");
do {
leaderAltitudeAbs = droneDict[leaderId].altitudeAbs;
leaderAltitudeRel = droneDict[leaderId].altitudeRel;
leaderLatitude = droneDict[leaderId].latitude;
leaderLongitude = droneDict[leaderId].longitude;
setTargetCoordinates(
leaderLatitude,
leaderLongitude,
leaderAltitudeAbs + altitudeDiff,
30.001
); );
sleep(500); } else {
} while(leaderAltitudeRel > PARACHUTE_ALTITUDE); me.loiter(100);
console.log(`[DEMO] Reached Checkpoint ${me.next_checkpoint}\n`);
me.next_checkpoint += 1;
me.sendMsg(JSON.stringify({next_checkpoint: me.next_checkpoint}));
me.direction_set = false;
me.must_wait = 50000;
}
return;
}
console.log("[DEMO] Stop following...\n"); if (!me.last_checkpoint_reached) {
nextCheckpoint = droneDict[leaderId].lastCheckpoint + 1; me.current_position = me.getCurrentPosition();
} me.distance = distance(
me.current_position.x,
me.onStart = function() { me.current_position.y,
INITIAL_ALTITUDE = getInitialAltitude(); CHECKPOINT_LIST[CHECKPOINT_LIST.length - 1].latitude,
START_ALTITUDE = INITIAL_ALTITUDE + FLIGH_ALTITUDE; CHECKPOINT_LIST[CHECKPOINT_LIST.length - 1].longitude
goToAltitude(START_ALTITUDE + 1, true, true);
waitForAltitude(START_ALTITUDE);
console.log("[DEMO] Setting loiter mode...\n");
loiter();
sleep(3000);
};
me.onUpdate = function() {
if(!IS_LEADER) {
followLeader(LEADER_ID, START_ALTITUDE, ALTITUDE_DIFF);
}
for (let i = nextCheckpoint; i < checkpointList.length; i++) {
console.log(`[DEMO] Going to Checkpoint ${i}\n`);
goTo(
checkpointList[i].latitude,
checkpointList[i].longitude,
checkpointList[i].altitude + FLIGH_ALTITUDE,
100,
1000,
105
); );
console.log(`[DEMO] Reached Checkpoint ${i}\n`); if (me.distance > EPSILON) {
setCheckpoint(i); console.log(
sleep(30000); "Waiting for drone to get to destination (last checkpoint: " +
} `${me.distance} m)`,
);
console.log("[DEMO] Setting altitude...\n"); } else {
goToAltitude(getAltitude() - getAltitudeRel() + PARACHUTE_ALTITUDE, true, true); me.loiter(100);
me.last_checkpoint_reached = true;
if(!IS_LEADER) { }
goTo( return;
checkpointList[checkpointList.length - 1].latitude, }
checkpointList[checkpointList.length - 1].longitude,
getAltitude(), if (!me.landing_alt_reached) {
100, me.landing_alt_reached = altitudeReached(
1000, me.getAltitudeAbs(),
105 me.landing_altitude
); );
me.setAltitude(me.landing_altitude);
return;
} }
while(distanceToLandingPoint > 20) { if (!me.going_to_landing_point) {
console.log(`[DEMO] Waiting to reach landing point (current distance is ${distanceToLandingPoint})`); exitOnFail(
distanceToLandingPoint = distance(getLatitude(), getLongitude(), landingPoint.latitude, landingPoint.longitude); 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"); console.log("[DEMO] Landing...\n");
land(); 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("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
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