diff --git a/README.md b/README.md index aff1785..e4421d3 100644 --- a/README.md +++ b/README.md @@ -41,13 +41,16 @@ by the bottom camera. 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. +with the drone taking off, travelling alongs a 2 x 2 meters square and then landing. ```js var autonomy = require('ardrone-autonomy'); var mission = autonomy.createMission(); -mission.takeoff() +mission.ftrim() // Makes a flat trim before takeoff + .takeoff() + .wait(4000) // Wait four seconds + .calibrate() // Calibrate the drone's compass (will not calibrate if it is not ready) .zero() // Sets the current state as the reference .altitude(1) // Climb to altitude = 1 meter .forward(2) @@ -71,6 +74,31 @@ mission.run(function (err, result) { ### Mission API +#### autonomy.createMission(options) + +Create a mission object. Any property in `options` can override the following default values: + +```javascript +{tag: { + x: 0, + y: 0, + yaw: 0 +} +pid: { + x_axis: {p_constant: 0.5, i_constant: 0, d_constant: 0.35}, + y_axis: {p_constant: 0.5, i_constant: 0, d_constant: 0.35}, + z_axis: {p_constant: 0.8, i_constant: 0, d_constant: 0.35}, + yaw_axis: {p_constant: 1.0, i_constant: 0, d_constant: 0.30} +}, +state: { + x: 0, + y: 0, + z: 0, + yaw: 0, + absoluteYaw: null +}} +``` + #### mission.log(path) Log the mission data, csv formatted, in the given file. Makes it really usefull to @@ -81,10 +109,18 @@ debug/plot the state and controller behavior. 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.ftrim() + +Add a flat trim step to the mission. Drone must be landed. + #### mission.takeoff() Add a takeoff step to the mission. +#### mission.calibrate() + +Add a compass calibration step to the mission. The drone is ready for calibration after about four seconds after takeoff + #### 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 @@ -174,3 +210,7 @@ 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. + +## AR.Drone SDK 2 + +It might be useful to use the official AR.Drone documentation when using mission.client().config(key, value). As of 2016, it can be downloaded [here](http://developer.parrot.com/docs/SDK2/ARDrone_SDK_2_0_1.zip) diff --git a/examples/square-2.js b/examples/square-2.js index 4d2d1e3..09d4a3a 100644 --- a/examples/square-2.js +++ b/examples/square-2.js @@ -15,6 +15,7 @@ var navdata_options = ( | navdata_option_mask(arDroneConstants.options.VISION_DETECT) | navdata_option_mask(arDroneConstants.options.MAGNETO) | navdata_option_mask(arDroneConstants.options.WIFI) + | navdata_option_mask(arDroneConstants.options.ZIMMU_3000) ); // Land on ctrl-c @@ -42,17 +43,19 @@ mission.client().config('detect:detect_type', 12); mission.log("mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt"); // Plan mission -mission.takeoff() +mission.ftrim() + .takeoff() + .wait(4000).calibrate() .zero() - .hover(500) - .altitude(2) - .forward(2) + .hover(1000) + .altitude(1.5) + .forward(1) .cw(90) - .forward(2) + .forward(1) .cw(90) - .forward(2) + .forward(1) .cw(90) - .forward(2) + .forward(1) .go({x:0, y:0}) .hover(500) .land(); diff --git a/examples/square.js b/examples/square.js index 1e93e04..b57677f 100644 --- a/examples/square.js +++ b/examples/square.js @@ -2,20 +2,8 @@ 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) -); +var chalk = require("chalk"); // Land on ctrl-c var exiting = false; @@ -32,6 +20,28 @@ process.on('SIGINT', function() { } }); +var options = undefined; +/*var options = {pid: { x_axis: {p_constant: 0.75, i_constant: 0.3, d_constant: 0.35}, + y_axis: {p_constant: 0.75, i_constant: 0.3, d_constant: 0.35}, + z_axis: {p_constant: 0.8, i_constant: 0.0, d_constant: 0.35}, + yaw_axis: {p_constant: 1.0, i_constant: 0.0, d_constant: 0.30}}}; +*/ +var mission = autonomy.createMission(options); +console.log("mission options: " + JSON.stringify(options)); + +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) + | navdata_option_mask(arDroneConstants.options.ZIMMU_3000) // To send GPS data +); + // Connect and configure the drone mission.client().config('general:navdata_demo', true); mission.client().config('general:navdata_options', navdata_options); @@ -39,29 +49,37 @@ 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 +var logFileName = "/tmp/mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt" +console.log("Mission Log: " + logFileName); +mission.log(logFileName); +console.log(".zero()"); +mission.zero() + .altitude(1.3) + .hover(250); 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); - } -}); + if (err) { + console.trace("Oops, something bad happened: %s", err.message); + mission.client().stop(); + mission.client().land(); + } else { + console.log(chalk.inverse.green("====Mission complete====")); + //setImmediate(doOver); + } + }); +function doOver () { + // Plan mission + console.log(chalk.green("====Planning mission====")); + mission.takeoff() + + .go({x:0, y:0}).hover(500) + .go({x:0, y:1}).hover(500) + .go({x:-1, y:1}).hover(500) + .go({x:0, y:1}).hover(500) + .go({x:0, y:0}).hover(500) + .go({x:1, y:0}).hover(500) + .go({x:0, y:0}).hover(500) + + // Execute mission + console.log(chalk.dim.blue("Running mission")); +} diff --git a/index.js b/index.js index ad7a05e..a3cb99f 100644 --- a/index.js +++ b/index.js @@ -12,6 +12,10 @@ exports.control = function(client, options) { exports.createMission = function(options, client) { var client = client || ardrone.createClient(options); + if (typeof options.droneConfiguration != "undefined" && Array.isArray(options.droneConfiguration)) + options.droneConfiguration.forEach(function (configuration) { + this.config(configuration.key, configuration.value, configuration.callback); + }, client); var control = new autonomy.Controller(client, options); var mission = new autonomy.Mission(client, control, options); diff --git a/lib/Camera.js b/lib/Camera.js index 882771b..e37921f 100644 --- a/lib/Camera.js +++ b/lib/Camera.js @@ -7,7 +7,21 @@ var util = require('util'); // 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 +// https://github.com/tum-vision/ardrone_autonomy/blob/master/calibrations/ardrone2_bottom/cal.yml +// The code is under the 'BSD' license, which is assumed to be the following license +// Copyright (c) 2012, Mani Monajjemi +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + var K_BOTTOM = $M([[686.994766, 0, 329.323208], [0, 688.195055, 159.323007], [0, 0, 1]]); @@ -25,20 +39,18 @@ function Camera(options) { * 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. +Camera.prototype.p2m = function(xC, yC, altitude) { + // See "7.1.3 Augmented reality data stream" of SDK 2.0.1 Documentation (Page 42) + // Read the third bullet point "xc[i], yc[i]: ... " // // 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; + 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 = $V([xC * xRatio, yC * yRatio, 1]); var P = this._invK.multiply(p).multiply(altitude); // X,Y are expressed in meters, in the drone coordinate system. @@ -51,5 +63,5 @@ Camera.prototype.p2m = function(x, y, altitude) { // | // Y return {x: P.e(1), y: P.e(2)}; -} +}; diff --git a/lib/Controller.js b/lib/Controller.js index 02f0dda..6b117ec 100644 --- a/lib/Controller.js +++ b/lib/Controller.js @@ -1,3 +1,8 @@ +// Controller.js + +// Should this file output debugging information to `console`? +var shouldSendDebugInfo = true; + var EventEmitter = require('events').EventEmitter; var Timers = require('timers'); var util = require('util'); @@ -5,14 +10,15 @@ 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 +var EPS_LINEAR = 0.05; // We are ok with 5 cm horizontal precision +var EPS_ALTITUDE = 0.1; // We are ok with 10 cm altitude precision +var EPS_ANGULAR = 0.1; // We are ok with 0.1 rad precision (5 deg) +var 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) { + 'use strict'; EventEmitter.call(this); options = options || {}; @@ -24,10 +30,24 @@ function Controller(client, options) { 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); + // Use user-defined PID values. If they are not defined, use default values + options.pid = options.pid || {x_axis: {p_constant: 0.5, i_constant: 0, d_constant: 0.35}, + y_axis: {p_constant: 0.5, i_constant: 0, d_constant: 0.35}, + z_axis: {p_constant: 0.8, i_constant: 0, d_constant: 0.35}, + yaw_axis: {p_constant: 1.0, i_constant: 0, d_constant: 0.30}}; + // PID controller for each axis: new PID( P constant, I constant, D constant ) + this._pid_x = new PID(options.pid.x_axis.p_constant, + options.pid.x_axis.i_constant, + options.pid.x_axis.d_constant); + this._pid_y = new PID(options.pid.y_axis.p_constant, + options.pid.y_axis.i_constant, + options.pid.y_axis.d_constant); + this._pid_z = new PID(options.pid.z_axis.p_constant, + options.pid.z_axis.i_constant, + options.pid.z_axis.d_constant); + this._pid_yaw = new PID(options.pid.yaw_axis.p_constant, + options.pid.yaw_axis.i_constant, + options.pid.yaw_axis.d_constant); // kalman filter is used for the drone state estimation this._ekf = new EKF(options); @@ -41,34 +61,53 @@ function Controller(client, options) { // Ensure that we don't enter the processing loop twice this._busy = false; - // The curretn target goal and an optional callback to trigger + // The current 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 known non-flight related drone info + this._drone_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) { + client.on('navdata', function(navdata) { + if (!this._busy && self._hasNecessaryData(navdata)) { this._busy = true; - self._processNavdata(d); - self._control(d); + self._processNavdata(navdata); + self._control(navdata); this._busy = false; } }); } +Controller.prototype._hasNecessaryData = function (navdata) { + 'use strict'; + //TODO: Make better test of navdata + if (typeof navdata.demo == "object" && typeof navdata.magneto == "object" && typeof navdata.magneto.heading.fusionUnwrapped != "undefined") { + // navdata is good + return true; + } + else { + // navdata is not enough + // Configure drone to send over all navdata + this._client.config("general:navdata_demo", false); + return false; + } +}; + /* * Enable auto-pilot. The controller will attempt to bring * the drone (and maintain it) to the goal. */ Controller.prototype.enable = function() { + 'use strict'; this._pid_x.reset(); this._pid_y.reset(); this._pid_z.reset(); @@ -81,24 +120,27 @@ Controller.prototype.enable = function() { * and send a stop command to the drone. */ Controller.prototype.disable = function() { + 'use strict'; this._enabled = false; this._client.stop(); -} +}; /* * Return the drone state (x,y,z,yaw) as estimated * by the Kalman Filter. */ Controller.prototype.state = function() { + 'use strict'; 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}); -} + 'use strict'; + this.disable(); +}; /* * Reset the kalman filter to its base state (default is x:0, y:0, yaw:0). @@ -108,55 +150,74 @@ Controller.prototype.hover = function() { * of the drone. */ Controller.prototype.zero = function() { + 'use strict'; 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(); + 'use strict'; + // 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; + if (state === null) { + if (shouldSendDebugInfo) { + console.warn("Unable to go forward since current location is unknown"); + } + } + else { + // Remap our target position in the world coordinates + var worldX = state.x + Math.cos(state.yaw) * distance; + var worldY = state.y + Math.sin(state.yaw) * distance; - // Assign the new goal - this._go({x: gx, y: gy, z: state.z, yaw: state.yaw}, callback); -} + // Assign the new goal + this._go({x: worldX, y: worldY, z: state.z, yaw: state.yaw}, callback); + } +}; /* * Move backward by the given distance (in meters). */ Controller.prototype.backward = function(distance, callback) { + 'use strict'; 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(); + 'use strict'; + // 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; + if (state === null) { + if (shouldSendDebugInfo) { + console.warn("Unable to go right since current location is unknown"); + } + } + else { + // Remap our target position in the world coordinates + var worldX = state.x - Math.sin(state.yaw) * distance; + var worldY = state.y + Math.cos(state.yaw) * distance; - // Assign the new goal - this._go({x: gx, y: gy, z: state.z, yaw: state.yaw}, callback); -} + // Assign the new goal + this._go({x: worldX, y: worldY, z: state.z, yaw: state.yaw}, callback); + } +}; /* * Move left by the given distance (in meters). */ Controller.prototype.left = function(distance, callback) { + 'use strict'; return this.right(-distance, callback); -} +}; /* * Turn clockwise of the given angle. Note that this does not @@ -164,49 +225,55 @@ Controller.prototype.left = function(distance, callback) { * will turn in the other direction, taking the shortest path. */ Controller.prototype.cw = function(angle, callback) { + 'use strict'; var state = this.state(); - var yaw = state.yaw.toDeg() + angle; + var yaw = toDeg(state.yaw) + angle; - return this._go({x: state.x, y: state.y, z: state.z, yaw: yaw.toRad()}, callback); -} + return this._go({x: state.x, y: state.y, z: state.z, yaw: toRad(yaw)}, callback); +}; /* * Turn counter clockwise of the given angle (in degrees) */ Controller.prototype.ccw = function(angle, callback) { + 'use strict'; return this.cw(-angle, callback); -} +}; /* * Climb ups by the given distance (in meters). */ Controller.prototype.up = function(distance, callback) { + 'use strict'; 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) { + 'use strict'; return this.up(-distance, callback); -} +}; /* * Go to the target altitude */ Controller.prototype.altitude = function(altitude, callback) { + 'use strict'; 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) { + 'use strict'; var state = this.state(); - return this._go({x: state.x, y: state.y, z: state.z, yaw: yaw.toRad()}, callback); -} + return this._go({x: state.x, y: state.y, z: state.z, yaw: toRad(yaw)}, callback); +}; /* * Sets a new goal and enable the controller. When the goal @@ -216,14 +283,16 @@ Controller.prototype.yaw = function(yaw, callback) { * yaw in degrees */ Controller.prototype.go = function(goal, callback) { - if (goal.yaw != undefined) { - goal.yaw = goal.yaw.toRad(); + 'use strict'; + if (goal.yaw !== undefined) { + goal.yaw = toRad(goal.yaw); } return this._go(goal, callback); -} +}; Controller.prototype._go = function(goal, callback) { + 'use strict'; // Since we are going to modify goal settings, we // disable the controller, just in case. this.disable(); @@ -233,13 +302,15 @@ Controller.prototype._go = function(goal, callback) { // Normalize the yaw, to make sure we don't spin 360deg for // nothing :-) - if (goal.yaw != undefined) { + if (goal.yaw !== undefined) { var yaw = goal.yaw; - goal.yaw = Math.atan2(Math.sin(yaw),Math.cos(yaw)); + while (yaw > Math.PI) { yaw -= 2 * Math.PI;} + while (yaw < -Math.PI) { yaw += 2 * Math.PI;} + goal.yaw = yaw; } // Make sure we don't attempt to go too low - if (goal.z != undefined) { + if (goal.z !== undefined) { goal.z = Math.max(goal.z, 0.5); } @@ -250,39 +321,43 @@ Controller.prototype._go = function(goal, callback) { // Keep track of the callback to trigger when we reach the goal this._callback = callback; - // (Re)-Enable the controller + // Reenable the controller this.enable(); -} +}; -Controller.prototype._processNavdata = function(d) { +Controller.prototype._processNavdata = function(navdata) { + 'use strict'; + // Save non-flight related drone info + this._drone_state = navdata.droneState; + // EKF prediction step - this._ekf.predict(d); - + this._ekf.predict(navdata); + // 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) { + if (navdata.visionDetect && navdata.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 + var cameraX = navdata.visionDetect.xc[0], + cameraY = navdata.visionDetect.yc[0], + cameraWidth = navdata.visionDetect.width[0], + cameraHeight = navdata.visionDetect.height[0], + yaw = navdata.visionDetect.orientationAngle[0], + distance = navdata.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); + var camera = this._camera.p2m(cameraX + cameraWidth/2, cameraY + cameraHeight/2, distance); // 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(); + measured.yaw = toRad(yaw); // Execute the EKS correction step this._ekf.correct(measured, this._tag); @@ -290,33 +365,38 @@ 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 -} + this._state.z = navdata.demo.altitude; + this._state.vx = navdata.demo.velocity.x / 1000; //We want m/s instead of mm/s + this._state.vy = navdata.demo.velocity.y / 1000; +}; -Controller.prototype._control = function(d) { +Controller.prototype._control = function(navdata) { + 'use strict'; // Do not control if not enabled - if (!this._enabled) return; + if (!this._enabled) { + return; + } // Do not control if no known state or no goal defines - if (this._goal == null || this._state == null) return; + 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 + var xError = (this._goal.x !== undefined) ? this._goal.x - this._state.x : 0, + yError = (this._goal.y !== undefined) ? this._goal.y - this._state.y : 0, + zError = (this._goal.z !== undefined) ? this._goal.z - this._state.z : 0, + yawError = (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); + while(yawError < -Math.PI) {yawError += (2 * Math.PI);} + while(yawError > Math.PI) {yawError -= (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)) { + if ((Math.abs(xError) < EPS_LINEAR) && (Math.abs(yError) < EPS_LINEAR) && (Math.abs(zError) < EPS_ALTITUDE) && (Math.abs(yawError) < EPS_ANGULAR)) { // Have we been here before ? - if (!this._goal.reached && this._last_ok != 0) { + 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 @@ -324,7 +404,7 @@ Controller.prototype._control = function(d) { // 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) { + if (this._callback !== null) { setTimeout(this._callback, 10); this._callback = null; } @@ -335,48 +415,57 @@ Controller.prototype._control = function(d) { } 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); - } + } + // If we just left the goal, we notify + else 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); + var xCommand = this._pid_x.getCommand(xError); + var yCommand = this._pid_y.getCommand(yError); + var zCommand = this._pid_z.getCommand(zError); + var yawCommand = this._pid_yaw.getCommand(yawError); - // Ceil commands and map them to drone orientation + // Map the commands to drone orientation and normalize them + // The drone only accepts directional speeds within [-1 units, 1 units] 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); + var normalizedXCommand = within(Math.cos(yaw) * xCommand + Math.sin(yaw) * yCommand, -1, 1); + var normalizedYCommand = within(-Math.sin(yaw) * xCommand + Math.cos(yaw) * yCommand, -1, 1); + var normalizedZCommand = within(zCommand, -1, 1); + var normalizedYawCommand = within(yawCommand, -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}, + error: {ex: xError, ey: yError, ez: zError, eyaw: yawError}, + control: {ux: xCommand, uy: yCommand, uz: zCommand, uyaw: yawCommand}, last_ok: this._last_ok, - tag: (d.visionDetect && d.visionDetect.nbDetected > 0) ? 1 : 0 + tag: (navdata.visionDetect && navdata.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); + // Check to see if commands are ok. For example, the drone will not go up if it is commanded soley to go up + // the drone must be in motion + /*if (Math.abs(normalizedZCommand) > 0.01 && !(Math.abs(normalizedXCommand) > 0.01 || Math.abs(normalizedYCommand) > 0.01 || Math.abs(normalizedYawCommand) > 0.01)) { + // The drone needs to go up but there is no other motion + // Give the drone a rotational command + normalizedYawCommand = 0.1; + }*/ + + // Send commands to drone if the are significant + if (Math.abs(normalizedXCommand) > 0.01) {this._client.front(normalizedXCommand);} + if (Math.abs(normalizedYCommand) > 0.01) {this._client.right(normalizedYCommand);} + if (Math.abs(normalizedZCommand) > 0.01) {this._client.up(normalizedZCommand);} + if (Math.abs(normalizedYawCommand) > 0.01) {this._client.clockwise(normalizedYawCommand);} -} +}; function within(x, min, max) { + 'use strict'; if (x < min) { return min; } else if (x > max) { @@ -385,3 +474,15 @@ function within(x, min, max) { return x; } } + +// Converts numeric degrees to radians +var toRad = function(number) { + 'use strict'; + return number * Math.PI / 180; +}; + +// Converts radians to numeric dregrees +var toDeg = function(number) { + 'use strict'; + return number * 180 / Math.PI; +}; diff --git a/lib/EKF.js b/lib/EKF.js index dab07d7..0353bfc 100644 --- a/lib/EKF.js +++ b/lib/EKF.js @@ -1,77 +1,186 @@ +// Extended Kalman Filter +// Learn more at https://en.wikipedia.org/wiki/Extended_Kalman_filter +// This file replaces StateEstimator.js, which is now not used + +// Should this file output debugging information to `console`? +var shouldSendDebugInfo = true; + 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 +var geolib = require('geolib'); module.exports = EKF; function EKF(options) { + 'use strict'; - options = options || {}; + options = options || {}; - this._options = options; - this._delta_t = options.delta_t || EKF.DELTA_T; + this._options = options; - this.reset(); + this.reset(); } EKF.prototype.state = function() { + 'use strict'; return this._state; -} +}; EKF.prototype.confidence = function() { + 'use strict'; 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 - ; + 'use strict'; + this._state = this._options.state || {x: 0, y: 0, yaw: 0, absoluteYaw: null}; + 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; + this._last_time = null; + this._last_gps_lat = null; + this._last_gps_lon = null; + this._last_gps_time = null; +}; +EKF.prototype.predict = function(navdata) { + 'use strict'; + var doNotUpdateState = false; + // LaTeX Math: \int (vx)dt = dx + try { + var pitch = toRad(navdata.demo.rotation.pitch), + roll = toRad(navdata.demo.rotation.roll), + yaw = normAngle(toRad(navdata.magneto.heading.fusionUnwrapped)), + velocityX = navdata.demo.velocity.x / 1000, // Convert milimeters/second to meters/second + velocityY = navdata.demo.velocity.y / 1000, + currentTime = navdata.time / 1000 // Convert miliseconds to seconds + ; + } + catch (error) { + if (error instanceof TypeError) + { + if(shouldSendDebugInfo) { + console.warn("Error reading navdata"); + } + return; + } + else { + throw error; + } + } + + // Get GPS data if it is available + if (typeof(navdata.gps) == "object" && navdata.gps !== null) { + if (typeof(navdata.gps.latitude) == "number" && typeof(navdata.gps.longitude) == "number") { + if (navdata.gps.latitude !== 0 || navdata.gps.longitude !== 0) { + var gpsLat = navdata.gps.latitude, + gpsLon = navdata.gps.longitude, + hasGpsData = true + ; + } + } + } + else { + // GPS data unavailable + var hasGpsData = false; + } + // 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) { + if (this._last_yaw === null) { this._last_yaw = yaw; + doNotUpdateState = true; + } + if (this._last_time === null) { + this._last_time = currentTime; + doNotUpdateState = true; + } + if (doNotUpdateState) { return; } - + + var deltaT = currentTime - this._last_time; + // We are not interested in the absolute yaw, but someone else might. + // Save the absolute yaw in case + this._state.absoluteYaw = yaw; + // Compute the odometry by integrating the motion over delta_t - var o = {dx: vx * dt, dy: vy * dt, dyaw: yaw - this._last_yaw}; + var odometry = {deltaX: velocityX * deltaT, deltaY: velocityY * deltaT, deltaYaw: 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; - + state.x += Math.cos(state.yaw) * odometry.deltaX - Math.sin(state.yaw) * odometry.deltaY; + state.y += Math.sin(state.yaw) * odometry.deltaX + Math.cos(state.yaw) * odometry.deltaY; + state.yaw += odometry.deltaYaw; + // Normalize the yaw value - state.yaw = Math.atan2(Math.sin(state.yaw),Math.cos(state.yaw)); + state.yaw = normAngle(state.yaw); + + // Overwrite odometry estimate when there is GPS data (which yields more accurate data) + if (hasGpsData) { + if(shouldSendDebugInfo) { + console.log("Analizing GPS data"); + } + // We need a prior GPS value to calculate distance + // If we don't have this, record the current GPS location for next time + if (this._last_gps_lat === null || this._last_gps_lon === null || (new Date().valueOf() - this._last_gps_time.valueOf()) > 4000) { + this._last_gps_lat = gpsLat; + this._last_gps_lon = gpsLon; + this._last_gps_time = new Date(); + return; + } + // Find displacement + var displacement = geolib.getDistance({latitude: gpsLat, longitude: gpsLon}, + {latitude: this._last_gps_lat, longitude: this._last_gps_lon}); + var bearing = geolib.getCompassDirection({latitude: gpsLat, longitude: gpsLon}, + {latitude: this._last_gps_lat, longitude: this._last_gps_lon}); + // Find X component of displacement + var displacementXComponent = displacement * Math.cos(bearing); + // Find Y component of displacement + var displacmenetYComponent = displacement * Math.sin(bearing); + + // In order to detect whether the drone's yaw is correct. + // Sometimes the drone rotates little by little and rotates its coordinate grid + + // Compare the GPS-based displacement to the estimate based displacement + var estimatedDisplacement = $V([state.x, state.y]); + var estimatedDisplacementAngle = estimatedDisplacement.angleFrom($V([1,0])); + var angleError = estimatedDisplacementAngle - bearing; + state.yaw -= angleError; + + // The former segment of code was just for angle calculation + // Now we will overwrite the state calculation with the more accuate GPS data + state.x = displacementXComponent; + state.y = displacmenetYComponent; + // Store GPS coordinates for the next calculation + this._last_gps_lat = gpsLat; + this._last_gps_lon = gpsLon; + // Store current time so that the GPS point is not used after + // it is not updated for a certain period of time, in the case of GPS signal loss + this._last_gps_time = new Date(); + } - // 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], + // Compute the G term (due to the Taylor approximation to linearize the function), + // where G is dimensionless magnetic moment (g-factor) + var gFactor = $M( + [[1, 0, -1 * Math.sin(state.yaw) * odometry.deltaX - Math.cos(state.yaw) * odometry.deltaY], + [0, 1, Math.cos(state.yaw) * odometry.deltaX - Math.sin(state.yaw) * odometry.deltaY], [0, 0, 1]] ); // Compute the new sigma - this._sigma = G.multiply(this._sigma).multiply(G.transpose()).add(this._q); -} + this._sigma = gFactor.multiply(this._sigma).multiply(gFactor.transpose()).add(this._q); + + // Store the current time for the next time + this._last_time = currentTime; +}; + +// Only used when a tag is detected /* * 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) @@ -82,6 +191,7 @@ EKF.prototype.predict = function(data) { * pose.yaw: yaw-rotation of marker in world-coordinate system */ EKF.prototype.correct = function(measure, pose) { + 'use strict'; // Compute expected measurement given our current state and the marker pose var state = this._state; var psi = state.yaw; @@ -90,7 +200,7 @@ EKF.prototype.correct = function(measure, pose) { // Normalized the measure yaw measure.yaw = normAngle(measure.yaw); this._m = {x: measure.x, y: measure.y, yaw: measure.yaw}; - + //TODO: Elaborate variable names 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; @@ -125,21 +235,20 @@ EKF.prototype.correct = function(measure, pose) { }; function normAngle(rad) { + 'use strict'; 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 numeric degrees to radians +var toRad = function(number) { + 'use strict'; + return number * Math.PI / 180; +}; -/** Converts radians to numeric dregrees */ -if (typeof(Number.prototype.toDeg) === "undefined") { - Number.prototype.toDeg = function() { - return this * 180 / Math.PI; - } -} +// Converts radians to numeric dregrees +var toDeg = function(number) { + 'use strict'; + return number * 180 / Math.PI; +}; diff --git a/lib/Mission.js b/lib/Mission.js index d793d5b..4dab152 100644 --- a/lib/Mission.js +++ b/lib/Mission.js @@ -1,9 +1,15 @@ -var async = require('async') - , fs = require('fs') +// Mission.js + +// Should this file output debugging information to `console`? +var shouldSendDebugInfo = true; + +var async = require('async'), + fs = require('fs') ; module.exports = Mission; function Mission(client, controller, options) { + 'use strict'; options = options || {}; @@ -15,21 +21,53 @@ function Mission(client, controller, options) { } Mission.prototype.client = function() { + 'use strict'; return this._client; -} +}; Mission.prototype.control = function() { + 'use strict'; return this._control; -} +}; Mission.prototype.run = function(callback) { - async.waterfall(this._steps, callback); -} + 'use strict'; + this._abortMission = false; + var self = this; + async.waterfall(this._steps, function (error, results) { + // Clean the buffer of commands (this._steps) + self._steps = []; + callback(error, results); + }); +}; + +Mission.prototype.abort = function () { + 'use strict'; + this._abortMission = true; + if (this._control._goal !== null) { + this._control._goal.reached = true; + } + this._client.stop(); +}; Mission.prototype.log = function(path) { + 'use strict'; var dataStream = fs.createWriteStream(path); var ekf = this._control._ekf; - + + // For a proper CSV file, list the headers of the data once + dataStream.write("controlData.state.x,controlData.state.y,controlData.state.z,controlData.state.yaw," + + + "controlData.state.vx,controlData.state.vy," + + "controlData.goal.x,controlData.goal.y,controlData.goal.z,controlData.goal.yaw," + + "controlData.error.ex,controlData.error.ey,controlData.error.ez,controlData.error.eyaw," + + "controlData.control.ux,controlData.control.uy,controlData.control.uz,controlData.control.uyaw," + + "controlData.last_ok,controlData.tag," + + "ekf._s.x,ekf._s.y,ekf._s.ekf._yaw (deg)," + + "ekf._m.x,ekf._m.y,ekf._m.yaw (deg)," + + "ekf._z.x,ekf._z.y,ekf._z.yaw (deg," + + "ekf._e.x,ekf._e.y,ekf._e.yaw (deg)" + + "\n"); + this._control.on('controlData', function(d) { var log = (d.state.x + "," + d.state.y + "," + @@ -56,188 +94,463 @@ Mission.prototype.log = function(path) { log = log + "," + ekf._s.x + "," + ekf._s.y + "," + - ekf._s.yaw.toDeg() + "," + + toDeg(ekf._s.yaw) + "," + ekf._m.x + "," + ekf._m.y + "," + - ekf._m.yaw.toDeg() + "," + + toDeg(ekf._m.yaw) + "," + ekf._z.x + "," + ekf._z.y + "," + - ekf._z.yaw.toDeg() + "," + + toDeg(ekf._z.yaw) + "," + ekf._e.x + "," + ekf._e.y + "," + - ekf._e.yaw.toDeg() + toDeg(ekf._e.yaw); } else { - log = log + ",0,0,0,0,0,0" + log = log + ",0,0,0,0,0,0"; } log = log + "\n"; dataStream.write(log); }); -} +}; -Mission.prototype.takeoff = function() { + +Mission.prototype.ftrim = function () { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._client.takeoff(cb); + if (shouldSendDebugInfo) { + console.log("async executing Mission.ftrim"); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting ftrim"); + } + cb(new Error("Mission Aborted")); + } + else { + self._client.ftrim(); + // We need to call the callback ourselves since .ftrim() does not accept a callback argument + cb(); + } }); + return this; +}; +Mission.prototype.calibrate = function () { + 'use strict'; + var self = this; + this._steps.push(function(cb) { + if (shouldSendDebugInfo) { + console.log("async executing Mission.calibrate"); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting calibrate"); + } + cb(new Error("Mission Aborted")); + } + else { + self._client.calibrate(0); + // We need to call the callback ourselves since .calibrate() does not accept a callback argument + // Calibration is usually finished in 1.5 seconds + setTimeout(cb, 4000); + } + }); return this; -} +}; + +Mission.prototype.takeoff = function() { + 'use strict'; + var self = this; + this._steps.push(function(cb) { + if (shouldSendDebugInfo) { + console.log("async executing Mission.takeoff"); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting takeoff"); + } + cb(new Error("Mission Aborted")); + } + else { + self._client.takeoff(cb); + } + }); + return this; +}; Mission.prototype.land = function() { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._client.land(cb); + if (shouldSendDebugInfo) { + console.log("async executing Mission.land"); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting landing"); + } + cb(new Error("Mission Aborted")); + } + else { + self._client.land(cb); + } }); return this; -} +}; Mission.prototype.hover = function(delay) { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._control.hover(); - setTimeout(cb, delay); + if (shouldSendDebugInfo) { + console.log("async executing Mission.hover(%ds)", delay); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting hover"); + } + cb(new Error("Mission Aborted")); + } + else { + self._control.hover(); + setTimeout(function() { + self._control.enable(); + cb(); + }, delay); + } }); return this; -} +}; Mission.prototype.wait = function(delay) { + 'use strict'; + var self = this; this._steps.push(function(cb) { - setTimeout(cb, delay); + if (shouldSendDebugInfo) { + console.log("async executing Mission.wait(%ds)", delay); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting wait"); + } + cb(new Error("Mission Aborted")); + } + else { + setTimeout(cb, delay); + } }); return this; -} +}; Mission.prototype.task = function(task) { + 'use strict'; + var self = this; this._steps.push(function(cb) { - task(cb); + if (shouldSendDebugInfo) { + console.log("async executing Mission.task"); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting task"); + } + cb(new Error("Mission Aborted")); + } + else { + task(cb); + } }); return this; -} +}; Mission.prototype.taskSync = function(task) { + 'use strict'; + var self = this; this._steps.push(function(cb) { - task(); - cb(); + if (shouldSendDebugInfo) { + console.log("async executing Mission.taskSync"); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting taskSync"); + } + cb(new Error("Mission Aborted")); + } + else { + task(); + cb(); + } }); return this; -} +}; Mission.prototype.zero = function() { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._control.zero(); - cb(); + if (shouldSendDebugInfo) { + console.log("async executing Mission.zero"); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting zero"); + } + cb(new Error("Mission Aborted")); + } + else { + self._control.zero(); + cb(); + } }); return this; -} +}; Mission.prototype.go = function(goal) { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._control.go(goal, cb); + if (shouldSendDebugInfo) { + console.log("async executing Mission.go: (%d,%d,%d)", goal.x, goal.y, goal.z); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting go"); + } + cb(new Error("Mission Aborted")); + } + else { + self._control.go(goal, cb); + } }); return this; -} +}; Mission.prototype.forward = function(distance) { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._control.forward(distance, cb); + if (shouldSendDebugInfo) { + console.log("async executing Mission.forward(%dm)", distance); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting forward"); + } + cb(new Error("Mission Aborted")); + } + else { + self._control.forward(distance, cb); + } }); return this; -} +}; Mission.prototype.backward = function(distance) { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._control.backward(distance, cb); + if (shouldSendDebugInfo) { + console.log("async executing Mission.backward(%dm)", distance); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting backward"); + } + cb(new Error("Mission Aborted")); + } + else { + self._control.backward(distance, cb); + } }); return this; -} +}; Mission.prototype.left = function(distance) { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._control.left(distance, cb); + if (shouldSendDebugInfo) { + console.log("async executing Mission.left(%dm)", distance); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting left"); + } + cb(new Error("Mission Aborted")); + } + else { + self._control.left(distance, cb); + } }); return this; -} +}; Mission.prototype.right = function(distance) { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._control.right(distance, cb); + if (shouldSendDebugInfo) { + console.log("async executing Mission.right(%dm)", distance); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting right"); + } + cb(new Error("Mission Aborted")); + } + else { + self._control.right(distance, cb); + } }); return this; -} +}; Mission.prototype.up = function(distance) { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._control.up(distance, cb); + if (shouldSendDebugInfo) { + console.log("async executing Mission.up(%dm)", distance); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting up"); + } + cb(new Error("Mission Aborted")); + } + else { + self._control.up(distance, cb); + } }); return this; -} +}; Mission.prototype.down = function(distance) { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._control.down(distance, cb); + if (shouldSendDebugInfo) { + console.log("async executing Mission.down(%dm)", distance); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting down"); + } + cb(new Error("Mission Aborted")); + } + else { + self._control.down(distance, cb); + } }); return this; -} +}; Mission.prototype.cw = function(angle) { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._control.cw(angle, cb); + if (shouldSendDebugInfo) { + console.log("async executing Mission.cw(%d˚)", angle); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting cw"); + } + cb(new Error("Mission Aborted")); + } + else { + self._control.cw(angle, cb); + } }); return this; -} +}; Mission.prototype.ccw = function(angle) { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._control.ccw(angle, cb); + if (shouldSendDebugInfo) { + console.log("async executing Mission.ccw(%d˚)", angle); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting ccw"); + } + cb(new Error("Mission Aborted")); + } + else { + self._control.ccw(angle, cb); + } }); return this; -} +}; Mission.prototype.altitude = function(altitude) { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._control.altitude(altitude, cb); + if (shouldSendDebugInfo) { + console.log("async executing Mission.altitude(%d)", altitude); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting altitude"); + } + cb(new Error("Mission Aborted")); + } + else { + self._control.altitude(altitude, cb); + } }); return this; -} +}; Mission.prototype.yaw = function(angle) { + 'use strict'; var self = this; this._steps.push(function(cb) { - self._control.yaw(angle,cb); + if (shouldSendDebugInfo) { + console.log("async executing Mission.yaw(%d˚)", angle); + } + if (self._abortMission) { + if (shouldSendDebugInfo) { + console.log("Aborting yaw"); + } + cb(new Error("Mission Aborted")); + } + else { + self._control.yaw(angle,cb); + } }); return this; -} - - - - +}; + +// Converts numeric degrees to radians +var toRad = function(number) { + 'use strict'; + return number * Math.PI / 180; +}; + +// Converts radians to numeric dregrees +var toDeg = function(number) { + 'use strict'; + return number * 180 / Math.PI; +}; diff --git a/lib/PID.js b/lib/PID.js index ae4719e..57a5173 100644 --- a/lib/PID.js +++ b/lib/PID.js @@ -1,28 +1,36 @@ +// This is a PID controller +// See the following for more info +// https://oscarliang.com/understanding-pid-for-quadcopter-rc-flight/ + module.exports = PID; function PID(kp, ki, kd) { + 'use strict'; this.configure(kp, ki, kd); this.reset(); } PID.prototype.configure = function(kp,ki,kd) { + 'use strict'; this._kp = kp; this._ki = ki; this._kd = kd; -} +}; PID.prototype.reset = function() { + 'use strict'; this._last_time = 0; this._last_error = Infinity; this._error_sum = 0; -} +}; PID.prototype.getCommand = function(e) { + 'use strict'; // Compute dt in seconds var time = Date.now(); - var dt = (time - this._last_time) / 1000 + var dt = (time - this._last_time) / 1000; var de = 0; - if (this._last_time != 0) { + if (this._last_time !== 0) { // Compute de (error derivation) if (this._last_error < Infinity) { de = (e - this._last_error) / dt; @@ -42,4 +50,4 @@ PID.prototype.getCommand = function(e) { + this._kd * de; return command; -} +}; diff --git a/lib/StateEstimator.js b/lib/StateEstimator.old.js similarity index 67% rename from lib/StateEstimator.js rename to lib/StateEstimator.old.js index 310097b..eaf5063 100644 --- a/lib/StateEstimator.js +++ b/lib/StateEstimator.old.js @@ -1,3 +1,6 @@ +// StateEstimator.js +// This file is replaced by EFK.js so this file is no longer used + var EventEmitter = require('events').EventEmitter; var util = require('util'); @@ -14,7 +17,9 @@ function StateEstimator(client, options) { 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."); + 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); @@ -25,28 +30,28 @@ 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 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; + 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)); @@ -62,5 +67,5 @@ StateEstimator.prototype._processNavData = function(data) { if (typeof(Number.prototype.toRad) === "undefined") { Number.prototype.toRad = function() { return this * Math.PI / 180; - } + }; } diff --git a/package.json b/package.json index cef7594..97196c5 100644 --- a/package.json +++ b/package.json @@ -20,7 +20,9 @@ }, "dependencies": { "sylvester": "0.0.21", - "async": "0.2.9" + "async": "0.2.9", + "geolib": "2.0.21", + "ar-drone": "^0.3.3" }, "devDependencies": { "utest": "0.0.6", diff --git a/tests/unit/test-Controller.js b/tests/unit/test-Controller.js index 8191478..64bb7f3 100644 --- a/tests/unit/test-Controller.js +++ b/tests/unit/test-Controller.js @@ -48,20 +48,20 @@ test('Controller', { // Test forward with yaw 90 var yaw = 90; - ctrl._state.yaw = yaw.toRad(); + ctrl._state.yaw = toRad(yaw); 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._state.yaw = toRad(yaw); 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._state.yaw = toRad(yaw); ctrl.forward(1); assert.equal(Math.round(ctrl._goal.x * 1000) / 1000, -Math.round(ctrl._goal.y * 1000) /1000); }, @@ -77,14 +77,14 @@ test('Controller', { // Test right with yaw 90 var yaw = 90; - ctrl._state.yaw = yaw.toRad(); + ctrl._state.yaw = toRad(yaw); 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._state.yaw = toRad(yaw); ctrl.right(1); assert.equal(Math.round(ctrl._goal.x * 1000) / 1000, -Math.round(ctrl._goal.y * 1000) /1000); }, @@ -169,3 +169,15 @@ test('Controller', { assert.equal(goal.yaw, state.yaw); } }); + +// Converts numeric degrees to radians +var toRad = function(number) { + 'use strict'; + return number * Math.PI / 180; +}; + +// Converts radians to numeric dregrees +var toDeg = function(number) { + 'use strict'; + return number * 180 / Math.PI; +};