Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_WPNav: add WP_PIVOT_DELAY parameter

Browse Source
This commit is contained in:
Tatsuya Yamaguchi 2021-04-20 16:42:33 +09:00 committed by Randy Mackay
parent 3fc58de7ce
commit 333588d7d4
2 changed files with 21 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -83,6 +83,15 @@ const AP_Param::GroupInfo AR_WPNav::var_info[] = {
// @User: Standard
AP_GROUPINFO("SPEED_MIN", 6, AR_WPNav, _speed_min, 0),
// @Param: PIVOT_DELAY
// @DisplayName: Delay after pivot turn
// @Description: Waiting time after pivot turn
// @Units: s
// @Range: 0 60
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("PIVOT_DELAY", 7, AR_WPNav, _pivot_delay, 0),
AP_GROUPEND
};
@ -315,10 +324,17 @@ void AR_WPNav::update_pivot_active_flag()
return;
}
// if within 10 degrees of the target heading, exit pivot steering
if (yaw_error < AR_WPNAV_PIVOT_ANGLE_ACCURACY) {
uint32_t now = AP_HAL::millis();
// if within 10 degrees of the target heading, set start time of pivot steering
if (_pivot_active && yaw_error < AR_WPNAV_PIVOT_ANGLE_ACCURACY && _pivot_start_ms == 0) {
_pivot_start_ms = now;
}
// exit pivot steering after the time set by pivot_delay has elapsed
if (_pivot_start_ms > 0 && now - _pivot_start_ms >= constrain_float(_pivot_delay.get(), 0.0f, 60.0f) * 1000.0f) {
_pivot_active = false;
return;
_pivot_start_ms = 0;
}
}

2
libraries/AR_WPNav/AR_WPNav.h
View File

@ -123,6 +123,7 @@ private:
AP_Float _overshoot; // maximum horizontal overshoot in meters
AP_Int16 _pivot_angle; // angle error that leads to pivot turn
AP_Int16 _pivot_rate; // desired turn rate during pivot turns in deg/sec
AP_Float _pivot_delay; // waiting time after pivot turn
// references
AR_AttitudeControl& _atc; // rover attitude control library
@ -135,6 +136,7 @@ private:
// variables for navigation
uint32_t _last_update_ms; // system time of last call to update
uint32_t _pivot_start_ms; // system time when pivot turn started
Location _origin; // origin Location (vehicle will travel from the origin to the destination)
Location _destination; // destination Location when in Guided_WP
bool _orig_and_dest_valid; // true if the origin and destination have been set