Diff: quadcopter.cpp

Revision:

32:e12b01c94b4a

Parent:

31:d473eacfc271

Child:

33:244dea7a4e81

--- a/quadcopter.cpp	Thu Apr 21 20:57:19 2016 +0000
+++ b/quadcopter.cpp	Fri Apr 22 00:52:32 2016 +0000
@@ -18,31 +18,32 @@
     zeroVelPwm = 0.11;
     maxPwm = 0.15;
 
-    // control gains set s.t. 100% joystick results in 15% (actually: (maxPwm-zeroVelPwm+0.1)) duty cycle. 
-    kp_f_     = (maxPwm - zeroVelPwm) * 4 / 0.5; 
-    kp_phi_   = 0.2 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
-    kp_theta_ = 0.2 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
+    // control gains set s.t. 100% joystick results in 15% (actually: (maxPwm-zeroVelPwm+0.1)) duty cycle.
+    kp_f_     = (maxPwm - zeroVelPwm) * 4 / 0.5;
+    kp_phi_   = 0.16 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
+    kp_theta_ = 0.12 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
     kp_psi_   = 0;
-   // kp_phi_ = 0;
+    // kp_phi_ = 0;
 //    kp_theta_ = 0;
 
     // derivative attitude control gains
-    kd_phi_   = 0.3 * (maxPwm - zeroVelPwm) * 2 / M_PI;
-    kd_theta_ = 0.3 * (maxPwm - zeroVelPwm) * 2 / M_PI;
+    kd_phi_   = 0.025 * (maxPwm - zeroVelPwm) * 2 / M_PI;
+    kd_theta_ = 0.025 * (maxPwm - zeroVelPwm) * 2 / M_PI;
     kd_psi_   = 0.1;
-   // kd_phi_   = 0;
+    // kd_phi_   = 0;
     //kd_theta_ = 0;
 
-    ki_phi_ = 0.5 * (maxPwm - zeroVelPwm)/(2*M_PI/4); // full control signal after 2s at pi/4 error
-    ki_theta_ = 0.5 *  (maxPwm - zeroVelPwm)/(2*M_PI/4);
-    
-    i_e_phi_ = 0; 
+// incresae ki_phi
+    ki_phi_ = 0.1 * (maxPwm - zeroVelPwm)/(2*M_PI/4); // full control signal after 2s at pi/4 error
+    ki_theta_ = 0 *  (maxPwm - zeroVelPwm)/(2*M_PI/4);
+
+    i_e_phi_ = 0;
     i_e_theta_ = 0;
     prev_time_ = 0;
-    
-    
+
+
     max_integral_phi_ = 0.05/(ki_phi_ * (0.5/l_ + 0.25));  // influence of integral part smaller than 0.05
-    max_integral_theta_ = 0.05/(ki_theta_ * (0.5/l_ + 0.25));  
+    max_integral_theta_ = 0.05/(ki_theta_ * (0.5/l_ + 0.25));
 
 
     // desired values (will come from joystick)
@@ -82,35 +83,63 @@
     orientation_.pitch   -= initial_offsets_->pitch;
     orientation_.heading -= initial_offsets_->heading;
 
+    static int current_filter = 0;
+    filters_.p[current_filter]     = gyro_event_.gyro.x * M_PI / 180;
+    filters_.q[current_filter]     = gyro_event_.gyro.y * M_PI / 180;
+    filters_.r[current_filter]     = gyro_event_.gyro.z * M_PI / 180;
+    filters_.phi[current_filter]   = orientation_.roll * M_PI / 180;
+    filters_.theta[current_filter] = -orientation_.pitch * M_PI / 180;
+    filters_.psi[current_filter]   = orientation_.heading * M_PI / 180;
+
+    current_filter = (current_filter + 1) % FILTER_SIZE;
+
+    double p_sum = 0;
+    double q_sum = 0;
+    double r_sum = 0;
+    double phi_sum = 0;
+    double theta_sum = 0;
+    double psi_sum = 0;
+    for (int i = 0; i < FILTER_SIZE; i++) {
+        p_sum     += filters_.p[i];
+        q_sum     += filters_.q[i];
+        r_sum     += filters_.r[i];
+        phi_sum   += filters_.phi[i];
+        theta_sum += filters_.theta[i];
+        psi_sum   += filters_.psi[i];
+    }
+
     // angular velocities in body coordinate system
-    state_.p = gyro_event_.gyro.x * M_PI / 180;
-    state_.q = gyro_event_.gyro.y * M_PI / 180;
-    state_.r = gyro_event_.gyro.z * M_PI / 180;
+    state_.p = p_sum / FILTER_SIZE;
+    state_.q = q_sum / FILTER_SIZE;
+    state_.r = r_sum / FILTER_SIZE;
 
-    state_.phi = orientation_.roll * M_PI / 180;
-    state_.theta = -orientation_.pitch * M_PI / 180;
-    state_.psi = orientation_.heading * M_PI / 180;
+    state_.phi = phi_sum / FILTER_SIZE;
+    state_.theta = theta_sum / FILTER_SIZE;
+    state_.psi = psi_sum / FILTER_SIZE;
+
     //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);
 }
 
 void Quadcopter::controller(float time)
 {
-    
+
     if (prev_time_ == 0) {
-        prev_time_ = time; 
+        prev_time_ = time;
         return;
-        }
-        
-    
+    }
+
+
     // PD controller
     double e_phi = desiredState_.phi - state_.phi;
     double e_theta = desiredState_.theta - state_.theta;
-    
-    float dt = time - prev_time_;    
-    i_e_phi_ = i_e_phi_ + e_phi * dt; 
-    i_e_theta_ = i_e_theta_ + e_theta * dt; 
+
+    float dt = time - prev_time_;
+    i_e_phi_ = i_e_phi_ + e_phi * dt;
+    i_e_theta_ = i_e_theta_ + e_theta * dt;
     i_e_phi_ = min(max_integral_phi_, i_e_phi_);
     i_e_theta_ = min(max_integral_theta_, i_e_theta_);
+    i_e_phi_ = max(-max_integral_phi_, i_e_phi_);
+    i_e_theta_ = max(-max_integral_theta_, i_e_theta_);
 
     controlInput_.f  = kp_f_ * F_des_;//m_*g_ + F_des_;
     controlInput_.mx = kp_phi_ * e_phi + kd_phi_ * (desiredState_.p - state_.p) + ki_phi_ * i_e_phi_;
@@ -129,12 +158,15 @@
 
     // cut off at max PWM
     //pc_->printf("m1: %f\tm2: %f\tm3: %f\tm4: %f\r\n", motorPwm_.m1, motorPwm_.m2, motorPwm_.m3, motorPwm_.m4);
-    motorPwm_.m1 = min(0.17, motorPwm_.m1);
+    motorPwm_.m1 = min(maxPwm, motorPwm_.m1);
     motorPwm_.m2 = min(maxPwm, motorPwm_.m2);
     motorPwm_.m3 = min(maxPwm, motorPwm_.m3);
     motorPwm_.m4 = min(maxPwm, motorPwm_.m4);
-    
-    prev_time_ = time; 
+
+    prev_time_ = time;
+
+    //pc_->printf("m1: %f\tm2: %f\tm3: %f\tm4: %f\r\n", motorPwm_.m1, motorPwm_.m2, motorPwm_.m3, motorPwm_.m4);
+
 }
 
 motors Quadcopter::getPwm()