From a58f61a29c776979b05bfd3e7a811b4fa0491157 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 601f331753..e27705c25d 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) @@ -167,8 +167,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 27542625dd..a4585d3926 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -71,7 +71,7 @@ public: 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; }