aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_WPNav: ensure that wp_radius greater than min

Browse Source 
Co-authored-by: Murata,Katsutoshi <ma2maru@gmail.com>
This commit is contained in:
Josh Henderson 2021-06-05 22:16:46 -04:00 committed by Randy Mackay
parent 117ca8a6ef
commit 757fc1d679
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -93,10 +93,6 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC
// init flags
_flags.reached_destination = false;
_flags.fast_waypoint = false;
// sanity check some parameters
_wp_accel_cmss = MIN(_wp_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f)));
_wp_radius_cm = MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN);
}
// get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL)
@ -127,10 +123,14 @@ AC_WPNav::TerrainSource AC_WPNav::get_terrain_source() const
/// should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination
void AC_WPNav::wp_and_spline_init(float speed_cms)
{
// sanity check parameters
// check _wp_accel_cmss is reasonable
if (_wp_accel_cmss <= 0) {
_wp_accel_cmss.set_and_save(WPNAV_ACCELERATION);
}
const float wp_accel_cmss = MIN(_wp_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f)));
_wp_accel_cmss.set_and_save_ifchanged((_wp_accel_cmss <= 0) ? WPNAV_ACCELERATION : wp_accel_cmss);
// check _wp_radius_cm is reasonable
_wp_radius_cm.set_and_save_ifchanged(MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN));
// initialise position controller
_pos_control.init_z_controller_stopping_point();