Improving controller

- Controller emit events
- Log directly to csv file in example repl
- Consider state stable after some delay
- Playing wit PID coefficients (not ideal yet)

examples/repl.js

@@ -33,32 +33,33 @@ client.config('detect:detect_type', 12);
 //
 repl._repl.context['ctrl'] = ctrl;
 
-// Log navdata and estimated state - for debugging
+// 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'));
 });
 
-client.on('navdata', function(data) {
+ctrl.on('controlData', function(d) {
-    var state = ctrl.state()
+   dataStream.write(d.state.x + "," +
-      , cmds  = ctrl.commands()
+                    d.state.y + "," +
-      , vx    = data.demo.velocity.x / 1000
+                    d.state.z + "," +
-      , vy    = data.demo.velocity.y / 1000
+                    d.state.yaw + "," +
-      , vz    = data.demo.velocity.z / 1000
+                    d.state.vx + "," +
-      , x     = state.x
+                    d.state.vy + "," +
-      , y     = state.y
+                    d.goal.x + "," +
-      , z     = state.z
+                    d.goal.y + "," +
-      , yaw   = state.yaw
+                    d.goal.z + "," +
-      , cx    = cmds.cx
+                    d.goal.yaw + "," +
-      , cy    = cmds.cy
+                    d.error.ex + "," +
-      , cz    = cmds.cz
+                    d.error.ey + "," +
-      , cyaw  = cmds.cyaw
+                    d.error.ez + "," +
-      , tag   = (data.visionDetect && data.visionDetect.nbDetected > 0)
+                    d.error.eyaw + "," +
-      ;
+                    d.control.ux + "," +
-
+                    d.control.uy + "," +
-  dataStream.write(x + "," + y + "," + z + "," + yaw + ","
+                    d.control.uz + "," +
-                   + vx + "," + vy + "," + vz +"," + cx + ","
+                    d.control.uyaw + "," +
-                   + cy + "," + cz + "," + cyaw + "," + tag + "\n");
+                    d.last_ok + "," +
+                    d.tag + "\n");
 });
 
 

lib/Controller.js

@@ -1,34 +1,58 @@
+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 precision
+EPS_LIN      = 0.05;  // We are ok with 5 cm precision
 EPS_ANG      = 0.05;  // We are ok with 0.05 rad precision (2.5 deg)
+STABLE_DELAY = 1000;  // 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 || {};
 
-    this._state   = options.state || {x: 0, y:0, z:1, yaw: 0};
+    // A ardrone client to pilot the drone
-    this._tag     = options.tag || {x: 0, y: 0, yaw: 0};
-    this._debug   = options.debug || false;
-    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.6, 0.001, 0.1);
-    this._pid_yaw = new PID(5.0, 0, 1.0);
-    this._ekf     = new EKF(options);
-    this._camera  = new Camera();
-    this._enabled = false;
     this._client  = client;
-    this._goal    = null;
-    this._cmds    = {cx: 0, cy: 0, cz: 0, cyaw: 0};
 
+    // 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.3,   0,  0.1);
+    this._pid_y   = new PID(0.3,   0,  0.1);
+    this._pid_z   = new PID(0.3,   0,  0.1);
+    this._pid_yaw = new PID(5.0,   0,  1.0);
+
+    // 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;
+
+    // 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) {
         self._processNavdata(d);
-        self._control();
+        self._control(d);
     });
 }
 
@@ -57,8 +81,11 @@ Controller.prototype.state = function() {
     return this._state;
 }
 
