Diff: quadcopter.cpp

Revision:

20:efa15ed008b4

Parent:

18:a00d6b065c6b

Child:

21:336faf452989

--- a/quadcopter.cpp	Fri Apr 08 23:38:22 2016 +0000
+++ b/quadcopter.cpp	Sun Apr 10 19:50:18 2016 +0000
@@ -13,10 +13,14 @@
 
     m_=1;
     g_=9.81;
+      l_=0.25;
+    gamma_=1;
+    
+    
     // proportional attitude control gains
-    kp_phi_ = 1;
-    kp_theta_ = 1;
-    kp_psi_ = 1;
+    kp_phi_ = 0.3;
+    kp_theta_ = 0.3;
+    kp_psi_ = 0.3;
 
     // derivative attitude control gains
     kd_phi_ = 0;
@@ -95,7 +99,7 @@
     // the angles, but for instance the velocity with the Joystick
 
     // PD controller
-    controlInput_.f = m_*g_ + F_des_;
+    controlInput_.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);
@@ -103,15 +107,27 @@
 
     //print("F: %f M_x: %f M_y: %f M_z: %f\n\r",  controlInput_.f, controlInput_.mz, controlInput_.my, controlInput_.mz);
     //        pc_->printf("F: %f\n\r",  F);
+    
+    // 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;
+    
 }
 
 motors Quadcopter::getPwm()
 {
-    //motors motorPwm_;// weired errror, should not be necessary
-    motorPwm_.m1=0;
-    motorPwm_.m2=0.5 + controlInput_.mx/2.5;
-    motorPwm_.m3=0;
-    motorPwm_.m4=0.5 - controlInput_.mx/2.5;
+   // motors motorPwm_
+    
+
+    
+    //old
+  //  motorPwm_.m1=0;
+   // motorPwm_.m2=0.5 + controlInput_.mx/2.5;
+    //motorPwm_.m3=0;
+    //motorPwm_.m4=0.5 - controlInput_.mx/2.5;
     return motorPwm_;
 }