OP-1507 - Handles GPS data a chunk at time instead of a single byte at time.

flight/modules/GPS/GPS.c

@@ -52,6 +52,10 @@
 #include "inc/ubx_autoconfig.h"
 #endif
 
+#include <pios_instrumentation_helper.h>
+PERF_DEFINE_COUNTER(counterBytesIn);
+PERF_DEFINE_COUNTER(counterRate);
+PERF_DEFINE_COUNTER(counterParse);
 // ****************
 // Private functions
 
@@ -79,6 +83,10 @@ void updateGpsSettings(UAVObjEvent *ev);
 // the new location with Set = true.
 #define GPS_HOMELOCATION_SET_DELAY 5000
 
+#define GPS_LOOP_DELAY_MS          6
+
+#define GPS_READ_BUFFER            128
+
 #ifdef PIOS_GPS_SETS_HOMELOCATION
 // Unfortunately need a good size stack for the WMM calculation
         #define STACK_SIZE_BYTES   1024
@@ -238,9 +246,16 @@ static void gpsTask(__attribute__((unused)) void *parameters)
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
     updateGpsSettings(0);
 #endif
+
+    TickType_t xLastWakeTime;
+    xLastWakeTime = xTaskGetTickCount();
+    PERF_INIT_COUNTER(counterBytesIn, 0x97510001);
+    PERF_INIT_COUNTER(counterRate, 0x97510002);
+    PERF_INIT_COUNTER(counterParse, 0x97510003);
+    uint8_t c[GPS_READ_BUFFER];
+
     // Loop forever
     while (1) {
-        uint8_t c;
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
         if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
             char *buffer   = 0;
@@ -253,12 +268,16 @@ static void gpsTask(__attribute__((unused)) void *parameters)
         }
 #endif
         // This blocks the task until there is something on the buffer
-        while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) {
+        uint16_t cnt;
+        while ((cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay)) > 0) {
+            PERF_TIMED_SECTION_START(counterParse);
+            PERF_TRACK_VALUE(counterBytesIn, cnt);
+            PERF_MEASURE_PERIOD(counterRate);
             int res;
             switch (gpsSettings.DataProtocol) {
 #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
             case GPSSETTINGS_DATAPROTOCOL_NMEA:
-                res = parse_nmea_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
+                res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
                 break;
 #endif
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
@@ -272,7 +291,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
                     ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
                     GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
 #endif
-                res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
+                res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
             }
             break;
 #endif
@@ -281,6 +300,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
                 break;
             }
 
+            PERF_TIMED_SECTION_END(counterParse);
             if (res == PARSER_COMPLETE) {
                 timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
                 timeOfLastUpdateMs = timeNowMs;
@@ -327,6 +347,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
                 AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
             }
         }
+        vTaskDelayUntil(&xLastWakeTime, GPS_LOOP_DELAY_MS / portTICK_RATE_MS);
     }
 }
 

flight/modules/GPS/NMEA.c

@@ -106,75 +106,80 @@ static const struct nmea_parser nmea_parsers[] = {
 #endif // PIOS_GPS_MINIMAL
 };
 
