diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index ec7da187e0..df92b124c8 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -30,6 +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 const AP_Param::GroupInfo AR_WPNav::var_info[] = { @@ -246,7 +247,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location _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(), // vertical accel (not used) - 1.0, // jerk time + AR_WPNAV_JERK_TIME_SEC, // jerk time _pos_control.get_jerk_max()); } @@ -273,7 +274,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location _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(), // vertical accel (not used) - 1.0, // jerk time + AR_WPNAV_JERK_TIME_SEC, // jerk time _pos_control.get_jerk_max()); // next destination provided so fast waypoint