AR_WPNav: Apply WP_SPEED changes instantly

libraries/AR_WPNav/AR_WPNav.cpp

@@ -99,6 +99,7 @@ AR_WPNav::AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control) :
 void AR_WPNav::init(float speed_max)
 {
     // determine max speed, acceleration and jerk
+    _check_speed_param_change = !is_positive(speed_max);
     if (is_positive(speed_max)) {
         _base_speed_max = speed_max;
     } else {
@@ -107,6 +108,7 @@ void AR_WPNav::init(float speed_max)
     _base_speed_max = MAX(AR_WPNAV_SPEED_MIN, _base_speed_max);
     const float accel_max = get_accel_max();
     const float jerk_max = get_jerk_max();
+    _last_speed_param_ms = _speed_max;
 
     // initialise position controller
     _pos_control.set_limits(_base_speed_max, accel_max, _atc.get_turn_lat_accel_max(), jerk_max);
@@ -183,6 +185,8 @@ bool AR_WPNav::set_speed_max(float speed_max)
     }
 
     _base_speed_max = speed_max;
+    // explicit override takes precedence; disable WP_SPEED param-refresh so it cannot overwrite the override
+    _check_speed_param_change = false;
     return true;
 }
 
@@ -605,6 +609,12 @@ bool AR_WPNav::set_origin_and_destination_to_stopping_point()
 // _atc.get_turn_lat_accel_max() and update position controller limits if required
 void AR_WPNav::update_limits()
 {
+    // refresh _base_speed_max if WP_SPEED param changed since init
+    if (_check_speed_param_change && !is_equal(_speed_max.get(), _last_speed_param_ms)) {
+        _base_speed_max = MAX(AR_WPNAV_SPEED_MIN, _speed_max.get());
+        _last_speed_param_ms = _speed_max;
+    }
+
     // update limits
     // Note this won't be applied to s-curve legs until the next waypoint, or (in 
     // the case of fast waypoints, the waypoint-after-next)

libraries/AR_WPNav/AR_WPNav.h

@@ -187,6 +187,8 @@ protected:
     float _base_speed_max;          // speed max (in m/s) derived from parameters or passed into init
     float _nudge_speed_max;         // "nudge" speed max (in m/s) normally from the pilot.  has no effect if less than _base_speed_max.  always positive.
     uint32_t _last_speed_update_ms; // system time that speed_max was last update.  used to ensure speed_max is not update too quickly
+    bool _check_speed_param_change; // true if WP_SPEED param should be monitored for changes during navigation
+    float _last_speed_param_ms;     // last recorded WP_SPEED value (m/s) for change detection
 
     // main outputs from navigation library
     float _desired_speed_limited;   // desired speed (above) but accel/decel limited