Add multicopter swarm flight script

multicopter_swarm.js

+/*
+ * 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));