AP_TECS: sync TECS with plane4.0

libraries/AP_TECS/AP_TECS.cpp

@@ -911,7 +911,7 @@ void AP_TECS::_update_pitch(void)
 void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
 {
     // Initialise states and variables if DT > 1 second or in climbout
-    if (_DT > 1.0f)
+    if (_DT > 1.0f || _need_reset)
     {
         _integTHR_state      = 0.0f;
         _integSEB_state      = 0.0f;
@@ -927,6 +927,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
         _flags.reached_speed_takeoff = false;
         _DT                = 0.1f; // when first starting TECS, use a
         // small time constant
+        _need_reset = false;
     }
     else if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)
     {
@@ -1067,6 +1068,13 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
         _PITCHminf = MAX(_land_pitch_min, _PITCHminf);
     }
 
+    if (_landing.is_flaring()) {
+        // ensure we don't violate the limits for flare pitch
+        if (_land_pitch_max != 0) {
+            _PITCHmaxf = MIN(_land_pitch_max, _PITCHmaxf);
+        }
+    }
+
     if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
         if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) {
             // we have reached our target speed in takeoff, allow for

libraries/AP_TECS/AP_TECS.h

@@ -117,6 +117,11 @@ public:
         _use_synthetic_airspeed_once = true;
     }
 
+    // reset on next loop
+    void reset(void) override {
+        _need_reset = true;
+    }
+
     // this supports the TECS_* user settable parameters
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -320,6 +325,9 @@ private:
 
     float _land_pitch_min = -90;
 
+    // need to reset on next loop
+    bool _need_reset;
+
     // internal variables to be logged
     struct {
         float SKE_weighting;