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

AR_WPNav: jerk_max maintained in position controller

This commit is contained in:
Randy Mackay 2022-01-05 20:24:16 +09:00
parent 54eee7d311
commit 537874c075
2 changed files with 4 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -220,7 +220,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location
MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()), MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()),
_pos_control.get_accel_max(), // vertical accel (not used) _pos_control.get_accel_max(), // vertical accel (not used)
1.0, // jerk time 1.0, // jerk time
_scurve_jerk); _pos_control.get_jerk_max());
} }
// handle next destination // handle next destination
@ -247,7 +247,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location
MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()), MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()),
_pos_control.get_accel_max(), // vertical accel (not used) _pos_control.get_accel_max(), // vertical accel (not used)
1.0, // jerk time 1.0, // jerk time
_scurve_jerk); _pos_control.get_jerk_max());
// next destination provided so fast waypoint // next destination provided so fast waypoint
_fast_waypoint = true; _fast_waypoint = true;
@ -393,8 +393,8 @@ void AR_WPNav::advance_wp_target_along_track(const Location &current_loc, float
} }
// change s-curve time speed with a time constant of maximum acceleration / maximum jerk // change s-curve time speed with a time constant of maximum acceleration / maximum jerk
float track_scaler_tc = 1.0f; float track_scaler_tc = 1.0f;
if (is_positive(_scurve_jerk)) { if (is_positive(_pos_control.get_jerk_max())) {
track_scaler_tc = _pos_control.get_accel_max() / _scurve_jerk; track_scaler_tc = _pos_control.get_accel_max() / _pos_control.get_jerk_max();
} }
_track_scalar_dt += (track_scaler_dt - _track_scalar_dt) * (dt / track_scaler_tc); _track_scalar_dt += (track_scaler_dt - _track_scalar_dt) * (dt / track_scaler_tc);

1
libraries/AR_WPNav/AR_WPNav.h
View File

@ -152,7 +152,6 @@ protected:
SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory
SCurve _scurve_this_leg; // current scurve trajectory SCurve _scurve_this_leg; // current scurve trajectory
SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory
float _scurve_jerk; // scurve jerk max in m/s/s/s
bool _fast_waypoint; // true if vehicle will stop at the next waypoint bool _fast_waypoint; // true if vehicle will stop at the next waypoint
bool _pivot_at_next_wp; // true if vehicle should pivot at next waypoint bool _pivot_at_next_wp; // true if vehicle should pivot at next waypoint
bool _overspeed_enabled; // if true scurve's position target will speedup to catch vehicles travelling faster than WP_SPEED bool _overspeed_enabled; // if true scurve's position target will speedup to catch vehicles travelling faster than WP_SPEED