wellton/ardrone-autonomy
1
0
Fork 0
Código Issues Pull Requests Ações Pacotes Projetos Versões Wiki Atividade

Improving controller

Ver Código-Fonte 
- Controller emit events
- Log directly to csv file in example repl
- Consider state stable after some delay
- Playing wit PID coefficients (not ideal yet)
Esse commit está contido em:
Laurent Eschenauer
2013-06-19 22:46:21 +02:00
pai 05ee621bfe
commit 657c5dd4a8
4 arquivos alterados com 116 adições e 91 exclusões
Baixar Arquivo de Patch Baixar Arquivo de Diff Expandir todos os arquivos Colapsar todos os arquivos
examples/repl.js
+22 -21
Ver Arquivo
@@ -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");
});
lib/Controller.js
+85 -55
Ver Arquivo
@@ -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) {
lib/EKF.js
+3 -14
Ver Arquivo
@@ -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
+6 -1
Ver Arquivo
@@ -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();
});