Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: added airspeed based pitch limit check

Browse Source 
prevent using too much pitch when at low airspeed, which can lead to
severe instability in SLT quadplanes
This commit is contained in:
Andrew Tridgell 2022-03-16 08:12:20 +11:00
parent eabdaae1e7
commit 6ebefbdb16
1 changed files with 60 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
ArduPlane/quadplane.cpp
View File

@ -3900,25 +3900,75 @@ bool SLT_Transition::active() const
return quadplane.assisted_flight && ((transition_state == TRANSITION_AIRSPEED_WAIT) || (transition_state == TRANSITION_TIMER));
}
/*
limit VTOL roll/pitch in POSITION1, POSITION2 and waypoint controller. This serves three roles:
1) an expanding envelope limit on pitch to prevent sudden pitch at the start of a back transition
2) limiting roll and pitch down to the Q_ANGLE_MAX, as the accel limits may push us beyond that for pitch up.
This is needed as the position controller doesn't have separate limits for pitch and roll
3) preventing us pitching up a lot when our airspeed may be low
enough that the real airspeed may be negative, which would result
in reversed control surfaces
*/
bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_cd)
{
if (quadplane.back_trans_pitch_limit_ms <= 0) {
// disabled
return false;
bool ret = false;
const int16_t angle_max = quadplane.aparm.angle_max;
/*
we always limit roll to Q_ANGLE_MAX
*/
int32_t new_roll_cd = constrain_int32(roll_cd, -angle_max, angle_max);
if (new_roll_cd != roll_cd) {
roll_cd = new_roll_cd;
ret = true;
}
/*
always limit pitch down to Q_ANGLE_MAX. We need to do this as
the position controller accel limits may exceed this limit
*/
if (pitch_cd < -angle_max) {
pitch_cd = -angle_max;
ret = true;
}
/*
prevent trying to fly backwards (negative airspeed) at high
pitch angles, which can result in a high degree of instability
in SLT aircraft. This can happen with a tailwind in a back
transition, where the position controller (which is unaware of
airspeed) demands high pitch to hit the desired landing point
*/
float airspeed;
if (pitch_cd > angle_max &&
plane.ahrs.airspeed_estimate(airspeed) && airspeed < 0.5 * plane.aparm.airspeed_min) {
const float max_limit_cd = linear_interpolate(angle_max, 4500,
airspeed,
0, 0.5 * plane.aparm.airspeed_min);
if (pitch_cd > max_limit_cd) {
pitch_cd = max_limit_cd;
ret = true;
}
}
if (quadplane.back_trans_pitch_limit_ms <= 0) {
// time based pitch envelope disabled
return ret;
}
const uint32_t limit_time_ms = quadplane.back_trans_pitch_limit_ms;
const uint32_t dt = AP_HAL::millis() - last_fw_mode_ms;
if (last_fw_mode_ms == 0 || dt > limit_time_ms) {
// we are beyond the time limit, don't apply envelope
last_fw_mode_ms = 0;
// past transition period, only constrain roll
int16_t limit_cd = MIN(quadplane.aparm.angle_max,plane.aparm.pitch_limit_max_cd);
roll_cd = constrain_int32(roll_cd, -limit_cd, limit_cd);
return false;
return ret;
}
// we limit pitch during initial transition
const float max_limit_cd = linear_interpolate(MAX(last_fw_nav_pitch_cd,0), MIN(quadplane.aparm.angle_max,plane.aparm.pitch_limit_max_cd),
const float max_limit_cd = linear_interpolate(MAX(last_fw_nav_pitch_cd,0), MIN(angle_max,plane.aparm.pitch_limit_max_cd),
dt,
0, limit_time_ms);
@ -3938,7 +3988,7 @@ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_
to prevent inability to progress to position if moving from a loiter
to landing
*/
const float min_limit_cd = linear_interpolate(MIN(last_fw_nav_pitch_cd,0), MAX(-quadplane.aparm.angle_max,plane.aparm.pitch_limit_min_cd),
const float min_limit_cd = linear_interpolate(MIN(last_fw_nav_pitch_cd,0), MAX(-angle_max,plane.aparm.pitch_limit_min_cd),
dt,
0, limit_time_ms);
@ -3947,7 +3997,7 @@ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_
return true;
}
return false;
return ret;
}
/*