diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 6211f09b32..0a98dc46f0 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -112,8 +112,10 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC _flags.reached_destination = false; _flags.fast_waypoint = false; - // initialise old WPNAV_SPEED value - _last_wp_speed_cms = get_default_speed_down(); + // initialise old WPNAV_SPEED values + _last_wp_speed_cms = _wp_speed_cms; + _last_wp_speed_up_cms = _wp_speed_up_cms; + _last_wp_speed_down_cms = get_default_speed_down(); } // get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL) @@ -579,6 +581,14 @@ bool AC_WPNav::update_wpnav() 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; + } + if (!is_equal(_wp_speed_down_cms.get(), _last_wp_speed_down_cms)) { + set_speed_down(_wp_speed_down_cms); + _last_wp_speed_down_cms = _wp_speed_down_cms; + } // advance the target if necessary if (!advance_wp_target_along_track(_pos_control.get_dt())) { diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 2182801bcc..f30bd581b2 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -230,6 +230,8 @@ protected: AP_Float _terrain_margin; // terrain following altitude margin. vehicle will stop if distance from target altitude is larger than this margin 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 // scurve SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory