.vscode/settings.json

@@ -134,12 +134,14 @@
         "croll",
         "cwise",
         "cyaw",
+        "datasheet",
         "deleteme",
         "DMEIE",
         "Doxyfile",
         "doxygen",
         "DRDY",
         "Duca",
+        "ECEF",
         "Ecompass",
         "Eigen",
         "elfs",
@@ -175,6 +177,7 @@
         "logdecoder",
         "Luca",
         "MEKF",
+        "microcontrollers",
         "MINC",
         "miosix",
         "mkdir",
@@ -184,6 +187,7 @@
         "NBAR",
         "NDTR",
         "NGPS",
+        "Nidasio",
         "NMAG",
         "NMEA",
         "NMEKF",
@@ -193,6 +197,7 @@
         "PINC",
         "Pitot",
         "Plin",
+        "POSLLH",
         "Qgbw",
         "Qget",
         "Qhandle",
@@ -218,6 +223,7 @@
         "testsuite",
         "TSCPP",
         "Ublox",
+        "ubloxgps",
         "UBXACK",
         "UBXGPS",
         "UBXNAV",

CMakeLists.txt

@@ -353,7 +353,7 @@ add_executable(test-ms5803 src/tests/sensors/test-ms5803.cpp)
 sbs_target(test-ms5803 stm32f429zi_skyward_death_stack_x)
 
 add_executable(test-ubxgps-serial src/tests/sensors/test-ubxgps-serial.cpp)
-sbs_target(test-ubxgps-serial stm32f429zi_skyward_death_stack_x)
+sbs_target(test-ubxgps-serial stm32f429zi_stm32f4discovery)
 
 add_executable(test-ubxgps-spi src/tests/sensors/test-ubxgps-spi.cpp)
 sbs_target(test-ubxgps-spi stm32f429zi_skyward_death_stack_x)

src/shared/drivers/adc/InternalADC.cpp

@@ -219,14 +219,14 @@ InternalADCData InternalADC::sampleImpl()
 
         // This should trigger the DMA stream for each channel's conversion
 
-        // Wait for tranfer end
+        // Wait for transfer end
         while (!(*statusReg & (transferCompleteMask | transferErrorMask)))
             ;
 
         // Clear the transfer complete flag
         *clearFlagReg |= transferCompleteMask;
 
-        // If and error has occurred (probaly due to a higher priority stream)
+        // If and error has occurred (probably due to a higher priority stream)
         // don't update the timestamp, the values should not have been updated
         if (*statusReg & transferErrorMask)
         {

src/shared/sensors/UBXGPS/UBXFrame.h

@@ -35,6 +35,8 @@ namespace Boardcore
  */
 enum class UBXMessage : uint16_t
 {
+    UBX_NAV_POSLLH = 0x0201,  // Geodetic position solution
+    UBX_NAV_SOL    = 0x0601,  // Navigation solution information
     UBX_NAV_PVT    = 0x0701,  // Navigation position velocity time solution
     UBX_ACK_NAK    = 0x0005,  // Message acknowledged
     UBX_ACK_ACK    = 0x0105,  // Message not acknowledged
@@ -200,6 +202,52 @@ public:
     bool isValid() const;
 };
 
+struct UBXPosllhFrame : public UBXFrame
+{
+    struct __attribute__((packed)) Payload
+    {
+        uint32_t iTOW;   // GPS time of week of the navigation epoch [ms]
+        int32_t lon;     // Longitude {1e-7} [deg]
+        int32_t lat;     // Latitude {1e-7} [deg]
+        int32_t height;  // Height above ellipsoid [mm]
+        int32_t hMSL;    // Height above mean sea level [mm]
+        uint32_t hAcc;   // Horizontal accuracy estimate [mm]
+        uint32_t vAcc;   // Vertical accuracy estimate [mm]
+    };
+
+    Payload& getPayload() const;
+
+    bool isValid() const;
+};
+
+struct UBXSolFrame : public UBXFrame
+{
+    struct __attribute__((packed)) Payload
+    {
+        uint32_t iTOW;   // GPS time of week of the navigation epoch [ms]
+        int32_t fTOW;    // Fractional nanoseconds remainder of rounded ms
+        int16_t week;    // GPS week (GPS time)
+        uint8_t gpsFix;  // GPS Fix type
+        uint8_t flags;   // Fix status flags
+        int32_t ecefX;   // ECEF X coordinate [cm]
+        int32_t ecefY;   // ECEF Y coordinate [cm]
+        int32_t ecefZ;   // ECEF Z coordinate [cm]
+        uint32_t pAcc;   // 3D position accuracy estimate [cm]
+        int32_t ecefVX;  // ECEF X velocity [cm/s]
+        int32_t ecefVY;  // ECEF y velocity [cm/s]
+        int32_t ecefVZ;  // ECEF z velocity [cm/s]
+        uint32_t sAcc;   // [cm/s]
+        uint16_t pDOP;   // Position DOP
+        uint8_t reserved1;
+        uint8_t numSV;  // Number of satellites used in Nav Solution
+        uint8_t reserved2;
+    };
+
+    Payload& getPayload() const;
+
+    bool isValid() const;
+};
+
 inline UBXFrame::UBXFrame(UBXMessage message, const uint8_t* payload,
                           uint16_t payloadLength)
     : message(static_cast<uint16_t>(message)), payloadLength(payloadLength)
@@ -300,9 +348,29 @@ inline UBXPvtFrame::Payload& UBXPvtFrame::getPayload() const
     return (Payload&)payload;
 }
 
