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

AC_WPNav: honour changes to wp-speed-up and wp-speed-down in-flight

This commit is contained in:
Peter Barker 2021-09-07 11:33:04 +10:00 committed by Peter Barker
parent a44b81f97d
commit 8a3a787b76
2 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -112,8 +112,10 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC
_flags.reached_destination = false;
_flags.fast_waypoint = false;
// initialise old WPNAV_SPEED value
_last_wp_speed_cms = get_default_speed_down();
// initialise old WPNAV_SPEED values
_last_wp_speed_cms = _wp_speed_cms;
_last_wp_speed_up_cms = _wp_speed_up_cms;
_last_wp_speed_down_cms = get_default_speed_down();
}
// get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL)
@ -579,6 +581,14 @@ bool AC_WPNav::update_wpnav()
set_speed_xy(_wp_speed_cms);
_last_wp_speed_cms = _wp_speed_cms;
}
if (!is_equal(_wp_speed_up_cms.get(), _last_wp_speed_up_cms)) {
set_speed_up(_wp_speed_up_cms);
_last_wp_speed_up_cms = _wp_speed_up_cms;
}
if (!is_equal(_wp_speed_down_cms.get(), _last_wp_speed_down_cms)) {
set_speed_down(_wp_speed_down_cms);
_last_wp_speed_down_cms = _wp_speed_down_cms;
}
// advance the target if necessary
if (!advance_wp_target_along_track(_pos_control.get_dt())) {

2
libraries/AC_WPNav/AC_WPNav.h
View File

@ -230,6 +230,8 @@ protected:
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
float _last_wp_speed_up_cms; // last recorded WPNAV_SPEED_UP, used for changing speed in-flight
float _last_wp_speed_down_cms; // last recorded WPNAV_SPEED_DN, used for changing speed in-flight
// scurve
SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory