Updated micro revision

Fix syntax error in README

.gitignore

@@ -1,6 +1,4 @@
+node_modules
 *.swp
-_site/
-examples/
-lib/
 mission*
-node_modules/
+pano*

Changes.md

@@ -0,0 +1,15 @@
+# Changes
+
+This file is a manually maintained list of changes for each release. Feel free
+to send corrections if you spot any mistakes.
+
+## v0.1.1 (2013-09-01)
+
+* Fixed issue with cw/ccw yaw rotation
+* Fixed issue with improper yaw reset on some moves
+* Added ctrl-c logic to example for emegency landing
+
+## v0.1.0 (2013-08-23)
+
+* Initial release to npm
+

LICENSE

@@ -0,0 +1,19 @@
+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.

README.md

@@ -0,0 +1,176 @@
+# ardrone-autonomy
+
+An autonomous flight library for the ARDrone, built on top of 
+the [node-ar-drone](https://github.com/felixge/node-ar-drone) library. 
+Instead of directly controlling the drone speed, you can use Autonomy 
+to plan and execute missions by describing the path, altitude and 
+orientation the drone must follow.
+
+If you are a #nodecopter enthusiast, then this library will enable you
+to focus on higher level use cases and experiments. You focus on where
+you want to go, the library takes your drone there.
+
+This work is based on the [Visual Navigation for Flying Robots](http://vision.in.tum.de/teaching/ss2013/visnav2013) course.
+
+**WARNING:**  This is early work. _Autonomous_ means that this library will move your drone
+automaticaly to reach a given target. There isn't much security in place yet, so if you 
+do something wrong, you may have your drone fly away :-)
+
+**!! Experiment with this library in a closed/controlled environment before going in the wild !!**
+
+## Features
+
+* **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.
+
+* **Camera projection and back-projection** to estimate the position of an object detected by the camera.
+Currently used to estimate a tag position in the drone coordinate system based on its detection
+by the bottom camera.
+
+* **PID Controler** to autonomously control the drone position.
+
+* **Mission planner** to prepare a flight/task plan and then execute it.
+
+### Planned features
+
+* **VSLAM** to improve the drone localization estimates.
+
+* **Object tracking** to detect and track objects in the video stream.
+
+## Mission
+
+This module exposes a high level API to plan and execute missions, by focusing on where
+the drone should go instead of its low-level movements. Here is a simple example, 
+with the drone taking off, travelling alongs a 2 x 2 meters square ane then landing.
+
+```js
+var autonomy = require('ardrone-autonomy');
+var mission  = autonomy.createMission();
+
+mission.takeoff()
+       .zero()       // Sets the current state as the reference
+       .altitude(1)  // Climb to altitude = 1 meter
+       .forward(2)   
+       .right(2)     
+       .backward(2) 
+       .left(2)
+       .hover(1000)  // Hover in place for 1 second
+       .land();
+
+mission.run(function (err, result) {
+    if (err) {
+        console.trace("Oops, something bad happened: %s", err.message);
+        mission.client().stop();
+        mission.client().land();
+    } else {
+        console.log("Mission success!");
+        process.exit(0);
+    }
+});
+```
+
+### Mission API
+
+#### mission.log(path)
+
+Log the mission data, csv formatted, in the given file. Makes it really usefull to
+debug/plot the state and controller behavior.
+
+#### mission.run(callback)
+
+Execute the mission. The callback has the form `function(err,result)` and will be triggered in
+case of error or at the end of the mission.
+
+#### mission.takeoff()
+
+Add a takeoff step to the mission.
+
+#### mission.forward/backward/left/right/up/down(distance)
+
+Add a movement step to the mission. The drone will move in the given direction by the distance (in meters) before
+proceeding to next step. The drone will also attempt to maintain all other degrees of freedom.
+
+#### mission.altitude(height)
+
+Add a altitude step to the mission. Will climb to the given height before proceeding to next step.
+
+#### mission.cw/ccw(angle)
+
+Add a rotation step to the mission. Will turn by the given angle (in Deg) before proceeding to the next step.
+
+#### mission.hover(delay)
+
+Add a hover step to the mission. Will hover in place for the given delay (in ms) before proceeding to next step.
+
+#### mission.wait(delay)
+
+Add a wait step to the mission. Will wait for the given delay (in ms) before proceeding to next step.
+
+#### mission.go(position)
+
+Add a movement step to the mission. Will go the given position before proceeding to next step. The position is a Controller goal such as {x: 0, y: 0, z: 1, yaw: 90}.
+
+#### mission.task(function(callback){..})
+
+Add a task step to the mission. Will execute the provided function before proceeding to the next step. A callback argument is passed to the function, it should be called when the 
+task is done.
+
+#### mission.taskSync(function)
+
+Add a task step to the mission. Will execute the provided function before proceeding to the next step.
+
+#### mission.zero()
+
+Add a zeroing step to the mission. This will set the current position/orientation as 
+the base state of the kalman filter (i.e. {x: 0, y:0, yaw:0}). If you are not using
+a tag as your base position, it is a good idea to zero() after takeoff.
+
+## Controller API
+
+This module exposes a high level API to control the position. It is built using an
+Extended Kalman Filter to estimate the position and a PID controller to move the drone
+to a given target.
+
+The easiest way to try the Controller is to play with the repl provided in the examples:
+
+```js
+$ node examples/repl.js
+// Make the drone takeoff
+drone> takeoff()
+// Move the drone to position (1,1)
+drone> ctrl.go({x: 1, y:1});
+// Climb to altitude 2 meters
+drone> ctrl.altitude(2);
+// Spin 90 deg to the right
+drone> ctrl.cw(90);
+// Go back to (0,0)
+drone> ctrl.go({x:0, y:0});
+// Hover in place
+drone> ctrl.hover();
+// Land
+drone> land();
+```
+
+## 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.

examples/hover.js

@@ -0,0 +1,43 @@
+var df = require('dateformat')
+  , autonomy = require('../')
+  , mission  = autonomy.createMission()
+  , arDrone = require('ar-drone')
+  , arDroneConstants = require('ar-drone/lib/constants')
+  ;
+
+function navdata_option_mask(c) {
+  return 1 << c;
+}
+
+// From the SDK.
+var navdata_options = (
+    navdata_option_mask(arDroneConstants.options.DEMO)
+  | navdata_option_mask(arDroneConstants.options.VISION_DETECT)
+  | navdata_option_mask(arDroneConstants.options.MAGNETO)
+  | navdata_option_mask(arDroneConstants.options.WIFI)
+);
+
+// Connect and configure the drone
+mission.client().config('general:navdata_demo', true);
+mission.client().config('general:navdata_options', navdata_options);
+mission.client().config('video:video_channel', 1);
+mission.client().config('detect:detect_type', 12);
+
+mission.log("mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt");
+
+mission.takeoff()
+       .go({x:0, y:0, z:1.5})
+       .hover(30000)
+       .land();
+
+mission.run(function (err, result) {
+    if (err) {
+        console.trace("Oops, something bad happened: %s", err.message);
+        mission.client().stop();
+        mission.client().land();
+    } else {
+        console.log("We are done!");
+        process.exit(0);
+    }
+});
+

examples/pano.js

@@ -0,0 +1,48 @@
+var df = require('dateformat')
+  , autonomy = require('../')
+  , mission  = autonomy.createMission()
+  ;
+
+// Land on ctrl-c
+var exiting = false;
+process.on('SIGINT', function() {
+    if (exiting) {
+        process.exit(0);
+    } else {
+        console.log('Got SIGINT. Landing, press Control-C again to force exit.');
+        exiting = true;
+        mission.control().disable();
+        mission.client().land(function() {
+            process.exit(0);
+        });
+    }
+});
+
+// Log mission data for debugging
+mission.log("mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt");
+
+// Plan pano mission
+mission.takeoff()
+       .zero()
+       .hover(1000)
+       .go({x:0, y:0})
+       .altitude(1.5)
+       .cw(90)
+       .cw(90)
+       .cw(90)
+       .cw(90)
+       .altitude(0.5)
+       .land();
+
+// Execute mission
+mission.run(function (err, result) {
+    if (err) {
+        console.trace("Oops, something bad happened: %s", err.message);
+        mission.client().stop();
+        mission.client().land();
+    } else {
+        console.log("We are done!");
+        process.exit(0);
+    }
+});
+

examples/repl.js

@@ -0,0 +1,65 @@
+var fs = require('fs');
+var path = require('path');
+var df = require('dateformat')
+  , arDrone = require('ar-drone')
+  , arDroneConstants = require('ar-drone/lib/constants')
+  , autonomy = require('..');
+
+var client  = arDrone.createClient();
+var ctrl    = new autonomy.Controller(client, {debug: false});
+var repl    = client.createRepl();
+
+function navdata_option_mask(c) {
+  return 1 << c;
+}
+
+// From the SDK.
+var navdata_options = (
+    navdata_option_mask(arDroneConstants.options.DEMO)
+  | navdata_option_mask(arDroneConstants.options.VISION_DETECT)
+  | navdata_option_mask(arDroneConstants.options.MAGNETO)
+  | navdata_option_mask(arDroneConstants.options.WIFI)
+);
+
+// Connect and configure the drone
+client.config('general:navdata_demo', true);
+client.config('general:navdata_options', navdata_options);
+client.config('video:video_channel', 1);
+client.config('detect:detect_type', 12);
+
+// Add a ctrl object to the repl. You can use the controller
+// from there. E.g.
+// ctrl.go({x:1, y:1});
+//
+repl._repl.context['ctrl'] = ctrl;
+
+// Log control data for debugging
+var folder = df(new Date(), "yyyy-mm-dd_hh-MM-ss");
+fs.mkdir(path.join('/tmp', folder), function() {
+  dataStream = fs.createWriteStream(path.join('/tmp', folder, 'data.txt'));
+});
+
+ctrl.on('controlData', function(d) {
+   dataStream.write(d.state.x + "," +
+                    d.state.y + "," +
+                    d.state.z + "," +
+                    d.state.yaw + "," +
+                    d.state.vx + "," +
+                    d.state.vy + "," +
+                    d.goal.x + "," +
+                    d.goal.y + "," +
+                    d.goal.z + "," +
+                    d.goal.yaw + "," +
+                    d.error.ex + "," +
+                    d.error.ey + "," +
+                    d.error.ez + "," +
+                    d.error.eyaw + "," +
+                    d.control.ux + "," +
+                    d.control.uy + "," +
+                    d.control.uz + "," +
+                    d.control.uyaw + "," +
+                    d.last_ok + "," +
+                    d.tag + "\n");
+});
+
+

examples/square-2.js

@@ -0,0 +1,71 @@
+var df = require('dateformat')
+  , autonomy = require('../')
+  , arDrone = require('ar-drone')
+  , arDroneConstants = require('ar-drone/lib/constants')
+  , mission  = autonomy.createMission()
+  ;
+
+function navdata_option_mask(c) {
+  return 1 << c;
+}
+
+// From the SDK.
+var navdata_options = (
+    navdata_option_mask(arDroneConstants.options.DEMO)
+  | navdata_option_mask(arDroneConstants.options.VISION_DETECT)
+  | navdata_option_mask(arDroneConstants.options.MAGNETO)
+  | navdata_option_mask(arDroneConstants.options.WIFI)
+);
+
+// Land on ctrl-c
+var exiting = false;
+process.on('SIGINT', function() {
+    if (exiting) {
+        process.exit(0);
+    } else {
+        console.log('Got SIGINT. Landing, press Control-C again to force exit.');
+        exiting = true;
+        mission.control().disable();
+        mission.client().land(function() {
+            process.exit(0);
+        });
+    }
+});
+
+// Connect and configure the drone
+mission.client().config('general:navdata_demo', true);
+mission.client().config('general:navdata_options', navdata_options);
+mission.client().config('video:video_channel', 1);
+mission.client().config('detect:detect_type', 12);
+
+// Log mission for debugging purposes
+mission.log("mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt");
+
+// Plan mission
+mission.takeoff()
+       .zero()
+       .hover(500)
+       .altitude(2)
+       .forward(2)
+       .cw(90)
+       .forward(2)
+       .cw(90)
+       .forward(2)
+       .cw(90)
+       .forward(2)
+       .go({x:0, y:0})
+       .hover(500)
+       .land();
+
+// Execute mission
+mission.run(function (err, result) {
+    if (err) {
+        console.trace("Oops, something bad happened: %s", err.message);
+        mission.client().stop();
+        mission.client().land();
+    } else {
+        console.log("We are done!");
+        process.exit(0);
+    }
+});
+

examples/square.js

@@ -0,0 +1,67 @@
+var df = require('dateformat')
+  , autonomy = require('../')
+  , arDrone = require('ar-drone')
+  , arDroneConstants = require('ar-drone/lib/constants')
+  , mission  = autonomy.createMission()
+  ;
+
+function navdata_option_mask(c) {
+  return 1 << c;
+}
+
+// From the SDK.
+var navdata_options = (
+    navdata_option_mask(arDroneConstants.options.DEMO)
+  | navdata_option_mask(arDroneConstants.options.VISION_DETECT)
+  | navdata_option_mask(arDroneConstants.options.MAGNETO)
+  | navdata_option_mask(arDroneConstants.options.WIFI)
+);
+
+// Land on ctrl-c
+var exiting = false;
+process.on('SIGINT', function() {
+    if (exiting) {
+        process.exit(0);
+    } else {
+        console.log('Got SIGINT. Landing, press Control-C again to force exit.');
+        exiting = true;
+        mission.control().disable();
+        mission.client().land(function() {
+            process.exit(0);
+        });
+    }
+});
+
+// Connect and configure the drone
+mission.client().config('general:navdata_demo', true);
+mission.client().config('general:navdata_options', navdata_options);
+mission.client().config('video:video_channel', 1);
+mission.client().config('detect:detect_type', 12);
+
+// Log mission for debugging purposes
+mission.log("mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt");
+
+// Plan mission
+mission.takeoff()
+       .zero()
+       .hover(500)
+       .altitude(2)
+       .forward(2)
+       .right(2)
+       .backward(2)
+       .go({x:0, y:0})
+       .hover(500)
+       .land();
+
+// Execute mission
+mission.run(function (err, result) {
+    if (err) {
+        console.trace("Oops, something bad happened: %s", err.message);
+        mission.client().stop();
+        mission.client().land();
+    } else {
+        console.log("We are done!");
+        process.exit(0);
+    }
+});
+

index.html

@@ -1,189 +0,0 @@
-<!DOCTYPE html>
-<html>
-  <head>
-    <meta charset='utf-8'>
-    <meta http-equiv="X-UA-Compatible" content="chrome=1">
-
-    <link href='http://fonts.googleapis.com/css?family=Audiowide' rel='stylesheet' type='text/css'>
-    <link rel="stylesheet" type="text/css" href="stylesheets/stylesheet.css" media="screen" />
-    <link rel="stylesheet" type="text/css" href="stylesheets/pygment_trac.css" media="screen" />
-    <link rel="stylesheet" type="text/css" href="stylesheets/print.css" media="print" />
-
-    <script type="text/javascript" src="https://ajax.googleapis.com/ajax/libs/jquery/1.8.0/jquery.min.js"></script>
-    <script type="text/javascript" src="https://blockchain.info//Resources/wallet/pay-now-button.js"></script>
-
-    <title>AR.Drone Autonomy</title>
-  </head>
-
-  <body>
-
-    <header>
-      <div class="container">
-          <h1>AR.Drone Autonomy</h1>
-          <h2>Autonomously flying an ARDrone in Javascript!</h2>
-          <a href="https://github.com/eschnou/ardrone-autonomy"><img style="position: absolute; top: 0; right: 0; border: 0;" src="https://s3.amazonaws.com/github/ribbons/forkme_right_red_aa0000.png" alt="Fork me on GitHub"></a>
-      </div>
-    </header>
-
-    <div class="container">
-      <section id="main_content">
-
-<p>An autonomous flight library for the <a href="http://ardrone2.parrot.com/">AR.Drone 2.0</a>, built on top of 
-the <a href="https://github.com/felixge/node-ar-drone">node-ar-drone</a> library. </p>
-
-<p>Instead of directly controlling the drone speed, you can use Autonomy 
-to plan and execute missions by describing the path, altitude and 
-orientation the drone must follow.</p>
-
-<p><strong>Autonomous means that this library will move your drone automaticaly to reach a given target. Experiment with this library in a closed/controlled environment before going in the wild !!</strong></p>
-
-<div class="screenshot">
-  <iframe width="560" height="315" src="//www.youtube.com/embed/wPXsG_fjncM" frameborder="0" allowfullscreen></iframe> 
-</div>
-
-<h2>Features</h2>
-
-<ul>
-<li><p><strong>Extended Kalman Filter</strong> leveraging the onboard tag detection as the observation source
-for an Extended Kalman Filter. This provides much more stable and usable state estimate.</p></li>
-<li><p><strong>Camera projection and back-projection</strong> to estimate the position of an object detected by the camera.
-Currently used to estimate a tag position in the drone coordinate system based on its detection
-by the bottom camera.</p></li>
-<li><p><strong>PID Controler</strong> to autonomously control the drone position.</p></li>
-<li><p><strong>Mission planner</strong> to prepare a flight/task plan and then execute it.</p></li>
-</ul><h3>
-<a name="planned-features" class="anchor" href="#planned-features"><span class="octicon octicon-link"></span></a>Planned features</h3>
-
-<ul>
-<li><p><strong>VSLAM</strong> to improve the drone localization estimates.</p></li>
-<li><p><strong>Object tracking</strong> to detect and track objects in the video stream.</p></li>
-</ul>
-
-<h2>Documentation</h2>
-
-<ul>
-    <li>API Documented in the <a href="https://github.com/eschnou/ardrone-autonomy">README</a></li>
-    <li><a href="http://www.slideshare.net/eschnou/20130807-advanced-programming-with-nodecopter">Advanced Programming with Nodecopter</a> on Slideshare</li>
-    <li><a href="https://eschnou.com/entry/advanced-programming-with-nodecopter-62-25019.html">Blog post</a> with additional example videos</li>
-</ul>
-
-<h2>Example</h2>
-
-<p>This module exposes a high level API to plan and execute missions, by focusing on where
-the drone should go instead of its low-level movements. Here is a simple example, 
-with the drone taking off, travelling alongs a 2 x 2 meters square ane then landing.</p>
-
-<div class="highlight highlight-js"><pre><span class="kd">var</span> <span class="nx">autonomy</span> <span class="o">=</span> <span class="nx">require</span><span class="p">(</span><span class="s1">'ardrone-autonomy'</span><span class="p">);</span>
-<span class="kd">var</span> <span class="nx">mission</span>  <span class="o">=</span> <span class="nx">autonomy</span><span class="p">.</span><span class="nx">createMission</span><span class="p">();</span>
-
-<span class="nx">mission</span><span class="p">.</span><span class="nx">takeoff</span><span class="p">()</span>
-       <span class="p">.</span><span class="nx">zero</span><span class="p">()</span>       <span class="c1">// Sets the current state as the reference</span>
-       <span class="p">.</span><span class="nx">altitude</span><span class="p">(</span><span class="mi">1</span><span class="p">)</span>  <span class="c1">// Climb to altitude = 1 meter</span>
-       <span class="p">.</span><span class="nx">forward</span><span class="p">(</span><span class="mi">2</span><span class="p">)</span>   
-       <span class="p">.</span><span class="nx">right</span><span class="p">(</span><span class="mi">2</span><span class="p">)</span>     
-       <span class="p">.</span><span class="nx">backward</span><span class="p">(</span><span class="mi">2</span><span class="p">)</span> 
-       <span class="p">.</span><span class="nx">left</span><span class="p">(</span><span class="mi">2</span><span class="p">)</span>
-       <span class="p">.</span><span class="nx">hover</span><span class="p">(</span><span class="mi">1000</span><span class="p">)</span>  <span class="c1">// Hover in place for 1 second</span>
-       <span class="p">.</span><span class="nx">land</span><span class="p">();</span>
-
-<span class="nx">mission</span><span class="p">.</span><span class="nx">run</span><span class="p">(</span><span class="kd">function</span> <span class="p">(</span><span class="nx">err</span><span class="p">,</span> <span class="nx">result</span><span class="p">)</span>
-    <span class="k">if</span> <span class="p">(</span><span class="nx">err</span><span class="p">)</span> <span class="p">{</span>
-        <span class="nx">console</span><span class="p">.</span><span class="nx">trace</span><span class="p">(</span><span class="s2">"Oops, something bad happened: %s"</span><span class="p">,</span> <span class="nx">err</span><span class="p">.</span><span class="nx">message</span><span class="p">);</span>
-        <span class="nx">mission</span><span class="p">.</span><span class="nx">client</span><span class="p">().</span><span class="nx">stop</span><span class="p">();</span>
-        <span class="nx">mission</span><span class="p">.</span><span class="nx">client</span><span class="p">().</span><span class="nx">land</span><span class="p">();</span>
-    <span class="p">}</span> <span class="k">else</span> <span class="p">{</span>
-        <span class="nx">console</span><span class="p">.</span><span class="nx">log</span><span class="p">(</span><span class="s2">"Mission success!"</span><span class="p">);</span>
-        <span class="nx">process</span><span class="p">.</span><span class="nx">exit</span><span class="p">(</span><span class="mi">0</span><span class="p">);</span>
-    <span class="p">}</span>
-<span class="p">});</span>
-</pre></div>
-
-<h2>Applications</h2>
-
-Here is a list of know apps built using autonomy. Please let me know if you build something and I'll be happy to add you in the list.
-
-<ul>
-    <li><p><strong><a href="https://github.com/eschnou/ardrone-panorama/">panorama</a></strong> autonomously fly to a given altitude and take pictures to form a 360 photo panorama.</li>
-</ul>
-
-<h2>Support</h2>
-
-<p>If you encounter issues, please add them to the <a href="https://github.com/eschnou/ardrone-autonomy/issues">issue tracker</a>. You can find
-me on twitter (<a href="http://twitter.com/eschnou">@eschnou</a>) or on the #nodecopter IRC channel 
-on #freenode.
-
-<h2>Thanks</h2>
-
-<p>This work is based on the <a href="http://vision.in.tum.de/teaching/ss2013/visnav2013">Visual Navigation for Flying Robots</a> course. My eternal gratitude for their team to post lectures and slides on the web. I learned a lot from them.</p>
-
-<p>Also a big thank you to <a href="https://github.com/felixge">@felixge</a> who came up with this crazy idea of flying a drone with Javascript and building the fantastic <a href="https://github.com/felixge/node-ar-drone">node-ar-drone</a> library.
-
-<h2>Donate</h2>
-
-<p>If you like this project, please consider donating. The less time I need to work, the more I can spend on open source projects :-)</p>
-<div style="font-size:16px;margin:0 auto;width:300px" class="blockchain-btn"
-     data-address="13q2eVG8KkmsFqGHqDWbjHeq3qicxi4rjh"
-     data-shared="false">
-    <div class="blockchain stage-begin">
-        <img src="https://blockchain.info//Resources/buttons/donate_64.png"/>
-    </div>
-    <div class="blockchain stage-loading" style="text-align:center">
-        <img src="https://blockchain.info//Resources/loading-large.gif"/>
-    </div>
-    <div class="blockchain stage-ready">
-         <p align="center">Please Donate To Bitcoin Address: <b>[[address]]</b></p>
-         <p align="center" class="qr-code"></p>
-    </div>
-    <div class="blockchain stage-paid">
-         Donation of <b>[[value]] BTC</b> Received. Thank You.
-    </div>
-    <div class="blockchain stage-error">
-        <font color="red">[[error]]</font>
-    </div>
-</div>
-
-<h2>License</h2>
-
-<p>The MIT License</p>
-
-<p>Copyright (c) 2013 by Laurent Eschenauer <a href="mailto:laurent@eschenauer.be">laurent@eschenauer.be</a></p>
-
-<p>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:</p>
-
-<p>The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.</p>
-
-<p>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.</p>
-      </section>
-    </div>
-
-    <!-- Piwik -->
-    <script type="text/javascript">
-      var _paq = _paq || [];
-      _paq.push(["trackPageView"]);
-      _paq.push(["enableLinkTracking"]);
-
-      (function() {
-        var u=(("https:" == document.location.protocol) ? "https" : "http") + "://analytics.eschnou.com/";
-        _paq.push(["setTrackerUrl", u+"piwik.php"]);
-        _paq.push(["setSiteId", "4"]);
-        var d=document, g=d.createElement("script"), s=d.getElementsByTagName("script")[0]; g.type="text/javascript";
-        g.defer=true; g.async=true; g.src=u+"piwik.js"; s.parentNode.insertBefore(g,s);
-      })();
-    </script>
-    <!-- End Piwik Code -->
-
-    
-  </body>
-</html>

