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

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

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
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,
// allowing us to use the compass heading at zero GPS velocity
groundSpeed = 0.1f;