Diff: quadcopter.cpp

Revision:

21:336faf452989

Parent:

20:efa15ed008b4

Child:

22:92401a4fec13

--- a/quadcopter.cpp	Sun Apr 10 19:50:18 2016 +0000
+++ b/quadcopter.cpp	Sun Apr 10 21:47:17 2016 +0000
@@ -18,9 +18,10 @@
     
     
     // proportional attitude control gains
-    kp_phi_ = 0.3;
-    kp_theta_ = 0.3;
-    kp_psi_ = 0.3;
+    kp_f_ =2.5;
+    kp_phi_ = 0.2;
+    kp_theta_ = 0.2;
+    kp_psi_ = 0.2;
 
     // derivative attitude control gains
     kd_phi_ = 0;
@@ -99,7 +100,7 @@
     // the angles, but for instance the velocity with the Joystick
 
     // PD controller
-    controlInput_.f = F_des_;//m_*g_ + F_des_;
+    controlInput_.f = kp_f_*F_des_;//m_*g_ + F_des_;
     controlInput_.mx = kp_phi_*(desiredState_.phi-state_.phi)+kd_phi_*(desiredState_.p-state_.p);
     controlInput_.my = kp_theta_*(desiredState_.theta-state_.theta)+kd_theta_*(desiredState_.q-state_.q);
     controlInput_.mz = kp_psi_*(desiredState_.psi-state_.psi)+kd_psi_*(desiredState_.r-state_.r);
@@ -110,10 +111,10 @@
     
     // set pwm values
     // make code faster by precomputing all the components that are used multiple times and hardcode 0.25/gamma...
-    motorPwm_.m1=0.5+ 0.25*controlInput_.f-0.5/l_*controlInput_.my+0.25/gamma_*controlInput_.mz;
-    motorPwm_.m2=0.5 +0.25*controlInput_.f+0.5/l_*controlInput_.mx-0.25/gamma_*controlInput_.mz; 
-    motorPwm_.m3=0.5+ 0.25*controlInput_.f+0.5/l_*controlInput_.my+0.25/gamma_*controlInput_.mz;
-    motorPwm_.m4=0.5 + 0.25*controlInput_.f-0.5/l_*controlInput_.mx-0.25/gamma_*controlInput_.mz;
+    motorPwm_.m1=0.5+ 0.25*controlInput_.f-0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz;
+    motorPwm_.m2=0.5 +0.25*controlInput_.f-0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz; 
+    motorPwm_.m3=0.5 + 0.25*controlInput_.f+0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz;
+    motorPwm_.m4=0.5 + 0.25*controlInput_.f+0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz;
     
 }