index.js

@@ -0,0 +1,20 @@
+var autonomy = exports;
+var ardrone = require('ar-drone');
+
+exports.EKF = require('./lib/EKF');
+exports.Camera = require('./lib/Camera');
+exports.Controller = require('./lib/Controller');
+exports.Mission = require('./lib/Mission');
+
+exports.control = function(client, options) {
+    return new autonomy.Controller(client, options);
+}
+
+exports.createMission = function(options) {
+    var client  = ardrone.createClient(options);
+    var control = new autonomy.Controller(client, options);
+    var mission = new autonomy.Mission(client, control, options);
+
+    return mission;
+}
+

javascripts/main.js

@@ -1 +0,0 @@
-console.log('This would be the main JS file.');

lib/Camera.js

@@ -0,0 +1,55 @@
+var sylvester    = require('sylvester');
+var util         = require('util');
+
+
+// TODO: Extend to support roll/pitch in back projection
+// TODO: Add support for front-facing camera
+// TODO: Make image aspect ratio configurable
+
+// AR Drone 2.0 Bottom Camera Intrinsic Matrix
+// https://github.com/tum-vision/ardrone_autonomy/blob/master/calibrations/ardrone2_bottom/cal.ymli
+var K_BOTTOM = $M([[686.994766, 0, 329.323208],
+                   [0, 688.195055, 159.323007],
+                   [0, 0, 1]]);
+
+module.exports = Camera;
+function Camera(options) {
+    this._options = options || {};
+    this._k = this._options.k || K_BOTTOM;
+
+    // We need to compute the inverse of K to back-project 2D to 3D
+    this._invK = this._k.inverse();
+}
+
+/*
+ * Given (x,y) pixel coordinates (e.g. obtained from tag detection)
+ * Returns a (X,Y) coordinate in drone space.
+ */
+Camera.prototype.p2m = function(x, y, altitude) {
+    // From the SDK Documentation:
+    // X and Y coordinates of detected tag or oriented roundel #i inside the picture,
+    // with (0; 0) being the top-left corner, and (1000; 1000) the right-bottom corner regardless
+    // the picture resolution or the source camera.
+    //
+    // But our camera intrinsic is built for 640 x 360 pixel grid, so we must do some mapping.
+    var xratio = 640 / 1000;
+    var yratio = 360 / 1000;
+
+    // Perform a simple back projection, we assume the drone is flat (no roll/pitch)
+    // for the moment. We ignore the drone translation and yaw since we want X,Y in the
+    // drone coordinate system.
+    var p = $V([x * xratio, y * yratio, 1]);
+    var P = this._invK.multiply(p).multiply(altitude);
+
+    // X,Y are expressed in meters, in the drone coordinate system.
+    // Which is:
+    //          <--- front-facing camera
+    //        |
+    //       / \------- X
+    //       \_/
+    //        |
+    //        |
+    //        Y
+    return {x: P.e(1), y: P.e(2)};
+}
+

