Separate UBLOX GPS SBAS configuration from GPS init.

Extracted NMEA and UBLOX code from the GPS hardware init method into
separate methods.  Introduced a state for changing baud rate rather than
re-using state_position.

src/main/io/gps.c

@@ -105,18 +105,55 @@ static const uint8_t ubloxInit[] = {
     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x03, 0x01, 0x0F, 0x49,           // set STATUS MSG rate
     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x06, 0x01, 0x12, 0x4F,           // set SOL MSG rate
     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1E, 0x67,           // set VELNED MSG rate
-    0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x31, 0xE5, // set SBAS to auto-detect
     0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A,             // set rate to 5Hz
 };
 
+// UBlox 6 Protocol documentation - GPS.G6-SW-10018-F
+// SBAS Configuration Settings Desciption, Page 4/210
+// 31.21 CFG-SBAS (0x06 0x16), Page 142/210
+// A.10 SBAS Configuration (UBX-CFG-SBAS), Page 198/210 - GPS.G6-SW-10018-F
+
+typedef enum {
+    SBAS_AUTO = 0,
+    SBAS_EGNOS,
+    SBAS_WAAS,
+    SBAS_MSAS,
+    SBAS_GAGAN
+} sbasMode_e;
+
+#define UBLOX_SBAS_MESSAGE_LENGTH 16
+typedef struct ubloxSbas_s {
+    sbasMode_e mode;
+    uint8_t message[UBLOX_SBAS_MESSAGE_LENGTH];
+} ubloxSbas_t;
+
+static const ubloxSbas_t ubloxSbas[] = {
+    { SBAS_AUTO,  { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x31, 0xE5}},
+    { SBAS_EGNOS, { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x51, 0x08, 0x00, 0x00, 0x8A, 0x41}},
+    { SBAS_WAAS,  { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x04, 0xE0, 0x04, 0x00, 0x19, 0x9D}},
+    { SBAS_MSAS,  { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x00, 0x02, 0x02, 0x00, 0x35, 0xEF}},
+    { SBAS_GAGAN, { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x80, 0x01, 0x00, 0x00, 0xB2, 0xE8}}
+};
+
+
 enum {
     GPS_UNKNOWN,
     GPS_INITIALIZING,
-    GPS_INITDONE,
+    GPS_CHANGE_BAUD,
-    GPS_RECEIVINGDATA,
+    GPS_CONFIGURE,
-    GPS_LOSTCOMMS,
+    GPS_RECEIVING_DATA,
+    GPS_LOST_COMMUNICATION,
 };
 
+typedef enum {
+    GPS_MESSAGE_STATE_IDLE = 0,
+    GPS_MESSAGE_STATE_INIT,
+    GPS_MESSAGE_STATE_SBAS,
+    GPS_MESSAGE_STATE_MAX = GPS_MESSAGE_STATE_SBAS
+} gpsMessageState_e;
+
+#define GPS_MESSAGE_STATE_ENTRY_COUNT (GPS_MESSAGE_STATE_MAX + 1)
+
 typedef struct gpsData_t {
     uint8_t state;                  // GPS thread state. Used for detecting cable disconnects and configuring attached devices
     uint8_t baudrateIndex;          // index into auto-detecting or current baudrate
@@ -124,9 +161,9 @@ typedef struct gpsData_t {
     uint32_t lastMessage;           // last time valid GPS data was received (millis)
     uint32_t lastLastMessage;       // last-last valid GPS message. Used to calculate delta.
 
-    uint32_t state_position;           // incremental variable for loops
+    uint32_t state_position;        // incremental variable for loops
-    uint32_t state_ts;                    // timestamp for last state_position increment
+    uint32_t state_ts;              // timestamp for last state_position increment
-
+    gpsMessageState_e messageState;
 } gpsData_t;
 
 static gpsData_t gpsData;
@@ -142,6 +179,7 @@ static void gpsSetState(uint8_t state)
     gpsData.state = state;
     gpsData.state_position = 0;
     gpsData.state_ts = millis();
+    gpsData.messageState = GPS_MESSAGE_STATE_IDLE;
 }
 
 void gpsUseProfile(gpsProfile_t *gpsProfileToUse)
@@ -190,53 +228,93 @@ void gpsInit(serialConfig_t *initialSerialConfig, uint8_t initialGpsProvider, gp
     gpsSetState(GPS_INITIALIZING);
 }
 
