diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 3caa8c16488ea..fec37a76629cc 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -901,13 +901,59 @@ class ModePosHold : public Mode { private: - void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw); - int16_t poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control); - void poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity); - void poshold_update_wind_comp_estimate(); - void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle); - void poshold_roll_controller_to_pilot_override(); - void poshold_pitch_controller_to_pilot_override(); + void update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw); + int16_t mix_controls(float mix_ratio, int16_t first_control, int16_t second_control); + void update_brake_angle_from_velocity(int16_t &brake_angle, float velocity); + void update_wind_comp_estimate(); + void get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle); + void roll_controller_to_pilot_override(); + void pitch_controller_to_pilot_override(); + + enum class RPMode { + PILOT_OVERRIDE=0, // pilot is controlling this axis (i.e. roll or pitch) + BRAKE, // this axis is braking towards zero + BRAKE_READY_TO_LOITER, // this axis has completed braking and is ready to enter loiter mode (both modes must be this value before moving to next stage) + BRAKE_TO_LOITER, // both vehicle's axis (roll and pitch) are transitioning from braking to loiter mode (braking and loiter controls are mixed) + LOITER, // both vehicle axis are holding position + CONTROLLER_TO_PILOT_OVERRIDE // pilot has input controls on this axis and this axis is transitioning to pilot override (other axis will transition to brake if no pilot input) + }; + + RPMode roll_mode; + RPMode pitch_mode; + + uint8_t braking_time_updated_roll : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking + uint8_t braking_time_updated_pitch : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking + + // pilot input related variables + float pilot_roll; // pilot requested roll angle (filtered to slow returns to zero) + float pilot_pitch; // pilot requested roll angle (filtered to slow returns to zero) + + // braking related variables + float brake_gain; // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from brake_rate) + int16_t brake_roll; // target roll angle during braking periods + int16_t brake_pitch; // target pitch angle during braking periods + int16_t brake_timeout_roll; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking + int16_t brake_timeout_pitch; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking + int16_t brake_angle_max_roll; // maximum lean angle achieved during braking. Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time + int16_t brake_angle_max_pitch; // maximum lean angle achieved during braking Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time + int16_t brake_to_loiter_timer; // cycles to mix brake and loiter controls in POSHOLD_BRAKE_TO_LOITER + + // loiter related variables + int16_t controller_to_pilot_timer_roll; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT + int16_t controller_to_pilot_timer_pitch; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT + int16_t controller_final_roll; // final roll angle from controller as we exit brake or loiter mode (used for mixing with pilot input) + int16_t controller_final_pitch; // final pitch angle from controller as we exit brake or loiter mode (used for mixing with pilot input) + + // wind compensation related variables + Vector2f wind_comp_ef; // wind compensation in earth frame, filtered lean angles from position controller + int16_t wind_comp_roll; // roll angle to compensate for wind + int16_t wind_comp_pitch; // pitch angle to compensate for wind + uint16_t wind_comp_start_timer; // counter to delay start of wind compensation for a short time after loiter is engaged + int8_t wind_comp_timer; // counter to reduce wind comp roll/pitch lean angle calcs to 10hz + + // final output + int16_t roll; // final roll angle sent to attitude controller + int16_t pitch; // final pitch angle sent to attitude controller }; diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index aaed8615e2c18..cd893b2e6418e 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -23,62 +23,6 @@ #define POSHOLD_STICK_RELEASE_SMOOTH_ANGLE 1800 // max angle required (in centi-degrees) after which the smooth stick release effect is applied #define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX 10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s -// PosHold states -enum PosHoldModeState { - PosHold_MotorStopped, - PosHold_Takeoff, - PosHold_Flying, - PosHold_Landed -}; - -// mission state enumeration -enum poshold_rp_mode { - POSHOLD_PILOT_OVERRIDE=0, // pilot is controlling this axis (i.e. roll or pitch) - POSHOLD_BRAKE, // this axis is braking towards zero - POSHOLD_BRAKE_READY_TO_LOITER, // this axis has completed braking and is ready to enter loiter mode (both modes must be this value before moving to next stage) - POSHOLD_BRAKE_TO_LOITER, // both vehicle's axis (roll and pitch) are transitioning from braking to loiter mode (braking and loiter controls are mixed) - POSHOLD_LOITER, // both vehicle axis are holding position - POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE // pilot has input controls on this axis and this axis is transitioning to pilot override (other axis will transition to brake if no pilot input) -}; - -static struct { - poshold_rp_mode roll_mode : 3; // roll mode: pilot override, brake or loiter - poshold_rp_mode pitch_mode : 3; // pitch mode: pilot override, brake or loiter - uint8_t braking_time_updated_roll : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking - uint8_t braking_time_updated_pitch : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking - - // pilot input related variables - float pilot_roll; // pilot requested roll angle (filtered to slow returns to zero) - float pilot_pitch; // pilot requested roll angle (filtered to slow returns to zero) - - // braking related variables - float brake_gain; // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from brake_rate) - int16_t brake_roll; // target roll angle during braking periods - int16_t brake_pitch; // target pitch angle during braking periods - int16_t brake_timeout_roll; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking - int16_t brake_timeout_pitch; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking - int16_t brake_angle_max_roll; // maximum lean angle achieved during braking. Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time - int16_t brake_angle_max_pitch; // maximum lean angle achieved during braking Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time - int16_t brake_to_loiter_timer; // cycles to mix brake and loiter controls in POSHOLD_BRAKE_TO_LOITER - - // loiter related variables - int16_t controller_to_pilot_timer_roll; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT - int16_t controller_to_pilot_timer_pitch; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT - int16_t controller_final_roll; // final roll angle from controller as we exit brake or loiter mode (used for mixing with pilot input) - int16_t controller_final_pitch; // final pitch angle from controller as we exit brake or loiter mode (used for mixing with pilot input) - - // wind compensation related variables - Vector2f wind_comp_ef; // wind compensation in earth frame, filtered lean angles from position controller - int16_t wind_comp_roll; // roll angle to compensate for wind - int16_t wind_comp_pitch; // pitch angle to compensate for wind - uint16_t wind_comp_start_timer; // counter to delay start of wind compensation for a short time after loiter is engaged - int8_t wind_comp_timer; // counter to reduce wind comp roll/pitch lean angle calcs to 10hz - - // final output - int16_t roll; // final roll angle sent to attitude controller - int16_t pitch; // final pitch angle sent to attitude controller -} poshold; - // poshold_init - initialise PosHold controller bool Copter::ModePosHold::init(bool ignore_checks) { @@ -93,20 +37,20 @@ bool Copter::ModePosHold::init(bool ignore_checks) } // initialise lean angles to current attitude - poshold.pilot_roll = 0; - poshold.pilot_pitch = 0; + pilot_roll = 0; + pilot_pitch = 0; // compute brake_gain - poshold.brake_gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) / 100.0f; + brake_gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) / 100.0f; if (ap.land_complete) { // if landed begin in loiter mode - poshold.roll_mode = POSHOLD_LOITER; - poshold.pitch_mode = POSHOLD_LOITER; + roll_mode = RPMode::LOITER; + pitch_mode = RPMode::LOITER; } else { // if not landed start in pilot override to avoid hard twitch - poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; - poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; + roll_mode = RPMode::PILOT_OVERRIDE; + pitch_mode = RPMode::PILOT_OVERRIDE; } // initialise loiter @@ -114,10 +58,10 @@ bool Copter::ModePosHold::init(bool ignore_checks) loiter_nav->init_target(); // initialise wind_comp each time PosHold is switched on - poshold.wind_comp_ef.zero(); - poshold.wind_comp_roll = 0; - poshold.wind_comp_pitch = 0; - poshold.wind_comp_timer = 0; + wind_comp_ef.zero(); + wind_comp_roll = 0; + wind_comp_pitch = 0; + wind_comp_timer = 0; return true; } @@ -171,8 +115,8 @@ void Copter::ModePosHold::run() loiter_nav->update(); // set poshold state to pilot override - poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; - poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; + roll_mode = RPMode::PILOT_OVERRIDE; + pitch_mode = RPMode::PILOT_OVERRIDE; break; case AltHold_Takeoff: @@ -201,8 +145,8 @@ void Copter::ModePosHold::run() pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); // set poshold state to pilot override - poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; - poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; + roll_mode = RPMode::PILOT_OVERRIDE; + pitch_mode = RPMode::PILOT_OVERRIDE; break; case AltHold_Landed_Ground_Idle: @@ -218,8 +162,8 @@ void Copter::ModePosHold::run() pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero // set poshold state to pilot override - poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; - poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; + roll_mode = RPMode::PILOT_OVERRIDE; + pitch_mode = RPMode::PILOT_OVERRIDE; break; case AltHold_Flying: @@ -251,8 +195,8 @@ void Copter::ModePosHold::run() float vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw(); // If not in LOITER, retrieve latest wind compensation lean angles related to current yaw - if (poshold.roll_mode != POSHOLD_LOITER || poshold.pitch_mode != POSHOLD_LOITER) { - poshold_get_wind_comp_lean_angles(poshold.wind_comp_roll, poshold.wind_comp_pitch); + if (roll_mode != RPMode::LOITER || pitch_mode != RPMode::LOITER) { + get_wind_comp_lean_angles(wind_comp_roll, wind_comp_pitch); } // Roll state machine @@ -260,92 +204,92 @@ void Copter::ModePosHold::run() // 1. dealing with pilot input // 2. calculating the final roll output to the attitude controller // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state - switch (poshold.roll_mode) { + switch (roll_mode) { - case POSHOLD_PILOT_OVERRIDE: + case RPMode::PILOT_OVERRIDE: // update pilot desired roll angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll); + update_pilot_lean_angle(pilot_roll, target_roll); // switch to BRAKE mode for next iteration if no pilot input - if (is_zero(target_roll) && (fabsf(poshold.pilot_roll) < 2 * g.poshold_brake_rate)) { + if (is_zero(target_roll) && (fabsf(pilot_roll) < 2 * g.poshold_brake_rate)) { // initialise BRAKE mode - poshold.roll_mode = POSHOLD_BRAKE; // Set brake roll mode - poshold.brake_roll = 0; // initialise braking angle to zero - poshold.brake_angle_max_roll = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking - poshold.brake_timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. - poshold.braking_time_updated_roll = false; // flag the braking time can be re-estimated + roll_mode = RPMode::BRAKE; // Set brake roll mode + brake_roll = 0; // initialise braking angle to zero + brake_angle_max_roll = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking + brake_timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. + braking_time_updated_roll = false; // flag the braking time can be re-estimated } // final lean angle should be pilot input plus wind compensation - poshold.roll = poshold.pilot_roll + poshold.wind_comp_roll; + roll = pilot_roll + wind_comp_roll; break; - case POSHOLD_BRAKE: - case POSHOLD_BRAKE_READY_TO_LOITER: + case RPMode::BRAKE: + case RPMode::BRAKE_READY_TO_LOITER: // calculate brake_roll angle to counter-act velocity - poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right); + update_brake_angle_from_velocity(brake_roll, vel_right); // update braking time estimate - if (!poshold.braking_time_updated_roll) { + if (!braking_time_updated_roll) { // check if brake angle is increasing - if (abs(poshold.brake_roll) >= poshold.brake_angle_max_roll) { - poshold.brake_angle_max_roll = abs(poshold.brake_roll); + if (abs(brake_roll) >= brake_angle_max_roll) { + brake_angle_max_roll = abs(brake_roll); } else { // braking angle has started decreasing so re-estimate braking time - poshold.brake_timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_roll))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. - poshold.braking_time_updated_roll = true; + brake_timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(brake_roll))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. + braking_time_updated_roll = true; } } // if velocity is very low reduce braking time to 0.5seconds - if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_roll > 50*LOOP_RATE_FACTOR)) { - poshold.brake_timeout_roll = 50*LOOP_RATE_FACTOR; + if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (brake_timeout_roll > 50*LOOP_RATE_FACTOR)) { + brake_timeout_roll = 50*LOOP_RATE_FACTOR; } // reduce braking timer - if (poshold.brake_timeout_roll > 0) { - poshold.brake_timeout_roll--; + if (brake_timeout_roll > 0) { + brake_timeout_roll--; } else { // indicate that we are ready to move to Loiter. - // Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER + // Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to RPMode::BRAKE_READY_TO_LOITER // logic for engaging loiter is handled below the roll and pitch mode switch statements - poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; + roll_mode = RPMode::BRAKE_READY_TO_LOITER; } // final lean angle is braking angle + wind compensation angle - poshold.roll = poshold.brake_roll + poshold.wind_comp_roll; + roll = brake_roll + wind_comp_roll; // check for pilot input if (!is_zero(target_roll)) { // init transition to pilot override - poshold_roll_controller_to_pilot_override(); + roll_controller_to_pilot_override(); } break; - case POSHOLD_BRAKE_TO_LOITER: - case POSHOLD_LOITER: + case RPMode::BRAKE_TO_LOITER: + case RPMode::LOITER: // these modes are combined roll-pitch modes and are handled below break; - case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: + case RPMode::CONTROLLER_TO_PILOT_OVERRIDE: // update pilot desired roll angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll); + update_pilot_lean_angle(pilot_roll, target_roll); // count-down loiter to pilot timer - if (poshold.controller_to_pilot_timer_roll > 0) { - poshold.controller_to_pilot_timer_roll--; + if (controller_to_pilot_timer_roll > 0) { + controller_to_pilot_timer_roll--; } else { // when timer runs out switch to full pilot override for next iteration - poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; + roll_mode = RPMode::PILOT_OVERRIDE; } // calculate controller_to_pilot mix ratio - controller_to_pilot_roll_mix = (float)poshold.controller_to_pilot_timer_roll / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; + controller_to_pilot_roll_mix = (float)controller_to_pilot_timer_roll / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; // mix final loiter lean angle and pilot desired lean angles - poshold.roll = poshold_mix_controls(controller_to_pilot_roll_mix, poshold.controller_final_roll, poshold.pilot_roll + poshold.wind_comp_roll); + roll = mix_controls(controller_to_pilot_roll_mix, controller_final_roll, pilot_roll + wind_comp_roll); break; } @@ -354,195 +298,195 @@ void Copter::ModePosHold::run() // 1. dealing with pilot input // 2. calculating the final pitch output to the attitude contpitcher // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state - switch (poshold.pitch_mode) { + switch (pitch_mode) { - case POSHOLD_PILOT_OVERRIDE: + case RPMode::PILOT_OVERRIDE: // update pilot desired pitch angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch); + update_pilot_lean_angle(pilot_pitch, target_pitch); // switch to BRAKE mode for next iteration if no pilot input - if (is_zero(target_pitch) && (fabsf(poshold.pilot_pitch) < 2 * g.poshold_brake_rate)) { + if (is_zero(target_pitch) && (fabsf(pilot_pitch) < 2 * g.poshold_brake_rate)) { // initialise BRAKE mode - poshold.pitch_mode = POSHOLD_BRAKE; // set brake pitch mode - poshold.brake_pitch = 0; // initialise braking angle to zero - poshold.brake_angle_max_pitch = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking - poshold.brake_timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. - poshold.braking_time_updated_pitch = false; // flag the braking time can be re-estimated + pitch_mode = RPMode::BRAKE; // set brake pitch mode + brake_pitch = 0; // initialise braking angle to zero + brake_angle_max_pitch = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking + brake_timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. + braking_time_updated_pitch = false; // flag the braking time can be re-estimated } // final lean angle should be pilot input plus wind compensation - poshold.pitch = poshold.pilot_pitch + poshold.wind_comp_pitch; + pitch = pilot_pitch + wind_comp_pitch; break; - case POSHOLD_BRAKE: - case POSHOLD_BRAKE_READY_TO_LOITER: + case RPMode::BRAKE: + case RPMode::BRAKE_READY_TO_LOITER: // calculate brake_pitch angle to counter-act velocity - poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw); + update_brake_angle_from_velocity(brake_pitch, -vel_fw); // update braking time estimate - if (!poshold.braking_time_updated_pitch) { + if (!braking_time_updated_pitch) { // check if brake angle is increasing - if (abs(poshold.brake_pitch) >= poshold.brake_angle_max_pitch) { - poshold.brake_angle_max_pitch = abs(poshold.brake_pitch); + if (abs(brake_pitch) >= brake_angle_max_pitch) { + brake_angle_max_pitch = abs(brake_pitch); } else { // braking angle has started decreasing so re-estimate braking time - poshold.brake_timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_pitch))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. - poshold.braking_time_updated_pitch = true; + brake_timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(brake_pitch))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. + braking_time_updated_pitch = true; } } // if velocity is very low reduce braking time to 0.5seconds - if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_pitch > 50*LOOP_RATE_FACTOR)) { - poshold.brake_timeout_pitch = 50*LOOP_RATE_FACTOR; + if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (brake_timeout_pitch > 50*LOOP_RATE_FACTOR)) { + brake_timeout_pitch = 50*LOOP_RATE_FACTOR; } // reduce braking timer - if (poshold.brake_timeout_pitch > 0) { - poshold.brake_timeout_pitch--; + if (brake_timeout_pitch > 0) { + brake_timeout_pitch--; } else { // indicate that we are ready to move to Loiter. - // Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER + // Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to RPMode::BRAKE_READY_TO_LOITER // logic for engaging loiter is handled below the pitch and pitch mode switch statements - poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; + pitch_mode = RPMode::BRAKE_READY_TO_LOITER; } // final lean angle is braking angle + wind compensation angle - poshold.pitch = poshold.brake_pitch + poshold.wind_comp_pitch; + pitch = brake_pitch + wind_comp_pitch; // check for pilot input if (!is_zero(target_pitch)) { // init transition to pilot override - poshold_pitch_controller_to_pilot_override(); + pitch_controller_to_pilot_override(); } break; - case POSHOLD_BRAKE_TO_LOITER: - case POSHOLD_LOITER: + case RPMode::BRAKE_TO_LOITER: + case RPMode::LOITER: // these modes are combined pitch-pitch modes and are handled below break; - case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: + case RPMode::CONTROLLER_TO_PILOT_OVERRIDE: // update pilot desired pitch angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch); + update_pilot_lean_angle(pilot_pitch, target_pitch); // count-down loiter to pilot timer - if (poshold.controller_to_pilot_timer_pitch > 0) { - poshold.controller_to_pilot_timer_pitch--; + if (controller_to_pilot_timer_pitch > 0) { + controller_to_pilot_timer_pitch--; } else { // when timer runs out switch to full pilot override for next iteration - poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; + pitch_mode = RPMode::PILOT_OVERRIDE; } // calculate controller_to_pilot mix ratio - controller_to_pilot_pitch_mix = (float)poshold.controller_to_pilot_timer_pitch / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; + controller_to_pilot_pitch_mix = (float)controller_to_pilot_timer_pitch / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; // mix final loiter lean angle and pilot desired lean angles - poshold.pitch = poshold_mix_controls(controller_to_pilot_pitch_mix, poshold.controller_final_pitch, poshold.pilot_pitch + poshold.wind_comp_pitch); + pitch = mix_controls(controller_to_pilot_pitch_mix, controller_final_pitch, pilot_pitch + wind_comp_pitch); break; } // - // Shared roll & pitch states (POSHOLD_BRAKE_TO_LOITER and POSHOLD_LOITER) + // Shared roll & pitch states (RPMode::BRAKE_TO_LOITER and RPMode::LOITER) // // switch into LOITER mode when both roll and pitch are ready - if (poshold.roll_mode == POSHOLD_BRAKE_READY_TO_LOITER && poshold.pitch_mode == POSHOLD_BRAKE_READY_TO_LOITER) { - poshold.roll_mode = POSHOLD_BRAKE_TO_LOITER; - poshold.pitch_mode = POSHOLD_BRAKE_TO_LOITER; - poshold.brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER; + if (roll_mode == RPMode::BRAKE_READY_TO_LOITER && pitch_mode == RPMode::BRAKE_READY_TO_LOITER) { + roll_mode = RPMode::BRAKE_TO_LOITER; + pitch_mode = RPMode::BRAKE_TO_LOITER; + brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER; // init loiter controller loiter_nav->init_target(inertial_nav.get_position()); // set delay to start of wind compensation estimate updates - poshold.wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER; + wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER; } // roll-mode is used as the combined roll+pitch mode when in BRAKE_TO_LOITER or LOITER modes - if (poshold.roll_mode == POSHOLD_BRAKE_TO_LOITER || poshold.roll_mode == POSHOLD_LOITER) { + if (roll_mode == RPMode::BRAKE_TO_LOITER || roll_mode == RPMode::LOITER) { // force pitch mode to be same as roll_mode just to keep it consistent (it's not actually used in these states) - poshold.pitch_mode = poshold.roll_mode; + pitch_mode = roll_mode; // handle combined roll+pitch mode - switch (poshold.roll_mode) { - case POSHOLD_BRAKE_TO_LOITER: + switch (roll_mode) { + case RPMode::BRAKE_TO_LOITER: // reduce brake_to_loiter timer - if (poshold.brake_to_loiter_timer > 0) { - poshold.brake_to_loiter_timer--; + if (brake_to_loiter_timer > 0) { + brake_to_loiter_timer--; } else { // progress to full loiter on next iteration - poshold.roll_mode = POSHOLD_LOITER; - poshold.pitch_mode = POSHOLD_LOITER; + roll_mode = RPMode::LOITER; + pitch_mode = RPMode::LOITER; } // calculate percentage mix of loiter and brake control - brake_to_loiter_mix = (float)poshold.brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER; + brake_to_loiter_mix = (float)brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER; // calculate brake_roll and pitch angles to counter-act velocity - poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right); - poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw); + update_brake_angle_from_velocity(brake_roll, vel_right); + update_brake_angle_from_velocity(brake_pitch, -vel_fw); // run loiter controller loiter_nav->update(); // calculate final roll and pitch output by mixing loiter and brake controls - poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, loiter_nav->get_roll()); - poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, loiter_nav->get_pitch()); + roll = mix_controls(brake_to_loiter_mix, brake_roll + wind_comp_roll, loiter_nav->get_roll()); + pitch = mix_controls(brake_to_loiter_mix, brake_pitch + wind_comp_pitch, loiter_nav->get_pitch()); // check for pilot input if (!is_zero(target_roll) || !is_zero(target_pitch)) { // if roll input switch to pilot override for roll if (!is_zero(target_roll)) { // init transition to pilot override - poshold_roll_controller_to_pilot_override(); + roll_controller_to_pilot_override(); // switch pitch-mode to brake (but ready to go back to loiter anytime) - // no need to reset poshold.brake_pitch here as wind comp has not been updated since last brake_pitch computation - poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; + // no need to reset brake_pitch here as wind comp has not been updated since last brake_pitch computation + pitch_mode = RPMode::BRAKE_READY_TO_LOITER; } // if pitch input switch to pilot override for pitch if (!is_zero(target_pitch)) { // init transition to pilot override - poshold_pitch_controller_to_pilot_override(); + pitch_controller_to_pilot_override(); if (is_zero(target_roll)) { // switch roll-mode to brake (but ready to go back to loiter anytime) - // no need to reset poshold.brake_roll here as wind comp has not been updated since last brake_roll computation - poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; + // no need to reset brake_roll here as wind comp has not been updated since last brake_roll computation + roll_mode = RPMode::BRAKE_READY_TO_LOITER; } } } break; - case POSHOLD_LOITER: + case RPMode::LOITER: // run loiter controller loiter_nav->update(); // set roll angle based on loiter controller outputs - poshold.roll = loiter_nav->get_roll(); - poshold.pitch = loiter_nav->get_pitch(); + roll = loiter_nav->get_roll(); + pitch = loiter_nav->get_pitch(); // update wind compensation estimate - poshold_update_wind_comp_estimate(); + update_wind_comp_estimate(); // check for pilot input if (!is_zero(target_roll) || !is_zero(target_pitch)) { // if roll input switch to pilot override for roll if (!is_zero(target_roll)) { // init transition to pilot override - poshold_roll_controller_to_pilot_override(); + roll_controller_to_pilot_override(); // switch pitch-mode to brake (but ready to go back to loiter anytime) - poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; + pitch_mode = RPMode::BRAKE_READY_TO_LOITER; // reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle - poshold.brake_pitch = 0; + brake_pitch = 0; } // if pitch input switch to pilot override for pitch if (!is_zero(target_pitch)) { // init transition to pilot override - poshold_pitch_controller_to_pilot_override(); + pitch_controller_to_pilot_override(); // if roll not overriden switch roll-mode to brake (but be ready to go back to loiter any time) if (is_zero(target_roll)) { - poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; - poshold.brake_roll = 0; + roll_mode = RPMode::BRAKE_READY_TO_LOITER; + brake_roll = 0; } // if roll not overridden switch roll-mode to brake (but be ready to go back to loiter any time) } @@ -557,18 +501,18 @@ void Copter::ModePosHold::run() // constrain target pitch/roll angles float angle_max = copter.aparm.angle_max; - poshold.roll = constrain_int16(poshold.roll, -angle_max, angle_max); - poshold.pitch = constrain_int16(poshold.pitch, -angle_max, angle_max); + roll = constrain_int16(roll, -angle_max, angle_max); + pitch = constrain_int16(pitch, -angle_max, angle_max); // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll, pitch, target_yaw_rate); // call z-axis position controller pos_control->update_z_controller(); } // poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received -void Copter::ModePosHold::poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw) +void Copter::ModePosHold::update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw) { // if raw input is large or reversing the vehicle's lean angle immediately set the fitlered angle to the new raw angle if ((lean_angle_filtered > 0 && lean_angle_raw < 0) || (lean_angle_filtered < 0 && lean_angle_raw > 0) || (fabsf(lean_angle_raw) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE)) { @@ -588,18 +532,18 @@ void Copter::ModePosHold::poshold_update_pilot_lean_angle(float &lean_angle_filt } } -// poshold_mix_controls - mixes two controls based on the mix_ratio +// mix_controls - mixes two controls based on the mix_ratio // mix_ratio of 1 = use first_control completely, 0 = use second_control completely, 0.5 = mix evenly -int16_t Copter::ModePosHold::poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control) +int16_t Copter::ModePosHold::mix_controls(float mix_ratio, int16_t first_control, int16_t second_control) { mix_ratio = constrain_float(mix_ratio, 0.0f, 1.0f); return (int16_t)((mix_ratio * first_control) + ((1.0f-mix_ratio)*second_control)); } -// poshold_update_brake_angle_from_velocity - updates the brake_angle based on the vehicle's velocity and brake_gain +// update_brake_angle_from_velocity - updates the brake_angle based on the vehicle's velocity and brake_gain // brake_angle is slewed with the wpnav.poshold_brake_rate and constrained by the wpnav.poshold_braking_angle_max // velocity is assumed to be in the same direction as lean angle so for pitch you should provide the velocity backwards (i.e. -ve forward velocity) -void Copter::ModePosHold::poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity) +void Copter::ModePosHold::update_brake_angle_from_velocity(int16_t &brake_angle, float velocity) { float lean_angle; int16_t brake_rate = g.poshold_brake_rate; @@ -611,9 +555,9 @@ void Copter::ModePosHold::poshold_update_brake_angle_from_velocity(int16_t &brak // calculate velocity-only based lean angle if (velocity >= 0) { - lean_angle = -poshold.brake_gain * velocity * (1.0f+500.0f/(velocity+60.0f)); + lean_angle = -brake_gain * velocity * (1.0f+500.0f/(velocity+60.0f)); } else { - lean_angle = -poshold.brake_gain * velocity * (1.0f+500.0f/(-velocity+60.0f)); + lean_angle = -brake_gain * velocity * (1.0f+500.0f/(-velocity+60.0f)); } // do not let lean_angle be too far from brake_angle @@ -623,13 +567,13 @@ void Copter::ModePosHold::poshold_update_brake_angle_from_velocity(int16_t &brak brake_angle = constrain_int16(brake_angle, -g.poshold_brake_angle_max, g.poshold_brake_angle_max); } -// poshold_update_wind_comp_estimate - updates wind compensation estimate +// update_wind_comp_estimate - updates wind compensation estimate // should be called at the maximum loop rate when loiter is engaged -void Copter::ModePosHold::poshold_update_wind_comp_estimate() +void Copter::ModePosHold::update_wind_comp_estimate() { // check wind estimate start has not been delayed - if (poshold.wind_comp_start_timer > 0) { - poshold.wind_comp_start_timer--; + if (wind_comp_start_timer > 0) { + wind_comp_start_timer--; return; } @@ -643,58 +587,58 @@ void Copter::ModePosHold::poshold_update_wind_comp_estimate() const Vector3f& accel_target = pos_control->get_accel_target(); // update wind compensation in earth-frame lean angles - if (is_zero(poshold.wind_comp_ef.x)) { + if (is_zero(wind_comp_ef.x)) { // if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction - poshold.wind_comp_ef.x = accel_target.x; + wind_comp_ef.x = accel_target.x; } else { // low pass filter the position controller's lean angle output - poshold.wind_comp_ef.x = (1.0f-TC_WIND_COMP)*poshold.wind_comp_ef.x + TC_WIND_COMP*accel_target.x; + wind_comp_ef.x = (1.0f-TC_WIND_COMP)*wind_comp_ef.x + TC_WIND_COMP*accel_target.x; } - if (is_zero(poshold.wind_comp_ef.y)) { + if (is_zero(wind_comp_ef.y)) { // if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction - poshold.wind_comp_ef.y = accel_target.y; + wind_comp_ef.y = accel_target.y; } else { // low pass filter the position controller's lean angle output - poshold.wind_comp_ef.y = (1.0f-TC_WIND_COMP)*poshold.wind_comp_ef.y + TC_WIND_COMP*accel_target.y; + wind_comp_ef.y = (1.0f-TC_WIND_COMP)*wind_comp_ef.y + TC_WIND_COMP*accel_target.y; } } -// poshold_get_wind_comp_lean_angles - retrieve wind compensation angles in body frame roll and pitch angles +// get_wind_comp_lean_angles - retrieve wind compensation angles in body frame roll and pitch angles // should be called at the maximum loop rate -void Copter::ModePosHold::poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle) +void Copter::ModePosHold::get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle) { // reduce rate to 10hz - poshold.wind_comp_timer++; - if (poshold.wind_comp_timer < POSHOLD_WIND_COMP_TIMER_10HZ) { + wind_comp_timer++; + if (wind_comp_timer < POSHOLD_WIND_COMP_TIMER_10HZ) { return; } - poshold.wind_comp_timer = 0; + wind_comp_timer = 0; // convert earth frame desired accelerations to body frame roll and pitch lean angles - roll_angle = atanf((-poshold.wind_comp_ef.x*ahrs.sin_yaw() + poshold.wind_comp_ef.y*ahrs.cos_yaw())/981)*(18000/M_PI); - pitch_angle = atanf(-(poshold.wind_comp_ef.x*ahrs.cos_yaw() + poshold.wind_comp_ef.y*ahrs.sin_yaw())/981)*(18000/M_PI); + roll_angle = atanf((-wind_comp_ef.x*ahrs.sin_yaw() + wind_comp_ef.y*ahrs.cos_yaw())/981)*(18000/M_PI); + pitch_angle = atanf(-(wind_comp_ef.x*ahrs.cos_yaw() + wind_comp_ef.y*ahrs.sin_yaw())/981)*(18000/M_PI); } -// poshold_roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis -void Copter::ModePosHold::poshold_roll_controller_to_pilot_override() +// roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis +void Copter::ModePosHold::roll_controller_to_pilot_override() { - poshold.roll_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; - poshold.controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; + roll_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE; + controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; // initialise pilot_roll to 0, wind_comp will be updated to compensate and poshold_update_pilot_lean_angle function shall not smooth this transition at next iteration. so 0 is the right value - poshold.pilot_roll = 0; + pilot_roll = 0; // store final controller output for mixing with pilot input - poshold.controller_final_roll = poshold.roll; + controller_final_roll = roll; } -// poshold_pitch_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis -void Copter::ModePosHold::poshold_pitch_controller_to_pilot_override() +// pitch_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis +void Copter::ModePosHold::pitch_controller_to_pilot_override() { - poshold.pitch_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; - poshold.controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; - // initialise pilot_pitch to 0, wind_comp will be updated to compensate and poshold_update_pilot_lean_angle function shall not smooth this transition at next iteration. so 0 is the right value - poshold.pilot_pitch = 0; + pitch_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE; + controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; + // initialise pilot_pitch to 0, wind_comp will be updated to compensate and update_pilot_lean_angle function shall not smooth this transition at next iteration. so 0 is the right value + pilot_pitch = 0; // store final loiter outputs for mixing with pilot input - poshold.controller_final_pitch = poshold.pitch; + controller_final_pitch = pitch; } #endif