Diff: quadcopter.cpp

Revision:

43:0e98d5488bf2

Parent:

42:d09dec5bb184

Child:

44:14bee84cdf59

--- a/quadcopter.cpp	Wed May 04 21:35:05 2016 +0000
+++ b/quadcopter.cpp	Thu May 05 03:22:59 2016 +0000
@@ -19,13 +19,13 @@
 
     // 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_   = 2.5 * (MAX_PWM - OFF_PWM) * l_ / 0.5 * 4 / M_PI;
-    kp_theta_ = 2.5 * (MAX_PWM - OFF_PWM) * l_ / 0.5 * 4 / M_PI;
+    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_psi_   = 0;
 
     // derivative attitude control gains
-    kd_phi_   = 0.0 * (MAX_PWM - OFF_PWM) * 2 / M_PI; // 0.25 maybe a good
-    kd_theta_ = 0.0 * (MAX_PWM - OFF_PWM) * 2 / M_PI;
+    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_psi_   = 0.1;
 
     // incresae ki_phi
@@ -67,19 +67,20 @@
 
     kalmanPitch.setAngle(state_.phi); // set initial pitch
     kalmanRoll.setAngle(state_.theta); // set initial theta
-    compAngleX = state_.phi;
-    compAngleY = state_.theta;
+    compAngleX = state_.phi + 0.03;
+    compAngleY = state_.theta + 0.055603443609023/2; // constant offset at props spinning in hover velocity /2 because of overcorrection
+
 }
 
 void Quadcopter::readSensorValues()
 {
     // precomputed gyro offsets
-    double offset_gyro_x = -0.048261598984772;
+    double offset_gyro_x = -0.048261598984772 + 5.5675e-04 + 0.0011; // correction due to offset in measurement when motors spin + when motors spin at hover velocity
     double offset_gyro_y = 0.499745812182741;
     double offset_gyro_z = 0.005005893401015;
-    double offset_acc_x = -0.233396269401444;
-    double offset_acc_y = 0.198868464759687;
-    double offset_acc_z = -0.485550588350806;
+    double offset_acc_x = -0.233396269401444 + 0.0344;
+    double offset_acc_y = 0.198868464759687 + 0.2434;
+    double offset_acc_z = -0.485550588350806 + 0.0305;
     
     if (prev_kalman_time == 0) {
         prev_kalman_time = controlTimer->read();
@@ -89,10 +90,10 @@
     accel_.getEvent(&accel_event_);
     // mag_.getEvent(&mag_event_);
     // detract offsets
-    accel_event_.acceleration.x -= offset_acc_x;
-    accel_event_.acceleration.y -= offset_acc_y;
-    accel_event_.acceleration.z -= offset_acc_z;
-    
+//    accel_event_.acceleration.x -= offset_acc_x;
+//    accel_event_.acceleration.y -= offset_acc_y;
+//    accel_event_.acceleration.z -= offset_acc_z;
+//    
     dof_.accelGetOrientation(&accel_event_, &orientation_);
     gyro_.getEvent(&gyro_event_);
 
@@ -106,9 +107,9 @@
 */
 
 
-    gyro_event_.gyro.x   -= offset_gyro_x;
-    gyro_event_.gyro.y   -= offset_gyro_y;
-    gyro_event_.gyro.z   -= offset_gyro_z;
+//    gyro_event_.gyro.x   -= offset_gyro_x;
+//    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;
@@ -166,7 +167,7 @@
     state_.p = kalmanRoll.getRate() * M_PI / 180;
     state_.q = kalmanPitch.getRate() * M_PI / 180;
 
-    double alphaX = 0.001;
+    double alphaX = 0.004;
     double alphaY = 0.001;
     //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;
@@ -174,7 +175,6 @@
     compAngleY = (1 - alphaY) * compAngleY + raw_q * dt + alphaY * raw_theta;
 
 
-
     state_.phi = compAngleX;
     state_.theta = compAngleY;
 
@@ -192,7 +192,7 @@
     // }
     // count++;
     // TODO: print HERE
-     pc_->printf("%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);
+     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()