AP_TECS: provide a much smoother transition before flare

this moves the pitch constraint smoothly between unconstrained and
fully constrained over two time constants before the flare. This
greatly reduces the sudden pitch changes at flare

libraries/AP_TECS/AP_TECS.cpp

@@ -2,6 +2,7 @@
 
 #include "AP_TECS.h"
 #include <AP_HAL.h>
+#include <stdio.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -393,6 +394,7 @@ void AP_TECS::_update_height_demand(void)
     // configured sink rate and adjust the demanded height to
     // be kinematically consistent with the height rate.
 	if (_flight_stage == FLIGHT_LAND_FINAL) {
+        _integ7_state = 0;
         if (_flare_counter == 0) {
             _hgt_rate_dem = _climb_rate;
             _land_hgt_dem = _hgt_dem_adj;
@@ -462,12 +464,18 @@ void AP_TECS::_update_energies(void)
 /*
   current time constant. It is lower in landing to try to give a precise approach
  */
-float AP_TECS::timeConstant(void)
+float AP_TECS::timeConstant(void) const
 {
     if (_flight_stage==FLIGHT_LAND_FINAL ||
         _flight_stage==FLIGHT_LAND_APPROACH) {
+        if (_landTimeConst < 0.1f) {
+            return 0.1f;
+        }
         return _landTimeConst;
     }
+    if (_timeConst < 0.1f) {
+        return 0.1f;
+    }
     return _timeConst;
 }
 
@@ -650,16 +658,25 @@ void AP_TECS::_update_pitch(void)
 
     // Calculate integrator state, constraining input if pitch limits are exceeded
     float integ7_input = SEB_error * _integGain;
-    if (_pitch_dem_unc > _PITCHmaxf) 
+    if (_pitch_dem > _PITCHmaxf) 
     {
-        integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc);
+        integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem);
     }
-    else if (_pitch_dem_unc < _PITCHminf)
+    else if (_pitch_dem < _PITCHminf)
     {
-        integ7_input = max(integ7_input, _PITCHminf - _pitch_dem_unc);
+        integ7_input = max(integ7_input, _PITCHminf - _pitch_dem);
     }
     _integ7_state = _integ7_state + integ7_input * _DT;
 
+#if 0
+	if (_flight_stage == FLIGHT_LAND_FINAL && fabsf(_climb_rate) > 0.2f) {
+        ::printf("_hgt_rate_dem=%.1f _hgt_dem_adj=%.1f climb=%.1f _flare_counter=%u _pitch_dem=%.1f SEB_dem=%.2f SEBdot_dem=%.2f SEB_error=%.2f SEBdot_error=%.2f\n",
+                 _hgt_rate_dem, _hgt_dem_adj, _climb_rate, _flare_counter, degrees(_pitch_dem),
+                 SEB_dem, SEBdot_dem, SEB_error, SEBdot_error);
+    }
+#endif
+
+
     // Apply max and min values for integrator state that will allow for no more than 
 	// 5deg of saturation. This allows for some pitch variation due to gusts before the 
 	// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
@@ -697,6 +714,10 @@ void AP_TECS::_update_pitch(void)
 	{
 		_pitch_dem = _last_pitch_dem - ptchRateIncr;
 	}
+
+    // re-constrain pitch demand
+    _pitch_dem = constrain_float(_pitch_dem, _PITCHminf, _PITCHmaxf);
+
 	_last_pitch_dem = _pitch_dem;
 }
 
@@ -784,6 +805,25 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
 
         // and allow zero throttle
         _THRminf = 0;
+    } else if (flight_stage == FLIGHT_LAND_APPROACH && (-_climb_rate) > _land_sink) {
+        // constrain the pitch in landing as we get close to the flare
+        // point. Use a simple linear limit from 15 meters after the
+        // landing point
+        float time_to_flare = (- hgt_afe / _climb_rate) - aparm.land_flare_sec;
+        if (time_to_flare < 0) {
+            // we should be flaring already
+            _PITCHminf = max(_PITCHminf, aparm.land_pitch_cd * 0.01f);
+        } else if (time_to_flare < timeConstant()*2) {
+            // smoothly move the min pitch to the flare min pitch over
+            // twice the time constant
+            float p = time_to_flare/(2*timeConstant());
+            float pitch_limit_cd = p*aparm.pitch_limit_min_cd + (1-p)*aparm.land_pitch_cd;
+#if 0
+            ::printf("ttf=%.1f hgt_afe=%.1f _PITCHminf=%.1f pitch_limit=%.1f climb=%.1f\n",
+                     time_to_flare, hgt_afe, _PITCHminf, pitch_limit_cd*0.01f, _climb_rate);
+#endif
+            _PITCHminf = max(_PITCHminf, pitch_limit_cd*0.01f);
+        }
     }
 	// convert to radians
 	_PITCHmaxf = radians(_PITCHmaxf);

libraries/AP_TECS/AP_TECS.h

@@ -283,7 +283,7 @@ private:
 	AverageFilterFloat_Size5 _vdot_filter;
 
 	// current time constant
-	float timeConstant(void);
+	float timeConstant(void) const;
 };
 
 #define TECS_LOG_FORMAT(msg) { msg, sizeof(AP_TECS::log_TECS_Tuning),	\