Diff: quadcopter.cpp

Revision:

25:d44610851105

Parent:

24:e220fbb70ded

Child:

26:7f50323c0c0d

--- a/quadcopter.cpp	Wed Apr 13 00:57:15 2016 +0000
+++ b/quadcopter.cpp	Wed Apr 13 19:16:03 2016 +0000
@@ -90,7 +90,7 @@
     // TODO print values to check what they are
     // TODO convert to Radians (*pi/180)
 
-    pc_->printf("Roll: %f\tPitch: %f\tYaw: %f\tVel x: %f\tVel y: %f\tVel z: %f\r\n", state_.phi, state_.theta, state_.psi, state_.p, state_.q, state_.r);
+    // pc_->printf("Roll: %f\tPitch: %f\tYaw: %f\tVel x: %f\tVel y: %f\tVel z: %f \r\n", state_.phi, state_.theta, state_.psi, state_.p, state_.q, state_.r);
     state_.phi = orientation_.roll * M_PI / 180;
     state_.theta = orientation_.pitch * M_PI / 180;
     state_.psi = orientation_.heading * M_PI / 180;
@@ -184,11 +184,13 @@
         pc_->printf("Receive failure\r\n");
     }
 
-    // TODO convert to radians (starting point: full joystick deflection = +-10° (converted to radian).
-
-    desiredState_.phi=roll-0.5;
-    desiredState_.theta=pitch-0.5;
-    desiredState_.psi=yaw-0.5;
-    F_des_=thrust-0.5;
+    // convert to radians, range is = +-40° or +-0.698132 radians
+    desiredState_.phi = ((roll - 0.5) * 80) * M_PI / 180;
+    desiredState_.theta = ((pitch - 0.5) * 80) * M_PI / 180;
+    desiredState_.psi = ((yaw - 0.5) * 80) * M_PI / 180;
+    F_des_ = ((thrust - 0.5) * 80) * M_PI / 180;
+    
+    // print id with thrust, yaw, pitch, and roll
+    pc_->printf("%lld: thrust: %f, yaw: %f, pitch: %f, roll: %f\r\n", id, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi);
 }