ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 45:9f74298eee78
- Parent:
- 44:14bee84cdf59
- Child:
- 46:4bcf2e679f96
diff -r 14bee84cdf59 -r 9f74298eee78 quadcopter.cpp --- a/quadcopter.cpp Thu May 05 04:05:33 2016 +0000 +++ b/quadcopter.cpp Thu May 05 14:42:30 2016 +0000 @@ -19,13 +19,14 @@ // 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_ = 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_phi_ = 0.3 * (MAX_PWM - OFF_PWM) * l_ / 0.5 * 4 / M_PI; //0.3 quite slow + kp_theta_ = 0.3 * (MAX_PWM - OFF_PWM) * l_ / 0.5 * 4 / M_PI; kp_psi_ = 0; // derivative attitude control gains + // TODO 0.15 for both 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_theta_ = 0.4 * (MAX_PWM - OFF_PWM) * 2 / M_PI; kd_psi_ = 0.1; // incresae ki_phi @@ -75,7 +76,7 @@ void Quadcopter::readSensorValues() { // precomputed gyro offsets - 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_x = -4.793711656441709e-04; double offset_gyro_y = 4.481595092024531e-06; double offset_gyro_z = -2.371472392638039e-05; double offset_acc_x = -0.270495085889571; @@ -111,30 +112,30 @@ 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; - filters_.q[current_filter] = gyro_event_.gyro.y * M_PI / 180; - filters_.r[current_filter] = gyro_event_.gyro.z * M_PI / 180; - filters_.phi[current_filter] = orientation_.roll * M_PI / 180; - filters_.theta[current_filter] = -orientation_.pitch * M_PI / 180; - filters_.psi[current_filter] = orientation_.heading * M_PI / 180; - - current_filter = (current_filter + 1) % FILTER_SIZE; +// static int current_filter = 0; +// filters_.p[current_filter] = gyro_event_.gyro.x * M_PI / 180; +// filters_.q[current_filter] = gyro_event_.gyro.y * M_PI / 180; +// filters_.r[current_filter] = gyro_event_.gyro.z * M_PI / 180; +// filters_.phi[current_filter] = orientation_.roll * M_PI / 180; +// filters_.theta[current_filter] = -orientation_.pitch * M_PI / 180; +// filters_.psi[current_filter] = orientation_.heading * M_PI / 180; +// +// current_filter = (current_filter + 1) % FILTER_SIZE; - double p_sum = 0; - double q_sum = 0; - double r_sum = 0; - double phi_sum = 0; - double theta_sum = 0; - double psi_sum = 0; - for (int i = 0; i < FILTER_SIZE; i++) { - p_sum += filters_.p[i]; - q_sum += filters_.q[i]; - r_sum += filters_.r[i]; - phi_sum += filters_.phi[i]; - theta_sum += filters_.theta[i]; - psi_sum += filters_.psi[i]; - } +// double p_sum = 0; +// double q_sum = 0; +// double r_sum = 0; +// double phi_sum = 0; +// double theta_sum = 0; +// double psi_sum = 0; +// for (int i = 0; i < FILTER_SIZE; i++) { +// p_sum += filters_.p[i]; +// q_sum += filters_.q[i]; +// r_sum += filters_.r[i]; +// phi_sum += filters_.phi[i]; +// theta_sum += filters_.theta[i]; +// psi_sum += filters_.psi[i]; +// } // double radMaxAngle = M_PI / 180 * 20; double phi_new = phi_sum / FILTER_SIZE; @@ -167,8 +168,8 @@ state_.p = kalmanRoll.getRate() * M_PI / 180; state_.q = kalmanPitch.getRate() * M_PI / 180; - double alphaX = 0.002; - double alphaY = 0.002; + double alphaX = 0.0002;//0.002; + double alphaY = 0.0001; //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 + 8.446703927263016e-04) * dt) + alphaX * raw_phi; // Calculate the angle using a Complimentary filter @@ -192,7 +193,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()