Skip to content

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

bouncy-flight.js: Fix missing function altitudeReached

parent 08d37fad
Hide whitespace changes
Inline Side-by-side
Showing with 204 additions and 198 deletions
bouncy-flight.js
View file @ d8c4e691
/*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 EPSILON = 9, var EPSILON = 9,
FLIGH_ALTITUDE = 100, FLIGH_ALTITUDE = 100,
PARACHUTE_ALTITUDE = 35, PARACHUTE_ALTITUDE = 35,
CHECKPOINT_LIST = [ CHECKPOINT_LIST = [
{ {
"altitude": 604, "altitude": 604,
"latitude": 45.641, "latitude": 45.641,
"longitude": 14.26472 "longitude": 14.26472
}, },
{ {
"altitude": 642, "altitude": 642,
"latitude": 45.6463889, "latitude": 45.6463889,
"longitude": 14.2516 "longitude": 14.2516
}, },
{ {
"altitude": 596, "altitude": 596,
"latitude": 45.656, "latitude": 45.656,
"longitude": 14.2516 "longitude": 14.2516
}, },
{ {
"altitude": 634, "altitude": 634,
"latitude": 45.65916, "latitude": 45.65916,
"longitude": 14.255 "longitude": 14.255
}, },
{ {
"altitude": 676, "altitude": 676,
"latitude": 45.6527, "latitude": 45.6527,
"longitude": 14.2775 "longitude": 14.2775
}, },
{ {
"altitude": 589, "altitude": 589,
"latitude": 45.6427, "latitude": 45.6427,
"longitude": 14.2519 "longitude": 14.2519
}, },
{ {
"altitude": 582, "altitude": 582,
"latitude": 45.641, "latitude": 45.641,
"longitude": 14.254 "longitude": 14.254
}, },
{ {
"altitude": 686, "altitude": 686,
"latitude": 45.6558, "latitude": 45.6558,
"longitude": 14.2775 "longitude": 14.2775
}, },
{ {
"altitude": 667, "altitude": 667,
"latitude": 45.6594, "latitude": 45.6594,
"longitude": 14.271 "longitude": 14.271
}, },
{ {
"altitude": 581, "altitude": 581,
"latitude": 45.641, "latitude": 45.641,
"longitude": 14.258 "longitude": 14.258
}, },
{ {
"altitude": 584, "altitude": 584,
"latitude": 45.653, "latitude": 45.653,
"longitude": 14.2516 "longitude": 14.2516
}, },
{ {
"altitude": 633, "altitude": 633,
"latitude": 45.6594, "latitude": 45.6594,
"longitude": 14.26194 "longitude": 14.26194
}, },
{ {
"altitude": 621, "altitude": 621,
"latitude": 45.641, "latitude": 45.641,
"longitude": 14.2716 "longitude": 14.2716
}, },
{ {
"altitude": 642, "altitude": 642,
"latitude": 45.644, "latitude": 45.644,
"longitude": 14.2775 "longitude": 14.2775
}, },
{ {
"altitude": 660, "altitude": 660,
"latitude": 45.6594, "latitude": 45.6594,
"longitude": 14.26638 "longitude": 14.26638
}, },
{ {
"altitude": 591, "altitude": 591,
"latitude": 45.6508, "latitude": 45.6508,
"longitude": 14.25194 "longitude": 14.25194
} }
]; ];
function distance(lat1, lon1, lat2, lon2) { function altitudeReached(altitude, target_altitude) {
var R = 6371e3, // meters console.log(
la1 = lat1 * Math.PI / 180, // la, lo in radians `[DEMO] Waiting for altitude... (${altitude} , ${target_altitude})`
la2 = lat2 * Math.PI / 180, );
lo1 = lon1 * Math.PI / 180, return Math.abs(altitude - target_altitude) < EPSILON_ALTITUDE;
lo2 = lon2 * Math.PI / 180, }
haversine_phi = Math.pow(Math.sin((la2 - la1) / 2), 2),
sin_lon = Math.sin((lo2 - lo1) / 2), function distance(lat1, lon1, lat2, lon2) {
h = haversine_phi + Math.cos(la1) * Math.cos(la2) * sin_lon * sin_lon; var R = 6371e3, // meters
return 2 * R * Math.asin(Math.sqrt(h)); la1 = lat1 * Math.PI / 180, // la, lo in radians
} la2 = lat2 * Math.PI / 180,
lo1 = lon1 * Math.PI / 180,
function exit_on_fail(ret, msg) { lo2 = lon2 * Math.PI / 180,
if (ret) { haversine_phi = Math.pow(Math.sin((la2 - la1) / 2), 2),
console.log(msg); sin_lon = Math.sin((lo2 - lo1) / 2),
me.exit(1); h = haversine_phi + Math.cos(la1) * Math.cos(la2) * sin_lon * sin_lon;
} return 2 * R * Math.asin(Math.sqrt(h));
} }
function mustWait(timestamp) { function exit_on_fail(ret, msg) {
if (me.timestamp === 0) { if (ret) {
me.timestamp = timestamp; console.log(msg);
} me.exit(1);
return timestamp - me.timestamp < me.must_wait; }
} }
function groundLevel(drone) { function mustWait(timestamp) {
return drone.getAltitudeAbs() - drone.getCurrentPosition().z; if (me.timestamp === 0) {
} me.timestamp = timestamp;
}
me.onStart = function () { return timestamp - me.timestamp < me.must_wait;
me.direction_set = false; }
me.landing_alt_reached = false;
me.next_checkpoint = 0; function groundLevel(drone) {
me.parachute_triggered = false; return drone.getAltitudeAbs() - drone.getCurrentPosition().z;
}; }
me.onUpdate = function (timestamp) { me.onStart = function () {
if (me.must_wait > 0) { me.direction_set = false;
if (!mustWait(timestamp)) { me.landing_alt_reached = false;
me.must_wait = 0; me.next_checkpoint = 0;
me.timestamp = 0; me.parachute_triggered = false;
} };
return;
} me.onUpdate = function (timestamp) {
if (me.must_wait > 0) {
if (!me.direction_set) { if (!mustWait(timestamp)) {
if (me.next_checkpoint < CHECKPOINT_LIST.length) { me.must_wait = 0;
exit_on_fail( me.timestamp = 0;
me.setTargetCoordinates( }
CHECKPOINT_LIST[me.next_checkpoint].latitude, return;
CHECKPOINT_LIST[me.next_checkpoint].longitude, }
CHECKPOINT_LIST[me.next_checkpoint].altitude + FLIGH_ALTITUDE
), if (!me.direction_set) {
"Failed to set checkpoint coordinates" if (me.next_checkpoint < CHECKPOINT_LIST.length) {
); exit_on_fail(
console.log(`[DEMO] Going to Checkpoint ${me.next_checkpoint}\n`); me.setTargetCoordinates(
} else { CHECKPOINT_LIST[me.next_checkpoint].latitude,
me.loiter(); CHECKPOINT_LIST[me.next_checkpoint].longitude,
console.log("[DEMO] Going to landing altitude...\n"); CHECKPOINT_LIST[me.next_checkpoint].altitude + FLIGH_ALTITUDE
me.landing_altitude = groundLevel(me) + PARACHUTE_ALTITUDE; ),
exit_on_fail( "Failed to set checkpoint coordinates"
me.setAltitude(me.landing_altitude), );
"Failed to set landing altitude" console.log(`[DEMO] Going to Checkpoint ${me.next_checkpoint}\n`);
); } else {
} me.loiter();
me.direction_set = true; console.log("[DEMO] Going to landing altitude...\n");
return; me.landing_altitude = groundLevel(me) + PARACHUTE_ALTITUDE;
} exit_on_fail(
me.setAltitude(me.landing_altitude),
if (me.next_checkpoint < CHECKPOINT_LIST.length) { "Failed to set landing altitude"
me.current_position = me.getCurrentPosition(); );
me.distance = distance( }
me.current_position.x, me.direction_set = true;
me.current_position.y, return;
CHECKPOINT_LIST[me.next_checkpoint].latitude, }
CHECKPOINT_LIST[me.next_checkpoint].longitude
); if (me.next_checkpoint < CHECKPOINT_LIST.length) {
if (me.distance > EPSILON) { me.current_position = me.getCurrentPosition();
console.log( me.distance = distance(
`Waiting for drone to get to destination (${me.distance} m)`); me.current_position.x,
} else { me.current_position.y,
console.log(`[DEMO] Reached Checkpoint ${me.next_checkpoint}\n`); CHECKPOINT_LIST[me.next_checkpoint].latitude,
me.next_checkpoint += 1; CHECKPOINT_LIST[me.next_checkpoint].longitude
me.direction_set = false; );
} if (me.distance > EPSILON) {
return; console.log(
} `Waiting for drone to get to destination (${me.distance} m)`);
} else {
if (!me.landing_alt_reached) { console.log(`[DEMO] Reached Checkpoint ${me.next_checkpoint}\n`);
me.landing_alt_reached = altitudeReached( me.next_checkpoint += 1;
me.getAltitudeAbs(), me.direction_set = false;
me.landing_altitude }
); return;
return; }
}
if (!me.landing_alt_reached) {
if (!me.parachute_triggered) { me.landing_alt_reached = altitudeReached(
console.log("[DEMO] Deploying parachute..."); me.getAltitudeAbs(),
exit_on_fail(me.triggerParachute(), "Failed to deploy parachute"); me.landing_altitude
me.parachute_triggered = true; );
} return;
}
if (me.landed()) {
me.exit(0); if (!me.parachute_triggered) {
} console.log("[DEMO] Deploying parachute...");
}; exit_on_fail(me.triggerParachute(), "Failed to deploy parachute");
}(console, me)); me.parachute_triggered = true;
}
\ No newline at end of file
if (me.landed()) {
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