+void gpsInitNmea(void)
+{
+    switch(gpsData.state) {
+        case GPS_INITIALIZING:
+        case GPS_CHANGE_BAUD:
+            serialSetBaudRate(gpsPort, gpsInitData[gpsData.baudrateIndex].baudrate);
+            gpsSetState(GPS_RECEIVING_DATA);
+            break;
+    }
+}
+
+void gpsInitUblox(void)
+{
+    uint32_t now;
+    // UBX will run at mcfg.gps_baudrate, it shouldn't be "autodetected". So here we force it to that rate
+
+    // Wait until GPS transmit buffer is empty
+    if (!isSerialTransmitBufferEmpty(gpsPort))
+        return;
+
+
+    switch (gpsData.state) {
+        case GPS_INITIALIZING:
+            now = millis();
+            if (now - gpsData.state_ts < GPS_BAUDRATE_CHATE_DELAY)
+                return;
+
+            if (gpsData.state_position < GPS_INIT_ENTRIES) {
+                // try different speed to INIT
+                serialSetBaudRate(gpsPort, gpsInitData[gpsData.state_position].baudrate);
+                // but print our FIXED init string for the baudrate we want to be at
+                serialPrint(gpsPort, gpsInitData[gpsData.baudrateIndex].ubx);
+
+                gpsData.state_position++;
+                gpsData.state_ts = now;
+            } else {
+                // we're now (hopefully) at the correct rate, next state will switch to it
+                gpsSetState(GPS_CHANGE_BAUD);
+            }
+            break;
+        case GPS_CHANGE_BAUD:
+            serialSetBaudRate(gpsPort, gpsInitData[gpsData.baudrateIndex].baudrate);
+            gpsSetState(GPS_CONFIGURE);
+            break;
+        case GPS_CONFIGURE:
+            if (gpsData.messageState == GPS_MESSAGE_STATE_IDLE) {
+                gpsData.messageState++;
+            }
+
+            if (gpsData.messageState == GPS_MESSAGE_STATE_INIT) {
+
+                if (gpsData.state_position < sizeof(ubloxInit)) {
+                    serialWrite(gpsPort, ubloxInit[gpsData.state_position]);
+                    gpsData.state_position++;
+                } else {
+                    gpsData.state_position = 0;
+                    gpsData.messageState++;
+                }
+            }
+
+            if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) {
+                uint8_t sbasIndex = SBAS_AUTO; // TODO allow configuration
+                if (gpsData.state_position < UBLOX_SBAS_MESSAGE_LENGTH) {
+                    serialWrite(gpsPort, ubloxSbas[sbasIndex].message[gpsData.state_position]);
+                    gpsData.state_position++;
+                } else {
+                    gpsData.messageState++;
+                }
+            }
+
+            if (gpsData.messageState >= GPS_MESSAGE_STATE_ENTRY_COUNT) {
+                gpsSetState(GPS_RECEIVING_DATA);
+                // ublox should be init'd, time to try receiving
+            }
+            break;
+    }
+}
+
 void gpsInitHardware(void)
 {
     switch (gpsProvider) {
         case GPS_NMEA:
-            // nothing to do, just set baud rate and try receiving some stuff and see if it parses
+            gpsInitNmea();
-            serialSetBaudRate(gpsPort, gpsInitData[gpsData.baudrateIndex].baudrate);
-            gpsSetState(GPS_RECEIVINGDATA);
             break;
 
         case GPS_UBLOX:
-            // UBX will run at mcfg.gps_baudrate, it shouldn't be "autodetected". So here we force it to that rate
+            gpsInitUblox();
-
-            // Wait until GPS transmit buffer is empty
-            if (!isSerialTransmitBufferEmpty(gpsPort))
-                break;
-
-            if (gpsData.state == GPS_INITIALIZING) {
-                uint32_t m = millis();
-                if (m - gpsData.state_ts < GPS_BAUDRATE_CHATE_DELAY)
-                    return;
-
-                if (gpsData.state_position < GPS_INIT_ENTRIES) {
-                    // try different speed to INIT
-                    serialSetBaudRate(gpsPort, gpsInitData[gpsData.state_position].baudrate);
-                    // but print our FIXED init string for the baudrate we want to be at
-                    serialPrint(gpsPort, gpsInitData[gpsData.baudrateIndex].ubx);
-
-                    gpsData.state_position++;
-                    gpsData.state_ts = m;
-                } else {
-                    // we're now (hopefully) at the correct rate, next state will switch to it
-                    gpsSetState(GPS_INITDONE);
-                }
-            } else {
-                // GPS_INITDONE, set our real baud rate and push some ublox config strings
-                if (gpsData.state_position == 0)
-                    serialSetBaudRate(gpsPort, gpsInitData[gpsData.baudrateIndex].baudrate);
-
-                if (gpsData.state_position < sizeof(ubloxInit)) {
-                    serialWrite(gpsPort, ubloxInit[gpsData.state_position]); // send ubx init binary
-
-                    gpsData.state_position++;
-                } else {
-                    // ublox should be init'd, time to try receiving some junk
-                    gpsSetState(GPS_RECEIVINGDATA);
-                }
-            }
             break;
     }
 
@@ -257,11 +335,12 @@ void gpsThread(void)
             break;
 
         case GPS_INITIALIZING:
-        case GPS_INITDONE:
+        case GPS_CHANGE_BAUD:
+        case GPS_CONFIGURE:
             gpsInitHardware();
             break;
 
-        case GPS_LOSTCOMMS:
+        case GPS_LOST_COMMUNICATION:
             gpsData.errors++;
             // try another rate
             gpsData.baudrateIndex++;
@@ -273,12 +352,12 @@ void gpsThread(void)
             gpsSetState(GPS_INITIALIZING);
             break;
 
-        case GPS_RECEIVINGDATA:
+        case GPS_RECEIVING_DATA:
             // check for no data/gps timeout/cable disconnection etc
             if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
                 // remove GPS from capability
                 sensorsClear(SENSOR_GPS);
-                gpsSetState(GPS_LOSTCOMMS);
+                gpsSetState(GPS_LOST_COMMUNICATION);
             }
             break;
     }
@@ -612,7 +691,7 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
 
 gpsEnablePassthroughResult_e gpsEnablePassthrough(void)
 {
-    if (gpsData.state != GPS_RECEIVINGDATA)
+    if (gpsData.state != GPS_RECEIVING_DATA)
         return GPS_PASSTHROUGH_NO_GPS;
 
     serialPort_t *gpsPassthroughPort = findOpenSerialPort(FUNCTION_GPS_PASSTHROUGH);