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

AP_L1_Control: check vehicle yaw (heading) and ground track agree

Browse Source
This commit is contained in:
Mark Agate 2023-09-01 07:56:10 -07:00 committed by Andrew Tridgell
parent 693e35b9c5
commit 1769453520
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -240,7 +240,11 @@ void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &nex
//Calculate groundspeed //Calculate groundspeed
float groundSpeed = _groundspeed_vector.length(); float groundSpeed = _groundspeed_vector.length();
if (groundSpeed < 0.1f) {
// check if we are moving in the direction of the front of the vehicle
const bool moving_forwards = fabsf(wrap_PI(_groundspeed_vector.angle() - get_yaw())) < M_PI_2;
if (groundSpeed < 0.1f || !moving_forwards) {
// use a small ground speed vector in the right direction, // use a small ground speed vector in the right direction,
// allowing us to use the compass heading at zero GPS velocity // allowing us to use the compass heading at zero GPS velocity
groundSpeed = 0.1f; groundSpeed = 0.1f;