AP_Soaring: Detect changes in active parameter/switch position.

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.is_active()) {
+    if (plane.g2.soaring_controller.update_active_state()) {
         return plane.g2.soaring_controller.get_vario_reading();
     }
 #endif

ArduPlane/soaring.cpp

@@ -9,9 +9,9 @@
 */
 void Plane::update_soaring() {
     
-    if (!g2.soaring_controller.is_active()) {
-        // This also sets the TECS gliding_requested to false.
-        g2.soaring_controller.set_throttle_suppressed(false);
+    // Check if soaring is active. Also sets throttle suppressed
+    // status on active state changes.
+    if (!g2.soaring_controller.update_active_state()) {
         return;
     }
     

libraries/AP_Soaring/AP_Soaring.cpp

@@ -355,6 +355,27 @@ bool SoaringController::is_active() const
     return RC_Channels::get_radio_in(soar_active_ch-1) >= 1400;
 }
 
+bool SoaringController::update_active_state()
+{
+    bool active = is_active();
+    bool state_changed = !(active == _last_update_active);
+
+    if (state_changed) {
+        if (active) {
+            // It's active, but wasn't on the last loop.
+            set_throttle_suppressed(true);
+        } else {
+            // It's not active, but was active on the last loop.
+            set_throttle_suppressed(false);
+        }
+    }
+
+    _last_update_active = active;
+
+    return active;
+}
+
+
 void SoaringController::set_throttle_suppressed(bool suppressed)
 {
     _throttle_suppressed = suppressed;

libraries/AP_Soaring/AP_Soaring.h

@@ -52,6 +52,8 @@ class SoaringController {
     float _thermalability;
     float _expected_sink;
 
+    bool _last_update_active;
+
 protected:
     AP_Int8 soar_active;
     AP_Int8 soar_active_ch;
@@ -112,6 +114,8 @@ public:
 
     bool check_drift(Vector2f prev_wp, Vector2f next_wp);
 
+    bool update_active_state();
+
 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;