Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2019-04-14 16:03:24 +10:00
parent 4fd3008b4e
commit 4f243aca02
2 changed files with 30 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_TECS/AP_TECS.cpp
View File

@ -1013,7 +1013,12 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
_PITCHminf = constrain_float(_PITCHminf, -_pitch_max_limit, _PITCHmaxf); _PITCHminf = constrain_float(_PITCHminf, -_pitch_max_limit, _PITCHmaxf);
_pitch_max_limit = 90; _pitch_max_limit = 90;
} }
if (!_landing.is_on_approach()) {
// reset land pitch min when not landing
_land_pitch_min = _PITCHminf;
}
if (_landing.is_flaring()) { if (_landing.is_flaring()) {
// in flare use min pitch from LAND_PITCH_CD // in flare use min pitch from LAND_PITCH_CD
_PITCHminf = MAX(_PITCHminf, _landing.get_pitch_cd() * 0.01f); _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 (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) { if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) {
// we have reached our target speed in takeoff, allow for // 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)_TAS_rate_dem,
(double)logging.SKE_weighting, (double)logging.SKE_weighting,
_flags_byte); _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, now,
(double)degrees(_PITCHmaxf),
(double)degrees(_PITCHminf),
(double)logging.SKE_error, (double)logging.SKE_error,
(double)logging.SPE_error, (double)logging.SPE_error,
(double)logging.SEB_delta, (double)logging.SEB_delta,
(double)load_factor, (double)load_factor);
(double)degrees(_PITCHmaxf),
(double)degrees(_PITCHminf));
} }

2
libraries/AP_TECS/AP_TECS.h
View File

@ -318,6 +318,8 @@ private:
float _distance_beyond_land_wp; float _distance_beyond_land_wp;
float _land_pitch_min = -90;
// internal variables to be logged // internal variables to be logged
struct { struct {
float SKE_weighting; float SKE_weighting;