Commit 8ca353c0 authored by Léo-Paul Géneau's avatar Léo-Paul Géneau 👾

add restricted_flight.js

parent 8624b8b3
/*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.6447727,
longitude: 14.2635345,
altitude: 608
},
POINT_B = {
latitude: 45.6447678,
longitude: 14.2635388,
altitude: 608
},
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) {
me.setTargetCoordinates(
POINT_A.latitude,
POINT_A.longitude,
POINT_A.altitude + FLIGH_ALTITUDE
);
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) {
me.setTargetCoordinates(
POINT_B.latitude,
POINT_B.longitude,
POINT_B.altitude + FLIGH_ALTITUDE
);
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) {
me.setTargetCoordinates(
POINT_A.latitude,
POINT_A.longitude,
POINT_A.altitude + FLIGH_ALTITUDE
);
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));
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