-Controller.prototype.commands = function() {
+/*
-    return this._cmds;
+ * 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});
 }
 
 /*
@@ -70,6 +97,9 @@ Controller.prototype.go = function(goal, callback) {
     // disable the controller, just in case.
     this.disable();
 
+    // If no goal given, assume an empty goal (which means the current state as goal)
+    goal = goal || {};
+
     // Normalize the yaw, to make sure we don't spin 360deg for
     // nothing :-)
     if (goal.yaw != undefined) {
@@ -88,7 +118,6 @@ Controller.prototype.go = function(goal, callback) {
 }
 
 Controller.prototype._processNavdata = function(d) {
-
     // EKF prediction step
     this._ekf.predict(d);
 
@@ -122,9 +151,11 @@ Controller.prototype._processNavdata = function(d) {
     // 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() {
+Controller.prototype._control = function(d) {
     // Do not control if not enabled
     if (!this._enabled) return;
 
@@ -157,19 +188,28 @@ Controller.prototype._control = function() {
 
     // If all commands are zero, we have reached our destination;
     // we go into hover mode and trigger the callback if defined.
-    //
+    var now = Date.now();
-    // TODO This does not work, since we may speed past the target
-    // we need a better way to estimate when we have reached the destination,
-    // probably should have error and speed < EPS for some time.
-    //
     if (ux == 0 && uy == 0 && uz == 0 && uyaw == 0) {
-        this._client.stop();
+        // If we have been in this position long enough we consider the target reached
-//        this.disable();
+        if (this._last_ok != 0) {
-//        if (this._callback != null) {
+            if ((now - this._last_ok) > STABLE_DELAY) {
-//            var cb = this._callback;
+                // Set the drone in hover mode
-//            this._callback = null;
+                this._client.stop();
-//            cb(this.state());
+
-//        }
+                // Trigger the callback. We clear the callback before calling
+                // it, just in case the callback adds a new callback and a new goal.
+                if (this._callback != null) {
+                    var cb = this._callback;
+                    this._callback = null;
+                    cb(this.state());
+                }
+
+                // Emit a state reached
+                this.emit('goalReached', this._state);
+            }
+        } else {
+            this._last_ok = now;
+        }
     }
     // Else map controller commands to drone commands
     else {
@@ -179,34 +219,24 @@ Controller.prototype._control = function() {
         var cz   = within(uz, -1, 1);
         var cyaw = within(uyaw, -1, 1);
 
-        // Send commands to drone
+        this._client.front(cx);
-        if (Math.abs(cx) > 0) {
+        this._client.right(cy);
-            this._client.front(cx);
+        this._client.up(cz);
-        }
+        this._client.clockwise(cyaw);
 
-        if (Math.abs(cy) > 0) {
+        // Reset last ok since we are in motion
-            this._client.right(cy);
+        this._last_ok = 0;
-        }
-
-        if (Math.abs(cz) > 0) {
-            this._client.up(cz);
-        }
-
-        if (Math.abs(cyaw) > 0) {
-            this._client.clockwise(cyaw);
-        }
     }
 
-    this._cmds = {cx: cx, cy: cy, cz: cz, cyaw: cyaw};
+    // Emit the control data for others
-
+    this.emit('controlData', {
-    if (this._debug) {
+        state:   this._state,
-        console.log("--------------------- Control step ----------------------------------------------");
+        goal:    this._goal,
-        console.log("Goal: \t %d,%d,%d,%d", this._goal.x, this._goal.y, this._goal.z, this._goal.yaw);
+        error:   {ex: ex, ey: ey, ez: ez, eyaw: eyaw},
-        console.log("State:\t %d,%d,%d,%d", this._state.x, this._state.y, this._state.z, this._state.yaw);
+        control: {ux: ux, uy: uy, uz: uz, uyaw: uyaw},
-        console.log("Error:\t %d,%d,%d,%d", ex, ey, ez, eyaw);
+        last_ok: this._last_ok,
-        console.log("Ctrl: \t %d,%d,%d,%d", ux, uy, uz, uyaw);
+        tag:     (d.visionDetect && d.visionDetect.nbDetected > 0) ? 1 : 0
-        console.log("Cmds: \t %d,%d,%d,%d", cx, cy, cz, cyaw);
+    });
-    }
 }
 
 function within(x, min, max) {

lib/EKF.js

@@ -13,7 +13,6 @@ function EKF(options) {
 
   this._options     = options;
   this._delta_t     = options.delta_t || EKF.DELTA_T;
-  this._tag         = options.tag     || {x: 0, y: 0, yaw: 0};
 
   this.reset();
 }
@@ -27,7 +26,7 @@ EKF.prototype.confidence = function() {
 }
 
 EKF.prototype.reset = function() {
-  this._state       = this._options.state   || {x: 0, y: 0, z: 1, yaw: 0};
+  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.1]);
@@ -40,9 +39,6 @@ EKF.prototype.predict = function(data) {
       , 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
-      , vz    = data.demo.velocity.z / 1000
-      , dphi  = yaw - this._last_yaw
-      , alt   = data.demo.altitude
       , dt    = this._delta_t
     ;
 
@@ -53,22 +49,15 @@ EKF.prototype.predict = function(data) {
         return;
     }
 
-    // A simplistic way to filter backround noise on the odometry
-    // TODO - We may want to calibrate by averaging noise over time
-    // when steady and then substract it.
-    vx   = (Math.abs(vx) < 0.15) ? 0 : vx;
-    vy   = (Math.abs(vy) < 0.15) ? 0 : vy;
-    dphi = (Math.abs(dphi) < 0.01) ? 0 : dphi;
-
     // Compute the odometry by integrating the motion over delta_t
-    var o = {dx: vx * dt, dy: vy * dt, dphi: dphi};
+    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.dphi;
+    state.yaw = state.yaw + o.dyaw;
 
     // Normalize the yaw value
     state.yaw = Math.atan2(Math.sin(state.yaw),Math.cos(state.yaw));

tests/simple.js

@@ -4,10 +4,15 @@ console.log("The test is broken. Will have to write proper tests :-)");
 var autonomy    = require('..');
 var client      = require('./mock/client');
 
-var controller  = new autonomy.Controller(new client(), {state: {x: 0, y:0, z:1, yaw: 0}, debug: false});
+var controller  = new autonomy.Controller(new client(), {state: {x: 0, y:0, z:1, yaw: 0}});
 
 console.log("State: %j", controller.state());
 
+controller.on('controlData', function(data) {
+    console.log("%j", data);
+});
+
 controller.go({x: 1, y: 1}, function(state) {
     console.log("Reached state %j", state);
+    controller.disable();
 });