Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: improved checking for valid yaw for moving baseline 

and fixed check for carrSolnFixed
This commit is contained in:
Andrew Tridgell 2019-10-07 17:00:11 +11:00
parent a0475874a7
commit 0b435559bd
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

@ -1101,21 +1101,27 @@ AP_GPS_UBLOX::_parse_gps(void)
{
const Vector3f &offset0 = gps._antenna_offset[0].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) |
static_cast<uint32_t>(RELPOSNED::relPosValid) |
static_cast<uint32_t>(RELPOSNED::gnssFixOK) |
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) |
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) &&
((_buffer.relposned.flags & invalid_mask) == 0)
#if defined(STRICT_LENGTH_CHECK)
// force the estimated position to be within
&& (fabsf(offset0.distance(offset1) - (_buffer.relposned.relPosLength * 1e-2)) < STRICT_LENGTH_CHECK)
#endif // defined(STRICT_LENGTH_CHECK)
) {
((_buffer.relposned.flags & invalid_mask) == 0) &&
rel_dist > min_separation &&
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();

3
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

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