ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 43:0e98d5488bf2
- Parent:
- 42:d09dec5bb184
- Child:
- 44:14bee84cdf59
diff -r d09dec5bb184 -r 0e98d5488bf2 quadcopter.cpp --- a/quadcopter.cpp Wed May 04 21:35:05 2016 +0000 +++ b/quadcopter.cpp Thu May 05 03:22:59 2016 +0000 @@ -19,13 +19,13 @@ // control gains set s.t. 100% joystick results in 15% (actually: (maxPwm-zeroVelPwm+0.1)) duty cycle. kp_f_ = (MAX_PWM - OFF_PWM) * 4 / 0.5; - kp_phi_ = 2.5 * (MAX_PWM - OFF_PWM) * l_ / 0.5 * 4 / M_PI; - kp_theta_ = 2.5 * (MAX_PWM - OFF_PWM) * l_ / 0.5 * 4 / M_PI; + kp_phi_ = 1 * (MAX_PWM - OFF_PWM) * l_ / 0.5 * 4 / M_PI; + kp_theta_ = 1 * (MAX_PWM - OFF_PWM) * l_ / 0.5 * 4 / M_PI; kp_psi_ = 0; // derivative attitude control gains - kd_phi_ = 0.0 * (MAX_PWM - OFF_PWM) * 2 / M_PI; // 0.25 maybe a good - kd_theta_ = 0.0 * (MAX_PWM - OFF_PWM) * 2 / M_PI; + kd_phi_ = 0.1 * (MAX_PWM - OFF_PWM) * 2 / M_PI; // 0.25 maybe a good + kd_theta_ = 0.1 * (MAX_PWM - OFF_PWM) * 2 / M_PI; kd_psi_ = 0.1; // incresae ki_phi @@ -67,19 +67,20 @@ kalmanPitch.setAngle(state_.phi); // set initial pitch kalmanRoll.setAngle(state_.theta); // set initial theta - compAngleX = state_.phi; - compAngleY = state_.theta; + compAngleX = state_.phi + 0.03; + compAngleY = state_.theta + 0.055603443609023/2; // constant offset at props spinning in hover velocity /2 because of overcorrection + } void Quadcopter::readSensorValues() { // precomputed gyro offsets - double offset_gyro_x = -0.048261598984772; + 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; - double offset_acc_y = 0.198868464759687; - double offset_acc_z = -0.485550588350806; + double offset_acc_x = -0.233396269401444 + 0.0344; + double offset_acc_y = 0.198868464759687 + 0.2434; + double offset_acc_z = -0.485550588350806 + 0.0305; if (prev_kalman_time == 0) { prev_kalman_time = controlTimer->read(); @@ -89,10 +90,10 @@ 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_); @@ -106,9 +107,9 @@ */ - 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; @@ -166,7 +167,7 @@ state_.p = kalmanRoll.getRate() * M_PI / 180; state_.q = kalmanPitch.getRate() * M_PI / 180; - double alphaX = 0.001; + double alphaX = 0.004; double alphaY = 0.001; //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; @@ -174,7 +175,6 @@ compAngleY = (1 - alphaY) * compAngleY + raw_q * dt + alphaY * raw_theta; - state_.phi = compAngleX; state_.theta = compAngleY; @@ -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\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); + 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()