Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_WPNav: integrate scurve snap

Browse Source
This commit is contained in:
Randy Mackay 2022-03-04 15:27:58 +09:00
parent c233f114bd
commit 0dd9a8028b
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -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_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_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_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 #define AR_WPNAV_ACCEL_MAX 20.0 // acceleration used when user has specified no acceleration limit
const AP_Param::GroupInfo AR_WPNav::var_info[] = { 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(),
_pos_control.get_speed_max(), // speed up (not used) _pos_control.get_speed_max(), // speed up (not used)
_pos_control.get_speed_max(), // speed down (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) _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()); _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(),
_pos_control.get_speed_max(), // speed up (not used) _pos_control.get_speed_max(), // speed up (not used)
_pos_control.get_speed_max(), // speed down (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) _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()); _pos_control.get_jerk_max());
// next destination provided so fast waypoint // next destination provided so fast waypoint
@ -439,7 +439,7 @@ void AR_WPNav::advance_wp_target_along_track(const Location &current_loc, float
// update target position, velocity and acceleration // update target position, velocity and acceleration
const float wp_radius = MAX(_radius, _turn_radius); 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 // pass new target to the position controller
init_pos_control_if_necessary(); init_pos_control_if_necessary();