diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index f5dac7082d..64c560203d 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -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()), _pos_control.get_accel_max(), // vertical accel (not used) 1.0, // jerk time - _scurve_jerk); + _pos_control.get_jerk_max()); } // 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()), _pos_control.get_accel_max(), // vertical accel (not used) 1.0, // jerk time - _scurve_jerk); + _pos_control.get_jerk_max()); // next destination provided so fast waypoint _fast_waypoint = true; @@ -393,8 +393,8 @@ void AR_WPNav::advance_wp_target_along_track(const Location ¤t_loc, float } // change s-curve time speed with a time constant of maximum acceleration / maximum jerk float track_scaler_tc = 1.0f; - if (is_positive(_scurve_jerk)) { - track_scaler_tc = _pos_control.get_accel_max() / _scurve_jerk; + if (is_positive(_pos_control.get_jerk_max())) { + 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); diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index df4c7ac6fa..06c4ab616e 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -152,7 +152,6 @@ 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_jerk; // scurve jerk max in m/s/s/s 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 _overspeed_enabled; // if true scurve's position target will speedup to catch vehicles travelling faster than WP_SPEED