check for anormal flight state and communication lost

multicopter_swarm.js

@@ -57,7 +57,8 @@
     DEFAULT_SPEED = 5,
     MAX_SPEED = 6,
     R = 6371e3,
-    SPEED_FACTOR = 0.2;
+    SPEED_FACTOR = 0.2,
+    TIMEOUT = 1000;
 
   function exitOnFail(ret, msg) {
     if (ret) {
@@ -154,11 +155,11 @@
       newSpeed,
       position = me.getCurrentPosition();
 
-    if (me.isLanding()) {
-      return sendLandingMessage(me);
+    if (leader_id !== me.id) {
+      neighbor_position = me.getDroneDict()[neighbor_id];
     }
 
-    if (!me.started) {
+    if (!me.path_planning){
       if (leader_id !== me.id) {
         console.log(
           "timestamp difference",
@@ -168,14 +169,40 @@
       return;
     }
 
+    if (me.landing && me.isLanding()) {
+      return sendLandingMessage(me);
+    }
+
+    if (!me.started) {
+      if(me.isReadyToFly()) {
+        me.started = true;
+      } else {
+        console.log("Taking off");
+        return me.sendMsg(JSON.stringify({id: me.id, timestamp: Date.now()}));
+      }
+    }
+
     if (!me.landing) {
-      if (!me.isReadyToFly()) {
-        return console.log("Taking off");
+      if(!me.isReadyToFly()) {
+        return console.log("emergency state");
       }
 
-      if (leader_id !== me.id) {
-        neighbor_position = me.getDroneDict()[neighbor_id];
+      me.timestamp_list.forEach(function (last_timestamp, id) {
+        if (me.id_list[id] !== me.id && Math.abs(timestamp - last_timestamp) >= TIMEOUT) {
+          console.log(
+            "Lost drone",
+            me.id_list[id],
+            "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);
+          return;
+        }
+      });
 
+      console.log("leader id", leader_id, "me id", me.id);
+      if (leader_id !== me.id) {
         distance2d = distance(
           position.latitude,
           position.longitude,
@@ -195,24 +222,14 @@
             distanceToTakeOffPoint
           );
           if (distanceToTakeOffPoint < 2 * TARGETED_DISTANCE) {
-            return;
+            return me.sendMsg(JSON.stringify({id: me.id, timestamp: Date.now()}));
           }
           me.following = true;
-          me.sendMsg(JSON.stringify(
-            {id: me.id, inAir: true, state: "Flying", type: "state"}
+          return me.sendMsg(JSON.stringify(
+            {id: me.id, inAir: true, state: "Flying", timestamp: Date.now(), 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),
@@ -227,7 +244,8 @@
           timestamp
         );
 
-        return console.log("distance to leader", distance2d);
+        console.log("distance to leader", distance2d);
+        return me.sendMsg(JSON.stringify({id: me.id, timestamp: Date.now()}));
       }
 
       checkpointIndex = (!me.reverse) ? me.next_checkpoint
@@ -248,9 +266,11 @@
         me.next_checkpoint += 1;
         me.next_checkpoint %= CHECKPOINT_LIST.length;
         me.direction_set = false;
-        me.sendMsg(JSON.stringify({
+        return me.sendMsg(JSON.stringify({
+          id: me.id,
           type: "checkpoint",
-          next_checkpoint: me.next_checkpoint
+          next_checkpoint: me.next_checkpoint,
+          timestamp: Date.now()
         }));
       }
     }
@@ -267,15 +287,17 @@
         me.next_point.latitude,
         me.next_point.longitude,
         DEFAULT_SPEED,
-        timestamp
+        Date.now()
       );
     }
+    return me.sendMsg(JSON.stringify({id: me.id, timestamp: Date.now()}));
   };
 
   me.onGetMsg = function (msg) {
     if (msg.hasOwnProperty("path_planning")) {
-      me.started = true;
+      me.path_planning = true;
       console.log("running");
+      me.timestamp_list = Array.apply(null, Array(me.id_list.length)).map(function () { return Date.now() });
       me.takeOff();
       return me.sendMsg(JSON.stringify(
         {id: me.id, inAir: true, state: "Flying", type: "state"}
@@ -284,12 +306,16 @@
 
     var msgDict = JSON.parse(msg);
 
+    if(msgDict.hasOwnProperty("id") && msgDict.hasOwnProperty("timestamp")) {
+      me.timestamp_list[msgDict.id] = msgDict.timestamp;
+    }
+
     if (msgDict.hasOwnProperty("status")) {
       switch (msgDict.status) {
 
       case "stopped":
         me.stopped = true;
-        if (me.id === 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 = {
@@ -311,28 +337,31 @@
       return;
     }
 
-    switch (msgDict.type) {
+    if (msgDict.hasOwnProperty("type")) {
+      switch (msgDict.type) {
 
-    case "checkpoint":
-      me.next_checkpoint = msgDict.next_checkpoint;
-      break;
+      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
-          };
+      case "state":
+        if (msgDict.state === "Landed" && 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.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;
+        break;
 
-    default:
-      console.log("Unknown message type: " + msgDict.type);
+      default:
+        console.log("Unknown message type: " + msgDict.type);
+      }
     }
   };
 }(console, me));