Add GPS functionality, Flat trim, Magnetic Calibration, and Custom PID Options

README.md

@@ -41,13 +41,16 @@ by the bottom camera.
 
 This module exposes a high level API to plan and execute missions, by focusing on where
 the drone should go instead of its low-level movements. Here is a simple example, 
-with the drone taking off, travelling alongs a 2 x 2 meters square ane then landing.
+with the drone taking off, travelling alongs a 2 x 2 meters square and then landing.
 
 ```js
 var autonomy = require('ardrone-autonomy');
 var mission  = autonomy.createMission();
 
-mission.takeoff()
+mission.ftrim()      // Makes a flat trim before takeoff
+       .takeoff()
+       .wait(4000)   // Wait four seconds
+       .calibrate()  // Calibrate the drone's compass (will not calibrate if it is not ready)
        .zero()       // Sets the current state as the reference
        .altitude(1)  // Climb to altitude = 1 meter
        .forward(2)   
@@ -71,6 +74,31 @@ mission.run(function (err, result) {
 
 ### Mission API
 
+#### autonomy.createMission(options)
+
+Create a mission object. Any property in `options` can override the following default values: 
+
+```javascript
+{tag: {
+    x: 0,
+    y: 0,
+    yaw: 0
+}
+pid: {
+    x_axis:   {p_constant: 0.5, i_constant: 0, d_constant: 0.35}, 
+    y_axis:   {p_constant: 0.5, i_constant: 0, d_constant: 0.35}, 
+    z_axis:   {p_constant: 0.8, i_constant: 0, d_constant: 0.35}, 
+    yaw_axis: {p_constant: 1.0, i_constant: 0, d_constant: 0.30}
+},
+state: {
+    x: 0,
+    y: 0,
+    z: 0,
+    yaw: 0,
+    absoluteYaw: null
+}}
+```
+
 #### mission.log(path)
 
 Log the mission data, csv formatted, in the given file. Makes it really usefull to
@@ -81,10 +109,18 @@ debug/plot the state and controller behavior.
 Execute the mission. The callback has the form `function(err,result)` and will be triggered in
 case of error or at the end of the mission.
 
+#### mission.ftrim()
+
+Add a flat trim step to the mission. Drone must be landed.
+
 #### mission.takeoff()
 
 Add a takeoff step to the mission.
 
+#### mission.calibrate()
+
+Add a compass calibration step to the mission. The drone is ready for calibration after about four seconds after takeoff
+
 #### mission.forward/backward/left/right/up/down(distance)
 
 Add a movement step to the mission. The drone will move in the given direction by the distance (in meters) before
@@ -174,3 +210,7 @@ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 THE SOFTWARE.
+
+## AR.Drone SDK 2
+
+It might be useful to use the official AR.Drone documentation when using mission.client().config(key, value). As of 2016, it can be downloaded [here](http://developer.parrot.com/docs/SDK2/ARDrone_SDK_2_0_1.zip)

examples/square-2.js

@@ -15,6 +15,7 @@ var navdata_options = (
   | navdata_option_mask(arDroneConstants.options.VISION_DETECT)
   | navdata_option_mask(arDroneConstants.options.MAGNETO)
   | navdata_option_mask(arDroneConstants.options.WIFI)
+  | navdata_option_mask(arDroneConstants.options.ZIMMU_3000)
 );
 
 // Land on ctrl-c
@@ -42,17 +43,19 @@ mission.client().config('detect:detect_type', 12);
 mission.log("mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt");
 
 // Plan mission
-mission.takeoff()
+mission.ftrim()
+       .takeoff()
+       .wait(4000).calibrate()
        .zero()
-       .hover(500)
-       .altitude(2)
-       .forward(2)
+       .hover(1000)
+       .altitude(1.5)
+       .forward(1)
        .cw(90)
-       .forward(2)
+       .forward(1)
        .cw(90)
-       .forward(2)
+       .forward(1)
        .cw(90)
-       .forward(2)
+       .forward(1)
        .go({x:0, y:0})
        .hover(500)
        .land();

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,36 +20,66 @@ 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);
 mission.client().config('video:video_channel', 1);
 mission.client().config('detect:detect_type', 12);
 
 // Log mission for debugging purposes
-mission.log("mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt");
-
-// Plan mission
-mission.takeoff()
-       .zero()
-       .hover(500)
-       .altitude(2)
-       .forward(2)
-       .right(2)
-       .backward(2)
-       .go({x:0, y:0})
-       .hover(500)
-       .land();
-
-// Execute mission
+var logFileName = "/tmp/mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt"
+console.log("Mission Log: " + logFileName);
+mission.log(logFileName);
+console.log(".zero()");
+mission.zero()
+    .altitude(1.3)
+    .hover(250);
 mission.run(function (err, result) {
-    if (err) {
-        console.trace("Oops, something bad happened: %s", err.message);
-        mission.client().stop();
-        mission.client().land();
-    } else {
-        console.log("We are done!");
-        process.exit(0);
-    }
-});
+        if (err) {
+            console.trace("Oops, something bad happened: %s", err.message);
+            mission.client().stop();
+            mission.client().land();
+        } else {
+            console.log(chalk.inverse.green("====Mission complete===="));
+            //setImmediate(doOver);
+        }
+    });
 
+function doOver () {
+    // Plan mission
+    console.log(chalk.green("====Planning mission===="));
+    mission.takeoff()
+
+        .go({x:0, y:0}).hover(500)
+        .go({x:0, y:1}).hover(500)
+        .go({x:-1, y:1}).hover(500)
+        .go({x:0, y:1}).hover(500)
+        .go({x:0, y:0}).hover(500)
+        .go({x:1, y:0}).hover(500)
+        .go({x:0, y:0}).hover(500)
+
+    // Execute mission
+    console.log(chalk.dim.blue("Running mission"));
+}

index.js

@@ -12,6 +12,10 @@ exports.control = function(client, options) {
 
 exports.createMission = function(options, client) {
     var client  = client || ardrone.createClient(options);
+    if (typeof options.droneConfiguration != "undefined" && Array.isArray(options.droneConfiguration))
+        options.droneConfiguration.forEach(function (configuration) {
+            this.config(configuration.key, configuration.value, configuration.callback);
+        }, client);
     var control = new autonomy.Controller(client, options);
     var mission = new autonomy.Mission(client, control, options);
 

lib/Camera.js

@@ -7,7 +7,21 @@ var util         = require('util');
 // TODO: Make image aspect ratio configurable
 
 // AR Drone 2.0 Bottom Camera Intrinsic Matrix
-// https://github.com/tum-vision/ardrone_autonomy/blob/master/calibrations/ardrone2_bottom/cal.ymli
+// https://github.com/tum-vision/ardrone_autonomy/blob/master/calibrations/ardrone2_bottom/cal.yml
+// The code is under the 'BSD' license, which is assumed to be the following license
+//    Copyright (c) 2012, Mani Monajjemi
+//    All rights reserved.
+//
+//    Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+//
+//    1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+//
+//    2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+//
+//    3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
+//
+//    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
 var K_BOTTOM = $M([[686.994766, 0, 329.323208],
                    [0, 688.195055, 159.323007],
                    [0, 0, 1]]);
@@ -25,20 +39,18 @@ function Camera(options) {
  * Given (x,y) pixel coordinates (e.g. obtained from tag detection)
  * Returns a (X,Y) coordinate in drone space.
  */
-Camera.prototype.p2m = function(x, y, altitude) {
-    // From the SDK Documentation:
-    // X and Y coordinates of detected tag or oriented roundel #i inside the picture,
-    // with (0; 0) being the top-left corner, and (1000; 1000) the right-bottom corner regardless
-    // the picture resolution or the source camera.
+Camera.prototype.p2m = function(xC, yC, altitude) {
+    // See "7.1.3 Augmented reality data stream" of SDK 2.0.1 Documentation (Page 42)
+    // Read the third bullet point "xc[i], yc[i]: ... "
     //
     // But our camera intrinsic is built for 640 x 360 pixel grid, so we must do some mapping.
-    var xratio = 640 / 1000;
-    var yratio = 360 / 1000;
+    var xRatio = 640 / 1000;
+    var yRatio = 360 / 1000;
 
     // Perform a simple back projection, we assume the drone is flat (no roll/pitch)
     // for the moment. We ignore the drone translation and yaw since we want X,Y in the
     // drone coordinate system.
-    var p = $V([x * xratio, y * yratio, 1]);
+    var p = $V([xC * xRatio, yC * yRatio, 1]);
     var P = this._invK.multiply(p).multiply(altitude);
 
     // X,Y are expressed in meters, in the drone coordinate system.
@@ -51,5 +63,5 @@ Camera.prototype.p2m = function(x, y, altitude) {
     //        |
     //        Y
     return {x: P.e(1), y: P.e(2)};
-}
+};