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: improved checking for valid yaw for moving baseline

Browse Source 
and fixed check for carrSolnFixed
This commit is contained in:
Andrew Tridgell 2019-10-07 17:00:11 +11:00
parent 2587b13ae7
commit 37fb028532
2 changed files with 16 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -1036,21 +1036,27 @@ AP_GPS_UBLOX::_parse_gps(void)
{ {
const Vector3f &offset0 = gps._antenna_offset[0].get(); const Vector3f &offset0 = gps._antenna_offset[0].get();
const Vector3f &offset1 = gps._antenna_offset[1].get(); const Vector3f &offset1 = gps._antenna_offset[1].get();
// note that we require the yaw to come from a fixed solution, not a float solution
// yaw from a float solution would only be acceptable with a very large separation between
// GPS modules
const uint32_t valid_mask = static_cast<uint32_t>(RELPOSNED::relPosHeadingValid) | const uint32_t valid_mask = static_cast<uint32_t>(RELPOSNED::relPosHeadingValid) |
static_cast<uint32_t>(RELPOSNED::relPosValid) | static_cast<uint32_t>(RELPOSNED::relPosValid) |
static_cast<uint32_t>(RELPOSNED::gnssFixOK) | static_cast<uint32_t>(RELPOSNED::gnssFixOK) |
static_cast<uint32_t>(RELPOSNED::isMoving) | static_cast<uint32_t>(RELPOSNED::isMoving) |
(2 << static_cast<uint32_t>(RELPOSNED::carrSoln)); // require fixed ambiguity static_cast<uint32_t>(RELPOSNED::carrSolnFixed);
const uint32_t invalid_mask = static_cast<uint32_t>(RELPOSNED::refPosMiss) | const uint32_t invalid_mask = static_cast<uint32_t>(RELPOSNED::refPosMiss) |
static_cast<uint32_t>(RELPOSNED::refObsMiss); static_cast<uint32_t>(RELPOSNED::refObsMiss) |
static_cast<uint32_t>(RELPOSNED::carrSolnFloat);
const float offset_dist = (offset0 - offset1).length();
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;
if (((_buffer.relposned.flags & valid_mask) == valid_mask) && if (((_buffer.relposned.flags & valid_mask) == valid_mask) &&
((_buffer.relposned.flags & invalid_mask) == 0) ((_buffer.relposned.flags & invalid_mask) == 0) &&
#if defined(STRICT_LENGTH_CHECK) rel_dist > min_separation &&
// force the estimated position to be within offset_dist > min_separation &&
&& (fabsf(offset0.distance(offset1) - (_buffer.relposned.relPosLength * 1e-2)) < STRICT_LENGTH_CHECK) fabsf(offset_dist - rel_dist) / MIN(offset_dist, rel_dist) < strict_length_error_allowed) {
#endif // defined(STRICT_LENGTH_CHECK)
) {
float rotation_offset_rad; float rotation_offset_rad;
if (offset0.is_zero()) { if (offset0.is_zero()) {
rotation_offset_rad = Vector2f(offset1.x, offset1.y).angle(); rotation_offset_rad = Vector2f(offset1.x, offset1.y).angle();

3
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -480,7 +480,8 @@ private:
gnssFixOK = 1U << 0, gnssFixOK = 1U << 0,
diffSoln = 1U << 1, diffSoln = 1U << 1,
relPosValid = 1U << 2, relPosValid = 1U << 2,
carrSoln = 1U << 3, carrSolnFloat = 1U << 3,
carrSolnFixed = 1U << 4,
isMoving = 1U << 5, isMoving = 1U << 5,
refPosMiss = 1U << 6, refPosMiss = 1U << 6,
refObsMiss = 1U << 7, refObsMiss = 1U << 7,