AP_TECS: inhibit raising TAS_dem during flare

magicrub · May 21, 2024 · d5e707c · d5e707c
1 parent 20a23d8
commit d5e707c
Showing 1 changed file with 11 additions and 5 deletions.
diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
@@ -458,8 +458,8 @@ void AP_TECS::_update_speed(float DT)
 
 void AP_TECS::_update_speed_demand(void)
 {
-    if (_options & OPTION_DESCENT_SPEEDUP) {
-        // Allow demanded speed to  go to maximum when descending at maximum descent rate
+    if ((_options & OPTION_DESCENT_SPEEDUP) && (_sink_fraction > 0.0f) && !_landing.is_flaring()) {
+        // Allow demanded speed to go to maximum when descending at maximum descent rate
         _TAS_dem = _TAS_dem + (_TASmax - _TAS_dem) * _sink_fraction;
     }
 
@@ -598,6 +598,8 @@ void AP_TECS::_update_height_demand(void)
         _hgt_dem_rate_ltd = _height;
         _hgt_dem_in_prev  = _height;
 
+        _sink_fraction = 0.0f; // do not add any additional sink/airspeed while flaring
+
         if (!_flare_initialised) {
             _flare_hgt_dem_adj = _hgt_dem;
             _flare_hgt_dem_ideal = _hgt_afe;
@@ -1094,8 +1096,9 @@ void AP_TECS::_update_pitch(void)
         // @Field: KI: Pitch demand kinetic energy integral
         // @Field: pmin: Pitch min
         // @Field: pmax: Pitch max
-        AP::logger().WriteStreaming("TEC2","TimeUS,PEW,KEW,EBD,EBE,EBDD,EBDE,EBDDT,Imin,Imax,I,KI,pmin,pmax",
-                                    "Qfffffffffffff",
+        // @Field: SF: Sink Fraction
+        AP::logger().WriteStreaming("TEC2","TimeUS,PEW,KEW,EBD,EBE,EBDD,EBDE,EBDDT,Imin,Imax,I,KI,pmin,pmax,SF",
+                                    "Qffffffffffffff",
                                     AP_HAL::micros64(),
                                     (double)SPE_weighting,
                                     (double)_SKE_weighting,
@@ -1109,7 +1112,8 @@ void AP_TECS::_update_pitch(void)
                                     (double)_integSEBdot,
                                     (double)_integKE,
                                     (double)_PITCHminf,
-                                    (double)_PITCHmaxf);
+                                    (double)_PITCHmaxf,
+                                    (double)_sink_fraction);
     }
 #endif
 }
@@ -1152,6 +1156,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
 
         _max_climb_scaler = 1.0f;
         _max_sink_scaler = 1.0f;
+        _sink_fraction = 0.0f;
 
         const float fc = 1.0f / (M_2PI * _timeConst);
         _pitch_demand_lpf.set_cutoff_frequency(fc);
@@ -1176,6 +1181,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
 
         _max_climb_scaler = 1.0f;
         _max_sink_scaler = 1.0f;
+        _sink_fraction = 0.0f;
 
         _pitch_demand_lpf.reset(_ahrs.get_pitch());
         _pitch_measured_lpf.reset(_ahrs.get_pitch());