AC_WPNav: bug fix sanity check of set_speed_xy

This corrects a bug that allowed the waypoint speed to be set to zero
This commit is contained in:
Randy Mackay 2014-10-21 22:09:42 +09:00
parent 39c8535223
commit e80328d3a5
1 changed files with 1 additions and 1 deletions

View File

@ -341,7 +341,7 @@ void AC_WPNav::wp_and_spline_init()
void AC_WPNav::set_speed_xy(float speed_cms)
{
// range check new target speed and update position controller
if (_wp_speed_cms >= WPNAV_WP_SPEED_MIN) {
if (speed_cms >= WPNAV_WP_SPEED_MIN) {
_wp_speed_cms = speed_cms;
_pos_control.set_speed_xy(_wp_speed_cms);
// flag that wp leash must be recalculated