+inline UBXPosllhFrame::Payload& UBXPosllhFrame::getPayload() const
+{
+    return (Payload&)payload;
+}
+
+inline UBXSolFrame::Payload& UBXSolFrame::getPayload() const
+{
+    return (Payload&)payload;
+}
+
 inline bool UBXPvtFrame::isValid() const
 {
     return UBXFrame::isValid() && getMessage() == UBXMessage::UBX_NAV_PVT;
 }
 
+inline bool UBXPosllhFrame::isValid() const
+{
+    return UBXFrame::isValid() && getMessage() == UBXMessage::UBX_NAV_POSLLH;
+}
+
+inline bool UBXSolFrame::isValid() const
+{
+    return UBXFrame::isValid() && getMessage() == UBXMessage::UBX_NAV_SOL;
+}
+
 }  // namespace Boardcore

src/shared/sensors/UBXGPS/UBXGPSSerial.cpp

@@ -55,14 +55,14 @@ bool UBXGPSSerial::init()
         return false;
     }
 
-    LOG_DEBUG(logger, "Resetting the device...");
+    // LOG_DEBUG(logger, "Resetting the device...");
 
-    if (!reset())
-    {
-        lastError = SensorErrors::INIT_FAIL;
-        LOG_ERR(logger, "Could not reset the device");
-        return false;
-    }
+    // if (!reset())
+    // {
+    //     lastError = SensorErrors::INIT_FAIL;
+    //     LOG_ERR(logger, "Could not reset the device");
+    //     return false;
+    // }
 
     LOG_DEBUG(logger, "Setting the UBX protocol...");
 
@@ -91,9 +91,16 @@ bool UBXGPSSerial::init()
         return false;
     }
 
-    LOG_DEBUG(logger, "Setting the PVT message rate...");
+    LOG_DEBUG(logger, "Setting the messages rate...");
+
+    if (!setPOSLLHMessageRate())
+    {
+        lastError = SensorErrors::INIT_FAIL;
+        LOG_ERR(logger, "Could not set the PVT message rate");
+        return false;
+    }
 
-    if (!setPVTMessageRate())
+    if (!setSOLMessageRate())
     {
         lastError = SensorErrors::INIT_FAIL;
         LOG_ERR(logger, "Could not set the PVT message rate");
@@ -289,6 +296,30 @@ bool UBXGPSSerial::setPVTMessageRate()
     return safeWriteUBXFrame(frame);
 }
 
+bool UBXGPSSerial::setPOSLLHMessageRate()
+{
+    uint8_t payload[] = {
+        0x01, 0x02,  // POSLLH message
+        0x01         // Rate = 1
+    };
+
+    UBXFrame frame{UBXMessage::UBX_CFG_MSG, payload, sizeof(payload)};
+
+    return safeWriteUBXFrame(frame);
+}
+
+bool UBXGPSSerial::setSOLMessageRate()
+{
+    uint8_t payload[] = {
+        0x01, 0x06,  // POSLLH message
+        0x01         // Rate = 1
+    };
+
+    UBXFrame frame{UBXMessage::UBX_CFG_MSG, payload, sizeof(payload)};
+
+    return safeWriteUBXFrame(frame);
+}
+
 bool UBXGPSSerial::readUBXFrame(UBXFrame& frame)
 {
     // Search UBX frame preamble byte by byte
@@ -311,7 +342,7 @@ bool UBXGPSSerial::readUBXFrame(UBXFrame& frame)
         else
         {
             i = 0;
-            LOG_DEBUG(logger, "Received unexpected byte: {:02x} {:#c}", c, c);
+            // LOG_DEBUG(logger, "Received unexpected byte: {:02x}", c);
         }
     }
 
