From 37c07e1d89a47323df423845faccd5e541ea0d30 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 23 Apr 2019 12:14:35 +1000 Subject: [PATCH] Copter: use enum class for roll/pitch mode This adds some type-safety and helps distinguish between the many defines which are used within PosHold mode Saves about 210 bytes of flash --- ArduCopter/mode.h | 20 ++++---- ArduCopter/mode_poshold.cpp | 92 ++++++++++++++++++------------------- 2 files changed, 56 insertions(+), 56 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 2a05dc29d1..fec37a7662 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -909,18 +909,18 @@ private: void roll_controller_to_pilot_override(); void pitch_controller_to_pilot_override(); - // 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) + 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) }; - 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 + 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 diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 65fce211c1..cd893b2e64 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -45,12 +45,12 @@ bool Copter::ModePosHold::init(bool ignore_checks) if (ap.land_complete) { // if landed begin in loiter mode - roll_mode = POSHOLD_LOITER; - pitch_mode = POSHOLD_LOITER; + roll_mode = RPMode::LOITER; + pitch_mode = RPMode::LOITER; } else { // if not landed start in pilot override to avoid hard twitch - roll_mode = POSHOLD_PILOT_OVERRIDE; - pitch_mode = POSHOLD_PILOT_OVERRIDE; + roll_mode = RPMode::PILOT_OVERRIDE; + pitch_mode = RPMode::PILOT_OVERRIDE; } // initialise loiter @@ -115,8 +115,8 @@ void Copter::ModePosHold::run() loiter_nav->update(); // set poshold state to pilot override - roll_mode = POSHOLD_PILOT_OVERRIDE; - pitch_mode = POSHOLD_PILOT_OVERRIDE; + roll_mode = RPMode::PILOT_OVERRIDE; + pitch_mode = RPMode::PILOT_OVERRIDE; break; case AltHold_Takeoff: @@ -145,8 +145,8 @@ void Copter::ModePosHold::run() pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); // set poshold state to pilot override - roll_mode = POSHOLD_PILOT_OVERRIDE; - pitch_mode = POSHOLD_PILOT_OVERRIDE; + roll_mode = RPMode::PILOT_OVERRIDE; + pitch_mode = RPMode::PILOT_OVERRIDE; break; case AltHold_Landed_Ground_Idle: @@ -162,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 - roll_mode = POSHOLD_PILOT_OVERRIDE; - pitch_mode = POSHOLD_PILOT_OVERRIDE; + roll_mode = RPMode::PILOT_OVERRIDE; + pitch_mode = RPMode::PILOT_OVERRIDE; break; case AltHold_Flying: @@ -195,7 +195,7 @@ 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 (roll_mode != POSHOLD_LOITER || pitch_mode != POSHOLD_LOITER) { + if (roll_mode != RPMode::LOITER || pitch_mode != RPMode::LOITER) { get_wind_comp_lean_angles(wind_comp_roll, wind_comp_pitch); } @@ -206,7 +206,7 @@ void Copter::ModePosHold::run() // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state 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 update_pilot_lean_angle(pilot_roll, target_roll); @@ -214,7 +214,7 @@ void Copter::ModePosHold::run() // switch to BRAKE mode for next iteration if no pilot input if (is_zero(target_roll) && (fabsf(pilot_roll) < 2 * g.poshold_brake_rate)) { // initialise BRAKE mode - roll_mode = POSHOLD_BRAKE; // Set brake roll mode + 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. @@ -225,8 +225,8 @@ void Copter::ModePosHold::run() 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 update_brake_angle_from_velocity(brake_roll, vel_right); @@ -252,9 +252,9 @@ void Copter::ModePosHold::run() 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 - roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; + roll_mode = RPMode::BRAKE_READY_TO_LOITER; } // final lean angle is braking angle + wind compensation angle @@ -267,12 +267,12 @@ void Copter::ModePosHold::run() } 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 update_pilot_lean_angle(pilot_roll, target_roll); @@ -282,7 +282,7 @@ void Copter::ModePosHold::run() controller_to_pilot_timer_roll--; } else { // when timer runs out switch to full pilot override for next iteration - roll_mode = POSHOLD_PILOT_OVERRIDE; + roll_mode = RPMode::PILOT_OVERRIDE; } // calculate controller_to_pilot mix ratio @@ -300,7 +300,7 @@ void Copter::ModePosHold::run() // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state 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 update_pilot_lean_angle(pilot_pitch, target_pitch); @@ -308,7 +308,7 @@ void Copter::ModePosHold::run() // switch to BRAKE mode for next iteration if no pilot input if (is_zero(target_pitch) && (fabsf(pilot_pitch) < 2 * g.poshold_brake_rate)) { // initialise BRAKE mode - pitch_mode = POSHOLD_BRAKE; // set brake pitch mode + 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. @@ -319,8 +319,8 @@ void Copter::ModePosHold::run() 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 update_brake_angle_from_velocity(brake_pitch, -vel_fw); @@ -346,9 +346,9 @@ void Copter::ModePosHold::run() 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 - pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; + pitch_mode = RPMode::BRAKE_READY_TO_LOITER; } // final lean angle is braking angle + wind compensation angle @@ -361,12 +361,12 @@ void Copter::ModePosHold::run() } 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 update_pilot_lean_angle(pilot_pitch, target_pitch); @@ -376,7 +376,7 @@ void Copter::ModePosHold::run() controller_to_pilot_timer_pitch--; } else { // when timer runs out switch to full pilot override for next iteration - pitch_mode = POSHOLD_PILOT_OVERRIDE; + pitch_mode = RPMode::PILOT_OVERRIDE; } // calculate controller_to_pilot mix ratio @@ -388,13 +388,13 @@ void Copter::ModePosHold::run() } // - // 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 (roll_mode == POSHOLD_BRAKE_READY_TO_LOITER && pitch_mode == POSHOLD_BRAKE_READY_TO_LOITER) { - roll_mode = POSHOLD_BRAKE_TO_LOITER; - pitch_mode = POSHOLD_BRAKE_TO_LOITER; + 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()); @@ -403,21 +403,21 @@ void Copter::ModePosHold::run() } // roll-mode is used as the combined roll+pitch mode when in BRAKE_TO_LOITER or LOITER modes - if (roll_mode == POSHOLD_BRAKE_TO_LOITER || 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) pitch_mode = roll_mode; // handle combined roll+pitch mode switch (roll_mode) { - case POSHOLD_BRAKE_TO_LOITER: + case RPMode::BRAKE_TO_LOITER: // reduce brake_to_loiter timer if (brake_to_loiter_timer > 0) { brake_to_loiter_timer--; } else { // progress to full loiter on next iteration - roll_mode = POSHOLD_LOITER; - pitch_mode = POSHOLD_LOITER; + roll_mode = RPMode::LOITER; + pitch_mode = RPMode::LOITER; } // calculate percentage mix of loiter and brake control @@ -442,7 +442,7 @@ void Copter::ModePosHold::run() roll_controller_to_pilot_override(); // switch pitch-mode to brake (but ready to go back to loiter anytime) // no need to reset brake_pitch here as wind comp has not been updated since last brake_pitch computation - pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; + pitch_mode = RPMode::BRAKE_READY_TO_LOITER; } // if pitch input switch to pilot override for pitch if (!is_zero(target_pitch)) { @@ -451,13 +451,13 @@ void Copter::ModePosHold::run() if (is_zero(target_roll)) { // switch roll-mode to brake (but ready to go back to loiter anytime) // no need to reset brake_roll here as wind comp has not been updated since last brake_roll computation - roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; + roll_mode = RPMode::BRAKE_READY_TO_LOITER; } } } break; - case POSHOLD_LOITER: + case RPMode::LOITER: // run loiter controller loiter_nav->update(); @@ -475,7 +475,7 @@ void Copter::ModePosHold::run() // init transition to pilot override roll_controller_to_pilot_override(); // switch pitch-mode to brake (but ready to go back to loiter anytime) - 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 brake_pitch = 0; } @@ -485,7 +485,7 @@ void Copter::ModePosHold::run() 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)) { - roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; + 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) @@ -622,7 +622,7 @@ void Copter::ModePosHold::get_wind_comp_lean_angles(int16_t &roll_angle, int16_t // 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() { - roll_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; + 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 pilot_roll = 0; @@ -633,7 +633,7 @@ void Copter::ModePosHold::roll_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() { - pitch_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; + 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;