diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 89a7cac8ec5..3a7d3d12c1f 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -21,6 +21,7 @@ #include #include #include +#include #include "platform.h" #include "build/build_config.h" @@ -67,8 +68,18 @@ #define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid)) // SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE +// note PRNs last upadted 2020-12-18 + +#define SBASMASK1_BASE 120 +#define SBASMASK1_BITS(prn) (1 << (prn-SBASMASK1_BASE)) + static const uint32_t ubloxScanMode1[] = { - 0x00000000, 0x00000851, 0x0004E004, 0x00020200, 0x00000180, 0x00000000, + 0x00000000, // AUTO + (SBASMASK1_BITS(123) | SBASMASK1_BITS(126) | SBASMASK1_BITS(136)), // SBAS + (SBASMASK1_BITS(131) | SBASMASK1_BITS(133) | SBASMASK1_BITS(138)), // WAAS + (SBASMASK1_BITS(129) | SBASMASK1_BITS(137)), // MAAS + (SBASMASK1_BITS(127) | SBASMASK1_BITS(128)), // GAGAN + 0x00000000, // NONE }; static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = { @@ -101,11 +112,35 @@ typedef struct { uint16_t time; } ubx_rate; +typedef struct { + uint8_t gnssId; + uint8_t resTrkCh; + uint8_t maxTrkCh; + uint8_t reserved1; +// flags + uint8_t enabled; + uint8_t undefined0; + uint8_t sigCfgMask; + uint8_t undefined1; +} ubx_gnss_element_t; + +typedef struct { + uint8_t msgVer; + uint8_t numTrkChHw; + uint8_t numTrkChUse; + uint8_t numConfigBlocks; + ubx_gnss_element_t config[0]; +} ubx_gnss_msg_t; + +#define MAX_GNSS 7 +#define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS) + typedef union { - uint8_t bytes[60]; // sizeof Galileo config + uint8_t bytes[MAX_GNSS_SIZE_BYTES]; // placeholder ubx_sbas sbas; ubx_msg msg; ubx_rate rate; + ubx_gnss_msg_t gnss; } ubx_payload; // UBX support @@ -409,25 +444,63 @@ static const uint8_t default_payload[] = { 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; -// Note the organisation of the bytes reflects the structure of the payload -// 4 bytes then 8*number of elements (7) -static const uint8_t galileo_payload[] = { - 0x00, 0x00, 0x20, 0x07, // GNSS / min / max / enable - 0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS / 8 / 16 / Y - 0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS / 1 / 3 / Y - 0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, // Galileo / 4 / 8 / Y - 0x03, 0x08, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01, // BeiDou / 8 / 16 / N - 0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x01, // IMES / 0 / 8 / N - 0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x01, // QZSS / 0 / 3 / N - 0x06, 0x08, 0x0e, 0x00, 0x01, 0x00, 0x01, 0x01 // GLONASS / 8 / 14 / Y -}; +#define GNSSID_SBAS 1 +#define GNSSID_GALILEO 2 + +static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block) +{ + gnss_block->gnssId = GNSSID_SBAS; + gnss_block->maxTrkCh = 3; + gnss_block->sigCfgMask = 1; + if (gpsState.gpsConfig->sbasMode == SBAS_NONE) { + gnss_block->enabled = 0; + gnss_block->resTrkCh = 0; + } else { + gnss_block->enabled = 1; + gnss_block->resTrkCh = 1; + } + + return 1; +} + +static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block) +{ + if (!capGalileo) { + return 0; + } + + gnss_block->gnssId = GNSSID_GALILEO; + gnss_block->maxTrkCh = 8; + gnss_block->sigCfgMask = 1; + if (gpsState.gpsConfig->ubloxUseGalileo) { + gnss_block->enabled = 1; + gnss_block->resTrkCh = 4; + } else { + gnss_block->enabled = 0; + gnss_block->resTrkCh = 0; + } -static void configureGalileo(void) + return 1; +} + +static void configureGNSS(void) { + int blocksUsed = 0; + send_buffer.message.header.msg_class = CLASS_CFG; send_buffer.message.header.msg_id = MSG_CFG_GNSS; - send_buffer.message.header.length = sizeof(galileo_payload); - memcpy(send_buffer.message.payload.bytes, galileo_payload, sizeof(galileo_payload)); + send_buffer.message.payload.gnss.msgVer = 0; + send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset + send_buffer.message.payload.gnss.numTrkChUse = 32; + + /* SBAS, always generated */ + blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]); + + /* Galileo */ + blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]); + + send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed; + send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed); sendConfigMessageUBLOX(); } @@ -741,7 +814,7 @@ STATIC_PROTOTHREAD(gpsConfigure) // Configure UBX binary messages gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - // u-Blox 9 + // 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) { configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0); @@ -840,13 +913,11 @@ STATIC_PROTOTHREAD(gpsConfigure) configureSBAS(); ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); - // Enable GALILEO - if (gpsState.gpsConfig->ubloxUseGalileo && capGalileo) { - // If GALILEO is not supported by the hardware we'll get a NAK, - // however GPS would otherwise be perfectly initialized, so we are just waiting for any response - gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - configureGalileo(); - ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + // Configure GNSS for M8N and later + if (gpsState.hwVersion >= 80000) { + gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); + configureGNSS(); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); } ptEnd(0);