Diff: quadcopter.cpp

Revision:

49:59c3427e6838

Parent:

48:0346cdff12a7

Child:

50:c9831bb5a660

diff -r 0346cdff12a7 -r 59c3427e6838 quadcopter.cpp
--- a/quadcopter.cpp	Fri May 06 01:44:29 2016 +0000
+++ b/quadcopter.cpp	Fri May 06 13:54:18 2016 +0000
@@ -200,7 +200,7 @@
     controlInput_.f  = kp_f_ * F_des_;
     controlInput_.mx = kp_phi_ * e_phi + kd_phi_ * (desiredState_.p - state_.p) + ki_phi_ * i_e_phi_;
     controlInput_.my = kp_theta_ * e_theta + kd_theta_ * (desiredState_.q - state_.q) + ki_theta_ * i_e_theta_;
-    controlInput_.mz = kd_psi_ * (desiredState_.r - state_.q); // feedforward desired yaw rate.  // kp_psi_*(desiredState_.psi-state_.psi)+kd_psi_*(desiredState_.r-state_.r);
+    controlInput_.mz = kd_psi_ * (desiredState_.r); //- state_.q); // feedforward desired yaw rate.  // kp_psi_*(desiredState_.psi-state_.psi)+kd_psi_*(desiredState_.r-state_.r);
 
     // set pwm values
     double forcePerMotor = 0.25 * controlInput_.f;
@@ -314,8 +314,8 @@
 //    }
 
     // convert to radians, range is = +-40° or +-0.698132 radians
-    desiredState_.phi = 1 * (-(roll * 30) * M_PI / 180); // minus, because joystick to right should result in positive moment
-    desiredState_.theta = 1 * (pitch * 30) * M_PI / 180;
+    desiredState_.phi = 1 * (-(roll * 20) * M_PI / 180); // minus, because joystick to right should result in positive moment
+    desiredState_.theta = 1 * (pitch * 20) * M_PI / 180;
     desiredState_.r = 1 * yaw; // number between 0 and 1 //((yaw - 0.5) * 80) * M_PI / 180;
     F_des_ = thrust; // number between 0 and 1 -> number between -0.5 and 0.5