Diff: quadcopter.cpp

Revision:

41:d103f9aa44f0

Parent:

40:09a59d5b7944

Child:

42:d09dec5bb184

--- a/quadcopter.cpp	Wed May 04 17:28:48 2016 +0000
+++ b/quadcopter.cpp	Wed May 04 18:46:06 2016 +0000
@@ -75,6 +75,14 @@
 
 void Quadcopter::readSensorValues()
 {
+    // precomputed gyro offsets
+    double offset_gyro_x = -0.048261598984772;
+    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;
+    
     if (prev_kalman_time == 0) {
         prev_kalman_time = controlTimer->read();
         return;
@@ -82,15 +90,27 @@
 
     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;
+    
     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;
+*/
+
+
+    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;
@@ -162,12 +182,12 @@
 
     prev_kalman_time = time;
 
-    /* RAW DATA
+    // RAW DATA
     pc_->printf("%f %f %f %f %f %f %f \r\n",
         prev_kalman_time,
         accel_event_.acceleration.x, accel_event_.acceleration.y, accel_event_.acceleration.z,
         gyro_event_.gyro.x, gyro_event_.gyro.y, gyro_event_.gyro.z);
-    */
+    
     // static int count = 0;
     // if (count % 100 == 0) {
     //  pc_->printf("%d\r\n", count);