// acro_run - runs the acro controller // should be called at 100hz or more void Sub::acro_run() { float target_roll, target_pitch, target_yaw; int16_t pilot_throttle_scaled; // if motors not running reset angle targets if(!motors.armed()) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // slow start if landed if (ap.land_complete) { motors.slow_start(true); } return; } // convert the input to the desired body frame rate get_pilot_desired_angle_rates(channel_roll->control_in, channel_pitch->control_in, channel_yaw->control_in, target_roll, target_pitch, target_yaw); // get pilot's desired throttle pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in); // run attitude controller attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); // output pilot's throttle without angle boost attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); //control_in is range 0-1000 //radio_in is raw pwm value motors.set_forward(channel_forward->radio_in); motors.set_strafe(channel_strafe->radio_in); }
// exit_mode - high level call to organise cleanup as a flight mode is exited void Sub::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode) { #if AUTOTUNE_ENABLED == ENABLED if (old_control_mode == AUTOTUNE) { autotune_stop(); } #endif // stop mission when we leave auto mode if (old_control_mode == AUTO) { if (mission.state() == AP_Mission::MISSION_RUNNING) { mission.stop(); } #if MOUNT == ENABLED camera_mount.set_mode_to_default(); #endif // MOUNT == ENABLED } if (old_control_mode == THROW) { throw_exit(); } // smooth throttle transition when switching from manual to automatic flight modes if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) { // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->control_in)); } // cancel any takeoffs in progress takeoff_stop(); }
void Copter::ModeAcro::run() { float target_roll, target_pitch, target_yaw; float pilot_throttle_scaled; // if not armed set throttle to zero and exit immediately if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) { motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt); return; } // clear landing flag set_land_complete(false); motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // convert the input to the desired body frame rate get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw); // get pilot's desired throttle pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in(), g2.acro_thr_mid); // run attitude controller attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); // output pilot's throttle without angle boost attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); }
// drift_run - runs the drift controller // should be called at 100hz or more void Copter::drift_run() { static float breaker = 0.0f; static float roll_input = 0.0f; float target_roll, target_pitch; float target_yaw_rate; int16_t pilot_throttle_scaled; // if not armed or motor interlock not enabled or landed and throttle at zero, set throttle to zero and exit immediately if(!motors.armed() || !motors.get_interlock() || (ap.land_complete && ap.throttle_zero)) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; } // convert pilot input to lean angles get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max); // get pilot's desired throttle pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in); // Grab inertial velocity const Vector3f& vel = inertial_nav.get_velocity(); // rotate roll, pitch input from north facing to vehicle's perspective float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel // gain sceduling for Yaw float pitch_vel2 = min(fabsf(pitch_vel), 2000); target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p; roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); roll_input = roll_input * .96f + (float)channel_yaw->control_in * .04f; //convert user input into desired roll velocity float roll_vel_error = roll_vel - (roll_input / DRIFT_SPEEDGAIN); // Roll velocity is feed into roll acceleration to minimize slip target_roll = roll_vel_error * -DRIFT_SPEEDGAIN; target_roll = constrain_int16(target_roll, -4500, 4500); // If we let go of sticks, bring us to a stop if(is_zero(target_pitch)) { // .14/ (.03 * 100) = 4.6 seconds till full breaking breaker += .03f; breaker = min(breaker, DRIFT_SPEEDGAIN); target_pitch = pitch_vel * breaker; } else { breaker = 0.0f; } // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // output pilot's throttle with angle boost attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt); }
// stabilize_init - initialise stabilize controller bool Copter::ModeStabilize::init(bool ignore_checks) { // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() && (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) { return false; } return true; }
bool Copter::ModeAcro::init(bool ignore_checks) { // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high if (motors->armed() && ap.land_complete && !_copter.flightmode->has_manual_throttle() && (get_pilot_desired_throttle(channel_throttle->get_control_in(), _copter.g2.acro_thr_mid) > _copter.get_non_takeoff_throttle())) { return false; } // set target altitude to zero for reporting pos_control->set_alt_target(0); return true; }
// exit_mode - high level call to organise cleanup as a flight mode is exited void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode) { #if AUTOTUNE_ENABLED == ENABLED if (old_control_mode == AUTOTUNE) { autotune_stop(); } #endif // stop mission when we leave auto mode if (old_control_mode == AUTO) { if (mission.state() == AP_Mission::MISSION_RUNNING) { mission.stop(); } #if MOUNT == ENABLED camera_mount.set_mode_to_default(); #endif // MOUNT == ENABLED } if (old_control_mode == THROW) { throw_exit(); } // smooth throttle transition when switching from manual to automatic flight modes if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) { // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->get_control_in())); } // cancel any takeoffs in progress takeoff_stop(); #if FRAME_CONFIG == HELI_FRAME // firmly reset the flybar passthrough to false when exiting acro mode. if (old_control_mode == ACRO) { attitude_control.use_flybar_passthrough(false, false); motors.set_acro_tail(false); } // if we are changing from a mode that did not use manual throttle, // stab col ramp value should be pre-loaded to the correct value to avoid a twitch // heli_stab_col_ramp should really only be active switching between Stabilize and Acro modes if (!mode_has_manual_throttle(old_control_mode)){ if (new_control_mode == STABILIZE){ input_manager.set_stab_col_ramp(1.0); } else if (new_control_mode == ACRO){ input_manager.set_stab_col_ramp(0.0); } } #endif //HELI_FRAME }
// exit_mode - high level call to organise cleanup as a flight mode is exited void Copter::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode) { #if AUTOTUNE_ENABLED == ENABLED if (old_control_mode == AUTOTUNE) { autotune_stop(); } #endif // stop mission when we leave auto mode if (old_control_mode == AUTO) { if (mission.state() == AP_Mission::MISSION_RUNNING) { mission.stop(); } #if MOUNT == ENABLED camera_mount.set_mode_to_default(); #endif // MOUNT == ENABLED } // smooth throttle transition when switching from manual to automatic flight modes if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) { // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->control_in)); } // cancel any takeoffs in progress takeoff_stop(); #if FRAME_CONFIG == HELI_FRAME // firmly reset the flybar passthrough to false when exiting acro mode. if (old_control_mode == ACRO) { attitude_control.use_flybar_passthrough(false, false); motors.set_acro_tail(false); } // reset RC Passthrough to motors motors.reset_radio_passthrough(); #endif //HELI_FRAME }
// stabilize_run - runs the main stabilize controller // should be called at 100hz or more void Copter::ModeStabilize::run() { float target_roll, target_pitch; float target_yaw_rate; float pilot_throttle_scaled; // if not armed set throttle to zero and exit immediately if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) { zero_throttle_and_relax_ac(); return; } // clear landing flag set_land_complete(false); motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // convert pilot input to lean angles get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // get pilot's desired throttle pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in()); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // body-frame rate controller is run directly from 100hz loop // output pilot's throttle attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt); }
// stabilize_run - runs the main stabilize controller // should be called at 100hz or more void Copter::stabilize_run() { float target_roll, target_pitch; float target_yaw_rate; int16_t pilot_throttle_scaled; // if not armed or throttle at zero, set throttle to zero and exit immediately if(!motors.armed() || ap.throttle_zero) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // slow start if landed if (ap.land_complete) { motors.slow_start(true); } return; } // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // convert pilot input to lean angles // To-Do: convert get_pilot_desired_lean_angles to return angles as floats get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); // get pilot's desired throttle pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in); // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // body-frame rate controller is run directly from 100hz loop // output pilot's throttle attitude_control.set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt); }
// flip_run - runs the flip controller // should be called at 100hz or more void Sub::flip_run() { float throttle_out; float recovery_angle; // if pilot inputs roll > 40deg or timeout occurs abandon flip if (!motors.armed() || (abs(channel_roll->get_control_in()) >= 4000) || (abs(channel_pitch->get_control_in()) >= 4000) || ((millis() - flip_start_time) > FLIP_TIMEOUT_MS)) { flip_state = Flip_Abandon; } // get pilot's desired throttle throttle_out = get_pilot_desired_throttle(channel_throttle->get_control_in()); // get corrected angle based on direction and axis of rotation // we flip the sign of flip_angle to minimize the code repetition int32_t flip_angle; if (flip_roll_dir != 0) { flip_angle = ahrs.roll_sensor * flip_roll_dir; } else { flip_angle = ahrs.pitch_sensor * flip_pitch_dir; } // state machine switch (flip_state) { case Flip_Start: // under 45 degrees request 400deg/sec roll or pitch attitude_control.input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0); // increase throttle throttle_out += FLIP_THR_INC; // beyond 45deg lean angle move to next stage if (flip_angle >= 4500) { if (flip_roll_dir != 0) { // we are rolling flip_state = Flip_Roll; } else { // we are pitching flip_state = Flip_Pitch_A; } } break; case Flip_Roll: // between 45deg ~ -90deg request 400deg/sec roll attitude_control.input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, 0.0, 0.0); // decrease throttle throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f); // beyond -90deg move on to recovery if ((flip_angle < 4500) && (flip_angle > -9000)) { flip_state = Flip_Recover; } break; case Flip_Pitch_A: // between 45deg ~ -90deg request 400deg/sec pitch attitude_control.input_rate_bf_roll_pitch_yaw(0.0f, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0); // decrease throttle throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f); // check roll for inversion if ((labs(ahrs.roll_sensor) > 9000) && (flip_angle > 4500)) { flip_state = Flip_Pitch_B; } break; case Flip_Pitch_B: // between 45deg ~ -90deg request 400deg/sec pitch attitude_control.input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0); // decrease throttle throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f); // check roll for inversion if ((labs(ahrs.roll_sensor) < 9000) && (flip_angle > -4500)) { flip_state = Flip_Recover; } break; case Flip_Recover: // use originally captured earth-frame angle targets to recover attitude_control.input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false, get_smoothing_gain()); // increase throttle to gain any lost altitude throttle_out += FLIP_THR_INC; if (flip_roll_dir != 0) { // we are rolling recovery_angle = fabsf(flip_orig_attitude.x - (float)ahrs.roll_sensor); } else { // we are pitching recovery_angle = fabsf(flip_orig_attitude.y - (float)ahrs.pitch_sensor); } // check for successful recovery if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) { // restore original flight mode if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) { // this should never happen but just in case set_mode(STABILIZE, MODE_REASON_UNKNOWN); } // log successful completion Log_Write_Event(DATA_FLIP_END); } break; case Flip_Abandon: // restore original flight mode if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) { // this should never happen but just in case set_mode(STABILIZE, MODE_REASON_UNKNOWN); } // log abandoning flip Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED); break; } // output pilot's throttle without angle boost attitude_control.set_throttle_out(throttle_out, false, g.throttle_filt); }
// manual_init - initialise manual controller bool Sub::manual_init(bool ignore_checks) { // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) { return false; } // set target altitude to zero for reporting pos_control.set_alt_target(0); return true; }
// drift_run - runs the drift controller // should be called at 100hz or more void Copter::ModeDrift::run() { static float braker = 0.0f; static float roll_input = 0.0f; float target_roll, target_pitch; float target_yaw_rate; float pilot_throttle_scaled; // if landed and throttle at zero, set throttle to zero and exit immediately if (!motors->armed() || !motors->get_interlock() || (ap.land_complete && ap.throttle_zero)) { zero_throttle_and_relax_ac(); return; } // clear landing flag above zero throttle if (!ap.throttle_zero) { set_land_complete(false); } // convert pilot input to lean angles get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max); // get pilot's desired throttle pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in()); // Grab inertial velocity const Vector3f& vel = inertial_nav.get_velocity(); // rotate roll, pitch input from north facing to vehicle's perspective float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel // gain sceduling for Yaw float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p; roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); roll_input = roll_input * .96f + (float)channel_yaw->get_control_in() * .04f; //convert user input into desired roll velocity float roll_vel_error = roll_vel - (roll_input / DRIFT_SPEEDGAIN); // Roll velocity is feed into roll acceleration to minimize slip target_roll = roll_vel_error * -DRIFT_SPEEDGAIN; target_roll = constrain_float(target_roll, -4500.0f, 4500.0f); // If we let go of sticks, bring us to a stop if(is_zero(target_pitch)){ // .14/ (.03 * 100) = 4.6 seconds till full braking braker += .03f; braker = MIN(braker, DRIFT_SPEEDGAIN); target_pitch = pitch_vel * braker; }else{ braker = 0.0f; } // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // output pilot's throttle with angle boost attitude_control->set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt); }
// run - runs the flip controller // should be called at 100hz or more void Copter::ModeFlip::run() { // if pilot inputs roll > 40deg or timeout occurs abandon flip if (!motors->armed() || (abs(channel_roll->get_control_in()) >= 4000) || (abs(channel_pitch->get_control_in()) >= 4000) || ((millis() - start_time_ms) > FLIP_TIMEOUT_MS)) { _state = FlipState::Abandon; } // get pilot's desired throttle float throttle_out = get_pilot_desired_throttle(); // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // get corrected angle based on direction and axis of rotation // we flip the sign of flip_angle to minimize the code repetition int32_t flip_angle; if (roll_dir != 0) { flip_angle = ahrs.roll_sensor * roll_dir; } else { flip_angle = ahrs.pitch_sensor * pitch_dir; } // state machine switch (_state) { case FlipState::Start: // under 45 degrees request 400deg/sec roll or pitch attitude_control->input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * roll_dir, FLIP_ROTATION_RATE * pitch_dir, 0.0); // increase throttle throttle_out += FLIP_THR_INC; // beyond 45deg lean angle move to next stage if (flip_angle >= 4500) { if (roll_dir != 0) { // we are rolling _state = FlipState::Roll; } else { // we are pitching _state = FlipState::Pitch_A; } } break; case FlipState::Roll: // between 45deg ~ -90deg request 400deg/sec roll attitude_control->input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * roll_dir, 0.0, 0.0); // decrease throttle throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f); // beyond -90deg move on to recovery if ((flip_angle < 4500) && (flip_angle > -9000)) { _state = FlipState::Recover; } break; case FlipState::Pitch_A: // between 45deg ~ -90deg request 400deg/sec pitch attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, FLIP_ROTATION_RATE * pitch_dir, 0.0); // decrease throttle throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f); // check roll for inversion if ((labs(ahrs.roll_sensor) > 9000) && (flip_angle > 4500)) { _state = FlipState::Pitch_B; } break; case FlipState::Pitch_B: // between 45deg ~ -90deg request 400deg/sec pitch attitude_control->input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * pitch_dir, 0.0); // decrease throttle throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f); // check roll for inversion if ((labs(ahrs.roll_sensor) < 9000) && (flip_angle > -4500)) { _state = FlipState::Recover; } break; case FlipState::Recover: { // use originally captured earth-frame angle targets to recover attitude_control->input_euler_angle_roll_pitch_yaw(orig_attitude.x, orig_attitude.y, orig_attitude.z, false); // increase throttle to gain any lost altitude throttle_out += FLIP_THR_INC; float recovery_angle; if (roll_dir != 0) { // we are rolling recovery_angle = fabsf(orig_attitude.x - (float)ahrs.roll_sensor); } else { // we are pitching recovery_angle = fabsf(orig_attitude.y - (float)ahrs.pitch_sensor); } // check for successful recovery if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) { // restore original flight mode if (!copter.set_mode(orig_control_mode, MODE_REASON_FLIP_COMPLETE)) { // this should never happen but just in case copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN); } // log successful completion Log_Write_Event(DATA_FLIP_END); } break; } case FlipState::Abandon: // restore original flight mode if (!copter.set_mode(orig_control_mode, MODE_REASON_FLIP_COMPLETE)) { // this should never happen but just in case copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN); } // log abandoning flip AP::logger().Write_Error(LogErrorSubsystem::FLIP, LogErrorCode::FLIP_ABANDONED); break; } // output pilot's throttle without angle boost attitude_control->set_throttle_out(throttle_out, false, g.throttle_filt); }