Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: protect against LOIT_SPEED divide-by-zero

Browse Source
This commit is contained in:
Randy Mackay 2017-03-14 20:03:37 +09:00
parent 83d39eaed8
commit 63dc5121bd
2 changed files with 9 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -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& ahrs, AC_PosContro
_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);

2
libraries/AC_WPNav/AC_WPNav.h
View File

@ -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