diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index ece7448ddb..93053c696c 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -287,6 +287,9 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f _track_desired = 0; _target = origin; _reached_destination = false; + + // reset limited speed to zero to slow initial acceleration away from wp + _limited_speed_xy_cms = 0; } /// advance_target_along_track - move target location along track from origin to destination diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 21a230787b..d5dec7c0d3 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -12,7 +12,7 @@ // loiter maximum velocities and accelerations #define WPNAV_LOITER_SPEED 750.0f // maximum default loiter speed in cm/s -#define MAX_LOITER_POS_ACCEL 500.0f // defines the velocity vs distant curve. maximum acceleration in cm/s/s that loiter position controller asks for from acceleration controller +#define MAX_LOITER_POS_ACCEL 250.0f // defines the velocity vs distant curve. maximum acceleration in cm/s/s that loiter position controller asks for from acceleration controller #define MAX_LOITER_VEL_ACCEL 800.0f // max acceleration in cm/s/s that the loiter velocity controller will ask from the lower accel controller. // should be 1.5 times larger than MAX_LOITER_POS_ACCEL. // max acceleration = max lean angle * 980 * pi / 180. i.e. 23deg * 980 * 3.141 / 180 = 393 cm/s/s @@ -28,7 +28,7 @@ #define WPNAV_ALT_HOLD_P 2.0f // hard coded estimate of throttle controller's altitude hold's P gain. To-Do: retrieve gain from throttle controller #define WPNAV_ALT_HOLD_ACCEL_MAX 250.0f // hard coded estimate of throttle controller's maximum acceleration in cm/s. To-Do: retrieve from throttle controller -#define WPNAV_WP_ACCELERATION 200.0f // acceleration in cm/s/s used to increase the speed of the intermediate point up to it's maximum speed held in _speed_xy_cms +#define WPNAV_WP_ACCELERATION 500.0f // acceleration in cm/s/s used to increase the speed of the intermediate point up to it's maximum speed held in _speed_xy_cms class AC_WPNav {