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

AC_WPNav: wpnav speed param check fixed

This commit is contained in:
Randy Mackay 2023-04-19 20:37:59 +09:00 committed by Andrew Tridgell
parent 5178ac92bc
commit d669a2b079
2 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -165,7 +165,8 @@ void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point)
_pos_control.init_z_controller_stopping_point();
_pos_control.init_xy_controller_stopping_point();
// initialize the desired wp speed if not already done
// initialize the desired wp speed
_check_wp_speed_change = !is_positive(speed_cms);
_wp_desired_speed_xy_cms = is_positive(speed_cms) ? speed_cms : _wp_speed_cms;
_wp_desired_speed_xy_cms = MAX(_wp_desired_speed_xy_cms, WPNAV_WP_SPEED_MIN);
@ -588,9 +589,12 @@ 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;
// check for changes in speed parameter values
if (_check_wp_speed_change) {
if (!is_equal(_wp_speed_cms.get(), _last_wp_speed_cms)) {
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);

2
libraries/AC_WPNav/AC_WPNav.h
View File

@ -247,6 +247,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
// WPNAV_SPEED param change checker
bool _check_wp_speed_change; // if true WPNAV_SPEED param should be checked for changes in-flight
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