Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: monitor WPNAV_SPEED for changes

Browse Source
This commit is contained in:
Peter Barker 2021-09-04 11:44:54 +10:00 committed by Randy Mackay
parent 2e52e2cba9
commit 3fade60249
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -111,6 +111,9 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC
// init flags
_flags.reached_destination = false;
_flags.fast_waypoint = false;
// initialise old WPNAV_SPEED value
_last_wp_speed_cms = _wp_speed_down_cms;
}
// get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL)
@ -571,6 +574,11 @@ bool AC_WPNav::update_wpnav()
{
bool ret = true;
if (!is_equal(_wp_speed_cms.get(), _last_wp_speed_cms)) {
set_speed_xy(_wp_speed_cms);
_last_wp_speed_cms = _wp_speed_cms;
}
// advance the target if necessary
if (!advance_wp_target_along_track(_pos_control.get_dt())) {
// To-Do: handle inability to advance along track (probably because of missing terrain data)

2
libraries/AC_WPNav/AC_WPNav.h
View File

@ -233,6 +233,8 @@ protected:
AP_Float _wp_jerk; // maximum jerk used to generate scurve trajectories in m/s/s/s
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
// scurve
SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory
SCurve _scurve_this_leg; // current scurve trajectory