diff --git a/examples/repl.js b/examples/repl.js index c640cb1..675c3e7 100644 --- a/examples/repl.js +++ b/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) { - var state = ctrl.state() - , cmds = ctrl.commands() - , vx = data.demo.velocity.x / 1000 - , vy = data.demo.velocity.y / 1000 - , vz = data.demo.velocity.z / 1000 - , x = state.x - , y = state.y - , z = state.z - , yaw = state.yaw - , cx = cmds.cx - , cy = cmds.cy - , cz = cmds.cz - , cyaw = cmds.cyaw - , tag = (data.visionDetect && data.visionDetect.nbDetected > 0) - ; - - dataStream.write(x + "," + y + "," + z + "," + yaw + "," - + vx + "," + vy + "," + vz +"," + cx + "," - + cy + "," + cz + "," + cyaw + "," + tag + "\n"); +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"); }); diff --git a/lib/Controller.js b/lib/Controller.js index 883aa8e..f319e10 100644 --- a/lib/Controller.js +++ b/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}; - 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; + // A ardrone client to pilot the drone 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. - // - // 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. - // + var now = Date.now(); if (ux == 0 && uy == 0 && uz == 0 && uyaw == 0) { - this._client.stop(); -// this.disable(); -// if (this._callback != null) { -// var cb = this._callback; -// this._callback = null; -// cb(this.state()); -// } + // If we have been in this position long enough we consider the target reached + if (this._last_ok != 0) { + if ((now - this._last_ok) > STABLE_DELAY) { + // Set the drone in hover mode + this._client.stop(); + + // 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 - if (Math.abs(cx) > 0) { - this._client.front(cx); - } + this._client.front(cx); + this._client.right(cy); + this._client.up(cz); + this._client.clockwise(cyaw); - if (Math.abs(cy) > 0) { - this._client.right(cy); - } - - if (Math.abs(cz) > 0) { - this._client.up(cz); - } - - if (Math.abs(cyaw) > 0) { - this._client.clockwise(cyaw); - } + // Reset last ok since we are in motion + this._last_ok = 0; } - this._cmds = {cx: cx, cy: cy, cz: cz, cyaw: cyaw}; - - if (this._debug) { - console.log("--------------------- Control step ----------------------------------------------"); - console.log("Goal: \t %d,%d,%d,%d", this._goal.x, this._goal.y, this._goal.z, this._goal.yaw); - console.log("State:\t %d,%d,%d,%d", this._state.x, this._state.y, this._state.z, this._state.yaw); - console.log("Error:\t %d,%d,%d,%d", ex, ey, ez, eyaw); - console.log("Ctrl: \t %d,%d,%d,%d", ux, uy, uz, uyaw); - console.log("Cmds: \t %d,%d,%d,%d", cx, cy, cz, cyaw); - } + // Emit the control data for others + 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 + }); } function within(x, min, max) { diff --git a/lib/EKF.js b/lib/EKF.js index ca48186..a25be7d 100644 --- a/lib/EKF.js +++ b/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)); diff --git a/tests/simple.js b/tests/simple.js index e9917c7..9c6bc92 100644 --- a/tests/simple.js +++ b/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(); });