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 */
/*global console, me*/
(function (console, me) {
"use strict";
var EPSILON = 9,
FLIGH_ALTITUDE = 100,
PARACHUTE_ALTITUDE = 35,
CHECKPOINT_LIST = [
{
"altitude": 604,
"latitude": 45.641,
"longitude": 14.26472
},
{
"altitude": 642,
"latitude": 45.6463889,
"longitude": 14.2516
},
{
"altitude": 596,
"latitude": 45.656,
"longitude": 14.2516
},
{
"altitude": 634,
"latitude": 45.65916,
"longitude": 14.255
},
{
"altitude": 676,
"latitude": 45.6527,
"longitude": 14.2775
},
{
"altitude": 589,
"latitude": 45.6427,
"longitude": 14.2519
},
{
"altitude": 582,
"latitude": 45.641,
"longitude": 14.254
},
{
"altitude": 686,
"latitude": 45.6558,
"longitude": 14.2775
},
{
"altitude": 667,
"latitude": 45.6594,
"longitude": 14.271
},
{
"altitude": 581,
"latitude": 45.641,
"longitude": 14.258
},
{
"altitude": 584,
"latitude": 45.653,
"longitude": 14.2516
},
{
"altitude": 633,
"latitude": 45.6594,
"longitude": 14.26194
},
{
"altitude": 621,
"latitude": 45.641,
"longitude": 14.2716
},
{
"altitude": 642,
"latitude": 45.644,
"longitude": 14.2775
},
{
"altitude": 660,
"latitude": 45.6594,
"longitude": 14.26638
},
{
"altitude": 591,
"latitude": 45.6508,
"longitude": 14.25194
}
];
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 exit_on_fail(ret, msg) {
if (ret) {
console.log(msg);
me.exit(1);
}
}
function mustWait(timestamp) {
if (me.timestamp === 0) {
me.timestamp = timestamp;
}
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) {
if (me.must_wait > 0) {
if (!mustWait(timestamp)) {
me.must_wait = 0;
me.timestamp = 0;
}
return;
}
if (!me.direction_set) {
if (me.next_checkpoint < CHECKPOINT_LIST.length) {
exit_on_fail(
me.setTargetCoordinates(
CHECKPOINT_LIST[me.next_checkpoint].latitude,
CHECKPOINT_LIST[me.next_checkpoint].longitude,
CHECKPOINT_LIST[me.next_checkpoint].altitude + FLIGH_ALTITUDE
),
"Failed to set checkpoint coordinates"
);
console.log(`[DEMO] Going to Checkpoint ${me.next_checkpoint}\n`);
} 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;
}
if (me.next_checkpoint < CHECKPOINT_LIST.length) {
me.current_position = me.getCurrentPosition();
me.distance = distance(
me.current_position.x,
me.current_position.y,
CHECKPOINT_LIST[me.next_checkpoint].latitude,
CHECKPOINT_LIST[me.next_checkpoint].longitude
);
if (me.distance > EPSILON) {
console.log(
`Waiting for drone to get to destination (${me.distance} m)`);
} else {
console.log(`[DEMO] Reached Checkpoint ${me.next_checkpoint}\n`);
me.next_checkpoint += 1;
me.direction_set = false;
}
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));
\ No newline at end of file
(function (console, me) {
"use strict";
var EPSILON = 9,
FLIGH_ALTITUDE = 100,
PARACHUTE_ALTITUDE = 35,
CHECKPOINT_LIST = [
{
"altitude": 604,
"latitude": 45.641,
"longitude": 14.26472
},
{
"altitude": 642,
"latitude": 45.6463889,
"longitude": 14.2516
},
{
"altitude": 596,
"latitude": 45.656,
"longitude": 14.2516
},
{
"altitude": 634,
"latitude": 45.65916,
"longitude": 14.255
},
{
"altitude": 676,
"latitude": 45.6527,
"longitude": 14.2775
},
{
"altitude": 589,
"latitude": 45.6427,
"longitude": 14.2519
},
{
"altitude": 582,
"latitude": 45.641,
"longitude": 14.254
},
{
"altitude": 686,
"latitude": 45.6558,
"longitude": 14.2775
},
{
"altitude": 667,
"latitude": 45.6594,
"longitude": 14.271
},
{
"altitude": 581,
"latitude": 45.641,
"longitude": 14.258
},
{
"altitude": 584,
"latitude": 45.653,
"longitude": 14.2516
},
{
"altitude": 633,
"latitude": 45.6594,
"longitude": 14.26194
},
{
"altitude": 621,
"latitude": 45.641,
"longitude": 14.2716
},
{
"altitude": 642,
"latitude": 45.644,
"longitude": 14.2775
},
{
"altitude": 660,
"latitude": 45.6594,
"longitude": 14.26638
},
{
"altitude": 591,
"latitude": 45.6508,
"longitude": 14.25194
}
];
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 exit_on_fail(ret, msg) {
if (ret) {
console.log(msg);
me.exit(1);
}
}
function mustWait(timestamp) {
if (me.timestamp === 0) {
me.timestamp = timestamp;
}
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) {
if (me.must_wait > 0) {
if (!mustWait(timestamp)) {
me.must_wait = 0;
me.timestamp = 0;
}
return;
}
if (!me.direction_set) {
if (me.next_checkpoint < CHECKPOINT_LIST.length) {
exit_on_fail(
me.setTargetCoordinates(
CHECKPOINT_LIST[me.next_checkpoint].latitude,
CHECKPOINT_LIST[me.next_checkpoint].longitude,
CHECKPOINT_LIST[me.next_checkpoint].altitude + FLIGH_ALTITUDE
),
"Failed to set checkpoint coordinates"
);
console.log(`[DEMO] Going to Checkpoint ${me.next_checkpoint}\n`);
} 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;
}
if (me.next_checkpoint < CHECKPOINT_LIST.length) {
me.current_position = me.getCurrentPosition();
me.distance = distance(
me.current_position.x,
me.current_position.y,
CHECKPOINT_LIST[me.next_checkpoint].latitude,
CHECKPOINT_LIST[me.next_checkpoint].longitude
);
if (me.distance > EPSILON) {
console.log(
`Waiting for drone to get to destination (${me.distance} m)`);
} else {
console.log(`[DEMO] Reached Checkpoint ${me.next_checkpoint}\n`);
me.next_checkpoint += 1;
me.direction_set = false;
}
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));
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