Diff: quadcopter.cpp

Revision:

33:244dea7a4e81

Parent:

32:e12b01c94b4a

Child:

34:eaea0ae92dfa

--- a/quadcopter.cpp	Fri Apr 22 00:52:32 2016 +0000
+++ b/quadcopter.cpp	Sat Apr 23 20:52:10 2016 +0000
@@ -166,7 +166,9 @@
     prev_time_ = time;
 
     //pc_->printf("m1: %f\tm2: %f\tm3: %f\tm4: %f\r\n", motorPwm_.m1, motorPwm_.m2, motorPwm_.m3, motorPwm_.m4);
+    pc_->printf("%f %f %f %f %f %f %f %f \r\n", time, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, state_.psi, state_.theta, state_.phi);
 
+    //pc_->printf("%f %f %f %f %f %f %f %f \r\n", time, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, state_.psi, state_.theta, state_.phi);
 }
 
 motors Quadcopter::getPwm()
@@ -231,13 +233,15 @@
         pc_->printf("Receive failure\r\n");
     }
 
+// TODO eliminate the zeros again after testing
     // convert to radians, range is = +-40° or +-0.698132 radians
-    desiredState_.phi = -((roll - 0.5) * 80) * M_PI / 180; // minus, because joystick to right should result in positive moment
-    desiredState_.theta = ((pitch - 0.5) * 80) * M_PI / 180;
-    desiredState_.r = yaw-0.5; // number between 0 and 1 //((yaw - 0.5) * 80) * M_PI / 180;
+    desiredState_.phi = 1 * (-((roll - 0.5) * 80) * M_PI / 180); // minus, because joystick to right should result in positive moment
+    desiredState_.theta = 1 * ((pitch - 0.5) * 80) * M_PI / 180;
+    desiredState_.r = 1 * (yaw-0.5); // number between 0 and 1 //((yaw - 0.5) * 80) * M_PI / 180;
     F_des_ = thrust-0.5; // number between 0 and 1 -> number between -0.5 and 0.5.  //((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);
+    //pc_->printf("%lld: thrust: %f yaw: %f pitch: %f roll: %f\r\n", id, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi);
+
 }