aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: account for rotation rate in moving baseline Z test

Browse Source 
this avoids switching GPS on rapid roll/pitch
This commit is contained in:
Andrew Tridgell 2021-04-15 11:35:54 +10:00 committed by Peter Barker
parent 2168633372
commit 8bc31ec5e9
1 changed files with 13 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -353,8 +353,20 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(const float reported_heading_deg,
#ifndef HAL_BUILD_AP_PERIPH
{
const Vector3f antenna_tilt = AP::ahrs().get_rotation_body_to_ned() * offset;
// get lag
float lag = 0.1;
get_lag(lag);
// get vehicle rotation, projected back in time using the gyro
auto &ahrs = AP::ahrs();
const Vector3f &gyro = ahrs.get_gyro();
Matrix3f rot_body_to_ned = ahrs.get_rotation_body_to_ned();
rot_body_to_ned.rotate(gyro * (-lag));
// apply rotation to the offset to get the Z offset in NED
const Vector3f antenna_tilt = rot_body_to_ned * offset;
const float alt_error = reported_D + antenna_tilt.z;
if (fabsf(alt_error) > permitted_error_length_pct * min_dist) {
// the vertical component is out of range, reject it
goto bad_yaw;