ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 44:14bee84cdf59
- Parent:
- 43:0e98d5488bf2
- Child:
- 45:9f74298eee78
diff -r 0e98d5488bf2 -r 14bee84cdf59 quadcopter.cpp --- 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()