Diff: quadcopter.cpp

Revision:

48:0346cdff12a7

Parent:

47:ae478c380136

Child:

49:59c3427e6838

--- a/quadcopter.cpp	Thu May 05 20:47:55 2016 +0000
+++ b/quadcopter.cpp	Fri May 06 01:44:29 2016 +0000
@@ -7,6 +7,8 @@
 #define M_PI           3.14159265358979323846
 #endif
 
+DigitalOut initializationLED(LED3);
+
 // constructor
 Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr, Timer *timer)
 {
@@ -35,7 +37,9 @@
 
     i_e_phi_ = 0;
     i_e_theta_ = 0;
-
+    
+    comp_x_offset = 0;
+    comp_y_offset = 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));
@@ -65,8 +69,9 @@
     // initialze previous values to zero
     prev_controller_time_ = 0;
     prev_sensor_time = 0;
+    start_sampling_time = 0;
 
-    readSensorValues();
+    //readSensorValues();
 
     //kalmanPitch.setAngle(state_.phi); // set initial pitch
     //§kalmanRoll.setAngle(state_.theta); // set initial theta
@@ -87,6 +92,7 @@
 
     if (prev_sensor_time == 0) {
         prev_sensor_time = controlTimer->read();
+        start_sampling_time = prev_sensor_time;
         return;
     }
     // get acceleration values
@@ -129,15 +135,29 @@
     //compAngleY = (1 - alphaY) * (com pAngleY + raw_q * dt) + alphaY * raw_theta;
     compAngleX = (1 - alphaX) * (compAngleX + (raw_p + 8.446703927263016e-04) * dt) + alphaX * raw_phi; // hardcoded value compensates drift with alpha == 0. 
     compAngleY = (1 - alphaY) * (compAngleY + (raw_q - 0.008936763695439) * dt) + alphaY * raw_theta;
-
+    
+    static int comp_avg_count = 0;
+    static double comp_sum_x = 0.0;
+    static double comp_sum_y = 0.0;
+    static bool sample_finished = false;
+    if (time - start_sampling_time < 5.0) {
+        comp_sum_x += compAngleX;
+        comp_sum_y += compAngleY;
+        comp_avg_count++;
+    } else if (!sample_finished) {
+        comp_x_offset = comp_sum_x / comp_avg_count;
+        comp_y_offset = comp_sum_y / comp_avg_count;
+        sample_finished = true;
+        initializationLED = 1;
+    }
+    
     // assign values to state
-    state_.phi = compAngleX;
-    state_.theta = compAngleY;
+    state_.phi = compAngleX - comp_x_offset;
+    state_.theta = compAngleY - comp_y_offset;
     state_.p = raw_p;
     state_.q = raw_q;
     state_.r = raw_r;
 
-
     prev_sensor_time = time;
 
     // Plotting
@@ -151,7 +171,7 @@
     //  pc_->printf("%d\r\n", count);
     // }
     // count++;
-    pc_->printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n", prev_sensor_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 %f %f\r\n", prev_sensor_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, comp_x_offset, comp_y_offset);
 }
 
 void Quadcopter::controller()