From 0dd9a8028b5ea1ccbd9f27e78c64f24341914133 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 4 Mar 2022 15:27:58 +0900 Subject: [PATCH] AR_WPNav: integrate scurve snap --- libraries/AR_WPNav/AR_WPNav.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 3dd77c4326..1d4bbb5b6e 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -30,7 +30,7 @@ extern const AP_HAL::HAL& hal; #define AR_WPNAV_SPEED_UPDATE_MIN_MS 500 // max speed cannot be updated more than once in this many milliseconds #define AR_WPNAV_RADIUS_DEFAULT 2.0f #define AR_WPNAV_OVERSPEED_RATIO_MAX 5.0f // if _overspeed_enabled the vehicle may travel as quickly as 5x WP_SPEED -#define AR_WPNAV_JERK_TIME_SEC 0.1 // jerk time (the time taken for jerk to climb to its maximum) is hard-coded to 0.1 +#define AR_WPNAV_SNAP_MAX 15.0f // scurve snap (change in jerk) in m/s/s/s/s #define AR_WPNAV_ACCEL_MAX 20.0 // acceleration used when user has specified no acceleration limit const AP_Param::GroupInfo AR_WPNav::var_info[] = { @@ -250,9 +250,9 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location _pos_control.get_speed_max(), _pos_control.get_speed_max(), // speed up (not used) _pos_control.get_speed_max(), // speed down (not used) - MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()), + _pos_control.get_accel_max(), // forward back acceleration _pos_control.get_accel_max(), // vertical accel (not used) - AR_WPNAV_JERK_TIME_SEC, // jerk time + AR_WPNAV_SNAP_MAX, // snap _pos_control.get_jerk_max()); } @@ -277,9 +277,9 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location _pos_control.get_speed_max(), _pos_control.get_speed_max(), // speed up (not used) _pos_control.get_speed_max(), // speed down (not used) - MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()), + _pos_control.get_accel_max(), // forward back acceleration _pos_control.get_accel_max(), // vertical accel (not used) - AR_WPNAV_JERK_TIME_SEC, // jerk time + AR_WPNAV_SNAP_MAX, // snap _pos_control.get_jerk_max()); // next destination provided so fast waypoint @@ -439,7 +439,7 @@ void AR_WPNav::advance_wp_target_along_track(const Location ¤t_loc, float // update target position, velocity and acceleration const float wp_radius = MAX(_radius, _turn_radius); - bool s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, wp_radius, _fast_waypoint, _track_scalar_dt * dt, target_pos_3d_ftype, target_vel, target_accel); + bool s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, wp_radius, _pos_control.get_lat_accel_max(), _fast_waypoint, _track_scalar_dt * dt, target_pos_3d_ftype, target_vel, target_accel); // pass new target to the position controller init_pos_control_if_necessary();