From c0458b786a878741ca52d98f41d091d9de37b57b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 30 Mar 2014 11:47:58 +0900 Subject: [PATCH] AC_WPNav: check acceleration is non zero --- libraries/AC_WPNav/AC_WPNav.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index ea08e7f4cd..373757e604 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -56,7 +56,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = { // @DisplayName: Waypoint Acceleration // @Description: Defines the horizontal acceleration in cm/s/s used during missions // @Units: cm/s/s - // @Range: 0 980 + // @Range: 50 500 // @Increment: 10 // @User: Standard AP_GROUPINFO("ACCEL", 5, AC_WPNav, _wp_accel_cms, WPNAV_ACCELERATION), @@ -281,6 +281,11 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto _pos_delta_unit = pos_delta/_track_length; } + // check _wp_accel_cms is reasonable + if (_wp_accel_cms <= 0) { + _wp_accel_cms.set_and_save(WPNAV_ACCELERATION); + } + // initialise position controller speed and acceleration _pos_control.set_speed_xy(_wp_speed_cms); _pos_control.set_accel_xy(_wp_accel_cms); @@ -526,6 +531,11 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V // mission is "active" if wpnav has been called recently and vehicle reached the previous waypoint bool prev_segment_exists = (_flags.reached_destination && ((hal.scheduler->millis() - _wp_last_update) < 1000)); + // check _wp_accel_cms is reasonable to avoid divide by zero + if (_wp_accel_cms <= 0) { + _wp_accel_cms.set_and_save(WPNAV_ACCELERATION); + } + // segment start types // stop - vehicle is not moving at origin // straight-fast - vehicle is moving, previous segment is straight. vehicle will fly straight through the waypoint before beginning it's spline path to the next wp