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

AC_WPNav: sanity check wp accel max vs lean angle max

This commit is contained in:
Leonard Hall 2017-11-16 16:13:53 +09:00 committed by Randy Mackay
parent dadc6a63c4
commit c59dc21c8b
1 changed files with 1 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -144,6 +144,7 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC
// sanity check some parameters // sanity check some parameters
_loiter_speed_cms = MAX(_loiter_speed_cms, WPNAV_LOITER_SPEED_MIN); _loiter_speed_cms = MAX(_loiter_speed_cms, WPNAV_LOITER_SPEED_MIN);
_wp_accel_cms = MIN(_wp_accel_cms, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f)));
_wp_radius_cm = MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN); _wp_radius_cm = MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN);
} }
@ -389,14 +390,6 @@ 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();
_pos_control.clear_desired_velocity_ff_z(); _pos_control.clear_desired_velocity_ff_z();