From e59f32074d6cf23bf1131d24b8f38bcc733d3bda Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 21 Jan 2020 07:50:34 +1100 Subject: [PATCH] AP_GPS: fixed yaw error when one GPS has zero position the M_PI correction was only valid for one zero side. Much simpler to always calculate difference --- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index c8dd934e44..09c1e5b415 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1344,14 +1344,8 @@ AP_GPS_UBLOX::_parse_gps(void) offset_dist > min_separation && fabsf(offset_dist - rel_dist) / MIN(offset_dist, rel_dist) < strict_length_error_allowed) { float rotation_offset_rad; - if (offset0.is_zero()) { - rotation_offset_rad = Vector2f(offset1.x, offset1.y).angle(); - } else if (offset1.is_zero()) { - rotation_offset_rad = Vector2f(offset0.x, offset0.y).angle(); - } else { - const Vector3f diff = offset0 - offset1; - rotation_offset_rad = Vector2f(diff.x, diff.y).angle(); - } + const Vector3f diff = offset0 - offset1; + rotation_offset_rad = Vector2f(diff.x, diff.y).angle(); if (state.instance != 0) { rotation_offset_rad += M_PI; }