From 9342ded2bb99924d5a42e9feed0163b63b4bdee7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Sep 2021 12:41:09 +1000 Subject: [PATCH] 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 --- libraries/AC_WPNav/AC_WPNav.cpp | 6 +++--- libraries/AC_WPNav/AC_WPNav.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 106ec40ff10de..6211f09b329d8 100644 --- 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 index 6c5a2829a45ad..2182801bcc6fe 100644 --- 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; }