From e1f7122566735bcd566290cf9169ea7406f6288c Mon Sep 17 00:00:00 2001 From: Samuel Tabor Date: Sun, 18 Aug 2019 16:02:15 +0100 Subject: [PATCH] AP_Soaring: Improve tracking of enabled/disabled status. --- ArduPlane/altitude.cpp | 2 +- ArduPlane/mode_loiter.cpp | 2 +- ArduPlane/navigation.cpp | 2 +- libraries/AP_Soaring/AP_Soaring.cpp | 48 +++++++++++++++++------------ libraries/AP_Soaring/AP_Soaring.h | 14 ++++++--- 5 files changed, 42 insertions(+), 26 deletions(-) diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 33841d6de2..7d3e25e929 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -40,7 +40,7 @@ void Plane::adjust_altitude_target() landing.adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, auto_state.wp_distance, target_altitude.offset_cm); } else if (landing.get_target_altitude_location(target_location)) { set_target_altitude_location(target_location); - } else if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) { + } else if (g2.soaring_controller.active_state() && 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 f926eba41f..5d993f4f3f 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.is_active() && + if (plane.g2.soaring_controller.active_state() && (plane.g2.soaring_controller.suppress_throttle() || plane.aparm.throttle_max==0)) { 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 c1ee18ec52..20ed6876a6 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -314,7 +314,7 @@ void Plane::update_fbwb_speed_height(void) change_target_altitude(alt_change_cm); if ((is_zero(elevator_input) && !is_zero(target_altitude.last_elevator_input)) || - (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed())) { + (g2.soaring_controller.active_state() && g2.soaring_controller.get_throttle_suppressed())) { // the user has just released the elevator, lock in // the current altitude // or we're in soaring mode with throttle suppressed diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index 614f19940c..b9407a3d66 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -179,20 +179,21 @@ bool SoaringController::suppress_throttle() bool SoaringController::check_thermal_criteria() { - bool thermalling_allowed = soar_active_ch>0 ? RC_Channels::get_radio_in(soar_active_ch-1) >= 1700 : true; + ActiveStatus status = active_state(); - return (soar_active + return (status == SOARING_STATUS_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 - && _vario.alt > alt_min - && thermalling_allowed); + && _vario.alt > alt_min); } SoaringController::LoiterStatus SoaringController::check_cruise_criteria(Vector2f prev_wp, Vector2f next_wp) { - if (!soar_active) { + ActiveStatus status = active_state(); + + if (status != SOARING_STATUS_AUTO_MODE_CHANGE) { _cruise_criteria_msg_last = SOARING_DISABLED; return SOARING_DISABLED; } @@ -281,7 +282,7 @@ void SoaringController::init_thermalling() void SoaringController::init_cruising() { - if (is_active()) { + if (active_state()) { _cruise_start_time_us = AP_HAL::micros64(); // Start glide. Will be updated on the next loop. set_throttle_suppressed(true); @@ -342,37 +343,46 @@ float SoaringController::McCready(float alt) return thermal_vspeed; } -bool SoaringController::is_active() const +SoaringController::ActiveStatus SoaringController::active_state() const { if (!soar_active) { - return false; + return SOARING_STATUS_DISABLED; } if (soar_active_ch <= 0) { // no activation channel - return true; + return SOARING_STATUS_MANUAL_MODE_CHANGE; } - // active when above 1400 - return RC_Channels::get_radio_in(soar_active_ch-1) >= 1400; + + 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; + } else if (radio_in >= 1400) { + return SOARING_STATUS_MANUAL_MODE_CHANGE; + } + + return SOARING_STATUS_DISABLED; } -bool SoaringController::update_active_state() +SoaringController::ActiveStatus SoaringController::update_active_state() { - bool active = is_active(); - bool state_changed = !(active == _last_update_active); + ActiveStatus status = active_state(); + bool state_changed = !(status == _last_update_status); if (state_changed) { - if (active) { - // It's active, but wasn't on the last loop. + if (status>=SOARING_STATUS_MANUAL_MODE_CHANGE) { + // It's enabled, but wasn't on the last loop. set_throttle_suppressed(true); } else { - // It's not active, but was active on the last loop. + // It's not enabled, but was enabled on the last loop. set_throttle_suppressed(false); } } - _last_update_active = active; + _last_update_status = status; - return active; + return status; } diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h index d25b767f2a..0af95e69df 100644 --- a/libraries/AP_Soaring/AP_Soaring.h +++ b/libraries/AP_Soaring/AP_Soaring.h @@ -52,8 +52,6 @@ class SoaringController { float _thermalability; float _expected_sink; - bool _last_update_active; - protected: AP_Int8 soar_active; AP_Int8 soar_active_ch; @@ -85,6 +83,12 @@ public: THERMAL_GOOD_TO_KEEP_LOITERING } LoiterStatus; + typedef enum ActiveStatus { + SOARING_STATUS_DISABLED, + SOARING_STATUS_MANUAL_MODE_CHANGE, + SOARING_STATUS_AUTO_MODE_CHANGE + } ActiveStatus; + AP_Float max_radius; // this supports the TECS_* user settable parameters @@ -97,7 +101,7 @@ public: void init_cruising(); void update_thermalling(); void update_cruising(); - bool is_active() const; + ActiveStatus active_state() const; void set_throttle_suppressed(bool suppressed); bool get_throttle_suppressed() const @@ -114,7 +118,9 @@ public: bool check_drift(Vector2f prev_wp, Vector2f next_wp); - bool update_active_state(); + ActiveStatus update_active_state(); + + ActiveStatus _last_update_status; private: // slow down messages if they are the same. During loiter we could smap the same message. Only show new messages during loiters