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

AP_GPS: UBLOX: Parse RELPOSNED messages 

This does not yet:
  - validate the receiver configuration
  - manage timing out stale GPS heading info
  - relPosNormalized usage isn't clear, which may defeat the STRICT_LENGTH_CHECK
This commit is contained in:
Michael du Breuil 2019-09-03 12:32:09 -07:00 committed by Andrew Tridgell
parent cb5bba8379
commit 2587b13ae7
3 changed files with 88 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_GPS/AP_GPS.h
View File

@ -145,11 +145,13 @@ public:
float speed_accuracy; ///< 3D velocity RMS accuracy estimate in m/s
float horizontal_accuracy; ///< horizontal RMS accuracy estimate in m
float vertical_accuracy; ///< vertical RMS accuracy estimate in m
float gps_yaw_accuracy; ///< heading accuracy of the GPS in degrees
bool have_vertical_velocity; ///< does GPS give vertical velocity? Set to true only once available.
bool have_speed_accuracy; ///< does GPS give speed accuracy? Set to true only once available.
bool have_horizontal_accuracy; ///< does GPS give horizontal position accuracy? Set to true only once available.
bool have_vertical_accuracy; ///< does GPS give vertical position accuracy? Set to true only once available.
bool have_gps_yaw; ///< does GPS give yaw? Set to true only once available.
bool have_gps_yaw_accuracy; ///< does the GPS give a heading accuracy estimate? Set to true only once available
uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds
uint32_t uart_timestamp_ms; ///< optional timestamp from set_uart_timestamp()
@ -270,11 +272,12 @@ public:
return false;
}
yaw_deg = state[instance].gps_yaw;
// None of the GPS backends can provide this yet, so we hard
// code a fixed value of 10 degrees, which seems like a
// reasonable guess. Once a backend can provide a proper
// estimate we can implement it
accuracy_deg = 10;
if (state[instance].have_gps_yaw_accuracy) {
accuracy_deg = state[instance].gps_yaw_accuracy;
} else {
// fall back to 10 degrees as a generic default
accuracy_deg = 10;
}
return true;
}
bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg) const {

42
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -1032,8 +1032,50 @@ AP_GPS_UBLOX::_parse_gps(void)
state.hdop = 130;
#endif
break;
case MSG_RELPOSNED:
{
const Vector3f &offset0 = gps._antenna_offset[0].get();
const Vector3f &offset1 = gps._antenna_offset[1].get();
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
const uint32_t invalid_mask = static_cast<uint32_t>(RELPOSNED::refPosMiss) |
static_cast<uint32_t>(RELPOSNED::refObsMiss);
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)
) {
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();
}
if (state.instance != 0) {
rotation_offset_rad += M_PI;
}
state.gps_yaw = wrap_360(_buffer.relposned.relPosHeading * 1e-5 + degrees(rotation_offset_rad));
state.have_gps_yaw = true;
state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
state.have_gps_yaw_accuracy = true;
} else {
state.have_gps_yaw = false;
state.have_gps_yaw_accuracy = false;
}
}
break;
case MSG_PVT:
Debug("MSG_PVT");
havePvtMsg = true;
// position
_check_new_itow(_buffer.pvt.itow);

38
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -281,6 +281,30 @@ private:
uint32_t headVeh;
uint8_t reserved2[4];
};
struct PACKED ubx_nav_relposned {
uint8_t version;
uint8_t reserved1;
uint16_t refStationId;
uint32_t iTOW;
int32_t relPosN;
int32_t relPosE;
int32_t relPosD;
int32_t relPosLength;
int32_t relPosHeading;
uint8_t reserved2[4];
int8_t relPosHPN;
int8_t relPosHPE;
int8_t relPosHPD;
int8_t relPosHPLength;
uint32_t accN;
uint32_t accE;
uint32_t accD;
uint32_t accLength;
uint32_t accHeading;
uint8_t reserved3[4];
uint32_t flags;
};
struct PACKED ubx_nav_velned {
uint32_t itow; // GPS msToW
int32_t ned_north;
@ -444,6 +468,7 @@ private:
#endif
ubx_cfg_sbas sbas;
ubx_nav_svinfo_header svinfo_header;
ubx_nav_relposned relposned;
#if UBLOX_RXM_RAW_LOGGING
ubx_rxm_raw rxm_raw;
ubx_rxm_rawx rxm_rawx;
@ -451,6 +476,18 @@ private:
ubx_ack_ack ack;
} _buffer;
enum class RELPOSNED {
gnssFixOK = 1U << 0,
diffSoln = 1U << 1,
relPosValid = 1U << 2,
carrSoln = 1U << 3,
isMoving = 1U << 5,
refPosMiss = 1U << 6,
refObsMiss = 1U << 7,
relPosHeadingValid = 1U << 8,
relPosNormalized = 1U << 9
};
enum ubs_protocol_bytes {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
@ -467,6 +504,7 @@ private:
MSG_SOL = 0x6,
MSG_PVT = 0x7,
MSG_TIMEGPS = 0x20,
MSG_RELPOSNED = 0x3c,
MSG_VELNED = 0x12,
MSG_CFG_CFG = 0x09,
MSG_CFG_RATE = 0x08,