From 4f243aca02770eb604f9875b0ff3fdfb44a2a43f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 14 Apr 2019 16:03:24 +1000
Subject: [PATCH] AP_TECS: prevent rapid changing of pitch limits on landing
 approach

when on landing approach we estimate time to flare based on two noisy
numbers, the vertical speed and height above ground. With noisy
rangefinders this can change rapidly, which resulted in the pitch
limit changing rapidly, leading to a porpoising movement

this limits the rate of change, and also prevents it coming down once
it has nosed up due to pending flare on approach
---
 libraries/AP_TECS/AP_TECS.cpp | 33 ++++++++++++++++++++++++++++-----
 libraries/AP_TECS/AP_TECS.h   |  2 ++
 2 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
index ce01a15029..5c9e90a298 100644
--- a/libraries/AP_TECS/AP_TECS.cpp
+++ b/libraries/AP_TECS/AP_TECS.cpp
@@ -1013,7 +1013,12 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
         _PITCHminf = constrain_float(_PITCHminf, -_pitch_max_limit, _PITCHmaxf);
         _pitch_max_limit = 90;
     }
-        
+
+    if (!_landing.is_on_approach()) {
+        // reset land pitch min when not landing
+        _land_pitch_min = _PITCHminf;
+    }
+    
     if (_landing.is_flaring()) {
         // in flare use min pitch from LAND_PITCH_CD
         _PITCHminf = MAX(_PITCHminf, _landing.get_pitch_cd() * 0.01f);
@@ -1047,6 +1052,21 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
         }
     }
 
+    if (_landing.is_on_approach()) {
+        // don't allow the lower bound of pitch to decrease, nor allow
+        // it to increase rapidly. This prevents oscillation of pitch
+        // demand while in landing approach based on rapidly changing
+        // time to flare estimate
+        if (_land_pitch_min <= -90) {
+            _land_pitch_min = _PITCHminf;
+        }
+        const float flare_pitch_range = 20;
+        const float delta_per_loop = (flare_pitch_range/_landTimeConst) * _DT;
+        _PITCHminf = MIN(_PITCHminf, _land_pitch_min+delta_per_loop);
+        _land_pitch_min = MAX(_land_pitch_min, _PITCHminf);
+        _PITCHminf = MAX(_land_pitch_min, _PITCHminf);
+    }
+
     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
@@ -1125,12 +1145,15 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
         (double)_TAS_rate_dem,
         (double)logging.SKE_weighting,
         _flags_byte);
-    AP::logger().Write("TEC2", "TimeUS,KErr,PErr,EDelta,LF,PMax,PMin", "Qffffff",
+    AP::logger().Write("TEC2", "TimeUS,pmax,pmin,KErr,PErr,EDelta,LF",
+                       "s------",
+                       "F------",
+                       "Qffffff",
                        now,
+                       (double)degrees(_PITCHmaxf),
+                       (double)degrees(_PITCHminf),
                        (double)logging.SKE_error,
                        (double)logging.SPE_error,
                        (double)logging.SEB_delta,
-                       (double)load_factor,
-                       (double)degrees(_PITCHmaxf),
-                       (double)degrees(_PITCHminf));
+                       (double)load_factor);
 }
diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
index 4c0eb8c4db..2f91f33656 100644
--- a/libraries/AP_TECS/AP_TECS.h
+++ b/libraries/AP_TECS/AP_TECS.h
@@ -318,6 +318,8 @@ private:
 
     float _distance_beyond_land_wp;
 
+    float _land_pitch_min = -90;
+
     // internal variables to be logged
     struct {
         float SKE_weighting;