From 4456732911a975e2bce6d8fa8819b4ad9779f324 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 1 Apr 2020 14:22:23 +1100 Subject: [PATCH] AP_GPS: fixed rotation of moving baseline GPS we were off by 180 degrees. Thanks to Jaime for noticing this! --- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 09c1e5b415..7bc7561ef7 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1333,6 +1333,10 @@ AP_GPS_UBLOX::_parse_gps(void) const float rel_dist = _buffer.relposned.relPosLength * 1.0e-2; const float strict_length_error_allowed = 0.2; // allow for up to 20% error const float min_separation = 0.05; + /* + RELPOSNED messages gives the NED distance from base to + rover. It comes from the rover + */ MB_Debug("RELPOSNED[%u]: od:%.2f rd:%.2f flags:0x%04x t=%u", state.instance+1, offset_dist, rel_dist, @@ -1344,7 +1348,7 @@ 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; - const Vector3f diff = offset0 - offset1; + const Vector3f diff = offset1 - offset0; rotation_offset_rad = Vector2f(diff.x, diff.y).angle(); if (state.instance != 0) { rotation_offset_rad += M_PI;