simulator-script.js: do not depend on initial coordinates

simulator-script.js

@@ -10,53 +10,82 @@
   "use strict";
 
   var ALTITUDE = 100,
-    EPSILON = 9,
-    CHECKPOINT_LIST = [
+    EPSILON = 0.4,
+    CHECKPOINT_LIST = [],
+    DIRECTION_LIST = [
       {
-        altitude: 585.1806861589965,
-        latitude: 45.64492790560583,
-        longitude: 14.25334942966329
+        distance: 1053,
+        bearing: 293
       },
       {
-        altitude: 589.8802607573035,
-        latitude: 45.64316335436476,
-        longitude: 14.26332880184475
+        distance: 800,
+        bearing: 104
       },
       {
-        altitude: 608.6648153348965,
-        latitude: 45.64911917196595,
-        longitude: 14.26214792790128
+        distance: 669,
+        bearing: 352
       },
       {
-        altitude: 606.1448368129072,
-        latitude: 45.64122685351364,
-        longitude: 14.26590493128597
+        distance: 925,
+        bearing: 162
       },
       {
-        altitude: 630.0829598206344,
-        latitude: 45.64543355564817,
-        longitude: 14.27242391207985
+        distance: 690,
+        bearing: 47
       },
       {
-        altitude: 616.1839898415284,
-        latitude: 45.6372792927328,
-        longitude: 14.27533492411138
+        distance: 935,
+        bearing: 166
       },
       {
-        altitude: 598.0603137354178,
-        latitude: 45.64061299543953,
-        longitude: 14.26161958465814
+        distance: 1129,
+        bearing: 289
       },
       {
-        altitude: 607.1243119862851,
-        latitude: 45.64032340702919,
-        longitude: 14.2682896662383
+        distance: 520,
+        bearing: 94
       }
-    ];
+    ],
+    R = 6371e3 // meters;
+
+  function to_deg(rad) {
+    return rad * 180 / Math.PI;
+  }
+
+  function to_rad(deg) {
+    return deg * Math.PI / 180;
+  }
+
+  function set_checkpoints(lat, lon) {
+    var bearing_rad,
+      lat_end,
+      lon_end,
+      lat_start = to_rad(lat),
+      lon_start = to_rad(lon),
+      relative_d;
+
+    DIRECTION_LIST.forEach(function (e) {
+      bearing_rad = to_rad(e.bearing);
+      relative_d = e.distance / R;
+
+      lat_end = Math.asin(Math.sin(lat_start) * Math.cos(relative_d)
+        + Math.cos(lat_start) * Math.sin(relative_d) * Math.cos(bearing_rad));
+      lon_end = lon_start + Math.atan2(
+        Math.sin(bearing_rad) * Math.sin(relative_d) * Math.cos(lat_start),
+        Math.cos(relative_d) - Math.sin(lat_start) * Math.sin(lon_start));
+
+      CHECKPOINT_LIST.push({
+        latitude: to_deg(lat_end),
+        longitude: to_deg(lon_end)
+      });
+
+      lat_start = lat_end;
+      lon_start = lon_end;
+    });
+  }
 
   function distance(lat1, lon1, lat2, lon2) {
-    var R = 6371e3, // meters
-      la1 = lat1 * Math.PI / 180, // lat, lon in radians
+    var la1 = lat1 * Math.PI / 180, // lat, lon in radians
       la2 = lat2 * Math.PI / 180,
       lo1 = lon1 * Math.PI / 180,
       lo2 = lon2 * Math.PI / 180,
@@ -66,50 +95,57 @@
     return 2 * R * Math.asin(Math.sqrt(h));
   }
 
-  me.onStart = function () {
+  me.onStart = function (timestamp) {
     me.direction_set = false;
     me.next_checkpoint = 0;
-    me.parachute_triggered = false;
+    me.takeOff();
   };
 
   me.onUpdate = function (timestamp) {
+    if (!me.isReadyToFly()) {
+      return;
+    }
+
+    me.current_position = me.getCurrentPosition();
+    if (CHECKPOINT_LIST.length === 0) {
+      set_checkpoints(me.current_position.latitude,
+                      me.current_position.longitude);
+    }
+
     if (!me.direction_set) {
       if (me.next_checkpoint < CHECKPOINT_LIST.length) {
         me.setTargetCoordinates(
           CHECKPOINT_LIST[me.next_checkpoint].latitude,
           CHECKPOINT_LIST[me.next_checkpoint].longitude,
-          CHECKPOINT_LIST[me.next_checkpoint].altitude
-            + ALTITUDE + ALTITUDE * me.id
+          ALTITUDE + ALTITUDE * me.id,
+          5
         );
-        console.log("[DEMO] Going to Checkpoint %d", me.next_checkpoint);
+        console.log("[DEMO] Going to Checkpoint", me.next_checkpoint);
       }
       me.direction_set = true;
       return;
     }
 
     if (me.next_checkpoint < CHECKPOINT_LIST.length) {
-      me.current_position = me.getCurrentPosition();
       me.distance = distance(
-        me.current_position.x,
-        me.current_position.y,
+        me.current_position.latitude,
+        me.current_position.longitude,
         CHECKPOINT_LIST[me.next_checkpoint].latitude,
         CHECKPOINT_LIST[me.next_checkpoint].longitude
       );
+
       if (me.distance <= EPSILON) {
-        console.log("[DEMO] Reached Checkpoint %d", me.next_checkpoint);
+        console.log("[DEMO] Reached Checkpoint", me.next_checkpoint);
         me.next_checkpoint += 1;
         me.direction_set = false;
       }
       return;
     }
 
-    if (!me.parachute_triggered) {
-      console.log("[DEMO] Deploying parachute...");
-      me.triggerParachute();
-      me.parachute_triggered = true;
+    if (!me.isLanding()) {
+      me.land();
     }
-
-    if (me.landed()) {
+    if (me.getCurrentPosition().altitude <= 0) {
       me.exit(0);
     }
   };