Diff: quadcopter.cpp

Revision:

44:14bee84cdf59

Parent:

43:0e98d5488bf2

Child:

45:9f74298eee78

--- a/quadcopter.cpp	Thu May 05 03:22:59 2016 +0000
+++ b/quadcopter.cpp	Thu May 05 04:05:33 2016 +0000
@@ -67,21 +67,21 @@
 
     kalmanPitch.setAngle(state_.phi); // set initial pitch
     kalmanRoll.setAngle(state_.theta); // set initial theta
-    compAngleX = state_.phi + 0.03;
-    compAngleY = state_.theta + 0.055603443609023/2; // constant offset at props spinning in hover velocity /2 because of overcorrection
+    compAngleX = state_.phi;
+    compAngleY = state_.theta; // constant offset at props spinning in hover velocity /2 because of overcorrection
 
 }
 
 void Quadcopter::readSensorValues()
 {
     // precomputed gyro offsets
-    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 + 0.0344;
-    double offset_acc_y = 0.198868464759687 + 0.2434;
-    double offset_acc_z = -0.485550588350806 + 0.0305;
-    
+    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_y = 4.481595092024531e-06;
+    double offset_gyro_z = -2.371472392638039e-05;
+    double offset_acc_x = -0.270495085889571;
+    double offset_acc_y = -0.053425306748466;
+    double offset_acc_z = -0.520254558282190;
+
     if (prev_kalman_time == 0) {
         prev_kalman_time = controlTimer->read();
         return;
@@ -90,26 +90,26 @@
     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_);
 
-/* old offset detraction
-    gyro_event_.gyro.x   -= initial_offsets_->gyro_x;
-    gyro_event_.gyro.y   -= initial_offsets_->gyro_y;
-    gyro_event_.gyro.z   -= initial_offsets_->gyro_z;
-    orientation_.roll    -= initial_offsets_->roll;
-    orientation_.pitch   -= initial_offsets_->pitch;
-    orientation_.heading -= initial_offsets_->heading;
-*/
+    /* old offset detraction
+        gyro_event_.gyro.x   -= initial_offsets_->gyro_x;
+        gyro_event_.gyro.y   -= initial_offsets_->gyro_y;
+        gyro_event_.gyro.z   -= initial_offsets_->gyro_z;
+        orientation_.roll    -= initial_offsets_->roll;
+        orientation_.pitch   -= initial_offsets_->pitch;
+        orientation_.heading -= initial_offsets_->heading;
+    */
 
 
-//    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;
@@ -167,12 +167,12 @@
     state_.p = kalmanRoll.getRate() * M_PI / 180;
     state_.q = kalmanPitch.getRate() * M_PI / 180;
 
-    double alphaX = 0.004;
-    double alphaY = 0.001;
+    double alphaX = 0.002;
+    double alphaY = 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 * 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
+    compAngleY = (1 - alphaY) * (compAngleY + (raw_q - 0.008936763695439) * dt) + alphaY * raw_theta;
 
 
     state_.phi = compAngleX;
@@ -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 %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()