Diff: quadcopter.cpp

Revision:

45:9f74298eee78

Parent:

44:14bee84cdf59

Child:

46:4bcf2e679f96

--- a/quadcopter.cpp	Thu May 05 04:05:33 2016 +0000
+++ b/quadcopter.cpp	Thu May 05 14:42:30 2016 +0000
@@ -19,13 +19,14 @@
 
     // control gains set s.t. 100% joystick results in 15% (actually: (maxPwm-zeroVelPwm+0.1)) duty cycle.
     kp_f_     = (MAX_PWM - OFF_PWM) * 4 / 0.5;
-    kp_phi_   = 1 * (MAX_PWM - OFF_PWM) * l_ / 0.5 * 4 / M_PI;
-    kp_theta_ = 1 * (MAX_PWM - OFF_PWM) * l_ / 0.5 * 4 / M_PI;
+    kp_phi_   = 0.3 * (MAX_PWM - OFF_PWM) * l_ / 0.5 * 4 / M_PI; //0.3 quite slow
+    kp_theta_ = 0.3 * (MAX_PWM - OFF_PWM) * l_ / 0.5 * 4 / M_PI;
     kp_psi_   = 0;
 
     // derivative attitude control gains
+    // TODO 0.15 for both
     kd_phi_   = 0.1 * (MAX_PWM - OFF_PWM) * 2 / M_PI; // 0.25 maybe a good
-    kd_theta_ = 0.1 * (MAX_PWM - OFF_PWM) * 2 / M_PI;
+    kd_theta_ = 0.4 * (MAX_PWM - OFF_PWM) * 2 / M_PI;
     kd_psi_   = 0.1;
 
     // incresae ki_phi
@@ -75,7 +76,7 @@
 void Quadcopter::readSensorValues()
 {
     // precomputed gyro offsets
-    double offset_gyro_x = -4.793711656441709e-04; // correction due to offset in measurement when motors spin + when motors spin at hover velocity
+    double offset_gyro_x = -4.793711656441709e-04; 
     double offset_gyro_y = 4.481595092024531e-06;
     double offset_gyro_z = -2.371472392638039e-05;
     double offset_acc_x = -0.270495085889571;
@@ -111,30 +112,30 @@
     gyro_event_.gyro.y   -= offset_gyro_y;
     gyro_event_.gyro.z   -= offset_gyro_z;
 
-    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;
+//    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];
-    }
+//    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];
+//    }
 
     // double radMaxAngle = M_PI / 180 * 20;
     double phi_new = phi_sum / FILTER_SIZE;
@@ -167,8 +168,8 @@
     state_.p = kalmanRoll.getRate() * M_PI / 180;
     state_.q = kalmanPitch.getRate() * M_PI / 180;
 
-    double alphaX = 0.002;
-    double alphaY = 0.002;
+    double alphaX = 0.0002;//0.002;
+    double alphaY = 0.0001; //0.002;
     //compAngleX = (1 - alphaX) * (compAngleX + raw_p * dt) + alphaX * raw_phi; // Calculate the angle using a Complimentary filter
     //compAngleY = (1 - alphaY) * (compAngleY + raw_q * dt) + alphaY * raw_theta;
     compAngleX = (1 - alphaX) * (compAngleX + (raw_p + 8.446703927263016e-04) * dt) + alphaX * raw_phi; // Calculate the angle using a Complimentary filter
@@ -192,7 +193,7 @@
     // }
     // count++;
     // TODO: print HERE
-    pc_->printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n", prev_kalman_time, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, state_.psi, state_.theta, state_.phi,state_.r, state_.p, state_.q, compAngleX, compAngleY, raw_phi, raw_theta,accel_event_.acceleration.x, accel_event_.acceleration.y, accel_event_.acceleration.z);
+    //pc_->printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n", prev_kalman_time, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, state_.psi, state_.theta, state_.phi,state_.r, state_.p, state_.q, compAngleX, compAngleY, raw_phi, raw_theta,accel_event_.acceleration.x, accel_event_.acceleration.y, accel_event_.acceleration.z);
 }
 
 void Quadcopter::controller()