Update state management

multicopter_swarm.js

@@ -75,19 +75,6 @@
     }
   }
 
-  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;
   }
@@ -137,7 +124,8 @@
     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 = [me.id];
+    me.timestamp_list = [0];
     me.id_list.sort();
     me.landing = false;
     me.last_cmd_timestamp = 0;
@@ -145,10 +133,6 @@
     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) {
@@ -174,40 +158,49 @@
           position.timestamp - neighbor_position.timestamp
         );
       }
-      return;
+      return me.sendMsg(JSON.stringify(
+        {id: me.id, okState: true, state: "Ready"}
+      ));
     }
 
     if (me.landing && me.isLanding()) {
-      return sendLandingMessage(me);
+      return me.sendMsg(JSON.stringify(
+        {id: drone.id, okState: false, state: "Landing"}
+      ));
     }
 
     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()}));
+        return me.sendMsg(JSON.stringify(
+          {id: me.id, okState: true, state: "Taking Off", timestamp: Date.now()}
+        ));
       }
     }
 
     if (!me.landing) {
       if(!me.isReadyToFly()) {
-        return console.log("emergency state");
+        return me.sendMsg(JSON.stringify(
+          {id: me.id, okState: false, state: "Emergency"}
+        ));
       }
 
-      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;
+      JSON.parse(JSON.stringify(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);
+          }
         }
-      });
+      );
 
       console.log("leader id", leader_id, "me id", me.id);
       if (leader_id !== me.id) {
@@ -230,12 +223,11 @@
             distanceToTakeOffPoint
           );
           if (distanceToTakeOffPoint < 2 * TARGETED_DISTANCE) {
-            return me.sendMsg(JSON.stringify({id: me.id, timestamp: Date.now()}));
+            return me.sendMsg(JSON.stringify(
+              {id: me.id, okState: true, state: "Waiting", timestamp: Date.now()}
+            ));
           }
           me.following = true;
-          return me.sendMsg(JSON.stringify(
-            {id: me.id, inAir: true, state: "Flying", timestamp: Date.now(), type: "state"}
-          ));
         }
 
         distanceDiff = distance2d - TARGETED_DISTANCE;
@@ -253,7 +245,9 @@
         );
 
         console.log("distance to leader", distance2d);
-        return me.sendMsg(JSON.stringify({id: me.id, timestamp: Date.now()}));
+        return me.sendMsg(JSON.stringify(
+          {id: me.id, okState: true, state: "Flying", timestamp: Date.now()}
+        ));
       }
 
       checkpointIndex = (!me.reverse) ? me.next_checkpoint
@@ -276,8 +270,9 @@
         me.direction_set = false;
         return me.sendMsg(JSON.stringify({
           id: me.id,
-          type: "checkpoint",
+          okState: true,
           next_checkpoint: me.next_checkpoint,
+          state: "Flying",
           timestamp: Date.now()
         }));
       }
@@ -286,7 +281,7 @@
     if (me.going_to_parachute &&
         pointReached(me, PARACHUTE_POINT_ARRAY[me.id], EPSILON)) {
       me.going_to_parachute = false;
-      return land(me);
+      return exitOnFail(drone.land(), "Failed to land");
     }
 
     if (!me.landing || me.going_to_parachute) {
@@ -298,7 +293,9 @@
         Date.now()
       );
     }
-    return me.sendMsg(JSON.stringify({id: me.id, timestamp: Date.now()}));
+    return me.sendMsg(JSON.stringify(
+      {id: me.id, okState: true, state: "Flying", timestamp: Date.now()}
+    ));
   };
 
   me.onGetMsg = function (msg) {
@@ -306,18 +303,11 @@
       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"}
-      ));
+      return me.takeOff();
     }
 
     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) {
 
@@ -345,15 +335,18 @@
       return;
     }
 
-    if (msgDict.hasOwnProperty("type")) {
-      switch (msgDict.type) {
+    if (msgDict.hasOwnProperty("state")) {
+      switch (msgDict.state) {
 
-      case "checkpoint":
-        me.next_checkpoint = msgDict.next_checkpoint;
+      case "Ready":
+        if (!me.id_list.includes(msgDict.id)) {
+          me.id_list[me.id_list.length - 1] = msgDict.id;
+          me.id_list.sort();
+        }
         break;
 
-      case "state":
-        if (msgDict.state === "Landed" && me.id_list.includes(msgDict.id)) {
+      case "Landing":
+        if (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])) {
@@ -368,8 +361,16 @@
         break;
 
       default:
-        console.log("Unknown message type: " + msgDict.type);
+        break;
       }
     }
+
+    if(msgDict.hasOwnProperty("timestamp")) {
+      me.timestamp_list[msgDict.id] = msgDict.timestamp;
+    }
+
+    if (msgDict.hasOwnProperty("next_checkpoint")) {
+      me.next_checkpoint = msgDict.next_checkpoint;
+    }
   };
 }(console, me));