Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2021-09-07 12:41:09 +10:00 committed by Randy Mackay
parent aade77db9a
commit 9342ded2bb
2 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -113,7 +113,7 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC
_flags.fast_waypoint = false; _flags.fast_waypoint = false;
// initialise old WPNAV_SPEED value // 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) // 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 // initialise position controller speed and acceleration
_pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); _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_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_max_speed_accel_z(-get_default_speed_down(), _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_correction_speed_accel_z(-get_default_speed_down(), _wp_speed_up_cms, _wp_accel_z_cmss);
// calculate scurve jerk and jerk time // calculate scurve jerk and jerk time
if (!is_positive(_wp_jerk)) { if (!is_positive(_wp_jerk)) {

2
libraries/AC_WPNav/AC_WPNav.h
View File

@ -68,7 +68,7 @@ public:
float get_default_speed_up() const { return _wp_speed_up_cms; } float get_default_speed_up() const { return _wp_speed_up_cms; }
/// get default target descent rate in cm/s during missions. Note: always positive /// 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 /// 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; } float get_accel_z() const { return _wp_accel_z_cmss; }