diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index d8463483554..7ef9eec6208 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/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) diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index 37b02a9afb2..82d0fc471a0 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/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