GPS - Add packet logging to the OLED display. Allow autobauding to be

enabled/disabled via cli (disabled by default now).  Fix missing rate
configuration for UBLOX SVINFO which would have resulted in missing
satallite counts.

src/main/config/config.c

@@ -104,7 +104,7 @@ profile_t *currentProfile;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 85;
+static const uint8_t EEPROM_CONF_VERSION = 86;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -364,7 +364,8 @@ static void resetConf(void)
     // gps/nav stuff
     masterConfig.gpsConfig.provider = GPS_NMEA;
     masterConfig.gpsConfig.sbasMode = SBAS_AUTO;
-    masterConfig.gpsConfig.gpsAutoConfig = GPS_AUTOCONFIG_ON;
+    masterConfig.gpsConfig.autoConfig = GPS_AUTOCONFIG_ON;
+    masterConfig.gpsConfig.autoBaud = GPS_AUTOBAUD_OFF;
 #endif
 
     resetSerialConfig(&masterConfig.serialConfig);

src/main/io/display.c

@@ -302,11 +302,6 @@ void showGpsPage() {
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 
-    tfp_sprintf(lineBuffer, "Errors: %d", gpsData.errors);
-    padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
-
     tfp_sprintf(lineBuffer, "Lat: %d, Lon: %d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER);
     padLineBuffer();
     i2c_OLED_set_line(rowIndex++);
@@ -317,15 +312,27 @@ void showGpsPage() {
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 
+#ifdef GPS_PH_DEBUG
     tfp_sprintf(lineBuffer, "Angles: P:%d, R:%d", GPS_angle[PITCH], GPS_angle[ROLL]);
     padLineBuffer();
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
+#endif
 
     tfp_sprintf(lineBuffer, "%d cm/s, gc: %d", GPS_speed, GPS_ground_course);
     padLineBuffer();
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
+
+    tfp_sprintf(lineBuffer, "Errors: %d", gpsData.errors);
+    padLineBuffer();
+    i2c_OLED_set_line(rowIndex++);
+    i2c_OLED_send_string(lineBuffer);
+
+    tfp_sprintf(lineBuffer, "%16s", gpsPacketLog);
+    padLineBuffer();
+    i2c_OLED_set_line(rowIndex++);
+    i2c_OLED_send_string(lineBuffer);
 }
 
 void showBatteryPage(void)
@@ -475,6 +482,12 @@ void updateDisplay(void)
     }
 }
 
+void displaySetPage(pageId_e pageId)
+{
+    pageState.pageId = pageId;
+    pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
+}
+
 void displayInit(rxConfig_t *rxConfigToUse)
 {
     delay(200);
@@ -484,29 +497,32 @@ void displayInit(rxConfig_t *rxConfigToUse)
     rxConfig = rxConfigToUse;
 
     memset(&pageState, 0, sizeof(pageState));
-    pageState.pageId = PAGE_WELCOME;
+    displaySetPage(PAGE_WELCOME);
 
     updateDisplay();
 
     displaySetNextPageChangeAt(micros() + (1000 * 1000 * 5));
 }
 
-void displayShowFixedPage(void) {
+void displayShowFixedPage(void)
-    pageState.pageId = PAGE_GPS;
+{
-    pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
+    displaySetPage(PAGE_GPS);
     displayDisablePageCycling();
 }
 
-void displaySetNextPageChangeAt(uint32_t futureMicros) {
+void displaySetNextPageChangeAt(uint32_t futureMicros)
+{
     pageState.nextPageAt = futureMicros;
 }
 
-void displayEnablePageCycling(void) {
+void displayEnablePageCycling(void)
+{
     pageState.pageFlags |= PAGE_STATE_FLAG_CYCLE_ENABLED;
     pageState.cycleIndex = CYCLE_PAGE_ID_COUNT - 1; // start at first page
 }
 
-void displayDisablePageCycling(void) {
+void displayDisablePageCycling(void)
+{
     pageState.pageFlags &= ~PAGE_STATE_FLAG_CYCLE_ENABLED;
 }
 

src/main/io/gps.c

@@ -52,7 +52,18 @@
 
 extern int16_t debug[4];
 
+#define LOG_NMEA_GGA 'g'
+#define LOG_NMEA_RMC 'r'
 
+#define LOG_UBLOX_SOL 'O'
+#define LOG_UBLOX_STATUS 'S'
+#define LOG_UBLOX_SVINFO 'I'
+#define LOG_UBLOX_POSLLH 'P'
+#define LOG_UBLOX_VELNED 'V'
+
+
+char gpsPacketLog[16];
+static char *gpsPacketLogChar = gpsPacketLog;
 // **********************
 // GPS
 // **********************
@@ -78,7 +89,7 @@ static gpsConfig_t *gpsConfig;
 #define GPS_TIMEOUT (2500)
 // How many entries in gpsInitData array below
 #define GPS_INIT_ENTRIES (GPS_BAUDRATE_MAX + 1)
-#define GPS_BAUDRATE_CHATE_DELAY (100)
+#define GPS_BAUDRATE_CHANGE_DELAY (200)
 
 static serialConfig_t *serialConfig;
 static serialPort_t *gpsPort;
@@ -97,7 +108,7 @@ static const gpsInitData_t gpsInitData[] = {
     { GPS_BAUDRATE_38400,   38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n", "$PMTK251,38400*27\r\n" },
     { GPS_BAUDRATE_19200,   19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n", "$PMTK251,19200*22\r\n" },
     // 9600 is not enough for 5Hz updates - leave for compatibility to dumb NMEA that only runs at this speed
-    { GPS_BAUDRATE_9600,     9600, "", "" }
+    { GPS_BAUDRATE_9600,     9600, "$PUBX,41,1,0003,0001,9600,0*16\r\n", "" }
 };
 
 #define GPS_INIT_DATA_ENTRY_COUNT (sizeof(gpsInitData) / sizeof(gpsInitData[0]))
@@ -111,11 +122,14 @@ static const uint8_t ubloxInit[] = {
     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x00, 0x00, 0xFA, 0x0F,           // GGA: Global positioning system fix data
     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x02, 0x00, 0xFC, 0x13,           // GSA: GNSS DOP and Active Satellites
     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x00, 0xFE, 0x17,           // RMC: Recommended Minimum data
+
     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47,           // set POSLLH MSG rate
     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, 0x30, 0x01, 0x3C, 0xA3,           // set SVINFO MSG rate
     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1E, 0x67,           // set VELNED MSG rate
-    0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A,             // set rate to 5Hz
+
+    0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A,             // set rate to 5Hz (measurement period: 200ms, navigation rate: 1 cycle)
 };
 
 // UBlox 6 Protocol documentation - GPS.G6-SW-10018-F
@@ -182,6 +196,8 @@ void gpsInit(serialConfig_t *initialSerialConfig, gpsConfig_t *initialGpsConfig)
     // clear error counter
     gpsData.errors = 0;
 
+    memset(gpsPacketLog, 0x00, sizeof(gpsPacketLog));
+
     gpsConfig = initialGpsConfig;
 
     // init gpsData structure. if we're not actually enabled, don't bother doing anything else
@@ -230,17 +246,25 @@ void gpsInitUblox(void)
     switch (gpsData.state) {
         case GPS_INITIALIZING:
             now = millis();
-            if (now - gpsData.state_ts < GPS_BAUDRATE_CHATE_DELAY)
+            if (now - gpsData.state_ts < GPS_BAUDRATE_CHANGE_DELAY)
                 return;
 
             if (gpsData.state_position < GPS_INIT_ENTRIES) {
                 // try different speed to INIT
-                serialSetBaudRate(gpsPort, gpsInitData[gpsData.state_position].baudrate);
+                uint32_t newBaudRate = gpsInitData[gpsData.state_position].baudrate;
-                // but print our FIXED init string for the baudrate we want to be at
+
+                gpsData.state_ts = now;
+
+                if (serialGetBaudRate(gpsPort) != newBaudRate) {
+                    // change the rate if needed and wait a little
+                    serialSetBaudRate(gpsPort, newBaudRate);
+                    return;
+                }
+
+                // 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);
@@ -251,6 +275,13 @@ void gpsInitUblox(void)
             gpsSetState(GPS_CONFIGURE);
             break;
         case GPS_CONFIGURE:
+
+            // Either use specific config file for GPS or let dynamically upload config
+            if( gpsConfig->autoConfig == GPS_AUTOCONFIG_OFF ) {
+                gpsSetState(GPS_RECEIVING_DATA);
+                break;
+            }
+
             if (gpsData.messageState == GPS_MESSAGE_STATE_IDLE) {
                 gpsData.messageState++;
             }
@@ -258,10 +289,7 @@ void gpsInitUblox(void)
             if (gpsData.messageState == GPS_MESSAGE_STATE_INIT) {
 
                 if (gpsData.state_position < sizeof(ubloxInit)) {
-                    //Either use specific config file for GPS or let dynamically upload config
+                    serialWrite(gpsPort, ubloxInit[gpsData.state_position]);
-                    if( gpsConfig->gpsAutoConfig == GPS_AUTOCONFIG_ON ) {
-                        serialWrite(gpsPort, ubloxInit[gpsData.state_position]);
-                    }
                     gpsData.state_position++;
                 } else {
                     gpsData.state_position = 0;
@@ -271,10 +299,7 @@ void gpsInitUblox(void)
 
             if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) {
                 if (gpsData.state_position < UBLOX_SBAS_MESSAGE_LENGTH) {
-                    //Either use specific config file for GPS or let dynamically upload config
+                    serialWrite(gpsPort, ubloxSbas[gpsConfig->sbasMode].message[gpsData.state_position]);
-                    if( gpsConfig->gpsAutoConfig == GPS_AUTOCONFIG_ON ) {
-                        serialWrite(gpsPort, ubloxSbas[gpsConfig->sbasMode].message[gpsData.state_position]);
-                    }
                     gpsData.state_position++;
                 } else {
                     gpsData.messageState++;
@@ -282,8 +307,8 @@ void gpsInitUblox(void)
             }
 
             if (gpsData.messageState >= GPS_MESSAGE_STATE_ENTRY_COUNT) {
+                // ublox should be initialised, try receiving
                 gpsSetState(GPS_RECEIVING_DATA);
-                // ublox should be init'd, time to try receiving
             }
             break;
     }
@@ -322,9 +347,11 @@ void gpsThread(void)
 
         case GPS_LOST_COMMUNICATION:
             gpsData.errors++;
-            // try another rate
+            if (gpsConfig->autoBaud) {
-            gpsData.baudrateIndex++;
+                // try another rate
-            gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
+                gpsData.baudrateIndex++;
+                gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
+            }
             gpsData.lastMessage = millis();
             // TODO - move some / all of these into gpsData
             GPS_numSat = 0;
@@ -554,8 +581,11 @@ static bool gpsNewFrameNMEA(char c)
         if (checksum_param) {   //parity checksum
             uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
             if (checksum == parity) {
+                *gpsPacketLogChar = '?';
+
                 switch (gps_frame) {
                 case FRAME_GGA:
+                  *gpsPacketLogChar = LOG_NMEA_GGA;
                   frameOK = 1;
                   if (STATE(GPS_FIX)) {
                         GPS_coord[LAT] = gps_Msg.latitude;
@@ -565,6 +595,7 @@ static bool gpsNewFrameNMEA(char c)
                     }
                     break;
                 case FRAME_RMC:
+                    *gpsPacketLogChar = LOG_NMEA_RMC;
                     GPS_speed = gps_Msg.speed;
                     GPS_ground_course = gps_Msg.ground_course;
                     break;
@@ -735,9 +766,16 @@ void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
 
 static bool UBLOX_parse_gps(void)
 {
-    int i;
+    uint32_t i;
+
+    for (i = sizeof(gpsPacketLog) - 1; i > 0 ; i--) {
+        gpsPacketLog[i] = gpsPacketLog[i-1];
+    }
+    *gpsPacketLogChar = '?';
+
     switch (_msg_id) {
     case MSG_POSLLH:
+        *gpsPacketLogChar = LOG_UBLOX_POSLLH;
         //i2c_dataset.time                = _buffer.posllh.time;
         GPS_coord[LON] = _buffer.posllh.longitude;
         GPS_coord[LAT] = _buffer.posllh.latitude;
@@ -750,11 +788,13 @@ static bool UBLOX_parse_gps(void)
         _new_position = true;
         break;
     case MSG_STATUS:
+        *gpsPacketLogChar = LOG_UBLOX_STATUS;
         next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
         if (!next_fix)
             DISABLE_STATE(GPS_FIX);
         break;
     case MSG_SOL:
+        *gpsPacketLogChar = LOG_UBLOX_SOL;
         next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
         if (!next_fix)
             DISABLE_STATE(GPS_FIX);
@@ -762,12 +802,14 @@ static bool UBLOX_parse_gps(void)
         GPS_hdop = _buffer.solution.position_DOP;
         break;
     case MSG_VELNED:
+        *gpsPacketLogChar = LOG_UBLOX_VELNED;
         // speed_3d                        = _buffer.velned.speed_3d;  // cm/s
         GPS_speed = _buffer.velned.speed_2d;    // cm/s
         GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
         _new_speed = true;
         break;
     case MSG_SVINFO:
+        *gpsPacketLogChar = LOG_UBLOX_SVINFO;
         GPS_numCh = _buffer.svinfo.numCh;
         if (GPS_numCh > 16)
             GPS_numCh = 16;
@@ -796,32 +838,32 @@ static bool gpsNewFrameUBLOX(uint8_t data)
     bool parsed = false;
 
     switch (_step) {
-        case 1:
+        case 1: // Sync char 2 (0x62)
             if (PREAMBLE2 == data) {
                 _step++;
                 break;
             }
             _step = 0;
-        case 0:
+        case 0: // Sync char 1 (0xB5)
             if (PREAMBLE1 == data)
                 _step++;
             break;
-        case 2:
+        case 2: // Class
             _step++;
             _class = data;
             _ck_b = _ck_a = data;   // reset the checksum accumulators
             break;
-        case 3:
+        case 3: // Id
             _step++;
             _ck_b += (_ck_a += data);       // checksum byte
             _msg_id = data;
             break;
-        case 4:
+        case 4: // Payload length (part 1)
             _step++;
             _ck_b += (_ck_a += data);       // checksum byte
             _payload_length = data; // payload length low byte
             break;
-        case 5:
+        case 5: // Payload length (part 2)
             _step++;
             _ck_b += (_ck_a += data);       // checksum byte
             _payload_length += (uint16_t)(data << 8);
@@ -870,6 +912,7 @@ gpsEnablePassthroughResult_e gpsEnablePassthrough(void)
             return GPS_PASSTHROUGH_NO_SERIAL_PORT;
         }
     }
+    serialSetBaudRate(gpsPort, serialConfig->gps_baudrate);
     if(!(gpsPort->mode & MODE_TX))
         serialSetMode(gpsPort, gpsPort->mode | MODE_TX);
 

src/main/io/gps.h

@@ -48,15 +48,22 @@ typedef enum {
 } gpsBaudRate_e;
 
 typedef enum {
-    GPS_AUTOCONFIG_ON = 0,
+    GPS_AUTOCONFIG_OFF = 0,
-    GPS_AUTOCONFIG_OFF
+    GPS_AUTOCONFIG_ON,
 } gpsAutoConfig_e;
+
+typedef enum {
+    GPS_AUTOBAUD_OFF = 0,
+    GPS_AUTOBAUD_ON
+} gpsAutoBaud_e;
+
 #define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600
 
 typedef struct gpsConfig_s {
     gpsProvider_e provider;
     sbasMode_e sbasMode;
-    gpsAutoConfig_e gpsAutoConfig;
+    gpsAutoConfig_e autoConfig;
+    gpsAutoBaud_e autoBaud;
 } gpsConfig_t;
 
 typedef enum {
@@ -95,6 +102,7 @@ typedef struct gpsData_t {
 extern gpsData_t gpsData;
 extern int32_t GPS_coord[2];               // LAT/LON
 
+extern char gpsPacketLog[16];
 extern uint8_t GPS_numSat;
 extern uint16_t GPS_hdop;                  // GPS signal quality
 extern uint8_t GPS_update;                 // it's a binary toogle to distinct a GPS position update

src/main/io/serial_cli.c

@@ -250,7 +250,8 @@ const clivalue_t valueTable[] = {
 
     { "gps_provider",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.gpsConfig.provider, 0, GPS_PROVIDER_MAX },
     { "gps_sbas_mode",              VAR_UINT8  | MASTER_VALUE,  &masterConfig.gpsConfig.sbasMode, 0, SBAS_MODE_MAX },
-    { "gps_auto_config",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.gpsConfig.gpsAutoConfig, 0, GPS_AUTOCONFIG_OFF },
+    { "gps_auto_config",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.gpsConfig.autoConfig, GPS_AUTOCONFIG_OFF, GPS_AUTOCONFIG_ON },
+    { "gps_auto_baud",              VAR_UINT8  | MASTER_VALUE,  &masterConfig.gpsConfig.autoBaud, GPS_AUTOBAUD_OFF, GPS_AUTOBAUD_ON },
 
 
     { "gps_pos_p",                  VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDPOS], 0, 200 },