Skip to content

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

Provide check and landing points through operator

parent e6a4e807
Hide whitespace changes
Inline Side-by-side
Showing with 112 additions and 100 deletions
multicopter_swarm.js
View file @ a0de0e00
......@@ -14,52 +14,8 @@
var ALTITUDE_DIFF = 5,
BASE_ALTITUDE = 25,
CHECKPOINT_LIST = [
{
latitude: 45.310770,
longitude: 5.950148
},
{
latitude: 45.311229,
longitude: 5.949820
},
{
latitude: 45.311246,
longitude: 5.950584
},
{
latitude: 45.310785,
longitude: 5.950877
}
],
EPSILON = 4,
PARACHUTE_POINT_ARRAY = [
{
latitude: 45.310479,
longitude: 5.950495
},
{
latitude: 45.310393,
longitude: 5.950677
},
{
latitude: 45.310376,
longitude: 5.950295
},
{
latitude: 45.310304,
longitude: 5.950459
},
{
latitude: 45.310235,
longitude: 5.950608
}
],
TARGETED_DISTANCE = 10,
TAKE_OFF_POINT = {
latitude: 45.310479,
longitude: 5.950495
},
MIN_CMD_INTERVAL = 500,
MIN_SPEED = 4,
DEFAULT_SPEED = 5,
......@@ -119,6 +75,14 @@
}
}
function communicate(drone, okState, state) {
var infos = {id: drone.id, okState: okState, state: state};
if (okState) {
infos.timestamp = Date.now();
}
return drone.sendMsg(JSON.stringify(infos));
}
me.onStart = function () {
me.direction_set = false;
me.flightAltitude = BASE_ALTITUDE + me.id * ALTITUDE_DIFF;
......@@ -126,9 +90,9 @@
me.going_to_parachute = false;
me.id_list = [me.id];
me.timestamp_list = [0];
me.id_list.sort();
me.landing = false;
me.last_cmd_timestamp = 0;
me.lost_a_drone = false;
me.next_checkpoint = 0;
me.started = false;
me.stopped = false;
......@@ -151,58 +115,55 @@
neighbor_position = me.getDroneDict()[neighbor_id];
}
if (!me.path_planning){
if (!me.checkpoint_list) {
if (leader_id !== me.id) {
console.log(
"timestamp difference",
position.timestamp - neighbor_position.timestamp
);
}
return me.sendMsg(JSON.stringify(
{id: me.id, okState: true, state: "Ready"}
));
return communicate(me, true, "Ready");
}
if (me.landing && me.isLanding()) {
return me.sendMsg(JSON.stringify(
{id: drone.id, okState: false, state: "Landing"}
));
return communicate(me, false, "Landing");
}
if (!me.started) {
if(me.isReadyToFly()) {
if (me.isReadyToFly()) {
me.started = true;
} else {
return me.sendMsg(JSON.stringify(
{id: me.id, okState: true, state: "Taking Off", timestamp: Date.now()}
));
return communicate(me, true, "Taking Off");
}
}
if (!me.landing) {
if(!me.isReadyToFly()) {
return me.sendMsg(JSON.stringify(
{id: me.id, okState: false, state: "Emergency"}
));
if (!me.isReadyToFly()) {
return communicate(me, false, "Emergency");
}
JSON.parse(JSON.stringify(me.timestamp_list)).forEach(
function (last_timestamp, id) {
if (me.id_list[id] !== me.id
function (last_timestamp, index) {
if (me.id_list[index] !== me.id
&& Math.abs(timestamp - last_timestamp) >= TIMEOUT) {
console.log(
"Lost drone",
me.id_list[id],
me.id_list[index],
"timestamp difference",
timestamp - last_timestamp
);
me.id_list.splice(me.id_list.indexOf(id), 1);
me.timestamp_list.splice(me.id_list.indexOf(id), 1);
me.id_list.splice(index, 1);
me.timestamp_list.splice(index, 1);
me.lost_a_drone = true;
}
}
);
console.log("leader id", leader_id, "me id", me.id);
if (me.lost_a_drone) {
me.lost_a_drone = false;
return communicate(me, true, "Flying");
}
if (leader_id !== me.id) {
distance2d = distance(
position.latitude,
......@@ -215,17 +176,15 @@
distanceToTakeOffPoint = distance(
neighbor_position.latitude,
neighbor_position.longitude,
TAKE_OFF_POINT.latitude,
TAKE_OFF_POINT.longitude
me.landing_point_list[0].latitude,
me.landing_point_list[0].longitude
);
console.log(
"Distance from neighbor to takeoff point",
distanceToTakeOffPoint
);
if (distanceToTakeOffPoint < 2 * TARGETED_DISTANCE) {
return me.sendMsg(JSON.stringify(
{id: me.id, okState: true, state: "Waiting", timestamp: Date.now()}
));
return communicate(me, true, "Waiting");
}
me.following = true;
}
......@@ -245,28 +204,26 @@
);
console.log("distance to leader", distance2d);
return me.sendMsg(JSON.stringify(
{id: me.id, okState: true, state: "Flying", timestamp: Date.now()}
));
return communicate(me, true, "Flying");
}
checkpointIndex = (!me.reverse) ? me.next_checkpoint
: CHECKPOINT_LIST.length - me.next_checkpoint - 1;
: me.checkpoint_list.length - me.next_checkpoint - 1;
if (!me.direction_set) {
console.log("Going to Checkpoint", checkpointIndex);
me.next_point = {
"latitude": CHECKPOINT_LIST[checkpointIndex].latitude,
"longitude": CHECKPOINT_LIST[checkpointIndex].longitude
"latitude": me.checkpoint_list[checkpointIndex].latitude,
"longitude": me.checkpoint_list[checkpointIndex].longitude
};
me.direction_set = true;
}
if (
pointReached(me, CHECKPOINT_LIST[checkpointIndex], EPSILON)
pointReached(me, me.checkpoint_list[checkpointIndex], EPSILON)
) {
console.log("Reached Checkpoint", checkpointIndex);
me.next_checkpoint += 1;
me.next_checkpoint %= CHECKPOINT_LIST.length;
me.next_checkpoint %= me.checkpoint_list.length;
me.direction_set = false;
return me.sendMsg(JSON.stringify({
id: me.id,
......@@ -279,9 +236,9 @@
}
if (me.going_to_parachute &&
pointReached(me, PARACHUTE_POINT_ARRAY[me.id], EPSILON)) {
pointReached(me, me.landing_point_list[me.id], EPSILON)) {
me.going_to_parachute = false;
return exitOnFail(drone.land(), "Failed to land");
return exitOnFail(me.land(), "Failed to land");
}
if (!me.landing || me.going_to_parachute) {
......@@ -293,16 +250,19 @@
Date.now()
);
}
return me.sendMsg(JSON.stringify(
{id: me.id, okState: true, state: "Flying", timestamp: Date.now()}
));
if (me.landing) {
return communicate(me, false, "Landed");
} else {
return communicate(me, true, "Flying");
}
};
me.onGetMsg = function (msg) {
if (msg.hasOwnProperty("path_planning")) {
me.path_planning = true;
if (msg.hasOwnProperty("checkpoint_list")) {
me.checkpoint_list = msg.checkpoint_list;
me.landing_point_list = msg.landing_point_list;
console.log("running");
me.timestamp_list = Array.apply(null, Array(me.id_list.length)).map(function () { return Date.now() });
return me.takeOff();
}
......@@ -313,12 +273,13 @@
case "stopped":
me.stopped = true;
if (me.id === (!me.reverse? me.id_list[me.id_list.length - 1] : me.id_list[0])) {
if (me.id ===
(!me.reverse ? me.id_list[me.id_list.length - 1] : me.id_list[0])) {
me.landing = true;
me.going_to_parachute = true;
me.next_point = {
"latitude": PARACHUTE_POINT_ARRAY[me.id].latitude,
"longitude": PARACHUTE_POINT_ARRAY[me.id].longitude
"latitude": me.landing_point_list[me.id].latitude,
"longitude": me.landing_point_list[me.id].longitude
};
console.log("Going to rally");
}
......@@ -340,21 +301,23 @@
case "Ready":
if (!me.id_list.includes(msgDict.id)) {
me.id_list[me.id_list.length - 1] = msgDict.id;
me.id_list[me.id_list.length] = msgDict.id;
me.id_list.sort();
}
break;
case "Landing":
if (me.id_list.includes(msgDict.id)) {
if (msgDict.id !== me.id && me.id_list.includes(msgDict.id)) {
me.id_list.splice(me.id_list.indexOf(msgDict.id), 1);
me.timestamp_list.splice(me.id_list.indexOf(id), 1);
if (me.stopped && me.id === (!me.reverse? me.id_list[me.id_list.length - 1] : me.id_list[0])) {
me.timestamp_list.splice(me.id_list.indexOf(msgDict.id), 1);
if (me.stopped && me.id ===
(!me.reverse ? me.id_list[me.id_list.length - 1] : me.id_list[0])
) {
me.landing = true;
me.going_to_parachute = true;
me.next_point = {
"latitude": PARACHUTE_POINT_ARRAY[me.id].latitude,
"longitude": PARACHUTE_POINT_ARRAY[me.id].longitude
"latitude": me.landing_point_list[me.id].latitude,
"longitude": me.landing_point_list[me.id].longitude
};
}
}
......@@ -365,8 +328,9 @@
}
}
if(msgDict.hasOwnProperty("timestamp")) {
me.timestamp_list[msgDict.id] = msgDict.timestamp;
if (me.id_list.includes(msgDict.id)
&& msgDict.hasOwnProperty("timestamp")) {
me.timestamp_list[me.id_list.indexOf(msgDict.id)] = msgDict.timestamp;
}
if (msgDict.hasOwnProperty("next_checkpoint")) {
......
operator.js
View file @ a0de0e00
......@@ -3,5 +3,53 @@
* start button of its GUI is pressed.
*/
var map = operator.getMapJSON();
operator.sendMsg({flag_positions: map.flag_list, path_planning: ""});
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global operator*/
(function (operator) {
"use strict";
var map = operator.getMapJSON();
operator.sendMsg({
checkpoint_list: [
{
latitude: 45.310770,
longitude: 5.950148
},
{
latitude: 45.311229,
longitude: 5.949820
},
{
latitude: 45.311246,
longitude: 5.950584
},
{
latitude: 45.310785,
longitude: 5.950877
}
],
flag_positions: map.flag_list,
landing_point_list: [
{
latitude: 45.310479,
longitude: 5.950495
},
{
latitude: 45.310393,
longitude: 5.950677
},
{
latitude: 45.310376,
longitude: 5.950295
},
{
latitude: 45.310304,
longitude: 5.950459
},
{
latitude: 45.310235,
longitude: 5.950608
}
]
});
}(operator));
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