@@ -394,15 +425,23 @@ void UBXGPSSerial::run()
 {
     while (!shouldStop())
     {
-        UBXPvtFrame pvt;
+        UBXFrame frame;
 
         // Try to read the message
-        if (!readUBXFrame(pvt))
+        if (!readUBXFrame(frame))
         {
             LOG_DEBUG(logger, "Unable to read a UBX message");
             continue;
         }
 
+        uint8_t data[frame.getLength()];
+        frame.writePacked(data);
+
+        UBXPvtFrame pvt;
+        pvt.readPacked(data);
+
+        if (pvt.isValid())
+        {
             UBXPvtFrame::Payload& pvtP = pvt.getPayload();
 
             threadSample.gpsTimestamp =
@@ -418,6 +457,35 @@ void UBXGPSSerial::run()
             threadSample.positionDOP   = (float)pvtP.pDOP / 1e2;
             threadSample.satellites    = pvtP.numSV;
             threadSample.fix           = pvtP.fixType;
+        }
+
+        UBXPosllhFrame posllh;
+        posllh.readPacked(data);
+
+        if (posllh.isValid())
+        {
+            UBXPosllhFrame::Payload& posllhP = posllh.getPayload();
+
+            threadSample.gpsTimestamp =
+                TimestampTimer::getInstance().getTimestamp();
+            threadSample.latitude  = (float)posllhP.lat / 1e7;
+            threadSample.longitude = (float)posllhP.lon / 1e7;
+            threadSample.height    = (float)posllhP.height / 1e3;
+        }
+
+        UBXSolFrame sol;
+        sol.readPacked(data);
+
+        if (sol.isValid())
+        {
+            UBXSolFrame::Payload& solP = sol.getPayload();
+
+            threadSample.gpsTimestamp =
+                TimestampTimer::getInstance().getTimestamp();
+            threadSample.positionDOP = (float)solP.pDOP / 1e2;
+            threadSample.satellites  = solP.numSV;
+            threadSample.fix         = solP.gpsFix;
+        }
 
         StackLogger::getInstance().updateStack(THID_GPS);
     }

src/shared/sensors/UBXGPS/UBXGPSSerial.h

@@ -132,6 +132,10 @@ private:
      */
     bool setPVTMessageRate();
 
+    bool setPOSLLHMessageRate();
+
+    bool setSOLMessageRate();
+
     /**
      * @brief Reads a UBX frame.
      *

src/tests/sensors/test-ubxgps-serial.cpp

@@ -30,38 +30,31 @@
 using namespace Boardcore;
 using namespace miosix;
 
-#define RATE 4
+typedef miosix::Gpio<GPIOD_BASE, 5> u2tx2;
+
+#define RATE 1
 
 int main()
 {
-    (void)TimestampTimer::getInstance();
-
-    printf("Welcome to the ublox test\n");
+    u2tx2::mode(Mode::ALTERNATE);
+    u2tx2::alternateFunction(7);
 
     // Keep GPS baud rate at default for easier testing
     UBXGPSSerial gps(38400, RATE, 2, "gps", 9600);
-    UBXGPSData dataGPS;
+    UBXGPSData data;
     printf("Gps allocated\n");
 
     // Init the gps
     if (gps.init())
-    {
         printf("Successful gps initialization\n");
-    }
     else
-    {
         printf("Failed gps initialization\n");
-    }
 
     // Perform the self test
     if (gps.selfTest())
-    {
         printf("Successful gps self test\n");
-    }
     else
-    {
         printf("Failed gps self test\n");
-    }
 
     // Start the gps thread
     gps.start();
@@ -69,23 +62,21 @@ int main()
 
     while (true)
     {
-        printf("a\n");
         // Give time to the thread
         Thread::sleep(1000 / RATE);
 
         // Sample
         gps.sample();
-        printf("b\n");
-        dataGPS = gps.getLastSample();
+        data = gps.getLastSample();
 
         // Print out the latest sample
         printf(
-            "[gps] timestamp: % 4.3f, fix: %01d lat: % f lon: % f "
-            "height: %4.1f nsat: %2d speed: %3.2f velN: % 3.2f velE: % 3.2f "
-            "track %3.1f\n",
-            (float)dataGPS.gpsTimestamp / 1000000, dataGPS.fix,
-            dataGPS.latitude, dataGPS.longitude, dataGPS.height,
-            dataGPS.satellites, dataGPS.speed, dataGPS.velocityNorth,
-            dataGPS.velocityEast, dataGPS.track);
+            "[%.2f] lat: % f lon: % f height: %4.1f velN: % 3.2f velE: % 3.2f "
+            "velD: % 3.2f speed: %3.2f track: %.2f posDOP: %.2f nSat: %2d fix: "
+            "%01d\n",
+            data.gpsTimestamp / 1e6, data.latitude, data.longitude, data.height,
+            data.velocityNorth, data.velocityEast, data.velocityDown,
+            data.speed, data.track, data.positionDOP, data.satellites,
+            data.fix);
     }
 }