From 8831db6a991d82f1ffae8d7ccb97de2db46488e6 Mon Sep 17 00:00:00 2001
From: Laurent Eschenauer <laurent@eschenauer.be>
Date: Fri, 30 Aug 2013 22:03:17 +0200
Subject: [PATCH] Attempt to fix yaw issues

---
 lib/Controller.js | 15 +++++++++------
 1 file changed, 9 insertions(+), 6 deletions(-)

diff --git a/lib/Controller.js b/lib/Controller.js
index e16e1bd..d76d0bd 100644
--- a/lib/Controller.js
+++ b/lib/Controller.js
@@ -165,13 +165,13 @@ Controller.prototype.left = function(distance, callback) {
  */
 Controller.prototype.cw = function(angle, callback) {
     var state = this.state();
-    var yaw   = state.yaw.toDeg();
+    var yaw   = state.yaw.toDeg() + angle;
 
-    return this.go({x: state.x, y: state.y, z: state.z, yaw: yaw + angle}, callback);
+    return this.go({x: state.x, y: state.y, z: state.z, yaw: yaw.toRad()}, callback);
 }
 
 /*
- * Turn counter clockwise of the given angle
+ * Turn counter clockwise of the given angle (in degrees)
  */
 Controller.prototype.ccw = function(angle, callback) {
     return this.cw(-angle, callback);
@@ -201,16 +201,19 @@ Controller.prototype.altitude = function(altitude, callback) {
 }
 
 /*
- * Go to the target yaw
+ * Go to the target yaw (argument in degree)
  */
 Controller.prototype.yaw = function(yaw, callback) {
     var state = this.state();
-    return this.go({x: state.x, y: state.y, z: state.z, yaw: yaw}, callback);
+    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
  * is reached, the callback is called with the current state.
+ *
+ * x,y,z in meters
+ * yaw in radians
  */
 Controller.prototype.go = function(goal, callback) {
     // Since we are going to modify goal settings, we
@@ -223,7 +226,7 @@ Controller.prototype.go = function(goal, callback) {
     // Normalize the yaw, to make sure we don't spin 360deg for
     // nothing :-)
     if (goal.yaw != undefined) {
-        var yaw = goal.yaw.toRad();
+        var yaw = goal.yaw;
         goal.yaw = Math.atan2(Math.sin(yaw),Math.cos(yaw));
     }