Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: fixed rotation of moving baseline GPS

Browse Source 
we were off by 180 degrees. Thanks to Jaime for noticing this!
This commit is contained in:
Andrew Tridgell 2020-04-01 14:22:23 +11:00
parent 46582944e2
commit 4456732911
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_GPS/AP_GPS_UBLOX.cpp
View File

@ -1333,6 +1333,10 @@ AP_GPS_UBLOX::_parse_gps(void)
const float rel_dist = _buffer.relposned.relPosLength * 1.0e-2; 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 strict_length_error_allowed = 0.2; // allow for up to 20% error
const float min_separation = 0.05; 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", MB_Debug("RELPOSNED[%u]: od:%.2f rd:%.2f flags:0x%04x t=%u",
state.instance+1, state.instance+1,
offset_dist, rel_dist, offset_dist, rel_dist,
@ -1344,7 +1348,7 @@ AP_GPS_UBLOX::_parse_gps(void)
offset_dist > min_separation && offset_dist > min_separation &&
fabsf(offset_dist - rel_dist) / MIN(offset_dist, rel_dist) < strict_length_error_allowed) { fabsf(offset_dist - rel_dist) / MIN(offset_dist, rel_dist) < strict_length_error_allowed) {
float rotation_offset_rad; float rotation_offset_rad;
const Vector3f diff = offset0 - offset1; const Vector3f diff = offset1 - offset0;
rotation_offset_rad = Vector2f(diff.x, diff.y).angle(); rotation_offset_rad = Vector2f(diff.x, diff.y).angle();
if (state.instance != 0) { if (state.instance != 0) {
rotation_offset_rad += M_PI; rotation_offset_rad += M_PI;