From eb16a23c1d2d1ad76d2f5e61e8eda370f3bf37b5 Mon Sep 17 00:00:00 2001 From: John Meyer Date: Fri, 12 Aug 2016 15:51:19 -0400 Subject: [PATCH 01/11] Initial Commit --- examples/square-2.js | 13 +- examples/square.js | 94 +++++++++------ index.js | 6 +- lib/Camera.js | 2 +- lib/Controller.js | 211 ++++++++++++++++++++------------ lib/EKF.js | 144 ++++++++++++++++++---- lib/Mission.js | 275 +++++++++++++++++++++++++++++++++++++----- lib/PID.js | 4 + lib/StateEstimator.js | 3 + package.json | 6 +- 10 files changed, 582 insertions(+), 176 deletions(-) diff --git a/examples/square-2.js b/examples/square-2.js index 4d2d1e3..c3b757e 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 @@ -44,15 +45,15 @@ mission.log("mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt"); // Plan mission mission.takeoff() .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..92465ac 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,39 @@ 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====")); + console.time(chalk.dim.blue("Call >> waterfall")); + 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")); + console.timeEnd(chalk.dim.blue("Call >> waterfall")); +} diff --git a/index.js b/index.js index ad7a05e..7ca29b8 100644 --- a/index.js +++ b/index.js @@ -1,5 +1,5 @@ var autonomy = exports; -var ardrone = require('ar-drone'); +var ardrone = require('ar-drone-custom'); exports.EKF = require('./lib/EKF'); exports.Camera = require('./lib/Camera'); @@ -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..d79cff5 100644 --- a/lib/Camera.js +++ b/lib/Camera.js @@ -7,7 +7,7 @@ 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 var K_BOTTOM = $M([[686.994766, 0, 329.323208], [0, 688.195055, 159.323007], [0, 0, 1]]); diff --git a/lib/Controller.js b/lib/Controller.js index 02f0dda..75b90c9 100644 --- a/lib/Controller.js +++ b/lib/Controller.js @@ -1,3 +1,8 @@ +// Controller.js +'use strict'; +// 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,10 +10,10 @@ 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); @@ -24,10 +29,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,29 +60,46 @@ 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) +{ + 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. @@ -97,7 +133,7 @@ Controller.prototype.state = function() { * 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}); + this.disable(); } /* @@ -117,15 +153,20 @@ Controller.prototype.zero = function() { * distance (in meters). */ Controller.prototype.forward = function(distance, callback) { - // Our starting position - var state = this.state(); + // 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 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); + // Assign the new goal + this._go({x: gx, y: gy, z: state.z, yaw: state.yaw}, callback); + } } /* @@ -140,15 +181,20 @@ Controller.prototype.backward = function(distance, callback) { * distance (in meters). */ Controller.prototype.right = function(distance, callback) { - // Our starting position - var state = this.state(); + // 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 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); + // Assign the new goal + this._go({x: gx, y: gy, z: state.z, yaw: state.yaw}, callback); + } } /* @@ -235,7 +281,9 @@ Controller.prototype._go = function(goal, callback) { // nothing :-) 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 @@ -250,26 +298,29 @@ 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) { + // 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 xc = navdata.visionDetect.xc[0] + , yc = navdata.visionDetect.yc[0] + , wc = navdata.visionDetect.width[0] + , hc = navdata.visionDetect.height[0] + , yaw = navdata.visionDetect.orientationAngle[0] + , dist = navdata.visionDetect.dist[0] / 100 // Need meters ; // Compute measure tag position (relative to drone) by @@ -290,12 +341,12 @@ 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) { // Do not control if not enabled if (!this._enabled) return; @@ -303,18 +354,18 @@ Controller.prototype._control = function(d) { 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) { // And for long enough ? @@ -335,44 +386,52 @@ 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); } diff --git a/lib/EKF.js b/lib/EKF.js index dab07d7..c880ab3 100644 --- a/lib/EKF.js +++ b/lib/EKF.js @@ -1,10 +1,17 @@ +// Extended Kalman Filter +// Learn more at https://en.wikipedia.org/wiki/Extended_Kalman_filter +// This file replaces StateEstimator.js, which is now not used +'use strict'; +// 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) { @@ -12,7 +19,6 @@ function EKF(options) { options = options || {}; this._options = options; - this._delta_t = options.delta_t || EKF.DELTA_T; this.reset(); } @@ -26,52 +32,140 @@ EKF.prototype.confidence = function() { } EKF.prototype.reset = function() { - this._state = this._options.state || {x: 0, y: 0, yaw: 0}; + 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(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 - ; - +EKF.prototype.predict = function(navdata) { + // LaTeX Math: \int (vx)dt = dx + try { + var pitch = navdata.demo.rotation.pitch.toRad() + , roll = navdata.demo.rotation.roll.toRad() + , yaw = normAngle((navdata.magneto.heading.fusionUnwrapped).toRad()) + , 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) { this._last_yaw = yaw; - return; + var doNotUpdateState = true; } - + if (this._last_time == null) { + this._last_time = currentTime; + var 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) diff --git a/lib/Mission.js b/lib/Mission.js index d793d5b..1c8088e 100644 --- a/lib/Mission.js +++ b/lib/Mission.js @@ -1,3 +1,8 @@ +// Mission.js +'use strict'; +// Should this file output debugging information to `console`? +var shouldSendDebugInfo = true; + var async = require('async') , fs = require('fs') ; @@ -23,13 +28,39 @@ Mission.prototype.control = function() { } Mission.prototype.run = function(callback) { - async.waterfall(this._steps, callback); + 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 () { + this._abortMission = true; + if (this._control._goal != null) + this._control._goal.reached = true; + this._client.stop(); } Mission.prototype.log = function(path) { 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.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()" + + "\n"); + this._control.on('controlData', function(d) { var log = (d.state.x + "," + d.state.y + "," + @@ -76,19 +107,74 @@ Mission.prototype.log = function(path) { }); } -Mission.prototype.takeoff = function() { + +Mission.prototype.ftrim = function () { + var self = this; + this._steps.push(function(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 () { var self = this; this._steps.push(function(cb) { - self._client.takeoff(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() { + 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() { 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; @@ -97,33 +183,73 @@ Mission.prototype.land = function() { Mission.prototype.hover = function(delay) { 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) { + 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) { + 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) { + 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; @@ -132,8 +258,17 @@ Mission.prototype.taskSync = function(task) { Mission.prototype.zero = function() { 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; @@ -142,7 +277,15 @@ Mission.prototype.zero = function() { Mission.prototype.go = function(goal) { 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; @@ -151,7 +294,15 @@ Mission.prototype.go = function(goal) { Mission.prototype.forward = function(distance) { 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; @@ -160,7 +311,15 @@ Mission.prototype.forward = function(distance) { Mission.prototype.backward = function(distance) { 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; @@ -169,7 +328,15 @@ Mission.prototype.backward = function(distance) { Mission.prototype.left = function(distance) { 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; @@ -178,7 +345,15 @@ Mission.prototype.left = function(distance) { Mission.prototype.right = function(distance) { 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; @@ -187,7 +362,15 @@ Mission.prototype.right = function(distance) { Mission.prototype.up = function(distance) { 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; @@ -196,7 +379,15 @@ Mission.prototype.up = function(distance) { Mission.prototype.down = function(distance) { 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; @@ -205,7 +396,15 @@ Mission.prototype.down = function(distance) { Mission.prototype.cw = function(angle) { 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; @@ -214,7 +413,15 @@ Mission.prototype.cw = function(angle) { Mission.prototype.ccw = function(angle) { 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; @@ -223,7 +430,15 @@ Mission.prototype.ccw = function(angle) { Mission.prototype.altitude = function(altitude) { 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; @@ -232,12 +447,16 @@ Mission.prototype.altitude = function(altitude) { Mission.prototype.yaw = function(angle) { 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; } - - - - diff --git a/lib/PID.js b/lib/PID.js index ae4719e..0a01416 100644 --- a/lib/PID.js +++ b/lib/PID.js @@ -1,3 +1,7 @@ +// 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) { this.configure(kp, ki, kd); diff --git a/lib/StateEstimator.js b/lib/StateEstimator.js index 310097b..e5f122b 100644 --- a/lib/StateEstimator.js +++ b/lib/StateEstimator.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'); diff --git a/package.json b/package.json index cef7594..60efabb 100644 --- a/package.json +++ b/package.json @@ -1,5 +1,5 @@ { - "name": "ardrone-autonomy", + "name": "ardrone-autonomy-custom", "version": "0.1.2", "description": "Building blocks for autonomous flying an AR.Drone.", "repository": { @@ -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", From cb9a9f7ec748399ed09f67723ee1b23e310cae57 Mon Sep 17 00:00:00 2001 From: 0x326 <0x326@users.noreply.github.com> Date: Fri, 12 Aug 2016 15:54:03 -0400 Subject: [PATCH 02/11] Update package.json Removes "-custom" postfix and reverts to original npm name --- package.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.json b/package.json index 60efabb..b1beed7 100644 --- a/package.json +++ b/package.json @@ -1,5 +1,5 @@ { - "name": "ardrone-autonomy-custom", + "name": "ardrone-autonomy", "version": "0.1.2", "description": "Building blocks for autonomous flying an AR.Drone.", "repository": { From e26bf20ce05193d532a2297a076dc5bfb21decad Mon Sep 17 00:00:00 2001 From: John Meyer Date: Tue, 16 Aug 2016 08:55:05 -0400 Subject: [PATCH 03/11] Add documentation about new mission commands --- README.md | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index aff1785..db30a30 100644 --- a/README.md +++ b/README.md @@ -47,7 +47,10 @@ with the drone taking off, travelling alongs a 2 x 2 meters square ane then land 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) @@ -81,10 +84,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 From f518391019902dd3d9a3a77d261802f1d322f4ef Mon Sep 17 00:00:00 2001 From: John Meyer Date: Tue, 16 Aug 2016 09:57:27 -0400 Subject: [PATCH 04/11] Update documentation about option override --- README.md | 29 +++++++++++++++++++++++++++++ lib/Controller.js | 6 +++--- 2 files changed, 32 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index db30a30..d4369eb 100644 --- a/README.md +++ b/README.md @@ -74,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 @@ -185,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/lib/Controller.js b/lib/Controller.js index 75b90c9..451a69a 100644 --- a/lib/Controller.js +++ b/lib/Controller.js @@ -11,9 +11,9 @@ var EKF = require('./EKF'); var Camera = require('./Camera'); 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 +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); From 9e178ecc57e56e8be887d42da704bdc17f0ed81e Mon Sep 17 00:00:00 2001 From: John Meyer Date: Tue, 16 Aug 2016 10:53:46 -0400 Subject: [PATCH 05/11] Clean up code Switch to function form of 'use strict'; Add brackets around one-line conditional statements; Use suggestions from linting software --- lib/Controller.js | 131 +++++---- lib/EKF.js | 109 +++++--- lib/Mission.js | 258 ++++++++++++------ lib/PID.js | 14 +- ...tateEstimator.js => StateEstimator.old.js} | 30 +- 5 files changed, 339 insertions(+), 203 deletions(-) rename lib/{StateEstimator.js => StateEstimator.old.js} (71%) diff --git a/lib/Controller.js b/lib/Controller.js index 451a69a..af68eee 100644 --- a/lib/Controller.js +++ b/lib/Controller.js @@ -1,5 +1,5 @@ // Controller.js -'use strict'; + // Should this file output debugging information to `console`? var shouldSendDebugInfo = true; @@ -18,6 +18,7 @@ var STABLE_DELAY = 200; // Time in ms to wait before declaring the drone on module.exports = Controller; util.inherits(Controller, EventEmitter); function Controller(client, options) { + 'use strict'; EventEmitter.call(this); options = options || {}; @@ -33,7 +34,7 @@ function Controller(client, options) { 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}} + 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, @@ -86,8 +87,8 @@ function Controller(client, options) { }); } -Controller.prototype._hasNecessaryData = function (navdata) -{ +Controller.prototype._hasNecessaryData = function (navdata) { + 'use strict'; if (typeof navdata.demo == "object" && typeof navdata.magneto == "object" && typeof navdata.magneto.heading.fusionUnwrapped != "undefined") { // navdata is good return true; @@ -105,6 +106,7 @@ Controller.prototype._hasNecessaryData = function (navdata) * 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(); @@ -117,24 +119,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() { + 'use strict'; this.disable(); -} +}; /* * Reset the kalman filter to its base state (default is x:0, y:0, yaw:0). @@ -144,21 +149,25 @@ 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 + 'use strict'; + // Our starting position var state = this.state(); - if (state == null) - if (shouldSendDebugInfo) + 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 gx = state.x + Math.cos(state.yaw) * distance; @@ -167,26 +176,30 @@ Controller.prototype.forward = function(distance, callback) { // 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) { + '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) { + 'use strict'; // Our starting position var state = this.state(); - if (state == null) - if (shouldSendDebugInfo) + 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 gx = state.x - Math.sin(state.yaw) * distance; @@ -195,14 +208,15 @@ Controller.prototype.right = function(distance, callback) { // 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) { + 'use strict'; return this.right(-distance, callback); -} +}; /* * Turn clockwise of the given angle. Note that this does not @@ -210,49 +224,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; 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) { + '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); -} +}; /* * Sets a new goal and enable the controller. When the goal @@ -262,14 +282,16 @@ Controller.prototype.yaw = function(yaw, callback) { * yaw in degrees */ Controller.prototype.go = function(goal, callback) { - if (goal.yaw != undefined) { + 'use strict'; + if (goal.yaw !== undefined) { goal.yaw = goal.yaw.toRad(); } 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(); @@ -279,7 +301,7 @@ 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; while (yaw > Math.PI) { yaw -= 2 * Math.PI;} while (yaw < -Math.PI) { yaw += 2 * Math.PI;} @@ -287,7 +309,7 @@ Controller.prototype._go = function(goal, callback) { } // 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); } @@ -300,9 +322,10 @@ Controller.prototype._go = function(goal, callback) { // Reenable the controller this.enable(); -} +}; Controller.prototype._processNavdata = function(navdata) { + 'use strict'; // Save non-flight related drone info this._drone_state = navdata.droneState; @@ -315,12 +338,12 @@ Controller.prototype._processNavdata = function(navdata) { // TODO: Add documentation about this if (navdata.visionDetect && navdata.visionDetect.nbDetected > 0) { // Fetch detected tag position, size and orientation - var xc = navdata.visionDetect.xc[0] - , yc = navdata.visionDetect.yc[0] - , wc = navdata.visionDetect.width[0] - , hc = navdata.visionDetect.height[0] - , yaw = navdata.visionDetect.orientationAngle[0] - , dist = navdata.visionDetect.dist[0] / 100 // Need meters + var xc = navdata.visionDetect.xc[0], + yc = navdata.visionDetect.yc[0], + wc = navdata.visionDetect.width[0], + hc = navdata.visionDetect.height[0], + yaw = navdata.visionDetect.orientationAngle[0], + dist = navdata.visionDetect.dist[0] / 100 // Need meters ; // Compute measure tag position (relative to drone) by @@ -344,30 +367,35 @@ Controller.prototype._processNavdata = function(navdata) { 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(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 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 + 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(yawError < -Math.PI) yawError += (2 * Math.PI); - while(yawError > Math.PI) yawError -= (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(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 @@ -375,7 +403,7 @@ Controller.prototype._control = function(navdata) { // 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; } @@ -388,7 +416,7 @@ Controller.prototype._control = function(navdata) { } } // If we just left the goal, we notify - else if (this._last_ok != 0) { + else if (this._last_ok !== 0) { // Reset last ok since we are in motion this._last_ok = 0; this._goal.reached = false; @@ -428,14 +456,15 @@ Controller.prototype._control = function(navdata) { }*/ // 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); + 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) { diff --git a/lib/EKF.js b/lib/EKF.js index c880ab3..b10631b 100644 --- a/lib/EKF.js +++ b/lib/EKF.js @@ -1,7 +1,7 @@ // Extended Kalman Filter // Learn more at https://en.wikipedia.org/wiki/Extended_Kalman_filter // This file replaces StateEstimator.js, which is now not used -'use strict'; + // Should this file output debugging information to `console`? var shouldSendDebugInfo = true; @@ -15,80 +15,93 @@ var geolib = require('geolib'); module.exports = EKF; function EKF(options) { + 'use strict'; - options = options || {}; + options = options || {}; - this._options = options; + 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, 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; -} + '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 = navdata.demo.rotation.pitch.toRad() - , roll = navdata.demo.rotation.roll.toRad() - , yaw = normAngle((navdata.magneto.heading.fusionUnwrapped).toRad()) - , 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 + var pitch = navdata.demo.rotation.pitch.toRad(), + roll = navdata.demo.rotation.roll.toRad(), + yaw = normAngle((navdata.magneto.heading.fusionUnwrapped).toRad()), + 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) + if(shouldSendDebugInfo) { console.warn("Error reading navdata"); + } return; } - else + 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; + 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 + } + 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; - var doNotUpdateState = true; + doNotUpdateState = true; } - if (this._last_time == null) { + if (this._last_time === null) { this._last_time = currentTime; - var doNotUpdateState = true; + doNotUpdateState = true; } - if (doNotUpdateState) + if (doNotUpdateState) { return; + } var deltaT = currentTime - this._last_time; // We are not interested in the absolute yaw, but someone else might. @@ -110,11 +123,12 @@ EKF.prototype.predict = function(navdata) { // Overwrite odometry estimate when there is GPS data (which yields more accurate data) if (hasGpsData) { - if(shouldSendDebugInfo) + 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) { + 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(); @@ -164,7 +178,8 @@ EKF.prototype.predict = function(navdata) { // 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) @@ -176,6 +191,7 @@ EKF.prototype.predict = function(navdata) { * 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; @@ -219,6 +235,7 @@ 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; @@ -226,14 +243,16 @@ function normAngle(rad) { /** Converts numeric degrees to radians */ if (typeof(Number.prototype.toRad) === "undefined") { - Number.prototype.toRad = function() { - return this * Math.PI / 180; - } + Number.prototype.toRad = function() { + 'use strict'; + 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; - } + Number.prototype.toDeg = function() { + 'use strict'; + return this * 180 / Math.PI; + }; } diff --git a/lib/Mission.js b/lib/Mission.js index 1c8088e..f310ef3 100644 --- a/lib/Mission.js +++ b/lib/Mission.js @@ -1,14 +1,15 @@ // Mission.js -'use strict'; + // Should this file output debugging information to `console`? var shouldSendDebugInfo = true; -var async = require('async') - , fs = require('fs') +var async = require('async'), + fs = require('fs') ; module.exports = Mission; function Mission(client, controller, options) { + 'use strict'; options = options || {}; @@ -20,14 +21,17 @@ 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) { + 'use strict'; this._abortMission = false; var self = this; async.waterfall(this._steps, function (error, results) { @@ -35,22 +39,25 @@ Mission.prototype.run = function(callback) { self._steps = []; callback(error, results); }); -} +}; Mission.prototype.abort = function () { + 'use strict'; this._abortMission = true; - if (this._control._goal != null) + 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," + 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," @@ -96,26 +103,29 @@ Mission.prototype.log = function(path) { ekf._z.yaw.toDeg() + "," + ekf._e.x + "," + ekf._e.y + "," + - ekf._e.yaw.toDeg() + ekf._e.yaw.toDeg(); } 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.ftrim = function () { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.ftrim"); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting ftrim"); + } cb(new Error("Mission Aborted")); } else { @@ -125,16 +135,19 @@ Mission.prototype.ftrim = function () { } }); return this; -} +}; Mission.prototype.calibrate = function () { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.calibrate"); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting calibrate"); + } cb(new Error("Mission Aborted")); } else { @@ -145,49 +158,60 @@ Mission.prototype.calibrate = function () { } }); return this; -} +}; Mission.prototype.takeoff = function() { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.takeoff"); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting takeoff"); + } cb(new Error("Mission Aborted")); } - else + else { self._client.takeoff(cb); + } }); return this; -} +}; Mission.prototype.land = function() { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.land"); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting landing"); + } cb(new Error("Mission Aborted")); } - else + else { self._client.land(cb); + } }); return this; -} +}; Mission.prototype.hover = function(delay) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.hover(%ds)", delay); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting hover"); + } cb(new Error("Mission Aborted")); } else { @@ -200,50 +224,61 @@ Mission.prototype.hover = function(delay) { }); return this; -} +}; Mission.prototype.wait = function(delay) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.wait(%ds)", delay); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting wait"); + } cb(new Error("Mission Aborted")); } - else + else { setTimeout(cb, delay); + } }); return this; -} +}; Mission.prototype.task = function(task) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.task"); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting task"); + } cb(new Error("Mission Aborted")); } - else + else { task(cb); + } }); return this; -} +}; Mission.prototype.taskSync = function(task) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.taskSync"); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting taskSync"); + } cb(new Error("Mission Aborted")); } else { @@ -253,16 +288,19 @@ Mission.prototype.taskSync = function(task) { }); return this; -} +}; Mission.prototype.zero = function() { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.zero"); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting zero"); + } cb(new Error("Mission Aborted")); } else { @@ -272,191 +310,235 @@ Mission.prototype.zero = function() { }); return this; -} +}; Mission.prototype.go = function(goal) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.go: (%d,%d,%d)", goal.x, goal.y, goal.z); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting go"); + } cb(new Error("Mission Aborted")); } - else + else { self._control.go(goal, cb); + } }); return this; -} +}; Mission.prototype.forward = function(distance) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.forward(%dm)", distance); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting forward"); + } cb(new Error("Mission Aborted")); } - else + else { self._control.forward(distance, cb); + } }); return this; -} +}; Mission.prototype.backward = function(distance) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.backward(%dm)", distance); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting backward"); + } cb(new Error("Mission Aborted")); } - else + else { self._control.backward(distance, cb); + } }); return this; -} +}; Mission.prototype.left = function(distance) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.left(%dm)", distance); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting left"); + } cb(new Error("Mission Aborted")); } - else + else { self._control.left(distance, cb); + } }); return this; -} +}; Mission.prototype.right = function(distance) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.right(%dm)", distance); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting right"); + } cb(new Error("Mission Aborted")); } - else + else { self._control.right(distance, cb); + } }); return this; -} +}; Mission.prototype.up = function(distance) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.up(%dm)", distance); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting up"); + } cb(new Error("Mission Aborted")); } - else + else { self._control.up(distance, cb); + } }); return this; -} +}; Mission.prototype.down = function(distance) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.down(%dm)", distance); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting down"); + } cb(new Error("Mission Aborted")); } - else + else { self._control.down(distance, cb); + } }); return this; -} +}; Mission.prototype.cw = function(angle) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.cw(%d˚)", angle); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting cw"); + } cb(new Error("Mission Aborted")); } - else + else { self._control.cw(angle, cb); + } }); return this; -} +}; Mission.prototype.ccw = function(angle) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.ccw(%d˚)", angle); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting ccw"); + } cb(new Error("Mission Aborted")); } - else + else { self._control.ccw(angle, cb); + } }); return this; -} +}; Mission.prototype.altitude = function(altitude) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.altitude(%d)", altitude); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting altitude"); + } cb(new Error("Mission Aborted")); } - else + else { self._control.altitude(altitude, cb); + } }); return this; -} +}; Mission.prototype.yaw = function(angle) { + 'use strict'; var self = this; this._steps.push(function(cb) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("async executing Mission.yaw(%d˚)", angle); + } if (self._abortMission) { - if (shouldSendDebugInfo) + if (shouldSendDebugInfo) { console.log("Aborting yaw"); + } cb(new Error("Mission Aborted")); } - else + else { self._control.yaw(angle,cb); + } }); return this; -} +}; diff --git a/lib/PID.js b/lib/PID.js index 0a01416..57a5173 100644 --- a/lib/PID.js +++ b/lib/PID.js @@ -4,29 +4,33 @@ 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; @@ -46,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 71% rename from lib/StateEstimator.js rename to lib/StateEstimator.old.js index e5f122b..eaf5063 100644 --- a/lib/StateEstimator.js +++ b/lib/StateEstimator.old.js @@ -17,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); @@ -28,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)); @@ -65,5 +67,5 @@ StateEstimator.prototype._processNavData = function(data) { if (typeof(Number.prototype.toRad) === "undefined") { Number.prototype.toRad = function() { return this * Math.PI / 180; - } + }; } From 9d02903b364f70939d5c128cd61263e4442a0400 Mon Sep 17 00:00:00 2001 From: John Meyer <0x326@users.noreply.github.com> Date: Tue, 16 Aug 2016 13:30:25 -0400 Subject: [PATCH 06/11] Update package.json --- package.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.json b/package.json index b1beed7..97196c5 100644 --- a/package.json +++ b/package.json @@ -22,7 +22,7 @@ "sylvester": "0.0.21", "async": "0.2.9", "geolib": "2.0.21", - "ar-drone": "0.3.3" + "ar-drone": "^0.3.3" }, "devDependencies": { "utest": "0.0.6", From 6faaf15fce4b92285fdfa7343af5d4f2c0ce27c3 Mon Sep 17 00:00:00 2001 From: John Meyer <0x326@users.noreply.github.com> Date: Tue, 16 Aug 2016 13:35:55 -0400 Subject: [PATCH 07/11] Fix Typo in README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index d4369eb..e4421d3 100644 --- a/README.md +++ b/README.md @@ -41,7 +41,7 @@ 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'); From f27be0b62f022de08b60e80e4a66d53283aebe70 Mon Sep 17 00:00:00 2001 From: John Meyer Date: Tue, 16 Aug 2016 15:16:39 -0400 Subject: [PATCH 08/11] Remove -custom postfix from require --- index.js | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/index.js b/index.js index 7ca29b8..a3cb99f 100644 --- a/index.js +++ b/index.js @@ -1,5 +1,5 @@ var autonomy = exports; -var ardrone = require('ar-drone-custom'); +var ardrone = require('ar-drone'); exports.EKF = require('./lib/EKF'); exports.Camera = require('./lib/Camera'); From 478abe2b336fa964de5f6504776126b833a8b32e Mon Sep 17 00:00:00 2001 From: John Meyer Date: Wed, 17 Aug 2016 15:29:08 -0400 Subject: [PATCH 09/11] Rename Variables and Insert License Rename variables under more descriptive names and add license for code snippet --- lib/Camera.js | 30 +++++++++++++++++++++--------- lib/Controller.js | 13 +++++++------ 2 files changed, 28 insertions(+), 15 deletions(-) diff --git a/lib/Camera.js b/lib/Camera.js index d79cff5..e37921f 100644 --- a/lib/Camera.js +++ b/lib/Camera.js @@ -8,6 +8,20 @@ var util = require('util'); // AR Drone 2.0 Bottom Camera Intrinsic Matrix // 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 af68eee..2fc6c45 100644 --- a/lib/Controller.js +++ b/lib/Controller.js @@ -89,6 +89,7 @@ function Controller(client, options) { 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; @@ -170,11 +171,11 @@ Controller.prototype.forward = function(distance, callback) { } else { // 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; + 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); + this._go({x: worldX, y: worldY, z: state.z, yaw: state.yaw}, callback); } }; @@ -202,11 +203,11 @@ Controller.prototype.right = function(distance, callback) { } else { // 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; + 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); + this._go({x: worldX, y: worldY, z: state.z, yaw: state.yaw}, callback); } }; From 6c1fa70c50e791291eb6d93af9989a8af79b046e Mon Sep 17 00:00:00 2001 From: John Meyer Date: Wed, 17 Aug 2016 16:06:49 -0400 Subject: [PATCH 10/11] Change Number.toRad to toRad(Number) Also upgrade variable names to more a descriptive name --- lib/Controller.js | 34 +++++++++++++++++++++++----------- lib/EKF.js | 32 ++++++++++++++------------------ lib/Mission.js | 28 ++++++++++++++++++++-------- tests/unit/test-Controller.js | 22 +++++++++++++++++----- 4 files changed, 74 insertions(+), 42 deletions(-) diff --git a/lib/Controller.js b/lib/Controller.js index 2fc6c45..6b117ec 100644 --- a/lib/Controller.js +++ b/lib/Controller.js @@ -227,9 +227,9 @@ Controller.prototype.left = function(distance, callback) { 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); }; /* @@ -272,7 +272,7 @@ Controller.prototype.altitude = function(altitude, callback) { 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); }; /* @@ -285,7 +285,7 @@ Controller.prototype.yaw = function(yaw, callback) { Controller.prototype.go = function(goal, callback) { 'use strict'; if (goal.yaw !== undefined) { - goal.yaw = goal.yaw.toRad(); + goal.yaw = toRad(goal.yaw); } return this._go(goal, callback); @@ -339,25 +339,25 @@ Controller.prototype._processNavdata = function(navdata) { // TODO: Add documentation about this if (navdata.visionDetect && navdata.visionDetect.nbDetected > 0) { // Fetch detected tag position, size and orientation - var xc = navdata.visionDetect.xc[0], - yc = navdata.visionDetect.yc[0], - wc = navdata.visionDetect.width[0], - hc = navdata.visionDetect.height[0], + 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], - dist = navdata.visionDetect.dist[0] / 100 // Need meters + 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); @@ -474,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 b10631b..0353bfc 100644 --- a/lib/EKF.js +++ b/lib/EKF.js @@ -52,9 +52,9 @@ EKF.prototype.predict = function(navdata) { var doNotUpdateState = false; // LaTeX Math: \int (vx)dt = dx try { - var pitch = navdata.demo.rotation.pitch.toRad(), - roll = navdata.demo.rotation.roll.toRad(), - yaw = normAngle((navdata.magneto.heading.fusionUnwrapped).toRad()), + 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 @@ -200,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; @@ -241,18 +241,14 @@ function normAngle(rad) { return rad; } -/** Converts numeric degrees to radians */ -if (typeof(Number.prototype.toRad) === "undefined") { - Number.prototype.toRad = function() { - 'use strict'; - 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() { - 'use strict'; - 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 f310ef3..4dab152 100644 --- a/lib/Mission.js +++ b/lib/Mission.js @@ -62,10 +62,10 @@ Mission.prototype.log = function(path) { + "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.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()" + + "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) { @@ -94,16 +94,16 @@ 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"; } @@ -542,3 +542,15 @@ Mission.prototype.yaw = function(angle) { 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/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; +}; From de80a6662be74bbaf98b3aebf0162cb4a53dc6f4 Mon Sep 17 00:00:00 2001 From: John Meyer Date: Wed, 7 Sep 2016 13:51:45 -0400 Subject: [PATCH 11/11] Update Examples --- examples/square-2.js | 4 +++- examples/square.js | 4 +--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/square-2.js b/examples/square-2.js index c3b757e..09d4a3a 100644 --- a/examples/square-2.js +++ b/examples/square-2.js @@ -43,7 +43,9 @@ 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(1000) .altitude(1.5) diff --git a/examples/square.js b/examples/square.js index 92465ac..b57677f 100644 --- a/examples/square.js +++ b/examples/square.js @@ -63,8 +63,7 @@ mission.run(function (err, result) { mission.client().land(); } else { console.log(chalk.inverse.green("====Mission complete====")); - console.time(chalk.dim.blue("Call >> waterfall")); - setImmediate(doOver); + //setImmediate(doOver); } }); @@ -83,5 +82,4 @@ function doOver () { // Execute mission console.log(chalk.dim.blue("Running mission")); - console.timeEnd(chalk.dim.blue("Call >> waterfall")); }