lib/Controller.js

@@ -0,0 +1,387 @@
+var EventEmitter = require('events').EventEmitter;
+var Timers  = require('timers');
+var util    = require('util');
+var PID     = require('./PID');
+var EKF     = require('./EKF');
+var Camera  = require('./Camera');
+
+EPS_LIN      = 0.1; // We are ok with 10 cm horizontal precision
+EPS_ALT      = 0.1; // We are ok with 10 cm altitude precision
+EPS_ANG      = 0.1; // We are ok with 0.1 rad precision (5 deg)
+STABLE_DELAY = 200; // Time in ms to wait before declaring the drone on target
+
+module.exports = Controller;
+util.inherits(Controller, EventEmitter);
+function Controller(client, options) {
+    EventEmitter.call(this);
+
+    options = options || {};
+
+    // A ardrone client to pilot the drone
+    this._client  = client;
+
+    // The position of a roundel tag to detect
+    this._tag     = options.tag || {x: 0, y: 0, yaw: 0};
+
+    // Configure the four PID required to control the drone
+    this._pid_x   = new PID(0.5, 0, 0.35);
+    this._pid_y   = new PID(0.5, 0, 0.35);
+    this._pid_z   = new PID(0.8, 0, 0.35);
+    this._pid_yaw = new PID(1.0, 0, 0.30);
+
+    // kalman filter is used for the drone state estimation
+    this._ekf     = new EKF(options);
+
+    // Used to process images and backproject them
+    this._camera  = new Camera();
+
+    // Control will only work if enabled
+    this._enabled = false;
+
+    // Ensure that we don't enter the processing loop twice
+    this._busy = false;
+
+    // The curretn target goal and an optional callback to trigger
+    // when goal is reached
+    this._goal     = null;
+    this._callback = null;
+
+    // The last known state
+    this._state   = null,
+
+    // The last time we have reached the goal (all control commands = 0)
+    this._last_ok = 0;
+
+    // Register the listener on navdata for our control loop
+    var self = this;
+    client.on('navdata', function(d) {
+        if (!this._busy && d.demo) {
+            this._busy = true;
+            self._processNavdata(d);
+            self._control(d);
+            this._busy = false;
+        }
+    });
+}
+
+/*
+ * Enable auto-pilot. The controller will attempt to bring
+ * the drone (and maintain it) to the goal.
+ */
+Controller.prototype.enable = function() {
+    this._pid_x.reset();
+    this._pid_y.reset();
+    this._pid_z.reset();
+    this._pid_yaw.reset();
+    this._enabled = true;
+};
+
+/*
+ * Disable auto-pilot. The controller will stop all actions
+ * and send a stop command to the drone.
+ */
+Controller.prototype.disable = function() {
+    this._enabled = false;
+    this._client.stop();
+}
+
+/*
+ * Return the drone state (x,y,z,yaw) as estimated
+ * by the Kalman Filter.
+ */
+Controller.prototype.state = function() {
+    return this._state;
+}
+
+/*
+ * Sets the goal to the current state and attempt to hover on top.
+ */
+Controller.prototype.hover = function() {
+    this._go({x: this._state.x, y: this._state.y, z: this._state.z, yaw: this._state.yaw});
+}
+
+/*
+ * Reset the kalman filter to its base state (default is x:0, y:0, yaw:0).
+ *
+ * This is especially usefull to set mark the drone position as the starting position
+ * after takeoff. We must disable, to ensure that the zeroing does not trigger a sudden move
+ * of the drone.
+ */
+Controller.prototype.zero = function() {
+    this.disable();
+    this._ekf.reset();
+}
+
+/*
+ * Move forward (direction faced by the front camera) by the given
+ * distance (in meters).
+ */
+Controller.prototype.forward = function(distance, callback) {
+   // Our starting position
+   var state = this.state();
+
+   // Remap our target position in the world coordinates
+   var gx = state.x + Math.cos(state.yaw) * distance;
+   var gy = state.y + Math.sin(state.yaw) * distance;
+
+   // Assign the new goal
+   this._go({x: gx, y: gy, z: state.z, yaw: state.yaw}, callback);
+}
+
+/*
+ * Move backward by the given distance (in meters).
+ */
+Controller.prototype.backward = function(distance, callback) {
+    return this.forward(-distance, callback);
+}
+
+/*
+ * Move right (front being the direction faced by the front camera) by the given
+ * distance (in meters).
+ */
+Controller.prototype.right = function(distance, callback) {
+   // Our starting position
+   var state = this.state();
+
+   // Remap our target position in the world coordinates
+   var gx = state.x - Math.sin(state.yaw) * distance;
+   var gy = state.y + Math.cos(state.yaw) * distance;
+
+   // Assign the new goal
+   this._go({x: gx, y: gy, z: state.z, yaw: state.yaw}, callback);
+}
+
+/*
+ * Move left by the given distance (in meters).
+ */
+Controller.prototype.left = function(distance, callback) {
+    return this.right(-distance, callback);
+}
+
+/*
+ * Turn clockwise of the given angle. Note that this does not
+ * force a clockwise motion, if the angle is > 180 then the drone
+ * will turn in the other direction, taking the shortest path.
+ */
+Controller.prototype.cw = function(angle, callback) {
+    var state = this.state();
+    var yaw   = state.yaw.toDeg() + angle;
+
+    return this._go({x: state.x, y: state.y, z: state.z, yaw: yaw.toRad()}, callback);
+}
+
+/*
+ * Turn counter clockwise of the given angle (in degrees)
+ */
+Controller.prototype.ccw = function(angle, callback) {
+    return this.cw(-angle, callback);
+}
+
+/*
+ * Climb ups by the given distance (in meters).
+ */
+Controller.prototype.up = function(distance, callback) {
+    var state = this.state();
+    return this._go({x: state.x, y: state.y, z: state.z + distance, yaw: state.yaw}, callback);
+}
+
+/*
+ * Lower itself by the given distance (in meters).
+ */
+Controller.prototype.down = function(distance, callback) {
+    return this.up(-distance, callback);
+}
+
+/*
+ * Go to the target altitude
+ */
+Controller.prototype.altitude = function(altitude, callback) {
+    var state = this.state();
+    return this._go({x: state.x, y: state.y, z: altitude, yaw: state.yaw}, callback);
+}
+
+/*
+ * Go to the target yaw (argument in degree)
+ */
+Controller.prototype.yaw = function(yaw, callback) {
+    var state = this.state();
+    return this._go({x: state.x, y: state.y, z: state.z, yaw: yaw.toRad()}, callback);
+}
+
+/*
+ * Sets a new goal and enable the controller. When the goal
+ * is reached, the callback is called with the current state.
+ *
+ * x,y,z in meters
+ * yaw in degrees
+ */
+Controller.prototype.go = function(goal, callback) {
+    if (goal.yaw != undefined) {
+        goal.yaw = goal.yaw.toRad();
+    }
+
+    return this._go(goal, callback);
+}
+
+Controller.prototype._go = function(goal, callback) {
+    // Since we are going to modify goal settings, we
+    // disable the controller, just in case.
+    this.disable();
+
+    // If no goal given, assume an empty goal
+    goal = goal || {};
+
+    // Normalize the yaw, to make sure we don't spin 360deg for
+    // nothing :-)
+    if (goal.yaw != undefined) {
+        var yaw = goal.yaw;
+        goal.yaw = Math.atan2(Math.sin(yaw),Math.cos(yaw));
+    }
+
+    // Make sure we don't attempt to go too low
+    if (goal.z != undefined) {
+        goal.z = Math.max(goal.z, 0.5);
+    }
+
+    // Update our goal
+    this._goal = goal;
+    this._goal.reached = false;
+
+    // Keep track of the callback to trigger when we reach the goal
+    this._callback = callback;
+
+    // (Re)-Enable the controller
+    this.enable();
+}
+
+Controller.prototype._processNavdata = function(d) {
+    // EKF prediction step
+    this._ekf.predict(d);
+
+    // If a tag is detected by the bottom camera, we attempt a correction step
+    // This require prior configuration of the client to detect the oriented
+    // roundel and to enable the vision detect in navdata.
+    // TODO: Add documentation about this
+    if (d.visionDetect && d.visionDetect.nbDetected > 0) {
+        // Fetch detected tag position, size and orientation
+        var xc = d.visionDetect.xc[0]
+          , yc = d.visionDetect.yc[0]
+          , wc = d.visionDetect.width[0]
+          , hc = d.visionDetect.height[0]
+          , yaw = d.visionDetect.orientationAngle[0]
+          , dist = d.visionDetect.dist[0] / 100 // Need meters
+          ;
+
+        // Compute measure tag position (relative to drone) by
+        // back-projecting the pixel position p(x,y) to the drone
+        // coordinate system P(X,Y).
+        // TODO: Should we use dist or the measure altitude ?
+        var camera = this._camera.p2m(xc + wc/2, yc + hc/2, dist);
+
+        // We convert this to the controller coordinate system
+        var measured = {x: -1 * camera.y, y: camera.x};
+
+        // Rotation is provided by the drone, we convert to radians
+        measured.yaw = yaw.toRad();
+
+        // Execute the EKS correction step
+        this._ekf.correct(measured, this._tag);
+    }
+
+    // Keep a local copy of the state
+    this._state = this._ekf.state();
+    this._state.z = d.demo.altitude;
+    this._state.vx = d.demo.velocity.x / 1000 //We want m/s instead of mm/s
+    this._state.vy = d.demo.velocity.y / 1000
+}
+
+Controller.prototype._control = function(d) {
+    // Do not control if not enabled
+    if (!this._enabled) return;
+
+    // Do not control if no known state or no goal defines
+    if (this._goal == null || this._state ==  null) return;
+
+    // Compute error between current state and goal
+    var ex   = (this._goal.x != undefined)   ? this._goal.x   - this._state.x   : 0
+      , ey   = (this._goal.y != undefined)   ? this._goal.y   - this._state.y   : 0
+      , ez   = (this._goal.z != undefined)   ? this._goal.z   - this._state.z   : 0
+      , eyaw = (this._goal.yaw != undefined) ? this._goal.yaw - this._state.yaw : 0
+      ;
+
+    // Normalize eyaw within [-180, 180]
+    while(eyaw < -Math.PI) eyaw += (2 * Math.PI);
+    while(eyaw >  Math.PI) eyaw -= (2 * Math.PI);
+
+    // Check if we are within the target area
+    if ((Math.abs(ex) < EPS_LIN) && (Math.abs(ey) < EPS_LIN) && (Math.abs(ez) < EPS_ALT) && (Math.abs(eyaw) < EPS_ANG)) {
+        // Have we been here before ?
+        if (!this._goal.reached && this._last_ok != 0) {
+            // And for long enough ?
+            if ((Date.now() - this._last_ok) > STABLE_DELAY) {
+                // Mark the goal has reached
+                this._goal.reached = true;
+
+                // We schedule the callback in the near future. This is to make
+                // sure we finish all our work before the callback is called.
+                if (this._callback != null) {
+                    setTimeout(this._callback, 10);
+                    this._callback = null;
+                }
+
+                // Emit a state reached
+                this.emit('goalReached', this._state);
+            }
+        } else {
+            this._last_ok = Date.now();
+        }
+    } else {
+        // If we just left the goal, we notify
+        if (this._last_ok != 0) {
+            // Reset last ok since we are in motion
+            this._last_ok = 0;
+            this._goal.reached = false;
+            this.emit('goalLeft', this._state);
+        }
+    }
+
+    // Get Raw command from PID
+    var ux = this._pid_x.getCommand(ex);
+    var uy = this._pid_y.getCommand(ey);
+    var uz = this._pid_z.getCommand(ez);
+    var uyaw = this._pid_yaw.getCommand(eyaw);
+
+    // Ceil commands and map them to drone orientation
+    var yaw  = this._state.yaw;
+    var cx   = within(Math.cos(yaw) * ux + Math.sin(yaw) * uy, -1, 1);
+    var cy   = within(-Math.sin(yaw) * ux + Math.cos(yaw) * uy, -1, 1);
+    var cz   = within(uz, -1, 1);
+    var cyaw = within(uyaw, -1, 1);
+
+    // Emit the control data for auditing
+    this.emit('controlData', {
+        state:   this._state,
+        goal:    this._goal,
+        error:   {ex: ex, ey: ey, ez: ez, eyaw: eyaw},
+        control: {ux: ux, uy: uy, uz: uz, uyaw: uyaw},
+        last_ok: this._last_ok,
+        tag:     (d.visionDetect && d.visionDetect.nbDetected > 0) ? 1 : 0
+    });
+
+    // Send commands to drone
+    if (Math.abs(cx) > 0.01) this._client.front(cx);
+    if (Math.abs(cy) > 0.01) this._client.right(cy);
+    if (Math.abs(cz) > 0.01) this._client.up(cz);
+    if (Math.abs(cyaw) > 0.01) this._client.clockwise(cyaw);
+
+}
+
+function within(x, min, max) {
+    if (x < min) {
+        return min;
+    } else if (x > max) {
+        return max;
+    } else {
+        return x;
+    }
+}

