diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 703242d10dd..ba8ef789d0b 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -564,10 +564,36 @@ static void configureRATE(uint16_t measRate) } } +static void ubloxSendSetCfgU8(ubx_config_data64_payload_t *kvPairs, uint8_t count) +{ + ubx_config_data64_t cfg = {}; + + ubloxCfgFillU8(&cfg, kvPairs, count); + + serialWriteBuf(gpsState.gpsPort, (uint8_t *)&cfg, cfg.header.length+8); + _ack_waiting_msg = cfg.header.msg_id; + _ack_state = UBX_ACK_WAITING; +} + /* */ static void configureSBAS(void) { + // M10 and later have no UBX-CFG-SBAS, only the configuration interface, so the + // message below never took effect there: whatever service was selected, the + // receiver kept searching the PRN list it was shipped with. + // The protocol version is what says whether the message exists, and it draws the + // line in the right place: the M10 platform reports 34 and the F10 40, while the + // M9 reports 32 and the F9 27, and both of those still have UBX-CFG-SBAS + if (ubloxVersionGTE(34, 0)) { + ubx_config_data64_payload_t scanValues[] = { + // Same layout as scanmode1: PRN120 is bit 0, and zero means all of them + {UBLOX_CFG_SBAS_PRNSCANMASK, ubloxScanMode1[gpsState.gpsConfig->sbasMode]} + }; + ubloxSendSetCfgU8(scanValues, 1); + return; + } + send_buffer.message.header.msg_class = CLASS_CFG; send_buffer.message.header.msg_id = MSG_CFG_SBAS; send_buffer.message.header.length = 8; diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 75f10901035..0aaae103bac 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -303,6 +303,11 @@ typedef struct { uint16_t value; } __attribute__((packed)) ubx_config_data16_payload_t; +typedef struct { + uint32_t key; + uint64_t value; +} __attribute__((packed)) ubx_config_data64_payload_t; + @@ -326,6 +331,18 @@ typedef struct { } data; } __attribute__((packed)) ubx_config_data16_t; +// Eight byte items are rare, a scan mask here and there, so this one only has room for a few +#define MAX_CONFIG_SET_VAL_VALUES_64 4 + +typedef struct { + ubx_header header; + ubx_config_data_header_v1_t configHeader; + union { + ubx_config_data64_payload_t payload[0]; + uint8_t buffer[(MAX_CONFIG_SET_VAL_VALUES_64 * sizeof(ubx_config_data64_payload_t)) + 2]; // key/value pairs + 2 checksum bytes + } data; +} __attribute__((packed)) ubx_config_data64_t; + typedef struct { diff --git a/src/main/io/gps_ublox_utils.c b/src/main/io/gps_ublox_utils.c index c9c1682a5f7..2c0b737fd79 100644 --- a/src/main/io/gps_ublox_utils.c +++ b/src/main/io/gps_ublox_utils.c @@ -90,6 +90,35 @@ int ubloxCfgFillU2(ubx_config_data16_t *cfg, ubx_config_data16_payload_t *kvPair return count; } +int ubloxCfgFillU8(ubx_config_data64_t *cfg, ubx_config_data64_payload_t *kvPairs, uint8_t count) +{ + if (count > MAX_CONFIG_SET_VAL_VALUES_64) + count = MAX_CONFIG_SET_VAL_VALUES_64; + + cfg->header.preamble1 = 0xb5; + cfg->header.preamble2 = 0x62; + cfg->header.msg_class = 0x06; + cfg->header.msg_id = 0x8A; + cfg->header.length = sizeof(ubx_config_data_header_v1_t) + ((sizeof(ubx_config_data64_payload_t) * count)); + cfg->configHeader.layers = 0x1; + cfg->configHeader.transaction = 0; + cfg->configHeader.reserved = 0; + cfg->configHeader.version = 1; + + for (int i = 0; i < count; ++i) { + cfg->data.payload[i].key = kvPairs[i].key; + cfg->data.payload[i].value = kvPairs[i].value; + } + + uint8_t *buf = (uint8_t *)cfg; + uint8_t ck_a, ck_b; + ublox_update_checksum(buf + 2, cfg->header.length + 4, &ck_a, &ck_b); + buf[cfg->header.length + 6] = ck_a; + buf[cfg->header.length + 7] = ck_b; + + return count; +} + void ubloxNavSat2NavSig(const ubx_nav_svinfo_channel *navSat, ubx_nav_sig_info *navSig) { memset(navSig, 0, sizeof(ubx_nav_sig_info)); diff --git a/src/main/io/gps_ublox_utils.h b/src/main/io/gps_ublox_utils.h index 996cbe2a68c..f01f540bf10 100644 --- a/src/main/io/gps_ublox_utils.h +++ b/src/main/io/gps_ublox_utils.h @@ -27,6 +27,7 @@ extern "C" { int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count); int ubloxCfgFillU2(ubx_config_data16_t *cfg, ubx_config_data16_payload_t *kvPairs, uint8_t count); +int ubloxCfgFillU8(ubx_config_data64_t *cfg, ubx_config_data64_payload_t *kvPairs, uint8_t count); void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b); diff --git a/src/test/unit/gps_ublox_unittest.cc b/src/test/unit/gps_ublox_unittest.cc index ea5164b32a0..c29dfe7e25d 100644 --- a/src/test/unit/gps_ublox_unittest.cc +++ b/src/test/unit/gps_ublox_unittest.cc @@ -89,6 +89,31 @@ TEST(GPSUbloxTest, TestUbloxCfgFillBytes) //EXPECT_FALSE(strcmp(buf, " 123.45")); } +TEST(GPSUbloxTest, TestUbloxCfgFillU8) +{ + // The SBAS scan mask is the one eight byte item INAV sends. EGNOS is PRN 121, + // 123, 126, 136 and 150, which puts bits 1, 3, 6, 16 and 30 in the mask + ubx_config_data64_t cfg = {}; + ubx_config_data64_payload_t kvPairs[] = { + { 0x50360006, 0x4001004A } + }; + + EXPECT_EQ(1, ubloxCfgFillU8(&cfg, kvPairs, 1)); + + const uint8_t expected[] = { + 0xB5, 0x62, 0x06, 0x8A, 0x10, 0x00, // CFG-VALSET, 16 byte payload + 0x01, 0x01, 0x00, 0x00, // version 1, RAM layer + 0x06, 0x00, 0x36, 0x50, // CFG-SBAS-PRNSCANMASK + 0x4A, 0x00, 0x01, 0x40, 0x00, 0x00, 0x00, 0x00, // the mask, little endian + 0xB9, 0xBF // checksum + }; + EXPECT_EQ(0, memcmp(expected, &cfg, sizeof(expected))); + + // Asking for more than the message holds gets what fits, not an overrun + ubx_config_data64_payload_t many[MAX_CONFIG_SET_VAL_VALUES_64 + 3] = {}; + EXPECT_EQ(MAX_CONFIG_SET_VAL_VALUES_64, ubloxCfgFillU8(&cfg, many, MAX_CONFIG_SET_VAL_VALUES_64 + 3)); +} + TEST(GPSUbloxTest, navSigStructureSizes) { EXPECT_TRUE(sizeof(ubx_nav_sig_info) == 16);