AC_WPNav: cope with negative WPNAV_SPEED_DN

a user set WPNAV_SPEED_DN to a negative value, with odd results. Take absolute value to cope. Even though the param docs clearly say range should be positive, it is one where it is easy to think it should be negative
Icatme · Sep 7, 2021 · 9342ded · 9342ded
1 parent aade77d
commit 9342ded
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 4 deletions.
diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -113,7 +113,7 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC
     _flags.fast_waypoint = false;
 
     // initialise old WPNAV_SPEED value
-    _last_wp_speed_cms = _wp_speed_down_cms;
+    _last_wp_speed_cms = get_default_speed_down();
 }
 
 // get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL)
@@ -168,8 +168,8 @@ void AC_WPNav::wp_and_spline_init(float speed_cms)
     // initialise position controller speed and acceleration
     _pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss);
     _pos_control.set_correction_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss);
-    _pos_control.set_max_speed_accel_z(-_wp_speed_down_cms, _wp_speed_up_cms, _wp_accel_z_cmss);
-    _pos_control.set_correction_speed_accel_z(-_wp_speed_down_cms, _wp_speed_up_cms, _wp_accel_z_cmss);
+    _pos_control.set_max_speed_accel_z(-get_default_speed_down(), _wp_speed_up_cms, _wp_accel_z_cmss);
+    _pos_control.set_correction_speed_accel_z(-get_default_speed_down(), _wp_speed_up_cms, _wp_accel_z_cmss);
 
     // calculate scurve jerk and jerk time
     if (!is_positive(_wp_jerk)) {

diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
@@ -68,7 +68,7 @@ class AC_WPNav
     float get_default_speed_up() const { return _wp_speed_up_cms; }
 
     /// get default target descent rate in cm/s during missions.  Note: always positive
-    float get_default_speed_down() const { return _wp_speed_down_cms; }
+    float get_default_speed_down() const { return fabsf(_wp_speed_down_cms); }
 
     /// get_speed_z - returns target descent speed in cm/s during missions.  Note: always positive
     float get_accel_z() const { return _wp_accel_z_cmss; }