From b9daae062c0fb6618a76626e853d3feebc0823cd Mon Sep 17 00:00:00 2001 From: Samuel Tabor Date: Sun, 5 Apr 2020 12:38:19 +0100 Subject: [PATCH] AP_Soaring: Use enum class rather than typedef enum for states and hide details of these. --- ArduPlane/GCS_Mavlink.cpp | 2 +- ArduPlane/altitude.cpp | 2 +- ArduPlane/mode_loiter.cpp | 2 +- ArduPlane/navigation.cpp | 2 +- ArduPlane/soaring.cpp | 6 +++-- libraries/AP_Soaring/AP_Soaring.cpp | 42 ++++++++++++++--------------- libraries/AP_Soaring/AP_Soaring.h | 27 ++++++++++--------- 7 files changed, 43 insertions(+), 40 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 7d10a43505..838e37f5a8 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -265,7 +265,7 @@ int16_t GCS_MAVLINK_Plane::vfr_hud_throttle() const float GCS_MAVLINK_Plane::vfr_hud_climbrate() const { #if SOARING_ENABLED == ENABLED - if (plane.g2.soaring_controller.update_active_state()) { + if (plane.g2.soaring_controller.is_active()) { return plane.g2.soaring_controller.get_vario_reading(); } #endif diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 363fda8c6f..ea3636f671 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -41,7 +41,7 @@ void Plane::adjust_altitude_target() } else if (landing.get_target_altitude_location(target_location)) { set_target_altitude_location(target_location); #if SOARING_ENABLED == ENABLED - } else if (g2.soaring_controller.active_state() && g2.soaring_controller.get_throttle_suppressed()) { + } else if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) { // Reset target alt to current alt, to prevent large altitude errors when gliding. set_target_altitude_location(current_loc); reset_offset_altitude(); diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index 2bef9679c0..344e7b0d64 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -10,7 +10,7 @@ bool ModeLoiter::_enter() plane.loiter_angle_reset(); #if SOARING_ENABLED == ENABLED - if (plane.g2.soaring_controller.active_state()) { + if (plane.g2.soaring_controller.is_active()) { plane.g2.soaring_controller.init_thermalling(); plane.g2.soaring_controller.get_target(plane.next_WP_loc); // ahead on flight path } diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index f4b31d383a..542e35545d 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -320,7 +320,7 @@ void Plane::update_fbwb_speed_height(void) } #if SOARING_ENABLED == ENABLED - if (g2.soaring_controller.active_state() && g2.soaring_controller.get_throttle_suppressed()) { + if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) { // we're in soaring mode with throttle suppressed set_target_altitude_current();; } diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp index 740d608cd7..57fab787fd 100644 --- a/ArduPlane/soaring.cpp +++ b/ArduPlane/soaring.cpp @@ -11,7 +11,9 @@ void Plane::update_soaring() { // Check if soaring is active. Also sets throttle suppressed // status on active state changes. - if (!g2.soaring_controller.update_active_state()) { + plane.g2.soaring_controller.update_active_state(); + + if (!g2.soaring_controller.is_active()) { return; } @@ -83,7 +85,7 @@ void Plane::update_soaring() { const SoaringController::LoiterStatus loiterStatus = g2.soaring_controller.check_cruise_criteria(prev_wp/100, next_wp/100); - if (loiterStatus == SoaringController::LoiterStatus::THERMAL_GOOD_TO_KEEP_LOITERING) { + if (loiterStatus == SoaringController::LoiterStatus::GOOD_TO_KEEP_LOITERING) { // Reset loiter angle, so that the loiter exit heading criteria // only starts expanding when we're ready to exit. plane.loiter.sum_cd = 0; diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index 3a3176fc72..6268db4ebf 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -184,7 +184,7 @@ bool SoaringController::check_thermal_criteria() { ActiveStatus status = active_state(); - return (status == SOARING_STATUS_AUTO_MODE_CHANGE + return (status == ActiveStatus::AUTO_MODE_CHANGE && ((AP_HAL::micros64() - _cruise_start_time_us) > ((unsigned)min_cruise_s * 1e6)) && (_vario.filtered_reading - _vario.get_exp_thermalling_sink()) > thermal_vspeed && _vario.alt < alt_max @@ -196,38 +196,38 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria(Vector2 { ActiveStatus status = active_state(); - if (status != SOARING_STATUS_AUTO_MODE_CHANGE) { - _cruise_criteria_msg_last = SOARING_DISABLED; - return SOARING_DISABLED; + if (status != ActiveStatus::AUTO_MODE_CHANGE) { + _cruise_criteria_msg_last = LoiterStatus::DISABLED; + return LoiterStatus::DISABLED; } - LoiterStatus result = THERMAL_GOOD_TO_KEEP_LOITERING; + LoiterStatus result = LoiterStatus::GOOD_TO_KEEP_LOITERING; const float alt = _vario.alt; if (alt > alt_max) { - result = ALT_TOO_HIGH; + result = LoiterStatus::ALT_TOO_HIGH; if (result != _cruise_criteria_msg_last) { gcs().send_text(MAV_SEVERITY_ALERT, "Reached upper alt = %dm", (int16_t)alt); } } else if (alt < alt_min) { - result = ALT_TOO_LOW; + result = LoiterStatus::ALT_TOO_LOW; if (result != _cruise_criteria_msg_last) { gcs().send_text(MAV_SEVERITY_ALERT, "Reached lower alt = %dm", (int16_t)alt); } } else if ((AP_HAL::micros64() - _thermal_start_time_us) > ((unsigned)min_thermal_s * 1e6)) { const float mcCreadyAlt = McCready(alt); if (_thermalability < mcCreadyAlt) { - result = THERMAL_WEAK; + result = LoiterStatus::THERMAL_WEAK; if (result != _cruise_criteria_msg_last) { gcs().send_text(MAV_SEVERITY_INFO, "Thermal weak: th %3.1fm/s alt %3.1fm Mc %3.1fm/s", (double)_thermalability, (double)alt, (double)mcCreadyAlt); } } else if (alt < (-_thermal_start_pos.z) || _vario.smoothed_climb_rate < 0.0) { - result = ALT_LOST; + result = LoiterStatus::ALT_LOST; if (result != _cruise_criteria_msg_last) { gcs().send_text(MAV_SEVERITY_INFO, "Not climbing"); } } else if (check_drift(prev_wp, next_wp)) { - result = DRIFT_EXCEEDED; + result = LoiterStatus::DRIFT_EXCEEDED; if (result != _cruise_criteria_msg_last) { gcs().send_text(MAV_SEVERITY_INFO, "Drifted too far"); } @@ -288,7 +288,7 @@ void SoaringController::init_thermalling() void SoaringController::init_cruising() { - if (active_state()) { + if (active_state()>=ActiveStatus::MANUAL_MODE_CHANGE) { _cruise_start_time_us = AP_HAL::micros64(); // Start glide. Will be updated on the next loop. set_throttle_suppressed(true); @@ -359,43 +359,43 @@ float SoaringController::McCready(float alt) SoaringController::ActiveStatus SoaringController::active_state() const { if (!soar_active) { - return SOARING_STATUS_DISABLED; + return ActiveStatus::DISABLED; } if (soar_active_ch <= 0) { // no activation channel - return SOARING_STATUS_AUTO_MODE_CHANGE; + return ActiveStatus::AUTO_MODE_CHANGE; } uint16_t radio_in = RC_Channels::get_radio_in(soar_active_ch-1); // active when above 1400, with auto mode changes when above 1700 if (radio_in >= 1700) { - return SOARING_STATUS_AUTO_MODE_CHANGE; + return ActiveStatus::AUTO_MODE_CHANGE; } else if (radio_in >= 1400) { - return SOARING_STATUS_MANUAL_MODE_CHANGE; + return ActiveStatus::MANUAL_MODE_CHANGE; } - return SOARING_STATUS_DISABLED; + return ActiveStatus::DISABLED; } -SoaringController::ActiveStatus SoaringController::update_active_state() +void SoaringController::update_active_state() { ActiveStatus status = active_state(); bool state_changed = !(status == _last_update_status); if (state_changed) { switch (status) { - case SOARING_STATUS_DISABLED: + case ActiveStatus::DISABLED: // It's not enabled, but was enabled on the last loop. gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Disabled."); set_throttle_suppressed(false); break; - case SOARING_STATUS_MANUAL_MODE_CHANGE: + case ActiveStatus::MANUAL_MODE_CHANGE: // It's enabled, but wasn't on the last loop. gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Enabled, manual mode changes."); set_throttle_suppressed(true); break; - case SOARING_STATUS_AUTO_MODE_CHANGE: + case ActiveStatus::AUTO_MODE_CHANGE: gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Enabled, automatic mode changes."); set_throttle_suppressed(true); break; @@ -403,8 +403,6 @@ SoaringController::ActiveStatus SoaringController::update_active_state() } _last_update_status = status; - - return status; } diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h index b65dc641a3..d2c9c0ef75 100644 --- a/libraries/AP_Soaring/AP_Soaring.h +++ b/libraries/AP_Soaring/AP_Soaring.h @@ -76,21 +76,21 @@ protected: public: SoaringController(AP_AHRS &ahrs, AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms); - typedef enum LoiterStatus { - SOARING_DISABLED, + enum class LoiterStatus { + DISABLED, ALT_TOO_HIGH, ALT_TOO_LOW, THERMAL_WEAK, ALT_LOST, DRIFT_EXCEEDED, - THERMAL_GOOD_TO_KEEP_LOITERING - } LoiterStatus; + GOOD_TO_KEEP_LOITERING + }; - typedef enum ActiveStatus { - SOARING_STATUS_DISABLED, - SOARING_STATUS_MANUAL_MODE_CHANGE, - SOARING_STATUS_AUTO_MODE_CHANGE - } ActiveStatus; + enum class ActiveStatus { + DISABLED, + MANUAL_MODE_CHANGE, + AUTO_MODE_CHANGE + }; AP_Float max_radius; @@ -104,7 +104,6 @@ public: void init_cruising(); void update_thermalling(); void update_cruising(); - ActiveStatus active_state() const; void set_throttle_suppressed(bool suppressed); bool get_throttle_suppressed() const @@ -121,11 +120,15 @@ public: bool check_drift(Vector2f prev_wp, Vector2f next_wp); - ActiveStatus update_active_state(); + void update_active_state(); - ActiveStatus _last_update_status; + bool is_active() const {return _last_update_status>=SoaringController::ActiveStatus::MANUAL_MODE_CHANGE;}; private: // slow down messages if they are the same. During loiter we could smap the same message. Only show new messages during loiters LoiterStatus _cruise_criteria_msg_last; + + ActiveStatus _last_update_status; + + ActiveStatus active_state() const; };