ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 36:40b134328376
- Parent:
- 35:35997980a8ba
- Child:
- 37:a983eb9fd9c5
diff -r 35997980a8ba -r 40b134328376 quadcopter.cpp --- a/quadcopter.cpp Tue Apr 26 18:50:55 2016 +0000 +++ b/quadcopter.cpp Fri Apr 29 18:24:15 2016 +0000 @@ -8,7 +8,7 @@ #endif // constructor -Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr, Timer *timer) +Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr, Timer *timer, Mutex *desired) { pc_= pcPntr; // enable printing g_= 9.81; @@ -17,17 +17,18 @@ zeroVelPwm = 0.11; maxPwm = 0.15; + desired_mutex = desired; // control gains set s.t. 100% joystick results in 15% (actually: (maxPwm-zeroVelPwm+0.1)) duty cycle. kp_f_ = (maxPwm - zeroVelPwm) * 4 / 0.5; - kp_phi_ = 0.25 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; - kp_theta_ = 0.2 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; + kp_phi_ = 10 * 0.25 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; + kp_theta_ = 10 * 0.2 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; kp_psi_ = 0; // kp_phi_ = 0; // kp_theta_ = 0; // derivative attitude control gains - kd_phi_ = 0.4 * (maxPwm - zeroVelPwm) * 2 / M_PI; //0.25 good + kd_phi_ = 0.0 * (maxPwm - zeroVelPwm) * 2 / M_PI; //0.25 good kd_theta_ = 0.0 * (maxPwm - zeroVelPwm) * 2 / M_PI; kd_psi_ = 0.1; // kd_phi_ = 0; @@ -78,6 +79,11 @@ void Quadcopter::readSensorValues() { + if (prev_kalman_time == 0) { + prev_kalman_time = controlTimer->read(); + return; + } + accel_.getEvent(&accel_event_); mag_.getEvent(&mag_event_); dof_.fusionGetOrientation(&accel_event_, &mag_event_, &orientation_); @@ -98,7 +104,7 @@ 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_.theta[current_filter] = orientation_.pitch * M_PI / 180; filters_.psi[current_filter] = orientation_.heading * M_PI / 180; current_filter = (current_filter + 1) % FILTER_SIZE; @@ -138,30 +144,32 @@ //state_.theta = theta_sum / FILTER_SIZE; state_.psi = psi_sum / FILTER_SIZE; - if (prev_kalman_time == 0) { - prev_kalman_time = controlTimer->read(); - return; - } - double raw_phi = state_.phi; double raw_theta = state_.theta; double raw_p = state_.p; double raw_q = state_.q; - float dt = controlTimer->read() - prev_kalman_time; + 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; - - compAngleX = 0.93 * (compAngleX + raw_p * dt) + 0.07 * raw_phi; // Calculate the angle using a Complimentary filter - compAngleY = 0.93 * (compAngleY + raw_q * dt) + 0.07 * raw_theta; + + double alphaX = 0.7; + double alphaY = 0.7; + 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; //state_.phi = compAngleX; //state_.theta = compAngleY; - prev_kalman_time = controlTimer->read(); - - 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); + prev_kalman_time = time; + static int count = 0; + if (count % 100 == 0) { + pc_->printf("%d\r\n", count); + } + count++; + //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); } void Quadcopter::controller() @@ -250,7 +258,11 @@ double Quadcopter::getForce() { - return F_des_; + double force; + desired_mutex->lock(); + force = F_des_; + desired_mutex->unlock(); + return force; } void Quadcopter::readRc() @@ -268,7 +280,7 @@ float roll; long long id; -// static int thrust_outliers = 0; + static int thrust_outliers = 0; receive = rf_receive_rssi(*mrf_, rxBuffer, rssi, rcLength_ + 1); if (receive > 0) { @@ -277,6 +289,9 @@ pc_->printf("Receive failure\r\n"); } + desired_mutex->lock(); +// thrust = thrust - 0.5; +// // if (thrust < -0.3) { // thrust_outliers++; // if (thrust_outliers < 3) { @@ -288,11 +303,14 @@ // TODO eliminate the zeros again after testing // convert to radians, range is = +-40° or +-0.698132 radians + + desiredState_.phi = 1 * (-((roll - 0.5) * 80) * M_PI / 180); // minus, because joystick to right should result in positive moment desiredState_.theta = 1 * ((pitch - 0.5) * 80) * M_PI / 180; desiredState_.r = 1 * (yaw - 0.5); // number between 0 and 1 //((yaw - 0.5) * 80) * M_PI / 180; F_des_ = thrust - 0.5; // number between 0 and 1 -> number between -0.5 and 0.5 + desired_mutex->unlock(); // print id with thrust, yaw, pitch, and roll // pc_->printf("%lld: thrust: %f yaw: %f pitch: %f roll: %f outliers: %d\r\n", id, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, thrust_outliers);