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

AC_WPNav: Increase corner speeds

This commit is contained in:
Leonard Hall 2022-03-02 13:59:00 +10:30 committed by Randy Mackay
parent 7673948d2a
commit 93aea67810
2 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -150,7 +150,8 @@ void AC_WPNav::wp_and_spline_init(float speed_cms)
// sanity check parameters
// check _wp_accel_cmss is reasonable
const float wp_accel_cmss = MIN(_wp_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max_cd() * 0.01f)));
_scurve_accel_corner = GRAVITY_MSS * 100.0f * tanf(ToRad(_pos_control.get_lean_angle_max_cd() * 0.01f));
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);
// check _wp_radius_cm is reasonable
@ -497,7 +498,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
if (!_this_leg_is_spline) {
// update target position, velocity and acceleration
target_pos = _origin;
s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _flags.fast_waypoint, _track_scalar_dt * vel_time_scalar * dt, target_pos, target_vel, target_accel);
s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _scurve_accel_corner, _flags.fast_waypoint, _track_scalar_dt * vel_time_scalar * dt, target_pos, target_vel, target_accel);
} else {
// splinetarget_vel
target_vel = curr_target_vel;

1
libraries/AC_WPNav/AC_WPNav.h
View File

@ -240,6 +240,7 @@ protected:
SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory
SCurve _scurve_this_leg; // current scurve trajectory
SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory
float _scurve_accel_corner; // scurve maximum corner acceleration in m/s/s
float _scurve_jerk; // scurve jerk max in m/s/s/s
float _scurve_snap; // scurve snap in m/s/s/s/s