From 8c6c2e46ccf26ce60b5d727988b77aea4893921d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 14 Mar 2017 20:03:37 +0900 Subject: [PATCH] AC_WPNav: protect against LOIT_SPEED divide-by-zero --- libraries/AC_WPNav/AC_WPNav.cpp | 15 ++++++++------- libraries/AC_WPNav/AC_WPNav.h | 2 +- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index c4f10d335b..d6b42cd8ab 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -46,7 +46,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = { // @DisplayName: Loiter Horizontal Maximum Speed // @Description: Defines the maximum speed in cm/s which the aircraft will travel horizontally while in loiter mode // @Units: cm/s - // @Range: 0 2000 + // @Range: 20 2000 // @Increment: 50 // @User: Standard AP_GROUPINFO("LOIT_SPEED", 4, AC_WPNav, _loiter_speed_cms, WPNAV_LOITER_SPEED), @@ -140,6 +140,9 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC _flags.recalc_wp_leash = false; _flags.new_wp_destination = false; _flags.segment_type = SEGMENT_STRAIGHT; + + // sanity check loiter speed + _loiter_speed_cms = MAX(_loiter_speed_cms, WPNAV_LOITER_SPEED_MIN); } /// @@ -181,6 +184,9 @@ void AC_WPNav::init_loiter_target() // initialise position controller _pos_control.init_xy_controller(); + // sanity check loiter speed + _loiter_speed_cms = MAX(_loiter_speed_cms, WPNAV_LOITER_SPEED_MIN); + // initialise pos controller speed and acceleration _pos_control.set_speed_xy(_loiter_speed_cms); _pos_control.set_accel_xy(_loiter_accel_cmss); @@ -232,18 +238,13 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit) // calculate a loiter speed limit which is the minimum of the value set by the WPNAV_LOITER_SPEED // parameter and the value set by the EKF to observe optical flow limits float gnd_speed_limit_cms = MIN(_loiter_speed_cms,ekfGndSpdLimit*100.0f); - gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, 10.0f); + gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, WPNAV_LOITER_SPEED_MIN); // range check nav_dt if( nav_dt < 0 ) { return; } - // check loiter speed and avoid divide by zero - if(gnd_speed_limit_cms < WPNAV_LOITER_SPEED_MIN) { - gnd_speed_limit_cms = WPNAV_LOITER_SPEED_MIN; - } - _pos_control.set_speed_xy(gnd_speed_limit_cms); _pos_control.set_accel_xy(_loiter_accel_cmss); _pos_control.set_jerk_xy(_loiter_jerk_max_cmsss); diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 50b55bfac2..45ac6cc686 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -15,7 +15,7 @@ #define WPNAV_ACCELERATION_MIN 50.0f // minimum acceleration in cm/s/s - used for sanity checking _wp_accel parameter #define WPNAV_LOITER_SPEED 500.0f // default loiter speed in cm/s -#define WPNAV_LOITER_SPEED_MIN 100.0f // minimum loiter speed in cm/s +#define WPNAV_LOITER_SPEED_MIN 20.0f // minimum loiter speed in cm/s #define WPNAV_LOITER_ACCEL 250.0f // default acceleration in loiter mode #define WPNAV_LOITER_ACCEL_MIN 25.0f // minimum acceleration in loiter mode #define WPNAV_LOITER_JERK_MAX_DEFAULT 1000.0f // maximum jerk in cm/s/s/s in loiter mode