-int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
+int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
 {
+    int ret = PARSER_INCOMPLETE;
     static uint8_t rx_count = 0;
     static bool start_flag  = false;
     static bool found_cr    = false;
+    uint8_t c;
 
-    // detect start while acquiring stream
+    for (int i = 0; i < len; i++) {
-    if (!start_flag && (c == '$')) { // NMEA identifier found
+        c = rx[i];
-        start_flag = true;
+        // detect start while acquiring stream
-        found_cr   = false;
+        if (!start_flag && (c == '$')) { // NMEA identifier found
-        rx_count   = 0;
+            start_flag = true;
-    } else if (!start_flag) {
+            found_cr   = false;
-        return PARSER_ERROR;
+            rx_count   = 0;
-    }
+        } else if (!start_flag) {
-
-    if (rx_count >= NMEA_MAX_PACKET_LENGTH) {
-        // The buffer is already full and we haven't found a valid NMEA sentence.
-        // Flush the buffer and note the overflow event.
-        gpsRxStats->gpsRxOverflow++;
-        start_flag = false;
-        found_cr   = false;
-        rx_count   = 0;
-        return PARSER_OVERRUN;
-    } else {
-        gps_rx_buffer[rx_count] = c;
-        rx_count++;
-    }
-
-    // look for ending '\r\n' sequence
-    if (!found_cr && (c == '\r')) {
-        found_cr = true;
-    } else if (found_cr && (c != '\n')) {
-        found_cr = false; // false end flag
-    } else if (found_cr && (c == '\n')) {
-        // The NMEA functions require a zero-terminated string
-        // As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
-        gps_rx_buffer[rx_count - 2] = 0;
-
-        // prepare to parse next sentence
-        start_flag = false;
-        found_cr   = false;
-        rx_count   = 0;
-        // Our rxBuffer must look like this now:
-        // [0]           = '$'
-        // ...           = zero or more bytes of sentence payload
-        // [end_pos - 1] = '\r'
-        // [end_pos]     = '\n'
-        //
-        // Prepare to consume the sentence from the buffer
-
-        // Validate the checksum over the sentence
-        if (!NMEA_checksum(&gps_rx_buffer[1])) { // Invalid checksum.  May indicate dropped characters on Rx.
-                                                 // PIOS_DEBUG_PinHigh(2);
-            gpsRxStats->gpsRxChkSumError++;
-            // PIOS_DEBUG_PinLow(2);
             return PARSER_ERROR;
-        } else { // Valid checksum, use this packet to update the GPS position
+        }
-            if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) {
-                // PIOS_DEBUG_PinHigh(2);
-                gpsRxStats->gpsRxParserError++;
-                // PIOS_DEBUG_PinLow(2);
-            } else {
-                gpsRxStats->gpsRxReceived++;
-            };
 
-            return PARSER_COMPLETE;
+        if (rx_count >= NMEA_MAX_PACKET_LENGTH) {
+            // The buffer is already full and we haven't found a valid NMEA sentence.
+            // Flush the buffer and note the overflow event.
+            gpsRxStats->gpsRxOverflow++;
+            start_flag = false;
+            found_cr   = false;
+            rx_count   = 0;
+            ret = PARSER_OVERRUN;
+        } else {
+            gps_rx_buffer[rx_count] = c;
+            rx_count++;
+        }
+
+        // look for ending '\r\n' sequence
+        if (!found_cr && (c == '\r')) {
+            found_cr = true;
+        } else if (found_cr && (c != '\n')) {
+            found_cr = false; // false end flag
+        } else if (found_cr && (c == '\n')) {
+            // The NMEA functions require a zero-terminated string
+            // As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
+            gps_rx_buffer[rx_count - 2] = 0;
+
+            // prepare to parse next sentence
+            start_flag = false;
+            found_cr   = false;
+            rx_count   = 0;
+            // Our rxBuffer must look like this now:
+            // [0]           = '$'
+            // ...           = zero or more bytes of sentence payload
+            // [end_pos - 1] = '\r'
+            // [end_pos]     = '\n'
+            //
+            // Prepare to consume the sentence from the buffer
+
+            // Validate the checksum over the sentence
+            if (!NMEA_checksum(&gps_rx_buffer[1])) { // Invalid checksum.  May indicate dropped characters on Rx.
+                                                     // PIOS_DEBUG_PinHigh(2);
+                gpsRxStats->gpsRxChkSumError++;
+                // PIOS_DEBUG_PinLow(2);
+                ret = PARSER_ERROR;
+            } else { // Valid checksum, use this packet to update the GPS position
+                if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) {
+                    // PIOS_DEBUG_PinHigh(2);
+                    gpsRxStats->gpsRxParserError++;
+                    // PIOS_DEBUG_PinLow(2);
+                } else {
+                    gpsRxStats->gpsRxReceived++;
+                };
+
+                ret = PARSER_COMPLETE;
+            }
         }
     }
-    return PARSER_INCOMPLETE;
+    return ret;
 }
 
 static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)

flight/modules/GPS/UBX.c

@@ -105,8 +105,9 @@ struct UBX_ACK_NAK ubxLastNak;
 #define UBX_PVT_TIMEOUT (1000)
 // parse incoming character stream for messages in UBX binary format
 
