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: Rework pitch limiting in QuadPlane::assign_tilt_to_fwd_thr

Browse Source 
Don't unnecessarily increase the forward pitch limit when position control forward action is saturated.
This commit is contained in:
Paul Riseborough 2023-09-13 22:45:02 +10:00 committed by Andrew Tridgell
parent acf8dbaeee
commit 3866f2b4b5
1 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduPlane/quadplane.cpp
View File

@ -2985,13 +2985,18 @@ void QuadPlane::assign_tilt_to_fwd_thr(void) {
float fwd_tilt_rad = radians(constrain_float(-0.01f * (float)plane.nav_pitch_cd, 0.0f, 45.0f)); float fwd_tilt_rad = radians(constrain_float(-0.01f * (float)plane.nav_pitch_cd, 0.0f, 45.0f));
q_fwd_throttle = MIN(q_fwd_thr_gain * tanf(fwd_tilt_rad), 1.0f); q_fwd_throttle = MIN(q_fwd_thr_gain * tanf(fwd_tilt_rad), 1.0f);
// Slowly relax forward tilt limit if the position controller is saturating in the forward direction because // Relax forward tilt limit if the position controller is saturating in the forward direction because
// the forward thrust motor could be failed // the forward thrust motor could be failed
const float fwd_tilt_range_cd = (float)aparm.angle_max - 100.0f * q_fwd_pitch_lim; const float fwd_tilt_range_cd = (float)aparm.angle_max - 100.0f * q_fwd_pitch_lim;
if (is_positive(fwd_tilt_range_cd)) { if (is_positive(fwd_tilt_range_cd)) {
// rate limit the forward tilt change to slew between the motor good and motor failed
// value over 10 seconds
const float fwd_pitch_lim_cd_tgt = plane.quadplane.pos_control->get_fwd_pitch_is_limited() ? (float)aparm.angle_max : 100.0f * q_fwd_pitch_lim; const float fwd_pitch_lim_cd_tgt = plane.quadplane.pos_control->get_fwd_pitch_is_limited() ? (float)aparm.angle_max : 100.0f * q_fwd_pitch_lim;
const float delta_max = 0.1f * fwd_tilt_range_cd * plane.G_Dt; const float delta_max = 0.1f * fwd_tilt_range_cd * plane.G_Dt;
q_fwd_pitch_lim_cd += constrain_float((fwd_pitch_lim_cd_tgt - q_fwd_pitch_lim_cd), -delta_max, delta_max); q_fwd_pitch_lim_cd += constrain_float((fwd_pitch_lim_cd_tgt - q_fwd_pitch_lim_cd), -delta_max, delta_max);
// Don't let the forward pitch limit be more than the forward pitch demand before limiting to
// avoid opening up the limit more than necessary
q_fwd_pitch_lim_cd = MIN(q_fwd_pitch_lim_cd, MAX(-(float)plane.nav_pitch_cd, 100.0f * q_fwd_pitch_lim));
} else { } else {
// take the lesser of the two limits // take the lesser of the two limits
q_fwd_pitch_lim_cd = (float)aparm.angle_max; q_fwd_pitch_lim_cd = (float)aparm.angle_max;
@ -3017,7 +3022,7 @@ void QuadPlane::assign_tilt_to_fwd_thr(void) {
// Diagnostics logging - remove when feature is fully flight tested. // Diagnostics logging - remove when feature is fully flight tested.
AP::logger().WriteStreaming("FWDT", AP::logger().WriteStreaming("FWDT",
"TimeUS,fts,qfpcd,npllcd,npcd,qft", // labels "TimeUS,fts,qfplcd,npllcd,npcd,qft", // labels
"Qfffff", // fmt "Qfffff", // fmt
AP_HAL::micros64(), AP_HAL::micros64(),
(double)fwd_thr_scaler, (double)fwd_thr_scaler,