From d669a2b079aecc86cdd0e0af9c0501cf3a127b7f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 19 Apr 2023 20:37:59 +0900 Subject: [PATCH] AC_WPNav: wpnav speed param check fixed --- libraries/AC_WPNav/AC_WPNav.cpp | 12 ++++++++---- libraries/AC_WPNav/AC_WPNav.h | 2 ++ 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index c18e9392c7..251e852ad9 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -165,7 +165,8 @@ void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point) _pos_control.init_z_controller_stopping_point(); _pos_control.init_xy_controller_stopping_point(); - // initialize the desired wp speed if not already done + // initialize the desired wp speed + _check_wp_speed_change = !is_positive(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); @@ -588,9 +589,12 @@ bool AC_WPNav::update_wpnav() { bool ret = true; - if (!is_equal(_wp_speed_cms.get(), _last_wp_speed_cms)) { - set_speed_xy(_wp_speed_cms); - _last_wp_speed_cms = _wp_speed_cms; + // check for changes in speed parameter values + if (_check_wp_speed_change) { + if (!is_equal(_wp_speed_cms.get(), _last_wp_speed_cms)) { + set_speed_xy(_wp_speed_cms); + _last_wp_speed_cms = _wp_speed_cms; + } } if (!is_equal(_wp_speed_up_cms.get(), _last_wp_speed_up_cms)) { set_speed_up(_wp_speed_up_cms); diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 8b0a02107e..e4563f7ce6 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -247,6 +247,8 @@ protected: AP_Float _wp_jerk; // maximum jerk used to generate scurve trajectories in m/s/s/s AP_Float _terrain_margin; // terrain following altitude margin. vehicle will stop if distance from target altitude is larger than this margin + // WPNAV_SPEED param change checker + bool _check_wp_speed_change; // if true WPNAV_SPEED param should be checked for changes in-flight float _last_wp_speed_cms; // last recorded WPNAV_SPEED, used for changing speed in-flight float _last_wp_speed_up_cms; // last recorded WPNAV_SPEED_UP, used for changing speed in-flight float _last_wp_speed_down_cms; // last recorded WPNAV_SPEED_DN, used for changing speed in-flight