David Lakata
ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 46:4bcf2e679f96
- Parent:
- 45:9f74298eee78
- Child:
- 47:ae478c380136
--- a/quadcopter.cpp Thu May 05 14:42:30 2016 +0000
+++ b/quadcopter.cpp Thu May 05 15:58:49 2016 +0000
@@ -8,14 +8,14 @@
#endif
// constructor
-Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr, Timer *timer, Mutex *desired)
+Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr, Timer *timer)
{
pc_= pcPntr; // enable printing
g_= 9.81;
l_= 0.25;
gamma_= 1;
- desired_mutex = desired;
+ //desired_mutex = desired;
// 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;
@@ -35,7 +35,6 @@
i_e_phi_ = 0;
i_e_theta_ = 0;
- prev_time_ = 0;
max_integral_phi_ = 0.05/(ki_phi_ * (0.5/l_ + 0.25)); // influence of integral part smaller than 0.05
@@ -47,7 +46,7 @@
dof_ = Adafruit_9DOF();
accel_ = Adafruit_LSM303_Accel_Unified(30301);
- mag_ = Adafruit_LSM303_Mag_Unified(30302);
+ //mag_ = Adafruit_LSM303_Mag_Unified(30302);
gyro_ = Adafruit_L3GD20_Unified(20);
// prepare for communication with remote control
@@ -56,134 +55,94 @@
rcLength_ = 250;
mrf_->SetChannel(3); //Set the Channel. 0 is default, 15 is max
- initial_offsets_ = (offset*) malloc(sizeof(offset));
+ //initial_offsets_ = (offset*) malloc(sizeof(offset));
initSensors(*this); // IMU
+ // define timer
controlTimer = timer;
controlTimer->start();
- prev_kalman_time = 0;
+ // initialze previous values to zero
+ prev_controller_time_ = 0;
+ prev_sensor_time = 0;
readSensorValues();
- kalmanPitch.setAngle(state_.phi); // set initial pitch
- kalmanRoll.setAngle(state_.theta); // set initial theta
- compAngleX = state_.phi;
- compAngleY = state_.theta; // constant offset at props spinning in hover velocity /2 because of overcorrection
+ //kalmanPitch.setAngle(state_.phi); // set initial pitch
+ //§kalmanRoll.setAngle(state_.theta); // set initial theta
+ //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 = -4.793711656441709e-04;
+ 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;
double offset_acc_y = -0.053425306748466;
double offset_acc_z = -0.520254558282190;
- if (prev_kalman_time == 0) {
- prev_kalman_time = controlTimer->read();
+ if (prev_sensor_time == 0) {
+ prev_sensor_time = controlTimer->read();
return;
}
-
+ // get acceleration values
accel_.getEvent(&accel_event_);
- // mag_.getEvent(&mag_event_);
- // detract offsets
+ // detract offsets form raw acceleromater data
accel_event_.acceleration.x -= offset_acc_x;
accel_event_.acceleration.y -= offset_acc_y;
accel_event_.acceleration.z -= offset_acc_z;
-//
+ // compute orientation with correctet acc data
dof_.accelGetOrientation(&accel_event_, &orientation_);
- gyro_.getEvent(&gyro_event_);
+ // compute roll and pitch values
+ double raw_phi = orientation_.roll * M_PI / 180;;
+ double raw_theta = orientation_.pitch * M_PI / 180;;
- /* 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;
- */
-
-
+ // get gyro values
+ gyro_.getEvent(&gyro_event_);
+ // detract offsets form raw gyro data
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;
-// 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;
+ // compute rates
+ double raw_p = gyro_event_.gyro.x * M_PI / 180;
+ double raw_q = gyro_event_.gyro.y * M_PI / 180;
+ double raw_r = gyro_event_.gyro.z * M_PI / 180;
-// 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];
-// }
+ float time = controlTimer->read();
+ float dt = time - prev_sensor_time; // TODO problems with this?
- // double radMaxAngle = M_PI / 180 * 20;
- double phi_new = phi_sum / FILTER_SIZE;
- // if (phi_new < radMaxAngle && phi_new > -radMaxAngle) {
- // state_.phi = phi_new;
- // }
- double theta_new = theta_sum / FILTER_SIZE;
- // if (theta_new < radMaxAngle && theta_new > -radMaxAngle) {
- // state_.theta = theta_new;
- // }
-
- state_.phi = phi_new;
- state_.theta = theta_new;
- state_.p = p_sum / FILTER_SIZE;
-
- state_.q = q_sum / FILTER_SIZE;
- state_.r = r_sum / FILTER_SIZE;
-
- //state_.theta = theta_sum / FILTER_SIZE;
- state_.psi = psi_sum / FILTER_SIZE;
-
- double raw_phi = state_.phi;
- double raw_theta = state_.theta;
- double raw_p = state_.p;
- double raw_q = state_.q;
- float time = controlTimer->read();
- float dt = time - prev_kalman_time;
- state_.phi = kalmanRoll.getAngle(state_.phi * 180 / M_PI, state_.p * 180 / M_PI, dt) * M_PI / 180;
- state_.theta = kalmanPitch.getAngle(state_.theta * 180 / M_PI, state_.q * 180 / M_PI, dt) * M_PI / 180;
- state_.p = kalmanRoll.getRate() * M_PI / 180;
- state_.q = kalmanPitch.getRate() * M_PI / 180;
-
+ // Kalman Filter
+ // state_.phi = kalmanRoll.getAngle(state_.phi * 180 / M_PI, state_.p * 180 / M_PI, dt) * M_PI / 180;
+ // state_.theta = kalmanPitch.getAngle(state_.theta * 180 / M_PI, state_.q * 180 / M_PI, dt) * M_PI / 180;
+ // state_.p = kalmanRoll.getRate() * M_PI / 180;
+ // state_.q = kalmanPitch.getRate() * M_PI / 180;
+
+ // complementary filter parameters
double alphaX = 0.0002;//0.002;
double alphaY = 0.0001; //0.002;
+ // different version of complementary filter
//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
+ 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;
+ // assign values to state
+ state_.phi = compAngleX;
+ state_.theta = compAngleY;
+ state_.p = raw_p;
+ state_.q = raw_q;
+ state_.r = raw_r;
+
- state_.phi = compAngleX;
- state_.theta = compAngleY;
+ prev_sensor_time = time;
- prev_kalman_time = time;
-
- // RAW DATA
+ // Plotting
/*pc_->printf("%f %f %f %f %f %f %f \r\n",
- prev_kalman_time,
+ prev_sensor_time,
accel_event_.acceleration.x, accel_event_.acceleration.y, accel_event_.acceleration.z,
gyro_event_.gyro.x, gyro_event_.gyro.y, gyro_event_.gyro.z);
*/
@@ -192,15 +151,14 @@
// pc_->printf("%d\r\n", count);
// }
// 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_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);
}
void Quadcopter::controller()
{
float time = controlTimer->read();
- if (prev_time_ == 0) {
- prev_time_ = time;
+ if (prev_controller_time_ == 0) {
+ prev_controller_time_ = time;
return;
}
@@ -208,15 +166,18 @@
double e_phi = desiredState_.phi - state_.phi;
double e_theta = desiredState_.theta - state_.theta;
- float dt = time - prev_time_;
+ float dt = time - prev_controller_time_;
+ // integrate errors
i_e_phi_ = i_e_phi_ + e_phi * dt;
i_e_theta_ = i_e_theta_ + e_theta * dt;
+ // limit integrals
i_e_phi_ = min(max_integral_phi_, i_e_phi_);
i_e_theta_ = min(max_integral_theta_, i_e_theta_);
i_e_phi_ = max(-max_integral_phi_, i_e_phi_);
i_e_theta_ = max(-max_integral_theta_, i_e_theta_);
-
- controlInput_.f = kp_f_ * F_des_;//m_*g_ + F_des_;
+
+ // compute control forces
+ controlInput_.f = kp_f_ * F_des_;
controlInput_.mx = kp_phi_ * e_phi + kd_phi_ * (desiredState_.p - state_.p) + ki_phi_ * i_e_phi_;
controlInput_.my = kp_theta_ * e_theta + kd_theta_ * (desiredState_.q - state_.q) + ki_theta_ * i_e_theta_;
controlInput_.mz = kd_psi_ * desiredState_.r; // feedforward desired yaw rate. // kp_psi_*(desiredState_.psi-state_.psi)+kd_psi_*(desiredState_.r-state_.r);
@@ -232,7 +193,6 @@
motorPwm_.m4 = MIN_PWM_4 + forcePerMotor - rollMomentPerMotor + yawMomentPerMotor;
// cut off at max PWM
- //pc_->printf("m1: %f\tm2: %f\tm3: %f\tm4: %f\r\n", motorPwm_.m1, motorPwm_.m2, motorPwm_.m3, motorPwm_.m4);
motorPwm_.m1 = min(MAX_PWM, motorPwm_.m1);
motorPwm_.m2 = min(MAX_PWM, motorPwm_.m2);
motorPwm_.m3 = min(MAX_PWM, motorPwm_.m3);
@@ -243,11 +203,12 @@
motorPwm_.m3 = max(MIN_PWM_3, motorPwm_.m3);
motorPwm_.m4 = max(MIN_PWM_4, motorPwm_.m4);
- prev_time_ = time;
+ prev_controller_time_ = time;
+ // Printing
+ //pc_->printf("%f %f %f %f %f %f %f %f \r\n", time, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, state_.psi, state_.theta, state_.phi);
//pc_->printf("m1: %f\tm2: %f\tm3: %f\tm4: %f\r\n", motorPwm_.m1, motorPwm_.m2, motorPwm_.m3, motorPwm_.m4);
- //pc_->printf("%f %f %f %f %f %f %f %f \r\n", time, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, state_.psi, state_.theta, state_.phi);
}
motors Quadcopter::getPwm()
@@ -265,10 +226,10 @@
return accel_;
}
-Adafruit_LSM303_Mag_Unified Quadcopter::getMag()
-{
- return mag_;
-}
+//Adafruit_LSM303_Mag_Unified Quadcopter::getMag()
+//{
+// return mag_;
+//}
Adafruit_L3GD20_Unified Quadcopter::getGyro()
{