Initial commit

LICENSE

@@ -0,0 +1,27 @@
+Copyright (c) 2012 Bernhard K. Weisshuhn (bkw@codingforce.com) and contributors
+
+ Permission is hereby granted, free of charge, to any person obtaining a copy
+ of this software and associated documentation files (the "Software"), to deal
+ in the Software without restriction, including without limitation the rights
+ to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ copies of the Software, and to permit persons to whom the Software is
+ furnished to do so, subject to the following conditions:
+
+ The above copyright notice and this permission notice shall be included in
+ all copies or substantial portions of the Software.
+
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ THE SOFTWARE.
+
+This code includes code from the following MIT licenced projects:
+
+* https://github.com/bjnortier/autopilot
+
+* https://github.com/Mikhus/canv-gauge
+
+

README.md

@@ -0,0 +1,58 @@
+# ardrone-autonomy
+
+Provides key building blocks to create autonomous flight applications
+with the AR.Drone. Works as a layer on top of the [node-ar-drone]() library.
+
+This work is based on the [Visual Navigation for Flying Robots](http://vision.in.tum.de/teaching/ss2013/visnav2013) course.
+
+**This work has just started, this branch is the development branch, there are no packaged release yet.**
+
+## Planned features
+
+Here is a list of all the cool stuff this library could provide one day.
+
+* **Simple State estimation** based on the integration of the drone odometry.
+
+* **Extended Kalman Filter** leveraging the onboard tag detection as the observation source
+for an Extended Kalman Filter. This provides much more stable and usable state estimate.
+
+* **PID Controler** to autonomously control the drone position.
+
+* **Visual motion estimation** estimates motion from visual image processing.
+
+
+## Usage
+
+```
+var client = require('ar-drone').createClient();
+var autonomy = require('autonomy').stateEstimate(client);
+
+autonomy.on('positon', function(position) {
+    console.log('Estimated position: %j', position);
+});
+
+```
+
+## License
+
+The MIT License
+
+Copyright (c) 2013 by Laurent Eschenauer <laurent@eschenauer.be>
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.

index.js

@@ -0,0 +1,9 @@
+var autonomy = exports;
+
+exports.StateEstimator = require('./lib/StateEstimator');
+
+exports.estimateState = function(client, options) {
+    var estimator = new autonomy.StateEstimator(client, options);
+    return estimator;
+}
+

lib/StateEstimator.js

@@ -0,0 +1,66 @@
+var EventEmitter = require('events').EventEmitter;
+var util         = require('util');
+
+module.exports = StateEstimator;
+util.inherits(StateEstimator, EventEmitter);
+function StateEstimator(client, options) {
+  EventEmitter.call(this);
+
+  options = options || {};
+
+  this._options           = options;
+  this._client            = client;
+  this._delta_t           = options.delta_t || StateEstimator.DELTA_T;
+  this._state             = {roll: 0, pitch: 0, yaw: 0, x: 0, y: 0, z: 0};
+  this._mode              = options.mode || "yaw";
+
+  if (this._client == null) throw new Error("This won't work if you don't pass a proper ardrone client.");
+
+  console.log('State estimator initialized in %s mode.', this._mode);
+
+  this._bind();
+}
+
+StateEstimator.DELTA_T = 1 / 15; // In demo mode, 15 navdata per second
+
+StateEstimator.prototype.state = function() {
+    return this._state;
+}
+
+StateEstimator.prototype._bind = function() {
+    var self = this;
+    this._client.on('navdata', function(data) {
+        self._processNavData(data);
+    });
+}
+
+StateEstimator.prototype._processNavData = function(data) {
+    var pitch = data.demo.rotation.pitch.toRad()
+      , roll  = data.demo.rotation.roll.toRad()
+      , yaw   = data.demo.rotation.yaw.toRad()
+      , mag   = data.magneto.heading.fusionUnwrapped.toRad()
+      , vx    = data.demo.velocity.x / 1000 //We want m/s instead of mm/s
+      , vy    = data.demo.velocity.y / 1000
+      , vz    = data.demo.velocity.z / 1000
+      , alt   = data.demo.altitude
+      , dt    = this._delta_t;
+      ;
+
+    var phi = (this._mode == "magneto" && mag != null) ? mag : yaw;
+    
+    this._state.x = this._state.x + dt * (vx * Math.cos(phi) - vy * Math.sin(phi));
+    this._state.y = this._state.y + dt * (vx * Math.sin(phi) + vy * Math.cos(phi));
+    this._state.z = alt;
+    this._state.roll = roll;
+    this._state.pitch = pitch;
+    this._state.yaw = yaw;
+
+    this.emit('state', this._state);
+};
+
+/** Converts numeric degrees to radians */
+if (typeof(Number.prototype.toRad) === "undefined") {
+  Number.prototype.toRad = function() {
+    return this * Math.PI / 180;
+  }
+}

package.json

@@ -0,0 +1,22 @@
+{
+  "name": "ardrone-autonomy",
+  "version": "0.1.0",
+  "description": "Building blocks for autonomous flying an AR.Drone.",
+  "repository": {
+    "type": "git",
+    "url": "git@github.com:eschnou/ardrone-autonomy.git"
+  },
+  "keywords": [
+    "drone",
+    "ardrone",
+    "nodecopter",
+    "parrot",
+    "autonomous",
+    "kalman",
+    "pid"
+  ],
+  "dependencies": {
+  },
+  "author": "Laurent Eschenauer <laurent@eschenauer.be>",
+  "license": "MIT"
+}