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;