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

AC_WPNav: limit WPNAV_ACCEL to that implied by ANGLE_MAX

Browse Source 
this prevents an overshoot and backtracking in the navigation code
when WPNAV_ACCEL is unachievable due to an angle limit
This commit is contained in:
Andrew Tridgell 2016-04-28 17:47:50 +10:00
parent c7664291f9
commit 4908350ccb
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -393,6 +393,14 @@ void AC_WPNav::wp_and_spline_init()
_wp_accel_cms.set_and_save(WPNAV_ACCELERATION); _wp_accel_cms.set_and_save(WPNAV_ACCELERATION);
} }
// also limit the accel using the maximum lean angle. This
// prevents the navigation controller from trying to move the
// target point at an unachievable rate
float accel_limit_cms = GRAVITY_MSS * 100 * tanf(radians(_attitude_control.lean_angle_max()*0.01f));
if (_wp_accel_cms > accel_limit_cms) {
_wp_accel_cms.set(accel_limit_cms);
}
// initialise position controller // initialise position controller
_pos_control.init_xy_controller(); _pos_control.init_xy_controller();