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

Copter: poshold wind effect comp limited to 2/3rds of angle max

This commit is contained in:
Randy Mackay 2020-09-08 21:12:53 +09:00
parent 3cc985332d
commit 5b71726c73
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/mode_poshold.cpp
View File

@ -22,6 +22,7 @@
// definitions that are independent of main loop rate
#define POSHOLD_STICK_RELEASE_SMOOTH_ANGLE 1800 // max angle required (in centi-degrees) after which the smooth stick release effect is applied
#define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX 10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s
#define POSHOLD_WIND_COMP_LEAN_PCT_MAX 0.6666f // wind compensation no more than 2/3rds of angle max to ensure pilot can always override
// poshold_init - initialise PosHold controller
bool ModePosHold::init(bool ignore_checks)
@ -595,6 +596,13 @@ void ModePosHold::update_wind_comp_estimate()
// low pass filter the position controller's lean angle output
wind_comp_ef.y = (1.0f-TC_WIND_COMP)*wind_comp_ef.y + TC_WIND_COMP*accel_target.y;
}
// limit acceleration
const float accel_lim_cmss = tanf(radians(POSHOLD_WIND_COMP_LEAN_PCT_MAX * copter.aparm.angle_max * 0.01f)) * 981.0f;
const float wind_comp_ef_len = wind_comp_ef.length();
if (!is_zero(accel_lim_cmss) && (wind_comp_ef_len > accel_lim_cmss)) {
wind_comp_ef *= accel_lim_cmss / wind_comp_ef_len;
}
}
// get_wind_comp_lean_angles - retrieve wind compensation angles in body frame roll and pitch angles