AC_WPNav: protect against divide-by-zero

ArduPilot · Sep 3, 2021 · b60028d · b60028d
1 parent 62062b6
commit b60028d
Showing 1 changed file with 6 additions and 2 deletions.
diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -151,12 +151,16 @@ void AC_WPNav::wp_and_spline_init(float speed_cms)
     // check _wp_radius_cm is reasonable
     _wp_radius_cm.set_and_save_ifchanged(MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN));
 
+    // check _wp_speed
+    _wp_speed_cms.set_and_save_ifchanged(MAX(_wp_speed_cms, WPNAV_WP_SPEED_MIN));
+
     // initialise position controller
     _pos_control.init_z_controller_stopping_point();
     _pos_control.init_xy_controller_stopping_point();
 
     // initialize the desired wp speed if not already done
     _wp_desired_speed_xy_cms = is_positive(speed_cms) ? speed_cms : _wp_speed_cms;
+    _wp_desired_speed_xy_cms = MAX(_wp_desired_speed_xy_cms, WPNAV_WP_SPEED_MIN);
 
     // initialise position controller speed and acceleration
     _pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss);
@@ -194,8 +198,8 @@ void AC_WPNav::wp_and_spline_init(float speed_cms)
 /// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation
 void AC_WPNav::set_speed_xy(float speed_cms)
 {
-    // range check target speed
-    if (speed_cms >= WPNAV_WP_SPEED_MIN) {
+    // range check target speed and protect against divide by zero
+    if (speed_cms >= WPNAV_WP_SPEED_MIN && is_positive(_wp_desired_speed_xy_cms)) {
         // update terrain following speed scalar
         _terrain_vel = speed_cms * _terrain_vel / _wp_desired_speed_xy_cms;