Skip to content

Commit

Permalink
Merge pull request #8058 from iNavFlight/de_ublox10
Browse files Browse the repository at this point in the history 
Refactor u-blox hardware version detection; Add support for u-blox 10 series
  • Loading branch information
@DzikuVx
DzikuVx authored May 28, 2022
2 parents 5fe2157 + dc3090f commit 63a60ad
Showing 1 changed file with 76 additions and 37 deletions.
113 changes: 76 additions & 37 deletions src/main/io/gps_ublox.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,14 @@
#define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1)
#define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid))

#define UBX_HW_VERSION_UNKNOWN 0
#define UBX_HW_VERSION_UBLOX5 500
#define UBX_HW_VERSION_UBLOX6 600
#define UBX_HW_VERSION_UBLOX7 700
#define UBX_HW_VERSION_UBLOX8 800
#define UBX_HW_VERSION_UBLOX9 900
#define UBX_HW_VERSION_UBLOX10 1000

// SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE
// note PRNs last upadted 2020-12-18

Expand Down Expand Up @@ -557,11 +565,45 @@ static void configureSBAS(void)
sendConfigMessageUBLOX();
}

static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize)
{
// ublox_5 hwVersion 00040005
if (strncmp(szBuf, "00040005", nBufSize) == 0) {
return UBX_HW_VERSION_UBLOX5;
}

// ublox_6 hwVersion 00040007
if (strncmp(szBuf, "00040007", nBufSize) == 0) {
return UBX_HW_VERSION_UBLOX6;
}

// ublox_7 hwVersion 00070000
if (strncmp(szBuf, "00070000", nBufSize) == 0) {
return UBX_HW_VERSION_UBLOX7;
}

// ublox_M8 hwVersion 00080000
if (strncmp(szBuf, "00080000", nBufSize) == 0) {
return UBX_HW_VERSION_UBLOX8;
}

// ublox_M9 hwVersion 00190000
if (strncmp(szBuf, "00190000", nBufSize) == 0) {
return UBX_HW_VERSION_UBLOX9;
}

// ublox_M10 hwVersion 000A0000
if (strncmp(szBuf, "000A0000", nBufSize) == 0) {
return UBX_HW_VERSION_UBLOX10;
}

return UBX_HW_VERSION_UNKNOWN;
}

static bool gpsParceFrameUBLOX(void)
{
switch (_msg_id) {
case MSG_POSLLH:
//i2c_dataset.time = _buffer.posllh.time;
gpsSol.llh.lon = _buffer.posllh.longitude;
gpsSol.llh.lat = _buffer.posllh.latitude;
gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm
Expand Down Expand Up @@ -647,11 +689,8 @@ static bool gpsParceFrameUBLOX(void)
break;
case MSG_VER:
if (_class == CLASS_MON) {
//uint32_t swver = _buffer.ver.swVersion;
// EXT CORE 3.01 (107900)
// 01234567890123456789012
gpsState.hwVersion = fastA2I(_buffer.ver.hwVersion);
if ((gpsState.hwVersion >= 80000) && (_buffer.ver.swVersion[9] > '2')) {
gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion));
if ((gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) && (_buffer.ver.swVersion[9] > '2')) {
// check extensions;
// after hw + sw vers; each is 30 bytes
for(int j = 40; j < _payload_length; j += 30) {
Expand Down Expand Up @@ -823,9 +862,8 @@ STATIC_PROTOTHREAD(gpsConfigure)
// Configure UBX binary messages
gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);

// u-Blox 9
// M9N does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence
if (gpsState.hwVersion >= 190000) {
// M9N & M10 does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence
if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX9) {
configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);

Expand Down Expand Up @@ -853,64 +891,65 @@ STATIC_PROTOTHREAD(gpsConfigure)
ptWait(_ack_state == UBX_ACK_GOT_ACK);
}
else {
// u-Blox 6/7/8
// u-Blox 6 doesn't support PVT, use legacy config
if ((gpsState.gpsConfig->provider == GPS_UBLOX) || (gpsState.hwVersion < 70000)) {
configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1);
// u-Blox 5/6/7/8 or unknown
// u-Blox 7-8 support PVT
if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7) {
configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);

configureMSG(MSG_CLASS_UBX, MSG_STATUS, 1);
configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);

configureMSG(MSG_CLASS_UBX, MSG_SOL, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);

configureMSG(MSG_CLASS_UBX, MSG_VELNED, 1);
configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);

configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 10);
configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);

// This may fail on old UBLOX units, advance forward on both ACK and NAK
configureMSG(MSG_CLASS_UBX, MSG_PVT, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
configureMSG(MSG_CLASS_UBX, MSG_PVT, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);

configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);

// Configure data rate to 5HZ
configureRATE(200);
if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) {
configureRATE(100); // 10Hz
}
else {
configureRATE(200); // 5Hz
}
ptWait(_ack_state == UBX_ACK_GOT_ACK);
}
// u-Blox 7-8 support PVT
// u-Blox 5/6 doesn't support PVT, use legacy config
// UNKNOWN also falls here, use as a last resort
else {
configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0);
configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);

configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0);
configureMSG(MSG_CLASS_UBX, MSG_STATUS, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);

configureMSG(MSG_CLASS_UBX, MSG_SOL, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);

configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0);
configureMSG(MSG_CLASS_UBX, MSG_VELNED, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);

configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0);
configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 10);
ptWait(_ack_state == UBX_ACK_GOT_ACK);

configureMSG(MSG_CLASS_UBX, MSG_PVT, 1);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
// This may fail on old UBLOX units, advance forward on both ACK and NAK
configureMSG(MSG_CLASS_UBX, MSG_PVT, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);

configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0);
ptWait(_ack_state == UBX_ACK_GOT_ACK);

if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= 70000)) {
configureRATE(100); // 10Hz
}
else {
configureRATE(200); // 5Hz
}
// Configure data rate to 5HZ
configureRATE(200);
ptWait(_ack_state == UBX_ACK_GOT_ACK);
}
}
Expand Down Expand Up @@ -1017,14 +1056,14 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS)));

// Attempt to detect GPS hw version
gpsState.hwVersion = 0;
gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN;
gpsState.autoConfigStep = 0;

do {
pollVersion();
gpsState.autoConfigStep++;
ptWaitTimeout((gpsState.hwVersion != 0), GPS_CFG_CMD_TIMEOUT_MS);
} while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == 0);
ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS);
} while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN);

// Configure GPS
ptSpawn(gpsConfigure);
Expand Down

0 comments on commit 63a60ad

Please sign in to comment.