lib/EKF.js

@@ -0,0 +1,145 @@
+var sylvester    = require('sylvester');
+var util         = require('util');
+
+var Matrix       = sylvester.Matrix;
+var Vector       = sylvester.Vector;
+
+EKF.DELTA_T = 1 / 15; // In demo mode, 15 navdata per second
+
+module.exports = EKF;
+function EKF(options) {
+
+  options = options || {};
+
+  this._options     = options;
+  this._delta_t     = options.delta_t || EKF.DELTA_T;
+
+  this.reset();
+}
+
+EKF.prototype.state = function() {
+    return this._state;
+}
+
+EKF.prototype.confidence = function() {
+    return this._sigma;
+}
+
+EKF.prototype.reset = function() {
+  this._state       = this._options.state   || {x: 0, y: 0, yaw: 0};
+  this._sigma       = Matrix.I(3);
+  this._q           = Matrix.Diagonal([0.0003, 0.0003, 0.0001]);
+  this._r           = Matrix.Diagonal([0.3, 0.3, 0.3]);
+  this._last_yaw    = null;
+}
+
+EKF.prototype.predict = function(data) {
+    var pitch = data.demo.rotation.pitch.toRad()
+      , roll  = data.demo.rotation.roll.toRad()
+      , yaw   = normAngle(data.demo.rotation.yaw.toRad())
+      , vx    = data.demo.velocity.x / 1000 //We want m/s instead of mm/s
+      , vy    = data.demo.velocity.y / 1000
+      , dt    = this._delta_t
+    ;
+
+    // We are not interested by the absolute yaw, but the yaw motion,
+    // so we need at least a prior value to get started.
+    if (this._last_yaw == null) {
+        this._last_yaw = yaw;
+        return;
+    }
+
+    // Compute the odometry by integrating the motion over delta_t
+    var o = {dx: vx * dt, dy: vy * dt, dyaw: yaw - this._last_yaw};
+    this._last_yaw  = yaw;
+
+    // Update the state estimate
+    var state = this._state;
+    state.x   = state.x + o.dx * Math.cos(state.yaw) - o.dy * Math.sin(state.yaw);
+    state.y   = state.y + o.dx * Math.sin(state.yaw) + o.dy * Math.cos(state.yaw);
+    state.yaw = state.yaw + o.dyaw;
+
+    // Normalize the yaw value
+    state.yaw = Math.atan2(Math.sin(state.yaw),Math.cos(state.yaw));
+
+    // Compute the G term (due to the Taylor approximation to linearize the function).
+    var G = $M(
+           [[1, 0, -1 * Math.sin(state.yaw) * o.dx - Math.cos(state.yaw) * o.dy],
+            [0, 1,  Math.cos(state.yaw) * o.dx - Math.sin(state.yaw) * o.dy],
+            [0, 0, 1]]
+            );
+
+    // Compute the new sigma
+    this._sigma = G.multiply(this._sigma).multiply(G.transpose()).add(this._q);
+}
+ /*
+  * measure.x:   x-position of marker in drone's xy-coordinate system (independant of roll, pitch)
+  * measure.y:   y-position of marker in drone's xy-coordinate system (independant of roll, pitch)
+  * measure.yaw: yaw rotation of marker, in drone's xy-coordinate system (independant of roll, pitch)
+  *
+  * pose.x:   x-position of marker in world-coordinate system
+  * pose.y:   y-position of marker in world-coordinate system
+  * pose.yaw: yaw-rotation of marker in world-coordinate system
+  */
+EKF.prototype.correct = function(measure, pose) {
+    // Compute expected measurement given our current state and the marker pose
+    var state  = this._state;
+    var psi    = state.yaw;
+    this._s    = {x: state.x, y: state.y, yaw: state.yaw};
+
+    // Normalized the measure yaw
+    measure.yaw = normAngle(measure.yaw);
+    this._m = {x: measure.x, y: measure.y, yaw: measure.yaw};
+
+    var z1 = Math.cos(psi) * (pose.x - state.x) + Math.sin(psi) * (pose.y - state.y);
+    var z2 = -1 * Math.sin(psi) * (pose.x - state.x) + Math.cos(psi) * (pose.y - state.y);
+    var z3 = pose.yaw - psi;
+    this._z = {x: z1, y: z2, yaw: z3};
+
+    // Compute the error
+    var e1 = measure.x - z1;
+    var e2 = measure.y - z2;
+    var e3 = measure.yaw - z3;
+    this._e = {x: e1, y: e2, yaw: e3};
+
+    // Compute the H term
+    var H = $M([[ -Math.cos(psi), -Math.sin(psi), Math.sin(psi) * (state.x - pose.x) - Math.cos(psi) * (state.y - pose.y)],
+                [  Math.sin(psi), -Math.cos(psi), Math.cos(psi) * (state.x - pose.x) + Math.sin(psi) * (state.y - pose.y)],
+                [  0, 0, -1]]);
+
+    // Compute the Kalman Gain
+    var Ht = H.transpose();
+    var K = this._sigma.multiply(Ht).multiply(H.multiply(this._sigma).multiply(Ht).add(this._r).inverse())
+
+    // Correct the pose estimate
+    var err = $V([e1, e2, e3]);
+    var c = K . multiply(err);
+    state.x = state.x + c.e(1);
+    state.y = state.y + c.e(2);
+
+//  TODO - This does not work, need more investigation.
+//  In the meanwhile, we don't correct yaw based on observation.
+//  state.yaw = state.yaw + c.e(3);
+
+    this._sigma = Matrix.I(3).subtract(K.multiply(H)).multiply(this._sigma);
+};
+
+function normAngle(rad) {
+    while (rad >  Math.PI) { rad -= 2 * Math.PI;}
+    while (rad < -Math.PI) { rad += 2 * Math.PI;}
+    return rad;
+}
+
+/** Converts numeric degrees to radians */
+if (typeof(Number.prototype.toRad) === "undefined") {
+  Number.prototype.toRad = function() {
+    return this * Math.PI / 180;
+  }
+}
+
+/** Converts radians to numeric dregrees */
+if (typeof(Number.prototype.toDeg) === "undefined") {
+  Number.prototype.toDeg = function() {
+    return this * 180 / Math.PI;
+  }
+}

