From 060f5530979585140f89dde345a60d32b1786c33 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 15 Mar 2015 13:52:17 +1100 Subject: [PATCH] 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 | 50 +++++++++++++++++++++++++++++++---- libraries/AP_TECS/AP_TECS.h | 2 +- 2 files changed, 46 insertions(+), 6 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 3099df5509..d631670203 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -2,6 +2,7 @@ #include "AP_TECS.h" #include +#include 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); diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index cd4331a1d2..5770299206 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/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), \