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

Add multicopter swarm flight script

parent 8d1d1ecb
...@@ -155,7 +155,7 @@ ...@@ -155,7 +155,7 @@
console.log( console.log(
"timestamp difference", "timestamp difference",
position.timestamp - me.drone_dict[neighbor_id].timestamp position.timestamp - me.drone_dict[neighbor_id].timestamp
); );
} }
return; return;
} }
......
/*
* In this flight all drone take off when start signal is sent. The leader
* follows continuously a list of checkpoints. The other drones follow each
* other (second drone follows leader, third drone follows second drone, ...).
* When the switch signal is sent, the last drone becomes the leader and follow
* the checkpoints in the reversed order. When stop signal is sent, each drone
* lands one at the time from the last one to the leader.
*/
/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
/*global console, me*/
(function (console, me) {
"use strict";
var ALTITUDE_DIFF = 5,
BASE_ALTITUDE = 25,
CHECKPOINT_LIST = [
{
latitude: 45.133816745111254,
longitude: 5.681756602776511
},
{
latitude: 45.13412766661184,
longitude: 5.681916517286368
},
{
latitude: 45.13399163453284,
longitude: 5.682459947632148
},
{
latitude: 45.133670102874454,
longitude: 5.68237448882777
}
],
EPSILON = 4,
PARACHUTE_POINT_ARRAY = [
{
latitude: 45.13342642944715,
longitude: 5.681879999829597
},
{
latitude: 45.133244783529136,
longitude: 5.681845964441365
},
{
latitude: 45.1333696,
longitude: 5.6817394
}
],
TARGETED_DISTANCE = 10,
TAKE_OFF_POINT = {
latitude: 45.13342642944715,
longitude: 5.681879999829597
},
MIN_CMD_INTERVAL = 500,
MIN_SPEED = 4,
DEFAULT_SPEED = 5,
MAX_SPEED = 6,
R = 6371e3,
SPEED_FACTOR = 0.2;
function exitOnFail(ret, msg) {
if (ret) {
console.log(msg);
me.exit(1);
}
}
function sendLandingMessage(drone) {
console.log("Landing");
drone.sendMsg(JSON.stringify(
{id: drone.id, inAir: false, state: "Landed", type: "state"}
));
}
function land(drone) {
console.log("Landing");
sendLandingMessage(drone);
exitOnFail(drone.land(), "Failed to land");
}
function toRad(angle) {
return Math.PI * angle / 180;
}
function distance(lat1, lon1, lat2, lon2) {
var la1 = toRad(lat1),
la2 = toRad(lat2),
lo1 = toRad(lon1),
lo2 = toRad(lon2),
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 leaderId(drone) {
var id_list = drone.id_list;
return (!drone.reverse) ? id_list[0] : id_list[id_list.length - 1];
}
function pointReached(drone, point, acceptance) {
var current_position = drone.getCurrentPosition(),
dist = distance(
current_position.latitude,
current_position.longitude,
point.latitude,
point.longitude
);
console.log("Distance to point", dist);
return dist <= acceptance;
}
function goTo(drone, latitude, longitude, speed, timestamp) {
if (timestamp - drone.last_cmd_timestamp >= MIN_CMD_INTERVAL) {
drone.setTargetCoordinates(
latitude,
longitude,
drone.flightAltitude,
speed
);
drone.last_cmd_timestamp = timestamp;
}
}
me.onStart = function () {
me.direction_set = false;
me.flightAltitude = BASE_ALTITUDE + me.id * ALTITUDE_DIFF;
me.following = false;
me.going_to_parachute = false;
me.id_list = Object.keys(me.getDroneDict()).map((x) => Number(x));
me.id_list.sort();
me.landing = false;
me.last_cmd_timestamp = 0;
me.next_checkpoint = 0;
me.started = false;
me.stopped = false;
me.reverse = false;
me.sendMsg(JSON.stringify(
{id: me.id, inAir: true, state: "Ready", type: "state"}
));
};
me.onUpdate = function (timestamp) {
var checkpointIndex,
distance2d,
distanceDiff,
distanceToTakeOffPoint,
leader_id = leaderId(me),
me_index = me.id_list.indexOf(me.id),
neighbor_id = me.id_list[(!me.reverse) ? me_index - 1 : me_index + 1],
neighbor_position,
newSpeed,
position = me.getCurrentPosition();
if (me.isLanding()) {
return sendLandingMessage(me);
}
if (!me.started) {
if (leader_id !== me.id) {
console.log(
"timestamp difference",
position.timestamp - neighbor_position.timestamp
);
}
return;
}
if (!me.landing) {
if (!me.isReadyToFly()) {
return console.log("Taking off");
}
if (leader_id !== me.id) {
neighbor_position = me.getDroneDict()[neighbor_id];
distance2d = distance(
position.latitude,
position.longitude,
neighbor_position.latitude,
neighbor_position.longitude
);
if (!me.following) {
distanceToTakeOffPoint = distance(
neighbor_position.latitude,
neighbor_position.longitude,
TAKE_OFF_POINT.latitude,
TAKE_OFF_POINT.longitude
);
console.log(
"Distance from neighbor to takeoff point",
distanceToTakeOffPoint
);
if (distanceToTakeOffPoint < 2 * TARGETED_DISTANCE) {
return;
}
me.following = true;
me.sendMsg(JSON.stringify(
{id: me.id, inAir: true, state: "Flying", type: "state"}
));
}
if (Math.abs(
position.timestamp - neighbor_position.timestamp
) >= 1000) {
console.log(
"timestamp difference",
position.timestamp - neighbor_position.timestamp
);
return;
}
distanceDiff = distance2d - TARGETED_DISTANCE;
newSpeed = Math.max(
Math.min(distanceDiff * SPEED_FACTOR + DEFAULT_SPEED, MAX_SPEED),
MIN_SPEED
);
goTo(
me,
neighbor_position.latitude,
neighbor_position.longitude,
newSpeed,
timestamp
);
return console.log("distance to leader", distance2d);
}
checkpointIndex = (!me.reverse) ? me.next_checkpoint
: 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
};
me.direction_set = true;
}
if (
pointReached(me, CHECKPOINT_LIST[checkpointIndex], EPSILON)
) {
console.log("Reached Checkpoint", checkpointIndex);
me.next_checkpoint += 1;
me.next_checkpoint %= CHECKPOINT_LIST.length;
me.direction_set = false;
me.sendMsg(JSON.stringify({
type: "checkpoint",
next_checkpoint: me.next_checkpoint
}));
}
}
if (me.going_to_parachute &&
pointReached(me, PARACHUTE_POINT_ARRAY[me.id], EPSILON)) {
me.going_to_parachute = false;
return land(me);
}
if (!me.landing || me.going_to_parachute) {
goTo(
me,
me.next_point.latitude,
me.next_point.longitude,
DEFAULT_SPEED,
timestamp
);
}
};
me.onGetMsg = function (msg) {
var msgDict = JSON.parse(msg);
if (msgDict.hasOwnProperty("status")) {
switch (msgDict.status) {
case "running":
me.started = true;
console.log("running");
me.takeOff();
me.sendMsg(JSON.stringify(
{id: me.id, inAir: true, state: "Flying", type: "state"}
));
break;
case "stopped":
me.stopped = true;
if (me.id === 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
};
console.log("Going to rally");
}
break;
case "switch":
me.reverse = !me.reverse;
me.direction_set = false;
break;
default:
console.log("Unknown status:", me.msgDict.status);
}
return;
}
switch (msgDict.type) {
case "checkpoint":
me.next_checkpoint = msgDict.next_checkpoint;
break;
case "state":
if (msgDict.state === "Landed" && me.id_list.includes(msgDict.id)) {
me.id_list.splice(me.id_list.indexOf(msgDict.id), 1);
if (me.stopped && me.id === 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
};
}
}
break;
default:
console.log("Unknown message type: " + msgDict.type);
}
};
}(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