AP_Soaring: Improve tracking of enabled/disabled status.

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();

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

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

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;
 }
 
 

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