Fix: the SBAS service choice never reaches M10 and F10 receivers

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;

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 {

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));

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);
 

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);