AC_WPNav: wpnav speed param check fixed

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,10 +589,13 @@ bool AC_WPNav::update_wpnav()
 {
     bool ret = true;
 
+    // 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);
         _last_wp_speed_up_cms = _wp_speed_up_cms;

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