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

AC_AttitudeControl: AC_PosControl: Prioritize crosstrack acceleration

This commit is contained in:
Leonard Hall 2021-12-14 12:44:49 +10:30 committed by Randy Mackay
parent 1e124ca957
commit 90db81354b
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -639,11 +639,13 @@ void AC_PosControl::update_xy_controller()
// Acceleration Controller
// limit acceleration using maximum lean angles
_limit_vector.xy().zero();
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max_cd(), get_lean_angle_max_cd());
float accel_max = GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f));
if (_accel_target.limit_length_xy(accel_max)) {
_limit_vector.xy() = _accel_target.xy();
// Define the limit vector before we constrain _accel_target
_limit_vector.xy() = _accel_target.xy();
if (!limit_accel_xy(_vel_desired.xy(), _accel_target.xy(), accel_max)) {
// _accel_target was not limited so we can zero the xy limit vector
_limit_vector.xy().zero();
}
// update angle targets that will be passed to stabilize controller