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 2022-01-28 06:08:24 +10:30 committed by Randy Mackay
parent 908b3ed659
commit fc97d0b976
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -658,13 +658,13 @@ void AC_PosControl::update_xy_controller()
// Acceleration Controller
// limit acceleration using maximum lean angles
_limit_vector.x = 0.0f;
_limit_vector.y = 0.0f;
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), 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.x = _accel_target.x;
_limit_vector.y = _accel_target.y;
// 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