Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: protect against divide-by-zero

Browse Source
This commit is contained in:
Randy Mackay 2021-09-03 13:22:51 +09:00
parent 173de60f25
commit fe2b4d09c3
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -150,12 +150,16 @@ void AC_WPNav::wp_and_spline_init(float speed_cms)
// check _wp_radius_cm is reasonable // check _wp_radius_cm is reasonable
_wp_radius_cm.set_and_save_ifchanged(MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN)); _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 // initialise position controller
_pos_control.init_z_controller_stopping_point(); _pos_control.init_z_controller_stopping_point();
_pos_control.init_xy_controller_stopping_point(); _pos_control.init_xy_controller_stopping_point();
// initialize the desired wp speed if not already done // 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 = 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 // initialise position controller speed and acceleration
_pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); _pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss);
@ -193,8 +197,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 /// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation
void AC_WPNav::set_speed_xy(float speed_cms) void AC_WPNav::set_speed_xy(float speed_cms)
{ {
// range check target speed // range check target speed and protect against divide by zero
if (speed_cms >= WPNAV_WP_SPEED_MIN) { if (speed_cms >= WPNAV_WP_SPEED_MIN && is_positive(_wp_desired_speed_xy_cms)) {
// update terrain following speed scalar // update terrain following speed scalar
_terrain_vel = speed_cms * _terrain_vel / _wp_desired_speed_xy_cms; _terrain_vel = speed_cms * _terrain_vel / _wp_desired_speed_xy_cms;