ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 35:35997980a8ba
- Parent:
- 34:eaea0ae92dfa
- Child:
- 36:40b134328376
diff -r eaea0ae92dfa -r 35997980a8ba quadcopter.cpp --- a/quadcopter.cpp Sun Apr 24 17:15:20 2016 +0000 +++ b/quadcopter.cpp Tue Apr 26 18:50:55 2016 +0000 @@ -20,21 +20,21 @@ // 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.3 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; - kp_theta_ = 0.3 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; + 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_psi_ = 0; // kp_phi_ = 0; // kp_theta_ = 0; // derivative attitude control gains - kd_phi_ = 0.025 * (maxPwm - zeroVelPwm) * 2 / M_PI; - kd_theta_ = 0.025 * (maxPwm - zeroVelPwm) * 2 / M_PI; + kd_phi_ = 0.4 * (maxPwm - zeroVelPwm) * 2 / M_PI; //0.25 good + kd_theta_ = 0.0 * (maxPwm - zeroVelPwm) * 2 / M_PI; kd_psi_ = 0.1; // kd_phi_ = 0; //kd_theta_ = 0; // incresae ki_phi - ki_phi_ = 0*0.1 * (maxPwm - zeroVelPwm)/(2*M_PI/4); // full control signal after 2s at pi/4 error + ki_phi_ = 0 * (maxPwm - zeroVelPwm)/(2*M_PI/4); // full control signal after 2s at pi/4 error ki_theta_ = 0 * (maxPwm - zeroVelPwm)/(2*M_PI/4); i_e_phi_ = 0; @@ -62,14 +62,14 @@ initial_offsets_ = (offset*) malloc(sizeof(offset)); initSensors(*this); // IMU - + controlTimer = timer; controlTimer->start(); - + prev_kalman_time = 0; - + readSensorValues(); - + kalmanPitch.setAngle(state_.phi); // set initial pitch kalmanRoll.setAngle(state_.theta); // set initial theta compAngleX = state_.phi; @@ -79,12 +79,8 @@ void Quadcopter::readSensorValues() { accel_.getEvent(&accel_event_); - if (dof_.accelGetOrientation(&accel_event_, &orientation_)) { - } - /* Calculate the heading using the magnetometer */ mag_.getEvent(&mag_event_); - if (dof_.magGetOrientation(SENSOR_AXIS_Z, &mag_event_, &orientation_)) { - } + dof_.fusionGetOrientation(&accel_event_, &mag_event_, &orientation_); gyro_.getEvent(&gyro_event_); @@ -95,6 +91,8 @@ orientation_.pitch -= initial_offsets_->pitch; orientation_.heading -= initial_offsets_->heading; + + 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; @@ -120,28 +118,50 @@ psi_sum += filters_.psi[i]; } - // angular velocities in body coordinate system +// 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_.phi = phi_sum / FILTER_SIZE; - state_.theta = theta_sum / FILTER_SIZE; + //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; 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; - compAngleX = 0.93 * (compAngleX + state_.p * dt) + 0.07 * state_.phi; // Calculate the angle using a Complimentary filter - compAngleY = 0.93 * (compAngleY + state_.q * dt) + 0.07 * state_.theta; + //state_.phi = compAngleX; + //state_.theta = compAngleY; - //pc_->printf("Roll: %f\tPitch: %f\tYaw: %f\tVel x: %f\tVel y: %f\tVel z: %f\r\n", state_.phi, state_.theta, state_.psi, state_.p, state_.q, state_.r); + 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); } void Quadcopter::controller() @@ -189,7 +209,6 @@ prev_time_ = time; //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 %f %f %f %f %f\r\n", time, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, state_.psi, state_.theta, state_.phi,state_.r, state_.p, state_.q, compAngleX, compAngleY); //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); } @@ -249,6 +268,8 @@ float roll; long long id; +// static int thrust_outliers = 0; + receive = rf_receive_rssi(*mrf_, rxBuffer, rssi, rcLength_ + 1); if (receive > 0) { sscanf(rxBuffer, "%lld,%f,%f,%f,%f", &id, &thrust, &yaw, &pitch, &roll); @@ -256,15 +277,24 @@ pc_->printf("Receive failure\r\n"); } +// if (thrust < -0.3) { +// thrust_outliers++; +// if (thrust_outliers < 3) { +// thrust = F_des_; +// } +// } else { +// thrust_outliers = 0; +// } + // 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. //((thrust - 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 // print id with thrust, yaw, pitch, and roll - //pc_->printf("%lld: thrust: %f yaw: %f pitch: %f roll: %f\r\n", id, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi); +// 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); }