AC_WPNav: monitor WPNAV_SPEED for changes

Icatme · Sep 7, 2021 · ec53a41 · ec53a41
1 parent d94191b
commit ec53a41
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 0 deletions.
diff --git 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
@@ -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