From b50e25bb1294b2e01d986a685e6660835e13ce1f Mon Sep 17 00:00:00 2001
From: Andrew Nesbitt <andrewnez@gmail.com>
Date: Tue, 1 Oct 2013 15:55:21 +0100
Subject: [PATCH] Add waypoints to map, follow mode and show battery life

---
 index.html | 113 ++++++++++++++++++++++++++++++++++++++++++++---------
 server.js  |  31 +++++++++------
 2 files changed, 114 insertions(+), 30 deletions(-)

diff --git a/index.html b/index.html
index bfda777..c833084 100644
--- a/index.html
+++ b/index.html
@@ -69,16 +69,19 @@ body{
   <div id='data'>
     <div id="phone-position">
       Phone:
-      <span class='lat'></span>,
-      <span class='lon'></span>
-      (+/-<span class='accuracy'></span>m)
+      <span class='lat'>0</span>,
+      <span class='lon'>0</span>
+      (+/-<span class='accuracy'>0</span>m)
     </div>
     <div id="drone-position">
       Drone: 
-      <span class='lat'></span>,
-      <span class='lon'></span>
+      <span class='lat'>0</span>,
+      <span class='lon'>0</span>
       <p>Distance:
-        <span class='distance'></span>
+        <span class='distance'>0</span>
+      </p>
+      <p>Battery:
+        <span class='battery'>0</span>%
       </p>
     </div>
   </div>
@@ -91,8 +94,12 @@ body{
   <script src="/socket.io/socket.io.js"></script>
   <script>
     var socket = io.connect('/');
-    var lat, lon, map, laptop, drone, phone, phonePath;
-    var stop = true;
+    var lat, lon, map, laptop, drone, phone, phonePath, waypointPath, dronePath, startPosition;
+    var targetLat, targetLon;
+    var waypointMarkers = [];
+    var activeWaypoints = [];
+    var waypoints = [];
+    var follow = false;
   
     var phoneIcon = L.icon({
         iconUrl: 'images/iphone.png'
@@ -120,39 +127,106 @@ body{
       map.addLayer(googleLayer);
     
       laptop = L.marker([lat, lon], {icon: laptopIcon}).addTo(map)
+      
+      map.on('click', function(e) {
+        waypointMarkers.push(L.marker(e.latlng).addTo(map))
+        waypoints.push([e.latlng.lat, e.latlng.lng])
+        if(waypointPath == undefined){
+          waypointPath = L.polyline(waypoints, {color: 'blue'}).addTo(map);
+        } else {
+          waypointPath.setLatLngs(waypoints)
+        }
+      });
     }
   
     function defaultMap(err){
       console.log(err)
       initMap({coords: { latitude: 51, longitude: 2 }})
     }
-  
+    
+    function clearWaypoints(){
+      waypoints = []
+      map.removeLayer(waypointPath)
+      waypointPath = undefined
+      $.each(waypointMarkers, function(i,m){map.removeLayer(m)})
+    }
+    
+    function setCurrentTarget(lat, lon){
+      targetLat = lat
+      targetLon = lon
+      socket.emit('go', {lat: targetLat, lon: targetLon})
+    }
+    
+    function clearCurrentTarget(){
+      targetLat = undefined
+      targetLon = undefined
+      socket.emit('stop')
+    }
+
      $(function(){
        $('#takeoff').click(function(){
+         follow = false
          socket.emit('takeoff')
+         if (drone != null){
+           startPosition = [drone._latlng.lat, drone._latlng.lng]
+         }
        })
        $('#land').click(function(){
+         follow = false
          socket.emit('land')
+         startPosition = []
        })
-       $('#move').click(function(){
-         stop = false;
+       $('#reset').click(function(){
+         socket.emit('reset')
        })
        $('#stop').click(function(){
-         stop = true;
+         follow = false
+         clearCurrentTarget()
+       })
+       $('#clear').click(function(){
+         follow = false
+         clearWaypoints()
+       })
+       $('#home').click(function(){
+         follow = false
+         activeWaypoints = [startPosition[0], startPosition[1]]
+         setCurrentTarget(startPosition[0], startPosition[1])
+       })
+       $('#go').click(function(){
+         follow = false
+         if(waypoints.length > 0){
+           activeWaypoints = waypoints.slice(0);
+           // go to next waypoint
+           setCurrentTarget(activeWaypoints[0][0], activeWaypoints[0][1])
+         }
+       })
+       $('#follow').click(function(){
+         follow = true
+       })
+       $('#manual').click(function(){
+         // control via xbox
+         follow = false
        })
      })
 
     socket.on('connect', function(){
-
-
-      console.log('connected')
-    
-      socket.on('position', function(data){
+      socket.on('waypointReached', function(data){
+       activeWaypoints.shift()
+       if(activeWaypoints.length > 0){
+          // go to next waypoint
+          setCurrentTarget(activeWaypoints[0][0], activeWaypoints[0][1])
+        }
+      })
+      socket.on('drone', function(data){
         if(data.lat != undefined){
           if (drone == null){
             drone = L.marker([data.lat, data.lon], {icon: droneIcon}).addTo(map)
+            dronePath = L.polyline([[data.lat, data.lon]], {color: 'green'}).addTo(map);
+          } else{
+            drone.setLatLng([data.lat, data.lon])
+            dronePath.addLatLng([data.lat, data.lon])
           }
-          drone.setLatLng([data.lat, data.lon])
+          $('#drone-position .battery').text(data.battery)
           $('#drone-position .lat').text(data.lat)
           $('#drone-position .lon').text(data.lon)
           $('#drone-position .distance').text(data.distance)
@@ -171,6 +245,9 @@ body{
             phone.setLatLng([data.lat, data.lon])
             phonePath.addLatLng([data.lat, data.lon])
           }
+          if(follow){
+            setCurrentTarget(data.lat, data.lon)
+          }
           $('#phone-position .lat').text(data.lat)
           $('#phone-position .lon').text(data.lon)
           $('#phone-position .accuracy').text(data.accuracy)
diff --git a/server.js b/server.js
index ed6c0bd..2110f51 100644
--- a/server.js
+++ b/server.js
@@ -29,29 +29,26 @@ io.sockets.on('connection', function(socket) {
   socket.on('land', function(data){
     console.log('land', data)
     client.land()
-  })  
+  })
   
+  socket.on('reset', function(data){
+    console.log('reset', data)
+    client.disableEmergency()
+  })
   socket.on('phone', function(data){
     console.log('phone', data)
     targetLat = data.lat
     targetLon = data.lon
     phoneAccuracy = data.accuracy
   })  
-  
   socket.on('stop', function(data){
-    console.log('stop', data)
-    targetYaw = null
-    targetLat = null
-    targetLon = null
-    client.stop()
+    stop()
   })  
 
   setInterval(function(){
-    io.sockets.emit('position', {lat: currentLat, lon: currentLon, distance: currentDistance})
-    io.sockets.emit('drone', {lat: currentLat, lon: currentLon, yaw: currentYaw})
+    io.sockets.emit('drone', {lat: currentLat, lon: currentLon, yaw: currentYaw, distance: currentDistance, battery: battery})
     io.sockets.emit('phone', {lat: targetLat, lon: targetLon, accuracy: phoneAccuracy})
   },1000)
-  
 });
 
 var arDrone  = require('ar-drone');
@@ -64,10 +61,19 @@ var client = arDrone.createClient();
 client.config('general:navdata_demo', 'FALSE');
 
 var targetLat, targetLon, targetYaw, cyaw, currentLat, currentLon,currentDistance, currentYaw, phoneAccuracy;
+var battery = 0;
+
+var stop = function(){
+  console.log('stop', data)
+  targetYaw = null
+  targetLat = null
+  targetLon = null
+  client.stop()
+}
 
 var handleNavData = function(data){
   if ( data.demo == null || data.gps == null) return;
-  
+  battery = data.demo.batteryPercentage
   currentLat = data.gps.latitude
   currentLon = data.gps.longitude
 
@@ -97,8 +103,9 @@ var handleNavData = function(data){
     client.front(0.05)
   } else {
     targetYaw = null
-    client.stop()
+    io.sockets.emit('waypointReached', {lat: targetLat, lon: targetLon})
     console.log('Reached ', targetLat, targetLon)
+    stop()
   }
 }