AP_TECS: Add flags to indicate gliding flight, and use these with AP_Soaring.

ArduPlane/ArduPlane.cpp

@@ -577,13 +577,6 @@ void Plane::update_alt()
         if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
             distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
         }
-
-        bool soaring_active = false;
-#if SOARING_ENABLED == ENABLED
-        if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
-            soaring_active = true;
-        }
-#endif
         
         SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
                                                  target_airspeed_cm,
@@ -592,8 +585,7 @@ void Plane::update_alt()
                                                  get_takeoff_pitch_min_cd(),
                                                  throttle_nudge,
                                                  tecs_hgt_afe(),
-                                                 aerodynamic_load_factor,
+                                                 aerodynamic_load_factor);
-                                                 soaring_active);
     }
 }
 

ArduPlane/mode_loiter.cpp

@@ -10,7 +10,8 @@ bool ModeLoiter::_enter()
     plane.loiter_angle_reset();
 
 #if SOARING_ENABLED == ENABLED
-    if (plane.g2.soaring_controller.is_active() && plane.g2.soaring_controller.suppress_throttle()) {
+    if (plane.g2.soaring_controller.is_active() &&
+       (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/servos.cpp

@@ -470,16 +470,6 @@ void Plane::set_servos_controlled(void)
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 
             constrain_int16(quadplane.forward_throttle_pct(), min_throttle, max_throttle));
     }
-
-#if SOARING_ENABLED == ENABLED
-    // suppress throttle when soaring is active
-    if ((control_mode == &mode_fbwb || control_mode == &mode_cruise ||
-        control_mode == &mode_auto || control_mode == &mode_loiter) &&
-        g2.soaring_controller.is_active() &&
-        g2.soaring_controller.get_throttle_suppressed()) {
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
-    }
-#endif
 }
 
 /*

ArduPlane/soaring.cpp

@@ -10,6 +10,8 @@
 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);
         return;
     }
     
@@ -22,18 +24,18 @@ void Plane::update_soaring() {
         break;
     case Mode::Number::FLY_BY_WIRE_B:
     case Mode::Number::CRUISE:
-        if (!g2.soaring_controller.suppress_throttle()) {
+        if (!g2.soaring_controller.suppress_throttle() && aparm.throttle_max > 0) {
             gcs().send_text(MAV_SEVERITY_INFO, "Soaring: forcing RTL");
             set_mode(mode_rtl, ModeReason::SOARING_FBW_B_WITH_MOTOR_RUNNING);
         }
         break;
     case Mode::Number::LOITER:
-        // Do nothing. We will switch back to auto/rtl before enabling throttle.
+        // Never use throttle in LOITER with soaring active.
+        g2.soaring_controller.set_throttle_suppressed(true);
         break;
     default:
-        // This does not affect the throttle since suppressed is only checked in the above three modes. 
+        // In any other mode allow throttle.
-        // It ensures that the soaring always starts with throttle suppressed though.
+        g2.soaring_controller.set_throttle_suppressed(false);
-        g2.soaring_controller.set_throttle_suppressed(true);
         break;
     }
 

libraries/AP_Soaring/AP_Soaring.cpp

@@ -153,12 +153,14 @@ bool SoaringController::suppress_throttle()
 
     if (_throttle_suppressed && (alt < alt_min)) {
         // Time to throttle up
-        _throttle_suppressed = false;
+        set_throttle_suppressed(false);
     } else if ((!_throttle_suppressed) && (alt > alt_cutoff)) {
         // Start glide
-        _throttle_suppressed = true;
+        set_throttle_suppressed(true);
+
         // Zero the pitch integrator - the nose is currently raised to climb, we need to go back to glide.
         _spdHgt.reset_pitch_I();
+
         _cruise_start_time_us = AP_HAL::micros64();
         // Reset the filtered vario rate - it is currently elevated due to the climb rate and would otherwise take a while to fall again,
         // leading to false positives.
@@ -275,10 +277,10 @@ void SoaringController::init_thermalling()
 
 void SoaringController::init_cruising()
 {
-    if (is_active() && suppress_throttle()) {
+    if (is_active()) {
         _cruise_start_time_us = AP_HAL::micros64();
         // Start glide. Will be updated on the next loop.
-        _throttle_suppressed = true;
+        set_throttle_suppressed(true);
     }
 }
 
@@ -344,3 +346,11 @@ bool SoaringController::is_active() const
     // active when above 1700
     return RC_Channels::get_radio_in(soar_active_ch-1) >= 1700;
 }
+
+void SoaringController::set_throttle_suppressed(bool suppressed)
+{
+    _throttle_suppressed = suppressed;
+
+    // Let the TECS know.
+    _spdHgt.set_gliding_requested_flag(suppressed);
+}

libraries/AP_Soaring/AP_Soaring.h

@@ -93,14 +93,13 @@ public:
     void update_thermalling();
     void update_cruising();
     bool is_active() const;
+    void set_throttle_suppressed(bool suppressed);
+
     bool get_throttle_suppressed() const
     {
         return _throttle_suppressed;
     }
-    void set_throttle_suppressed(bool suppressed)
+
-    {
-        _throttle_suppressed = suppressed;
-    }
     float get_vario_reading() const
     {
         return _vario.displayed_reading;

libraries/AP_SpdHgtControl/AP_SpdHgtControl.h

@@ -31,8 +31,7 @@ public:
 										int32_t ptchMinCO_cd,
 										int16_t throttle_nudge,
                                         float hgt_afe,
-										float load_factor,
+										float load_factor) = 0;
-                                        bool soaring_active) = 0;
 
 	// demanded throttle in percentage
 	// should return 0 to 100
@@ -66,6 +65,12 @@ public:
     // reset on next loop
     virtual void reset(void) = 0;
 
+    // set gliding requested flag
+    virtual void set_gliding_requested_flag(bool gliding_requested) = 0;
+
+    // set propulsion failed flag
+    virtual void set_propulsion_failed_flag(bool propulsion_failed) = 0;
+
 	// add new controllers to this enum. Users can then
 	// select which controller to use by setting the
 	// SPDHGT_CONTROLLER parameter

libraries/AP_TECS/AP_TECS.cpp

@@ -641,6 +641,10 @@ void AP_TECS::_update_throttle_with_airspeed(void)
     {
         _throttle_dem = 1.0f;
     }
+    else if (_flags.is_gliding)
+    {
+        _throttle_dem = 0.0f;
+    }
     else
     {
         // Calculate gain scaler from specific energy error to throttle
@@ -755,6 +759,11 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge)
         _throttle_dem = nomThr;
     }
 
+    if (_flags.is_gliding)
+    {
+        _throttle_dem = 0.0f;
+    }
+
     // Calculate additional throttle for turn drag compensation including throttle nudging
     const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned();
     // Use the demanded rate of change of total energy as the feed-forward demand, but add
@@ -780,7 +789,7 @@ void AP_TECS::_detect_bad_descent(void)
     // 2) Specific total energy error > 0
     // This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
     float STEdot = _SPEdot + _SKEdot;
-    if ((!_flags.underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_flags.badDescent && !_flags.underspeed && (_STE_error > 0.0f)))
+    if (((!_flags.underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_flags.badDescent && !_flags.underspeed && (_STE_error > 0.0f))) && !_flags.is_gliding)
     {
         _flags.badDescent = true;
     }
@@ -807,7 +816,7 @@ void AP_TECS::_update_pitch(void)
         // height. This is needed as the usual relationship of speed
         // and height is broken by the VTOL motors
         SKE_weighting = 0.0f;        
-    } else if ( _flags.underspeed || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
+    } else if ( _flags.underspeed || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND || _flags.is_gliding) {
         SKE_weighting = 2.0f;
     } else if (_flags.is_doing_auto_land) {
         if (_spdWeightLand < 0) {
@@ -902,7 +911,9 @@ void AP_TECS::_update_pitch(void)
 
 
     // Add a feedforward term from demanded airspeed to pitch.
-    _pitch_dem_unc += (_TAS_dem_adj - _pitch_ff_v0) * _pitch_ff_k;
+    if (_flags.is_gliding) {
+        _pitch_dem_unc += (_TAS_dem_adj - _pitch_ff_v0) * _pitch_ff_k;
+    }
 
     // Constrain pitch demand
     _pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
@@ -980,14 +991,14 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
                                     int32_t ptchMinCO_cd,
                                     int16_t throttle_nudge,
                                     float hgt_afe,
-                                    float load_factor,
+                                    float load_factor)
-                                    bool soaring_active)
 {
     // Calculate time in seconds since last update
     uint64_t now = AP_HAL::micros64();
     _DT = (now - _update_pitch_throttle_last_usec) * 1.0e-6f;
     _update_pitch_throttle_last_usec = now;
 
+    _flags.is_gliding = _flags.gliding_requested || _flags.propulsion_failed || aparm.throttle_max==0;
     _flags.is_doing_auto_land = (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND);
     _distance_beyond_land_wp = distance_beyond_land_wp;
     _flight_stage = flight_stage;
@@ -1141,8 +1152,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
     // Detect bad descent due to demanded airspeed being too high
     _detect_bad_descent();
 
-    // when soaring is active we never trigger a bad descent
+    if (_options & OPTION_GLIDER_ONLY) {
-    if (soaring_active || (_options & OPTION_GLIDER_ONLY)) {
         _flags.badDescent = false;        
     }
 

libraries/AP_TECS/AP_TECS.h

@@ -52,8 +52,7 @@ public:
                                int32_t ptchMinCO_cd,
                                int16_t throttle_nudge,
                                float hgt_afe,
-                               float load_factor,
+                               float load_factor) override;
-                               bool soaring_active) override;
 
     // demanded throttle in percentage
     // should return -100 to 100, usually positive unless reverse thrust is enabled via _THRminf < 0
@@ -107,6 +106,17 @@ public:
         _path_proportion = constrain_float(path_proportion, 0.0f, 1.0f);
     }
 
+    // set soaring flag
+    void set_gliding_requested_flag(bool gliding_requested) override {
+        _flags.gliding_requested = gliding_requested;
+    }
+
+    // set propulsion failed flag
+    void set_propulsion_failed_flag(bool propulsion_failed) override {
+        _flags.propulsion_failed = propulsion_failed;
+    }
+
+
     // set pitch max limit in degrees
     void set_pitch_max_limit(int8_t pitch_limit) {
         _pitch_max_limit = pitch_limit;
@@ -272,6 +282,18 @@ private:
 
         // true when we have reached target speed in takeoff
         bool reached_speed_takeoff:1;
+
+        // true if the soaring feature has requested gliding flight
+        bool gliding_requested:1;
+
+        // true when we are in gliding flight, in one of three situations;
+        //   - THR_MAX=0
+        //   - gliding has been requested e.g. by soaring feature
+        //   - engine failure detected (detection not implemented currently)
+        bool is_gliding:1;
+
+        // true if a propulsion failure is detected.
+        bool propulsion_failed:1;
     };
     union {
         struct flags _flags;