From ec53a41ffde9ca3bdf9f93a4c0f0116b151c9506 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 4 Sep 2021 11:44:54 +1000 Subject: [PATCH] AC_WPNav: monitor WPNAV_SPEED for changes --- libraries/AC_WPNav/AC_WPNav.cpp | 8 ++++++++ libraries/AC_WPNav/AC_WPNav.h | 2 ++ 2 files changed, 10 insertions(+) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 500212e80757de..106ec40ff10de4 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -111,6 +111,9 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC // init flags _flags.reached_destination = false; _flags.fast_waypoint = false; + + // initialise old WPNAV_SPEED value + _last_wp_speed_cms = _wp_speed_down_cms; } // get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL) @@ -572,6 +575,11 @@ 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; + } + // advance the target if necessary if (!advance_wp_target_along_track(_pos_control.get_dt())) { // To-Do: handle inability to advance along track (probably because of missing terrain data) diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index dee8379b56d9c8..6c5a2829a45adb 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -229,6 +229,8 @@ class AC_WPNav 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 + float _last_wp_speed_cms; // last recorded WPNAV_SPEED, used for changing speed in-flight + // scurve SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory SCurve _scurve_this_leg; // current scurve trajectory