ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 48:0346cdff12a7
- Parent:
- 47:ae478c380136
- Child:
- 49:59c3427e6838
diff -r ae478c380136 -r 0346cdff12a7 quadcopter.cpp --- 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()