[ublox] Fixed buffer overflow and swapped getTick for getTimestamp

src/shared/drivers/gps/Gps.cpp

@@ -20,6 +20,7 @@
  * THE SOFTWARE.
  */
 
+#include <TimestampTimer.h>
 #include <drivers/gps/Gps.h>
 #include <drivers/nmea/nmea.h>
 
@@ -70,14 +71,9 @@ void Gps::run()
             selfTestFlag   = false;
         }
 
-        int i = 0;
-        read(fd, msg, 1);
-        while (msg[i] != '\n')
-        {
-            i++;
-            read(fd, &msg[i], 1);
-        }
-        msg[++i] = '\0';
+        // If the message was truncated don't even bother with parsing...
+        if (!readMessageIn(msg, sizeof(msg)))
+            continue;
 
         sentence_id = minmea_sentence_id(msg, 0);
 
@@ -100,7 +96,7 @@ void Gps::run()
                     {
                         {
                             data.fix           = true;
-                            data.gps_timestamp = getTick();
+                            data.gps_timestamp = TimestampTimer::getTimestamp();
                             deg                = frame_rmc.latitude.value /
                                   frame_rmc.latitude.scale / 100;
                             data.latitude =
@@ -160,9 +156,14 @@ void Gps::run()
                 }
                 break;
 
-            default:
+            case MINMEA_INVALID:
+            case MINMEA_UNKNOWN:
                 TRACE("Unrecognized NMEA message: %s", msg);
                 break;
+
+            default:
+                TRACE("Unexpected NMEA message\n");
+                break;
         }
     }
 }
@@ -261,14 +262,8 @@ bool Gps::selfTestInThread()
 
     for (int attempts = 0; attempts < 4; attempts++)
     {
-        int i = 0;
-        read(fd, msg, 1);
-        while (msg[i] != '\n')
-        {
-            i++;
-            read(fd, &msg[i], 1);
-        }
-        msg[++i] = '\0';
+        if (!readMessageIn(msg, sizeof(msg)))
+            continue;
 
         sentence_id = minmea_sentence_id(msg, 0);
         if (sentence_id != MINMEA_INVALID && sentence_id != MINMEA_UNKNOWN)
@@ -369,6 +364,28 @@ void Gps::ubxChecksum(uint8_t* msg, int len)
     msg[len - 1] = ck_b;
 }
 
+bool Gps::readMessageIn(char* msg, int len)
+{
+    int i = 0;
+    char c;
+
+    // We need to read a whole message, even if there is a buffer overflow.
+    do
+    {
+        read(fd, &c, 1);
+
+        // Prevent buffer overflows
+        if (i < len)
+            msg[i] = c;
+
+        i++;
+    } while (c != '\n');
+
+    msg[i++] = '\0';
+
+    return i < len;
+}
+
 void Gps::sendSBASMessage(int mode, int usage, int maxChannelNum, int PRNs[3])
 {
     uint8_t msg[SET_SBAS_MSG_LEN];

src/shared/drivers/gps/Gps.h

@@ -103,6 +103,12 @@ private:
      */
     void ubxChecksum(uint8_t *msg, int len);
 
+    /*
+     * Reads a message with proper bound checks.
+     * Returns false in case of buffer overflow.
+     */
+    bool readMessageIn(char *msg, int len);
+
     /*
      * Packs SBAS message:
      *

src/tests/drivers/test-ublox.cpp

 #include <Common.h>
+#include <TimestampTimer.h>
 #include <drivers/gps/Gps.h>
 #include <drivers/serial.h>
 #include <fcntl.h>
@@ -13,8 +14,10 @@ int main()
     devFs->addDevice("gps",
                      intrusive_ref_ptr<Device>(new STM32Serial(2, 38400)));
 
+    TimestampTimer::enableTimestampTimer();
+
     // Keep GPS baud rate at default for easier testing
-    Gps gps(38400);
+    Gps gps(38400, 25);
     struct GPSData dataGPS;
 
     printf("init gps: %d\n", gps.init());
@@ -31,12 +34,13 @@ int main()
 
         dataGPS = gps.getLastSample();
         printf(
-            "fix: %d t: %lld lat: %f lon: %f height: %f nsat: %d speed: %f "
+            "timestamp: %.3f, fix: %d t: %lld lat: %f lon: %f height: %f nsat: "
+            "%d speed: %f "
             "velN: %f velE: "
             "%f track %f\n",
-            dataGPS.fix, dataGPS.gps_timestamp, dataGPS.latitude,
-            dataGPS.longitude, dataGPS.height, dataGPS.num_satellites,
-            dataGPS.speed, dataGPS.velocity_north, dataGPS.velocity_east,
-            dataGPS.track);
+            (float)dataGPS.gps_timestamp / 1000000, dataGPS.fix,
+            dataGPS.gps_timestamp, dataGPS.latitude, dataGPS.longitude,
+            dataGPS.height, dataGPS.num_satellites, dataGPS.speed,
+            dataGPS.velocity_north, dataGPS.velocity_east, dataGPS.track);
     }
 }