-int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
+int parse_ubx_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
 {
+    int ret = PARSER_INCOMPLETE; // message not (yet) complete
     enum proto_states {
         START,
         UBX_SY2,
@@ -119,83 +120,85 @@ int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsD
         UBX_CHK2,
         FINISHED
     };
-
+    uint8_t c;
     static enum proto_states proto_state = START;
     static uint8_t rx_count = 0;
     struct UBXPacket *ubx   = (struct UBXPacket *)gps_rx_buffer;
 
-    switch (proto_state) {
+    for (int i = 0; i < len; i++) {
-    case START: // detect protocol
+        c = rx[i];
-        if (c == UBX_SYNC1) { // first UBX sync char found
+        switch (proto_state) {
-            proto_state = UBX_SY2;
+        case START: // detect protocol
-        }
+            if (c == UBX_SYNC1) { // first UBX sync char found
-        break;
+                proto_state = UBX_SY2;
-    case UBX_SY2:
-        if (c == UBX_SYNC2) { // second UBX sync char found
-            proto_state = UBX_CLASS;
-        } else {
-            proto_state = START; // reset state
-        }
-        break;
-    case UBX_CLASS:
-        ubx->header.class = c;
-        proto_state      = UBX_ID;
-        break;
-    case UBX_ID:
-        ubx->header.id   = c;
-        proto_state      = UBX_LEN1;
-        break;
-    case UBX_LEN1:
-        ubx->header.len  = c;
-        proto_state      = UBX_LEN2;
-        break;
-    case UBX_LEN2:
-        ubx->header.len += (c << 8);
-        if (ubx->header.len > sizeof(UBXPayload)) {
-            gpsRxStats->gpsRxOverflow++;
-            proto_state = START;
-        } else {
-            rx_count    = 0;
-            proto_state = UBX_PAYLOAD;
-        }
-        break;
-    case UBX_PAYLOAD:
-        if (rx_count < ubx->header.len) {
-            ubx->payload.payload[rx_count] = c;
-            if (++rx_count == ubx->header.len) {
-                proto_state = UBX_CHK1;
             }
-        } else {
+            break;
-            gpsRxStats->gpsRxOverflow++;
+        case UBX_SY2:
-            proto_state = START;
+            if (c == UBX_SYNC2) { // second UBX sync char found
+                proto_state = UBX_CLASS;
+            } else {
+                proto_state = START; // reset state
+            }
+            break;
+        case UBX_CLASS:
+            ubx->header.class = c;
+            proto_state      = UBX_ID;
+            break;
+        case UBX_ID:
+            ubx->header.id   = c;
+            proto_state      = UBX_LEN1;
+            break;
+        case UBX_LEN1:
+            ubx->header.len  = c;
+            proto_state      = UBX_LEN2;
+            break;
+        case UBX_LEN2:
+            ubx->header.len += (c << 8);
+            if (ubx->header.len > sizeof(UBXPayload)) {
+                gpsRxStats->gpsRxOverflow++;
+                proto_state = START;
+            } else {
+                rx_count    = 0;
+                proto_state = UBX_PAYLOAD;
+            }
+            break;
+        case UBX_PAYLOAD:
+            if (rx_count < ubx->header.len) {
+                ubx->payload.payload[rx_count] = c;
+                if (++rx_count == ubx->header.len) {
+                    proto_state = UBX_CHK1;
+                }
+            } else {
+                gpsRxStats->gpsRxOverflow++;
+                proto_state = START;
+            }
+            break;
+        case UBX_CHK1:
+            ubx->header.ck_a = c;
+            proto_state = UBX_CHK2;
+            break;
+        case UBX_CHK2:
+            ubx->header.ck_b = c;
+            if (checksum_ubx_message(ubx)) { // message complete and valid
+                parse_ubx_message(ubx, GpsData);
+                proto_state = FINISHED;
+            } else {
+                gpsRxStats->gpsRxChkSumError++;
+                proto_state = START;
+            }
+            break;
+        default: break;
         }
-        break;
+
-    case UBX_CHK1:
+        if (proto_state == START) {
-        ubx->header.ck_a = c;
+            ret = (ret != PARSER_COMPLETE) ? PARSER_ERROR : PARSER_COMPLETE; // parser couldn't use this byte
-        proto_state = UBX_CHK2;
+        } else if (proto_state == FINISHED) {
-        break;
+            gpsRxStats->gpsRxReceived++;
-    case UBX_CHK2:
-        ubx->header.ck_b = c;
-        if (checksum_ubx_message(ubx)) { // message complete and valid
-            parse_ubx_message(ubx, GpsData);
-            proto_state = FINISHED;
-        } else {
-            gpsRxStats->gpsRxChkSumError++;
             proto_state = START;
+            ret = PARSER_COMPLETE; // message complete & processed
         }
-        break;
-    default: break;
     }
-
+    return ret;
-    if (proto_state == START) {
-        return PARSER_ERROR; // parser couldn't use this byte
-    } else if (proto_state == FINISHED) {
-        gpsRxStats->gpsRxReceived++;
-        proto_state = START;
-        return PARSER_COMPLETE; // message complete & processed
-    }
-
-    return PARSER_INCOMPLETE; // message not (yet) complete
 }
 
 

flight/modules/GPS/inc/NMEA.h

@@ -39,6 +39,6 @@
 
 extern bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData);
 extern bool NMEA_checksum(char *nmea_sentence);
-extern int parse_nmea_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
+extern int parse_nmea_stream(uint8_t *, uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
 
 #endif /* NMEA_H */

flight/modules/GPS/inc/UBX.h

@@ -400,7 +400,7 @@ extern struct UBX_ACK_NAK ubxLastNak;
 bool checksum_ubx_message(struct UBXPacket *);
 uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);
 
-int parse_ubx_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
+int parse_ubx_stream(uint8_t *rx, uint8_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
 void load_mag_settings();
 
 #endif /* UBX_H */

flight/targets/boards/revolution/firmware/pios_board.c

@@ -216,7 +216,7 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
 #define PIOS_COM_TELEM_RF_RX_BUF_LEN     512
 #define PIOS_COM_TELEM_RF_TX_BUF_LEN     512
 
-#define PIOS_COM_GPS_RX_BUF_LEN          32
+#define PIOS_COM_GPS_RX_BUF_LEN          128
 #define PIOS_COM_GPS_TX_BUF_LEN          32
 
 #define PIOS_COM_TELEM_USB_RX_BUF_LEN    65