Initial commit

.gitignore

@@ -0,0 +1 @@
+node_modules

PID.js

@@ -0,0 +1,46 @@
+// from https://github.com/eschnou/ardrone-autonomy/blob/401d7c920af34457a1ebf88e82004c517f5d6a24/lib/PID.js
+module.exports = PID;
+function PID(kp, ki, kd) {
+    this.configure(kp, ki, kd);
+    this.reset();
+}
+
+PID.prototype.configure = function(kp,ki,kd) {
+    this._kp = kp;
+    this._ki = ki;
+    this._kd = kd;
+}
+
+PID.prototype.reset = function() {
+    this._last_time = 0;
+    this._last_error = Infinity;
+    this._error_sum = 0;
+}
+
+PID.prototype.getCommand = function(e) {
+    // Compute dt in seconds
+    var time = Date.now();
+    var dt = (time - this._last_time) / 1000
+
+    var de = 0;
+    if (this._last_time != 0) {
+        // Compute de (error derivation)
+        if (this._last_error < Infinity) {
+            de = (e - this._last_error) / dt;
+        }
+
+        // Integrate error
+        this._error_sum += e * dt;
+    }
+
+    // Update our trackers
+    this._last_time = time;
+    this._last_error = e;
+
+    // Compute commands
+    var command = this._kp * e
+                + this._ki * this._error_sum
+                + this._kd * de;
+
+    return command;
+}

index.js

@@ -0,0 +1,68 @@
+var arDrone  = require('ar-drone');
+var PID      = require('./PID');
+var vincenty = require('node-vincenty');
+
+var yawPID = new PID(1.0, 0, 0.30);
+var client = arDrone.createClient();
+
+client.config('general:navdata_demo', 'FALSE');
+
+client.takeoff()
+
+var targetLat, targetLon, targetYaw, cyaw;
+
+setTimeout(function(){
+  // end of garden: 51.392059 -2.3224394
+  targetLat = 51.392059
+  targetLon = -2.3224394
+}, 6000)
+
+var handleNavData = function(data){
+  if ( data.demo == null) return;
+
+  var currentLat = data.gps.latitude
+  var currentLon = data.gps.longitude
+  console.log('lat/lon:', currentLat, currentLon);
+
+  var currentYaw = data.demo.rotation.yaw;
+
+  if (targetLat == null || targetLon == null || currentYaw ==  null || currentLat == null || currentLon == null) return;
+
+  var bearing = vincenty.distVincenty(currentLat, currentLon, targetLat, targetLon)
+
+  if(bearing.distance > 1){
+    console.log('distance', bearing.distance)
+    console.log('bearing:', bearing.initialBearing)
+    targetYaw = bearing.initialBearing
+
+    console.log('currentYaw:', currentYaw);
+    var eyaw = targetYaw - currentYaw;
+    console.log('eyaw:', eyaw);
+
+    var uyaw = yawPID.getCommand(eyaw);
+    console.log('uyaw:', uyaw);
+
+    var cyaw = within(uyaw, -1, 1);
+    console.log('cyaw:', cyaw);
+
+    client.clockwise(cyaw)
+    client.front(0.05)
+  } else {
+    targetYaw = null
+    client.stop()
+    console.log('Reached ', targetLat, targetLon)
+  }
+}
+
+client.on('navdata', handleNavData);
+
+function within(x, min, max) {
+  if (x < min) {
+      return min;
+  } else if (x > max) {
+      return max;
+  } else {
+      return x;
+  }
+}
+

log.js

@@ -0,0 +1,14 @@
+var arDrone  = require('ar-drone');
+var client = arDrone.createClient();
+
+client.config('general:navdata_demo', 'FALSE');
+
+setInterval(function(){
+  client.once('navdata', function(data){
+    var currentLat = data.gps.latitude
+    var currentLon = data.gps.longitude
+    console.log('lat/lon:', currentLat, currentLon);
+    console.log('battery', data.demo.batteryPercentage)
+  });
+}, 1000)
+

package.json

@@ -0,0 +1,27 @@
+{
+  "name": "ar-drone-gps",
+  "version": "0.0.1",
+  "description": "",
+  "main": "index.js",
+  "dependencies": {
+    "ar-drone": "git://github.com/andrew/node-ar-drone.git#gps",
+    "node-vincenty": "0.0.6"
+  },
+  "devDependencies": {},
+  "scripts": {
+    "test": "echo \"Error: no test specified\" && exit 1"
+  },
+  "repository": {
+    "type": "git",
+    "url": "https://github.com/andrew/ar-drone-gps"
+  },
+  "keywords": [
+    "nodecopter",
+    "gps"
+  ],
+  "author": "Andrew Nesbitt",
+  "license": "MIT",
+  "bugs": {
+    "url": "https://github.com/andrew/ar-drone-gps/issues"
+  }
+}