[ublox] Fixed wrong sample rate and updated internals.

Fixed a bug in the message packing routing causing the device to reject any sample rate change command.

src/shared/drivers/gps/Gps.cpp

@@ -22,6 +22,7 @@
 
 #include <drivers/gps/Gps.h>
 #include <drivers/nmea/nmea.h>
+
 #include <cmath>
 
 #include "drivers/serial.h"
@@ -173,8 +174,6 @@ bool Gps::serialComSetup()
         0xD0, 0x08, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0x07, 0x00,
         0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff};
 
-    uint8_t msg[SET_RATE_MSG_LEN];
-
     intrusive_ref_ptr<DevFs> devFs = FilesystemManager::instance().getDevFs();
     if (!devFs->remove(serialPortName))
         return false;
@@ -207,7 +206,8 @@ bool Gps::serialComSetup()
     bytes 6 and 7 for rate
     last 2 bytes for checksum
     */
-    packRateMessage(msg, betweenReadings, 1);
+    uint8_t msg[SET_RATE_MSG_LEN];
+    packRateMessage(msg, betweenReadings);
     write(fd, msg, SET_RATE_MSG_LEN);
 
     /*
@@ -311,6 +311,7 @@ void Gps::packSBASMessge(uint8_t* msg, int mode, int usage, int maxChannelNum,
     {
         msg[9 + i] = 0x00;
     }
+
     for (int i = 0; i < 3; i++)
     {
         int bit =
@@ -330,10 +331,10 @@ void Gps::packSBASMessge(uint8_t* msg, int mode, int usage, int maxChannelNum,
     ubxChecksum(msg, SET_SBAS_MSG_LEN);
 }
 
-void Gps::packRateMessage(uint8_t* msg, int inbetweenReadings, int timeRef)
+void Gps::packRateMessage(uint8_t* msg, int inbetweenReadings, int navRate,
+                          TimeRef timeRef)
 {
-    // To avoid GCC warning
-    (void)timeRef;
+    uint16_t timeRefIdx = static_cast<uint16_t>(timeRef);
 
     msg[0] = UbxHeader[0];
     msg[1] = UbxHeader[1];
@@ -345,6 +346,12 @@ void Gps::packRateMessage(uint8_t* msg, int inbetweenReadings, int timeRef)
     msg[6] = inbetweenReadings & 0x00ff;
     msg[7] = (inbetweenReadings & 0xff00) >> 8;
 
+    msg[8] = navRate & 0x00ff;
+    msg[9] = (navRate & 0xff00) >> 8;
+
+    msg[10] = timeRefIdx & 0x00ff;
+    msg[11] = (timeRefIdx & 0xff00) >> 8;
+
     ubxChecksum(msg, SET_RATE_MSG_LEN);
 }
 
@@ -369,9 +376,9 @@ void Gps::sendSBASMessage(int mode, int usage, int maxChannelNum, int PRNs[3])
     write(fd, msg, SET_SBAS_MSG_LEN);
 }
 
-void Gps::sendRateMessage(int inbetweenReadings, int timeRef)
+void Gps::sendRateMessage(int inbetweenReadings, int navRate, TimeRef timeRef)
 {
     uint8_t msg[SET_RATE_MSG_LEN];
-    packRateMessage(msg, inbetweenReadings, timeRef);
+    packRateMessage(msg, inbetweenReadings, navRate, timeRef);
     write(fd, msg, SET_RATE_MSG_LEN);
 }

src/shared/drivers/gps/Gps.h

@@ -32,26 +32,21 @@
 using miosix::FastMutex;
 using miosix::Thread;
 
-struct GPSData_old
-{
-    /// timestamp in ms (anakin time, not GPS time). getTick()-timestamp tells
-    /// you how "old" the data is.
-    long long timestamp;
-
-    double latitude;      ///< [deg] //TODO: cast to float??
-    double longitude;     ///< [deg] //TODO: cast to float??
-    double altitude;      ///< [m]   //TODO: cast to float??
-    float velocityNorth;  ///< [m/s]
-    float velocityEast;   ///< [m/s]
-    float speed;          ///< [m/s]
-    int numSatellites;    ///< [1]
-    float track;          ///< [deg]
-    bool fix;
-};
-
 class Gps : public Sensor<GPSData>, public ActiveObject
 {
 public:
+    /*
+     * Which time reference to use
+     */
+    enum class TimeRef
+    {
+        UTC     = 0,
+        GPS     = 1,
+        GLONASS = 2,
+        BEIDOU  = 3,
+        GALILEO = 4
+    };
+
     Gps(int baudrate = 460800, int sampleRate = 10, int serialPortNum = 2,
         const char *portName = "gps");
 
@@ -81,10 +76,10 @@ public:
 
     /*
      * Packs and sends a UBX configuration message to the GPS module to set the
-     * rate. By default it samples the gps every 100 ms and uses GPS time (1, to
-     * use UTC time set 0)
+     * rate. By default it samples the gps every 100 ms and uses GPS time
      */
-    void sendRateMessage(int inbetweenReadings = 100, int timeRef = 1);
+    void sendRateMessage(int inbetweenReadings = 100, int navRate = 1,
+                         TimeRef timeRef = TimeRef::GPS);
 
 private:
     /*
@@ -129,11 +124,11 @@ private:
      * Length: 14 bytes
      * bytes 0-5 as UBX standard (header x2, class x1, id x1, length x2)
      * bytes 6-7 measurement rate, time between readings, default 100
-     * bytes 8-9 navRate, must be set to 1
-     * bytes timeRef, Alignment to reference time: 0 = UTC time, 1 = GPS time
+     * bytes 8-9 navRate, measurements taken before creating a solution, default
+     * 1 bytes timeRef, Alignment to reference time, default GPS
      */
     void packRateMessage(uint8_t *msg, int inbetweenReadings = 100,
-                         int timeRef = 1);
+                         int navRate = 1, TimeRef timeRef = TimeRef::GPS);
 
     struct GPSData data;
     mutable FastMutex mutex;

src/tests/drivers/test-ublox.cpp

@@ -26,7 +26,7 @@ int main()
 
     while (1)
     {
-        Thread::sleep(1000);
+        Thread::sleep(2000);
         gps.sample();
 
         dataGPS = gps.getLastSample();