Attempt to fix yaw issues

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));
     }