add restricted_flight.js

restricted_flight.js

+/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
+/*global console, me*/
+
+(function (console, me) {
+  "use strict";
+
+  var ALTITUDE_DIFF = 40,
+    EPSILON = 9,
+    EPSILON_ALTITUDE = 7,
+    FLIGH_ALTITUDE = 120,
+    LANDING_POINT = {
+      latitude: 45.6398451,
+      longitude: 14.2699217
+    },
+    LEADER_ID = 0,
+    IS_LEADER = me.id === LEADER_ID,
+    PARACHUTE_ALTITUDE = 100,
+    POINT_A = {
+      latitude: 45.6447727,
+      longitude: 14.2635345,
+      altitude: 608
+    },
+    POINT_B = {
+      latitude: 45.6447678,
+      longitude: 14.2635388,
+      altitude: 608
+    },
+    ROUND_NB = 1;
+
+  function altitudeReached(altitude, target_altitude) {
+    console.log(
+      "[DEMO] Waiting for altitude... ("
+        + altitude + " , " + target_altitude + ")"
+    );
+    return Math.abs(altitude - target_altitude) < EPSILON_ALTITUDE;
+  }
+
+  function distance(lat1, lon1, lat2, lon2) {
+    var R = 6371e3, // meters
+      la1 = lat1 * Math.PI / 180, // la, lo in radians
+      la2 = lat2 * Math.PI / 180,
+      lo1 = lon1 * Math.PI / 180,
+      lo2 = lon2 * Math.PI / 180,
+      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 exitOnFail(ret, msg) {
+    if (ret) {
+      console.log(msg);
+      me.exit(1);
+    }
+  }
+
+  function leaderStartAltitude(drone) {
+    return drone.start_altitude - ALTITUDE_DIFF * me.id;
+  }
+
+  function leaderReachedInitAltitude(drone) {
+    return drone.drone_dict[LEADER_ID].altitudeAbs >= leaderStartAltitude(me);
+  }
+
+  me.onStart = function () {
+    me.going_to_point_a = false;
+    me.going_to_landing_point = false;
+    me.init_alt_reached = false;
+    me.landing = false;
+    me.landing_altitude = me.getInitialAltitude() + PARACHUTE_ALTITUDE;
+    me.parachute_triggered = false;
+    me.round_count = 0;
+    me.start_altitude = me.getInitialAltitude() + FLIGH_ALTITUDE;
+
+    if (!IS_LEADER) {
+      me.follow_leader = true;
+      me.leader_init_alt_reached = false;
+      me.start_altitude += ALTITUDE_DIFF * me.id;
+    }
+    exitOnFail(
+      me.setAltitude(me.start_altitude + 1),
+      "Failed to set start altitude"
+    );
+  };
+
+  me.onUpdate = function (timestamp) {
+    if (!me.init_alt_reached) {
+      me.init_alt_reached = altitudeReached(
+        me.getAltitudeAbs(),
+        me.start_altitude
+      );
+      if (me.init_alt_reached && IS_LEADER) {
+        me.setTargetCoordinates(
+          POINT_A.latitude,
+          POINT_A.longitude,
+          POINT_A.altitude + FLIGH_ALTITUDE
+        );
+        me.going_to_point_a = true;
+        me.sendMsg(JSON.stringify({going_to_point_a: me.going_to_point_a}));
+      }
+      return;
+    }
+
+    if (!IS_LEADER && me.follow_leader) {
+      if (me.drone_dict[LEADER_ID].altitudeAbs === 0) {
+        return console.log("[DEMO] Waiting for leader to send its altitude");
+      }
+
+      if (!me.leader_init_alt_reached) {
+        me.leader_init_alt_reached = leaderReachedInitAltitude(me);
+        return console.log(
+          "[DEMO] Waiting for leader to reach altitude "
+            + leaderStartAltitude(me)
+            + "(currently " + me.drone_dict[LEADER_ID].altitudeAbs + ")"
+        );
+      }
+
+      if (me.drone_dict[LEADER_ID].altitudeRel > PARACHUTE_ALTITUDE) {
+        exitOnFail(
+          me.setTargetCoordinates(
+            me.drone_dict[LEADER_ID].latitude,
+            me.drone_dict[LEADER_ID].longitude,
+            me.drone_dict[LEADER_ID].altitudeAbs + ALTITUDE_DIFF * me.id
+          ),
+          "Failed to follow leader"
+        );
+      } else {
+        me.follow_leader = false;
+        console.log("[DEMO] Stop following...\n");
+      }
+      return;
+    }
+
+    if (!me.going_to_landing_point) {
+      me.current_position = me.getCurrentPosition();
+      me.distance = distance(
+        me.current_position.x,
+        me.current_position.y,
+        (me.going_to_point_a) ? POINT_A.latitude : POINT_B.latitude,
+        (me.going_to_point_a) ? POINT_A.longitude : POINT_B.longitude
+      );
+      if (me.distance > EPSILON) {
+        console.log(
+          "Waiting for drone to get to destination (" + me.distance + " m)"
+        );
+      } else {
+        if (me.going_to_point_a) {
+          me.setTargetCoordinates(
+            POINT_B.latitude,
+            POINT_B.longitude,
+            POINT_B.altitude + FLIGH_ALTITUDE
+          );
+          me.going_to_point_a = false;
+          me.sendMsg(JSON.stringify({going_to_point_a: me.going_to_point_a}));
+        } else {
+          me.round_count += 1;
+          if (me.round_count < ROUND_NB) {
+            me.setTargetCoordinates(
+              POINT_A.latitude,
+              POINT_A.longitude,
+              POINT_A.altitude + FLIGH_ALTITUDE
+            );
+            me.going_to_point_a = true;
+            me.sendMsg(JSON.stringify({going_to_point_a: me.going_to_point_a}));
+          } else {
+            exitOnFail(
+              me.setTargetCoordinates(
+                LANDING_POINT.latitude,
+                LANDING_POINT.longitude,
+                me.landing_altitude
+              ),
+              "Failed to set landing point coordinates"
+            );
+            me.going_to_landing_point = true;
+          }
+        }
+      }
+      return;
+    }
+
+    if (!me.landing) {
+      me.current_position = me.getCurrentPosition();
+      me.distance = distance(
+        me.current_position.x,
+        me.current_position.y,
+        LANDING_POINT.latitude,
+        LANDING_POINT.longitude
+      );
+      if (me.distance > EPSILON) {
+        console.log(
+          "[DEMO] Waiting to reach landing point (current distance is "
+            + me.distance + ")"
+        );
+      } else {
+        console.log("[DEMO] Landing...\n");
+        me.landing = true;
+      }
+      return;
+    }
+
+    if (!me.parachute_triggered) {
+      console.log("[DEMO] Deploying parachute...");
+      exitOnFail(me.triggerParachute(), "Failed to deploy parachute");
+      me.parachute_triggered = true;
+    }
+
+    if (me.landed()) {
+      me.exit(0);
+    }
+  };
+
+  me.onGetMsg = function (msg) {
+    me.msgDict = JSON.parse(msg);
+    if (me.follow_leader && me.msgDict.hasOwnProperty("going_to_point_a")) {
+      me.going_to_point_a = me.msgDict.going_to_point_a;
+    }
+  };
+}(console, me));