From 8bc31ec5e9d356b73084bdb9faa7d1caa5071f4f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Apr 2021 11:35:54 +1000 Subject: [PATCH] AP_GPS: account for rotation rate in moving baseline Z test this avoids switching GPS on rapid roll/pitch --- libraries/AP_GPS/GPS_Backend.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 85786800c4..24f52829f1 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -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;