lib/Mission.js

@@ -0,0 +1,243 @@
+var async = require('async')
+  , fs    = require('fs')
+  ;
+
+module.exports = Mission;
+function Mission(client, controller, options) {
+
+    options = options || {};
+
+    this._options   = options;
+    this._client    = client;
+    this._control   = controller;
+
+    this._steps     = [];
+}
+
+Mission.prototype.client = function() {
+    return this._client;
+}
+
+Mission.prototype.control = function() {
+    return this._control;
+}
+
+Mission.prototype.run = function(callback) {
+    async.waterfall(this._steps, callback);
+}
+
+Mission.prototype.log = function(path) {
+    var dataStream = fs.createWriteStream(path);
+    var ekf = this._control._ekf;
+
+    this._control.on('controlData', function(d) {
+        var log = (d.state.x + "," +
+                   d.state.y + "," +
+                   d.state.z + "," +
+                   d.state.yaw + "," +
+                   d.state.vx + "," +
+                   d.state.vy + "," +
+                   d.goal.x + "," +
+                   d.goal.y + "," +
+                   d.goal.z + "," +
+                   d.goal.yaw + "," +
+                   d.error.ex + "," +
+                   d.error.ey + "," +
+                   d.error.ez + "," +
+                   d.error.eyaw + "," +
+                   d.control.ux + "," +
+                   d.control.uy + "," +
+                   d.control.uz + "," +
+                   d.control.uyaw + "," +
+                   d.last_ok + "," +
+                   d.tag);
+
+        if (d.tag > 0) {
+            log = log + "," +
+                  ekf._s.x + "," +
+                  ekf._s.y + "," +
+                  ekf._s.yaw.toDeg() + "," +
+                  ekf._m.x + "," +
+                  ekf._m.y + "," +
+                  ekf._m.yaw.toDeg() + "," +
+                  ekf._z.x + "," +
+                  ekf._z.y + "," +
+                  ekf._z.yaw.toDeg() + "," +
+                  ekf._e.x + "," +
+                  ekf._e.y + "," +
+                  ekf._e.yaw.toDeg()
+        } else {
+            log = log + ",0,0,0,0,0,0"
+        }
+
+        log = log + "\n";
+
+        dataStream.write(log);
+    });
+}
+
+Mission.prototype.takeoff = function() {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._client.takeoff(cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.land = function() {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._client.land(cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.hover = function(delay) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.hover();
+        setTimeout(cb, delay);
+    });
+
+    return this;
+}
+
+Mission.prototype.wait = function(delay) {
+    this._steps.push(function(cb) {
+        setTimeout(cb, delay);
+    });
+
+    return this;
+}
+
+Mission.prototype.task = function(task) {
+    this._steps.push(function(cb) {
+        task(cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.taskSync = function(task) {
+    this._steps.push(function(cb) {
+        task();
+        cb();
+    });
+
+    return this;
+}
+
+Mission.prototype.zero = function() {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.zero();
+        cb();
+    });
+
+    return this;
+}
+
+Mission.prototype.go = function(goal) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.go(goal, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.forward = function(distance) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.forward(distance, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.backward = function(distance) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.backward(distance, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.left = function(distance) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.left(distance, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.right = function(distance) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.right(distance, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.up = function(distance) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.up(distance, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.down = function(distance) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.down(distance, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.cw = function(angle) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.cw(angle, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.ccw = function(angle) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.ccw(angle, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.altitude = function(altitude) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.altitude(altitude, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.yaw = function(angle) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.yaw(angle,cb);
+    });
+
+    return this;
+}
+
+
+
+

lib/PID.js

@@ -0,0 +1,45 @@
+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;
+}

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,32 @@
+{
+  "name": "ardrone-autonomy",
+  "version": "0.1.2",
+  "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"
+  ],
+  "scripts": {
+    "test": "node tests/run.js"
+  },
+  "dependencies": {
+    "sylvester": "0.0.21",
+    "async": "0.2.9"
+  },
+  "devDependencies": {
+    "utest": "0.0.6",
+    "urun": "0.0.6",
+    "sinon": "1.4.2"
+  },
+  "author": "Laurent Eschenauer <laurent@eschenauer.be>",
+  "license": "MIT"
+}

stylesheets/pygment_trac.css

@@ -1,68 +0,0 @@
-.highlight .c { color: #999988; font-style: italic } /* Comment */
-.highlight .err { color: #a61717; background-color: #e3d2d2 } /* Error */
-.highlight .k { font-weight: bold } /* Keyword */
-.highlight .o { font-weight: bold } /* Operator */
-.highlight .cm { color: #999988; font-style: italic } /* Comment.Multiline */
-.highlight .cp { color: #999999; font-weight: bold } /* Comment.Preproc */
-.highlight .c1 { color: #999988; font-style: italic } /* Comment.Single */
-.highlight .cs { color: #999999; font-weight: bold; font-style: italic } /* Comment.Special */
-.highlight .gd { color: #000000; background-color: #ffdddd } /* Generic.Deleted */
-.highlight .gd .x { color: #000000; background-color: #ffaaaa } /* Generic.Deleted.Specific */
-.highlight .ge { font-style: italic } /* Generic.Emph */
-.highlight .gr { color: #aa0000 } /* Generic.Error */
-.highlight .gh { color: #999999 } /* Generic.Heading */
-.highlight .gi { color: #000000; background-color: #ddffdd } /* Generic.Inserted */
-.highlight .gi .x { color: #000000; background-color: #aaffaa } /* Generic.Inserted.Specific */
-.highlight .go { color: #888888 } /* Generic.Output */
-.highlight .gp { color: #555555 } /* Generic.Prompt */
-.highlight .gs { font-weight: bold } /* Generic.Strong */
-.highlight .gu { color: #800080; font-weight: bold; } /* Generic.Subheading */
-.highlight .gt { color: #aa0000 } /* Generic.Traceback */
-.highlight .kc { font-weight: bold } /* Keyword.Constant */
-.highlight .kd { font-weight: bold } /* Keyword.Declaration */
-.highlight .kn { font-weight: bold } /* Keyword.Namespace */
-.highlight .kp { font-weight: bold } /* Keyword.Pseudo */
-.highlight .kr { font-weight: bold } /* Keyword.Reserved */
-.highlight .kt { color: #445588; font-weight: bold } /* Keyword.Type */
-.highlight .m { color: #009999 } /* Literal.Number */
-.highlight .s { color: #d14 } /* Literal.String */
-.highlight .na { color: #008080 } /* Name.Attribute */
-.highlight .nb { color: #0086B3 } /* Name.Builtin */
-.highlight .nc { color: #445588; font-weight: bold } /* Name.Class */
-.highlight .no { color: #008080 } /* Name.Constant */
-.highlight .ni { color: #800080 } /* Name.Entity */
-.highlight .ne { color: #990000; font-weight: bold } /* Name.Exception */
-.highlight .nf { color: #990000; font-weight: bold } /* Name.Function */
-.highlight .nn { color: #555555 } /* Name.Namespace */
-.highlight .nt { color: #CBDFFF } /* Name.Tag */
-.highlight .nv { color: #008080 } /* Name.Variable */
-.highlight .ow { font-weight: bold } /* Operator.Word */
-.highlight .w { color: #bbbbbb } /* Text.Whitespace */
-.highlight .mf { color: #009999 } /* Literal.Number.Float */
-.highlight .mh { color: #009999 } /* Literal.Number.Hex */
-.highlight .mi { color: #009999 } /* Literal.Number.Integer */
-.highlight .mo { color: #009999 } /* Literal.Number.Oct */
-.highlight .sb { color: #d14 } /* Literal.String.Backtick */
-.highlight .sc { color: #d14 } /* Literal.String.Char */
-.highlight .sd { color: #d14 } /* Literal.String.Doc */
-.highlight .s2 { color: #d14 } /* Literal.String.Double */
-.highlight .se { color: #d14 } /* Literal.String.Escape */
-.highlight .sh { color: #d14 } /* Literal.String.Heredoc */
-.highlight .si { color: #d14 } /* Literal.String.Interpol */
-.highlight .sx { color: #d14 } /* Literal.String.Other */
-.highlight .sr { color: #009926 } /* Literal.String.Regex */
-.highlight .s1 { color: #d14 } /* Literal.String.Single */
-.highlight .ss { color: #990073 } /* Literal.String.Symbol */
-.highlight .bp { color: #999999 } /* Name.Builtin.Pseudo */
-.highlight .vc { color: #008080 } /* Name.Variable.Class */
-.highlight .vg { color: #008080 } /* Name.Variable.Global */
-.highlight .vi { color: #008080 } /* Name.Variable.Instance */
-.highlight .il { color: #009999 } /* Literal.Number.Integer.Long */
-
-.type-csharp .highlight .k { color: #0000FF }
-.type-csharp .highlight .kt { color: #0000FF }
-.type-csharp .highlight .nf { color: #000000; font-weight: normal }
-.type-csharp .highlight .nc { color: #2B91AF }
-.type-csharp .highlight .nn { color: #000000 }
-.type-csharp .highlight .s { color: #A31515 }
-.type-csharp .highlight .sc { color: #A31515 }

stylesheets/stylesheet.css

@@ -1,246 +0,0 @@
-body {
-  margin: 0;
-  padding: 0;
-  background: #151515 url("../images/bkg.png") 0 0;
-  color: #eaeaea;
-  font: 16px;
-  line-height: 1.5;
-  font-family: Monaco, "Bitstream Vera Sans Mono", "Lucida Console", Terminal, monospace;
-}
-
-/* General & 'Reset' Stuff */
-
-.container {
-  width: 90%;
-  max-width: 600px;
-  margin: 0 auto;
-}
-
-section {
-  display: block;
-  margin: 0 0 20px 0;
-}
-
-h1, h2, h3, h4, h5, h6 {
-  margin: 0 0 20px;
-}
-
-li {
-  line-height: 1.4 ;
-}
-
-/* Header, <header>
-   header   - container
-   h1       - project name
-   h2       - project description
-*/
-
-header {
-  background: rgba(0, 0, 0, 0.1);
-  width: 100%;
-  border-bottom: 1px dashed #b5e853;
-  padding: 20px 0;
-  margin: 0 0 40px 0;
-}
-
-header h1 {
-  font-size: 30px;
-  line-height: 1.5;
-  margin: 0 0 0 -30px;
-  font-family: Audiowide,Monaco, "Bitstream Vera Sans Mono", "Lucida Console", Terminal, monospace;
-  color: #b5e853;
-  text-shadow: 0 1px 1px rgba(0, 0, 0, 0.1),
-               0 0 5px rgba(181, 232, 83, 0.1),
-               0 0 10px rgba(181, 232, 83, 0.1);
-  letter-spacing: -1px;
-  -webkit-font-smoothing: antialiased;
-}
-
-header h1:before {
-  content: "./ ";
-  font-size: 24px;
-}
-
-header h2 {
-  font-size: 18px;
-  font-weight: 300;
-  color: #666;
-}
-
-#downloads .btn {
-  display: inline-block;
-  text-align: center;
-  margin: 0;
-}
-
-/* Main Content
-*/
-
-#main_content {
-  width: 100%;
-  -webkit-font-smoothing: antialiased;
-}
-section img {
-  max-width: 100%
-}
-
-h1, h2, h3, h4, h5, h6 {
-  font-weight: normal;
-  font-family: Monaco, "Bitstream Vera Sans Mono", "Lucida Console", Terminal, monospace;
-  color: #b5e853;
-  letter-spacing: -0.03em;
-  text-shadow: 0 1px 1px rgba(0, 0, 0, 0.1),
-               0 0 5px rgba(181, 232, 83, 0.1),
-               0 0 10px rgba(181, 232, 83, 0.1);
-}
-
-#main_content h1 {
-  font-size: 30px;
-}
-
-#main_content h2 {
-  font-size: 24px;
-}
-
-#main_content h3 {
-  font-size: 18px;
-}
-
-#main_content h4 {
-  font-size: 14px;
-}
-
-#main_content h5 {
-  font-size: 12px;
-  text-transform: uppercase;
-  margin: 0 0 5px 0;
-}
-
-#main_content h6 {
-  font-size: 12px;
-  text-transform: uppercase;
-  color: #999;
-  margin: 0 0 5px 0;
-}
-
-dt {
-  font-style: italic;
-  font-weight: bold;
-}
-
-ul li {
-  list-style: none;
-}
-
-ul li:before {
-  content: ">>";
-  font-family: Monaco, "Bitstream Vera Sans Mono", "Lucida Console", Terminal, monospace;
-  font-size: 13px;
-  color: #b5e853;
-  margin-left: -37px;
-  margin-right: 21px;
-  line-height: 16px;
-}
-
-blockquote {
-  color: #aaa;
-  padding-left: 10px;
-  border-left: 1px dotted #666;
-}
-
-pre {
-  background: rgba(0, 0, 0, 0.9);
-  border: 1px solid rgba(255, 255, 255, 0.15);
-  padding: 10px;
-  font-size: 14px;
-  color: #b5e853;
-  border-radius: 2px;
-  -moz-border-radius: 2px;
-  -webkit-border-radius: 2px;
-  text-wrap: normal;
-  overflow: auto;
-  overflow-y: hidden;
-}
-
-table {
-  width: 100%;
-  margin: 0 0 20px 0;
-}
-
-th {
-  text-align: left;
-  border-bottom: 1px dashed #b5e853;
-  padding: 5px 10px;
-}
-
-td {
-  padding: 5px 10px;
-}
-
-hr {
-  height: 0;
-  border: 0;
-  border-bottom: 1px dashed #b5e853;
-  color: #b5e853;
-}
-
-/* Buttons
-*/
-
-.btn {
-  display: inline-block;
-  background: -webkit-linear-gradient(top, rgba(40, 40, 40, 0.3), rgba(35, 35, 35, 0.3) 50%, rgba(10, 10, 10, 0.3) 50%, rgba(0, 0, 0, 0.3));
-  padding: 8px 18px;
-  border-radius: 50px;
-  border: 2px solid rgba(0, 0, 0, 0.7);
-  border-bottom: 2px solid rgba(0, 0, 0, 0.7);
-  border-top: 2px solid rgba(0, 0, 0, 1);
-  color: rgba(255, 255, 255, 0.8);
-  font-family: Helvetica, Arial, sans-serif;
-  font-weight: bold;
-  font-size: 13px;
-  text-decoration: none;
-  text-shadow: 0 -1px 0 rgba(0, 0, 0, 0.75);
-  box-shadow: inset 0 1px 0 rgba(255, 255, 255, 0.05);
-}
-
-.btn:hover {
-  background: -webkit-linear-gradient(top, rgba(40, 40, 40, 0.6), rgba(35, 35, 35, 0.6) 50%, rgba(10, 10, 10, 0.8) 50%, rgba(0, 0, 0, 0.8));
-}
-
-.btn .icon {
-  display: inline-block;
-  width: 16px;
-  height: 16px;
-  margin: 1px 8px 0 0;
-  float: left;
-}
-
-.btn-github .icon {
-  opacity: 0.6;
-  background: url("../images/blacktocat.png") 0 0 no-repeat;
-}
-
-/* Links
-   a, a:hover, a:visited
-*/
-
-a {
-  color: #63c0f5;
-  text-shadow: 0 0 5px rgba(104, 182, 255, 0.5);
-}
-
-/* Clearfix */
-
-.cf:before, .cf:after {
-  content:"";
-  display:table;
-}
-
-.cf:after {
-  clear:both;
-}
-
-.cf {
-  zoom:1;
-}

tests/common.js

@@ -0,0 +1,5 @@
+var common = exports;
+var path   = require('path');
+
+common.root     = path.join(__dirname, '..');
+common.lib      = path.join(common.root, 'lib');

tests/integration/mission.js

@@ -0,0 +1,32 @@
+var df = require('dateformat')
+  , common = require('../common')
+  , autonomy = require(common.root)
+  , mockClient = require('./mock/client')
+  ;
+
+var client     = new mockClient();
+var controller = new autonomy.Controller(client);
+var mission    = new autonomy.Mission(client, controller);
+
+mission.log("mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt");
+
+mission.takeoff()
+       .hover(1000)
+       .forward(1)
+       .right(1)
+       .backward(1)
+       .left(1)
+       .hover(1000)
+       .land();
+
+mission.run(function (err, result) {
+    if (err) {
+        console.trace("Oops, something bad happened: %s", err.message);
+        mission.client().stop();
+        mission.client().land();
+    } else {
+        console.log("We are done!");
+        process.exit(0);
+    }
+});
+

tests/integration/mock/client.js

@@ -0,0 +1,92 @@
+var EventEmitter = require('events').EventEmitter;
+var Timers       = require('timers');
+var util         = require('util');
+
+DT = 30;
+
+module.exports = Client;
+util.inherits(Client, EventEmitter);
+function Client(options) {
+    EventEmitter.call(this);
+
+    options = options || {};
+
+    this._state = options.state || {x: 0, y: 0, z: 1, yaw: 0};
+    this._speed = {vx: 0, vy: 0, vz: 0, vyaw: 0};
+
+    var self = this;
+    Timers.setInterval(function() {
+        self._sendNavdata();
+    }, DT);
+}
+
+Client.prototype.takeoff = function(callback) {
+    setTimeout(callback, 1000);
+}
+
+Client.prototype.land = function(callback) {
+    setTimeout(callback, 1000);
+}
+
+Client.prototype.front = function(speed) {
+    this._speed.vx = speed;
+}
+
+Client.prototype.back = function(speed) {
+    this._speed.vx = -speed;
+}
+
+Client.prototype.right = function(speed) {
+    this._speed.vy = speed;
+}
+
+Client.prototype.left = function(speed) {
+    this._speed.vy = -speed;
+}
+
+Client.prototype.up = function(speed) {
+    this._speed.vz = speed;
+}
+
+Client.prototype.down = function(speed) {
+    this._speed.vz = -speed;
+}
+
+Client.prototype.clockwise = function(speed) {
+    this._speed.vyaw = speed;
+}
+
+Client.prototype.counterClockwise = function(speed) {
+    this._speed.vyaw = -speed;
+}
+
+Client.prototype.stop = function() {
+    this._speed = {vx: 0, vy: 0, vz: 0, vyaw: 0};
+}
+
+Client.prototype._sendNavdata = function() {
+    // First we update the state based on speed
+    this._state.z = Math.max(0, this._state.z + this._speed.vz);
+    this._state.yaw = this._state.yaw + this._speed.vyaw;
+
+    var navdata = {
+        demo: {
+            rotation: {
+                pitch: 0,
+                roll: 0,
+                yaw: this._state.yaw
+            },
+            velocity: {
+                x: this._speed.vx * 1000,
+                y: this._speed.vy * 1000,
+                z: this._speed.vz * 1000
+            },
+            altitude: this._state.z
+        },
+        visionDetect: {
+            nbDetected: 0
+        }
+    };
+
+    this.emit('navdata', navdata);
+}

tests/run.js

@@ -0,0 +1,2 @@
+var urun = require('urun');
+urun(__dirname);

tests/unit/test-Controller.js

@@ -0,0 +1,171 @@
+var common   = require('../common');
+var assert   = require('assert');
+var test     = require('utest');
+var sinon    = require('sinon');
+var autonomy = require(common.root);
+
+test('Controller', {
+    before: function() {
+        this.mockClient      = {};
+        this.mockClient.on   = sinon.stub();
+        this.mockClient.stop = sinon.stub();
+    },
+
+    'controller binds on navdata': function() {
+        var ctrl = new autonomy.Controller(this.mockClient);
+        assert.equal(this.mockClient.on.callCount, 1);
+    },
+
+    'disabling the controller stops the drone': function() {
+        var ctrl = new autonomy.Controller(this.mockClient);
+        ctrl.disable();
+        assert.equal(this.mockClient.stop.callCount, 1);
+        assert.equal(ctrl._enabled, false);
+    },
+
+    'hover assigns current state as goal': function() {
+        var ctrl  = new autonomy.Controller(this.mockClient);
+        var state = {x: 1, y: 2, z: 3, yaw: 0};
+
+        ctrl._state = state;
+        ctrl.hover();
+        var goal = ctrl._goal;
+
+        assert.equal(goal.x, state.x);
+        assert.equal(goal.y, state.y);
+        assert.equal(goal.z, state.z);
+        assert.equal(goal.yaw, state.yaw);
+    },
+
+    'forward mapping works with different yaw': function() {
+        var ctrl  = new autonomy.Controller(this.mockClient);
+        ctrl._state = {x: 0, y: 0, z: 1, yaw: 0};
+
+        // Test forward with yaw 0
+        ctrl.forward(1);
+        assert.equal(ctrl._goal.x, 1);
+        assert.equal(ctrl._goal.y, 0);
+
+        // Test forward with yaw 90
+        var yaw = 90;
+        ctrl._state.yaw = yaw.toRad();
+        ctrl.forward(1);
+        assert.equal(Math.round(ctrl._goal.x * 1000) / 1000, 0);
+        assert.equal(ctrl._goal.y, 1);
+
+        // Test forward with yaw 45
+        var yaw = 45;
+        ctrl._state.yaw = yaw.toRad();
+        ctrl.forward(1);
+        assert.equal(Math.round(ctrl._goal.x * 1000) / 1000, Math.round(ctrl._goal.y * 1000) /1000);
+
+        // Test forward with yaw -45
+        var yaw = -45;
+        ctrl._state.yaw = yaw.toRad();
+        ctrl.forward(1);
+        assert.equal(Math.round(ctrl._goal.x * 1000) / 1000, -Math.round(ctrl._goal.y * 1000) /1000);
+    },
+
+    'right mapping works with different yaw': function() {
+        var ctrl  = new autonomy.Controller(this.mockClient);
+        ctrl._state = {x: 0, y: 0, z: 1, yaw: 0};
+
+        // Test right with yaw 0
+        ctrl.right(1);
+        assert.equal(ctrl._goal.x, 0);
+        assert.equal(ctrl._goal.y, 1);
+
+        // Test right with yaw 90
+        var yaw = 90;
+        ctrl._state.yaw = yaw.toRad();
+        ctrl.right(1);
+        assert.equal(Math.round(ctrl._goal.x * 1000) / 1000, -1);
+        assert.equal(Math.round(ctrl._goal.y * 1000) / 1000, 0);
+
+        // Test right with yaw 45
+        var yaw = 45;
+        ctrl._state.yaw = yaw.toRad();
+        ctrl.right(1);
+        assert.equal(Math.round(ctrl._goal.x * 1000) / 1000, -Math.round(ctrl._goal.y * 1000) /1000);
+    },
+
+    'backward is the inverse of forward': function() {
+        var ctrl    = new autonomy.Controller(this.mockClient);
+        var cb      = function() {};
+        ctrl._state = {x: 0, y: 0, z: 1, yaw: 0};
+        sinon.spy(ctrl, 'forward');
+
+        ctrl.backward(1, cb);
+        assert(ctrl.forward.calledWith(-1, cb));
+    },
+
+    'left is the inverse of right': function() {
+        var ctrl    = new autonomy.Controller(this.mockClient);
+        var cb      = function() {};
+        ctrl._state = {x: 0, y: 0, z: 1, yaw: 0};
+        sinon.spy(ctrl, 'right');
+
+        ctrl.left(1, cb);
+        assert(ctrl.right.calledWith(-1, cb));
+    },
+
+    'zero reset the kalman filter': function() {
+        var ctrl    = new autonomy.Controller(this.mockClient);
+        sinon.spy(ctrl._ekf, 'reset');
+
+        ctrl.zero();
+        assert(ctrl._ekf.reset.calledOnce);
+    },
+
+    'up': function() {
+        var ctrl  = new autonomy.Controller(this.mockClient);
+        var state = {x: 1, y: 2, z: 3, yaw: 0};
+
+        ctrl._state = state;
+        ctrl.up(1);
+        var goal = ctrl._goal;
+
+        assert.equal(goal.x, state.x);
+        assert.equal(goal.y, state.y);
+        assert.equal(goal.z, state.z + 1);
+        assert.equal(goal.yaw, state.yaw);
+    },
+
+    'down is invserse of up': function() {
+        var ctrl    = new autonomy.Controller(this.mockClient);
+        var cb      = function() {};
+        ctrl._state = {x: 0, y: 0, z: 1, yaw: 0};
+        sinon.spy(ctrl, 'up');
+
+        ctrl.down(1, cb);
+        assert(ctrl.up.calledWith(-1, cb));
+    },
+
+    'cannot go too low': function() {
+        var ctrl  = new autonomy.Controller(this.mockClient);
+        var state = {x: 0, y: 0, z: 1, yaw: 0};
+
+        ctrl._state = state;
+        ctrl.down(1);
+        var goal = ctrl._goal;
+
+        assert.equal(goal.x, state.x);
+        assert.equal(goal.y, state.y);
+        assert.equal(goal.z, 0.5);
+        assert.equal(goal.yaw, state.yaw);
+    },
+
+    'altitude': function() {
+        var ctrl  = new autonomy.Controller(this.mockClient);
+        var state = {x: 0, y: 0, z: 1, yaw: 0};
+
+        ctrl._state = state;
+        ctrl.altitude(3);
+        var goal = ctrl._goal;
+
+        assert.equal(goal.x, state.x);
+        assert.equal(goal.y, state.y);
+        assert.equal(goal.z, 3);
+        assert.equal(goal.yaw, state.yaw);
+    }
+});