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

AC_WPNav: use angle/accel functions

This commit is contained in:
Andrew Tridgell 2022-03-15 09:25:36 +11:00
parent ca4151be2c
commit f824be726c
2 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_WPNav/AC_Loiter.cpp
View File

@ -211,7 +211,7 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit*100.0f);
gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, LOITER_SPEED_MIN);
float pilot_acceleration_max = GRAVITY_MSS*100.0f * tanf(radians(get_angle_max_cd()*0.01f));
float pilot_acceleration_max = angle_to_accel(get_angle_max_cd()*0.01) * 100;
// range check nav_dt
if (nav_dt < 0) {

2
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -150,7 +150,7 @@ void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point)
// sanity check parameters
// check _wp_accel_cmss is reasonable
_scurve_accel_corner = GRAVITY_MSS * 100.0f * tanf(ToRad(_pos_control.get_lean_angle_max_cd() * 0.01f));
_scurve_accel_corner = angle_to_accel(_pos_control.get_lean_angle_max_cd() * 0.01) * 100;
const float wp_accel_cmss = MIN(_wp_accel_cmss, _scurve_accel_corner);
_wp_accel_cmss.set_and_save_ifchanged((_wp_accel_cmss <= 0) ? WPNAV_ACCELERATION : wp_accel_cmss);