diff --git a/.vscode/settings.json b/.vscode/settings.json
index e1a98af224ab9e9c71af4a5ba32200cd6d082039..5791694b122349dd5977372e80e5978a984cdf42 100644
--- a/.vscode/settings.json
+++ b/.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",
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c8fdb7953d78dbc335ce4b0cddd2a3a6681fe1e2..002a51ede24064c42d385c9ca7622101b0241e9f 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/src/shared/drivers/adc/InternalADC.cpp b/src/shared/drivers/adc/InternalADC.cpp
index acfe5e21f31f2b8e394d516c4688086f8d351ddc..32576a66240a1d87266533b5cb07abb0136870e1 100644
--- a/src/shared/drivers/adc/InternalADC.cpp
+++ b/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)
{
diff --git a/src/shared/sensors/UBXGPS/UBXFrame.h b/src/shared/sensors/UBXGPS/UBXFrame.h
index 9f1593baaa01a1ce8b3d7be31488e9ba12cfd05d..50b8ca12af6bc9526a4445623410ffe17f7e1546 100644
--- a/src/shared/sensors/UBXGPS/UBXFrame.h
+++ b/src/shared/sensors/UBXGPS/UBXFrame.h
@@ -35,14 +35,16 @@ namespace Boardcore
*/
enum class UBXMessage : uint16_t
{
- UBX_NAV_PVT = 0x0701, // Navigation position velocity time solution
- UBX_ACK_NAK = 0x0005, // Message acknowledged
- UBX_ACK_ACK = 0x0105, // Message not acknowledged
- UBX_CFG_PRT = 0x0006, // Port configuration
- UBX_CFG_MSG = 0x0106, // Set message rate
- UBX_CFG_RST = 0x0406, // Reset receiver
- UBX_CFG_RATE = 0x0806, // Navigation/measurement rate settings
- UBX_CFG_NAV5 = 0x2406, // Navigation engine settings
+ 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
+ UBX_CFG_PRT = 0x0006, // Port configuration
+ UBX_CFG_MSG = 0x0106, // Set message rate
+ UBX_CFG_RST = 0x0406, // Reset receiver
+ UBX_CFG_RATE = 0x0806, // Navigation/measurement rate settings
+ UBX_CFG_NAV5 = 0x2406, // Navigation engine settings
};
/**
@@ -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
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp b/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
index 4bd60787c5a033840be60e2b64a2bb4c4e218bda..d6a3a9bec5bd35bc4093f24dc65f646a55a474b4 100644
--- a/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
+++ b/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 (!setPVTMessageRate())
+ if (!setPOSLLHMessageRate())
+ {
+ lastError = SensorErrors::INIT_FAIL;
+ LOG_ERR(logger, "Could not set the PVT message rate");
+ return false;
+ }
+
+ 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,30 +425,67 @@ 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;
}
- UBXPvtFrame::Payload& pvtP = pvt.getPayload();
-
- threadSample.gpsTimestamp =
- TimestampTimer::getInstance().getTimestamp();
- threadSample.latitude = (float)pvtP.lat / 1e7;
- threadSample.longitude = (float)pvtP.lon / 1e7;
- threadSample.height = (float)pvtP.height / 1e3;
- threadSample.velocityNorth = (float)pvtP.velN / 1e3;
- threadSample.velocityEast = (float)pvtP.velE / 1e3;
- threadSample.velocityDown = (float)pvtP.velD / 1e3;
- threadSample.speed = (float)pvtP.gSpeed / 1e3;
- threadSample.track = (float)pvtP.headMot / 1e5;
- threadSample.positionDOP = (float)pvtP.pDOP / 1e2;
- threadSample.satellites = pvtP.numSV;
- threadSample.fix = pvtP.fixType;
+ uint8_t data[frame.getLength()];
+ frame.writePacked(data);
+
+ UBXPvtFrame pvt;
+ pvt.readPacked(data);
+
+ if (pvt.isValid())
+ {
+ UBXPvtFrame::Payload& pvtP = pvt.getPayload();
+
+ threadSample.gpsTimestamp =
+ TimestampTimer::getInstance().getTimestamp();
+ threadSample.latitude = (float)pvtP.lat / 1e7;
+ threadSample.longitude = (float)pvtP.lon / 1e7;
+ threadSample.height = (float)pvtP.height / 1e3;
+ threadSample.velocityNorth = (float)pvtP.velN / 1e3;
+ threadSample.velocityEast = (float)pvtP.velE / 1e3;
+ threadSample.velocityDown = (float)pvtP.velD / 1e3;
+ threadSample.speed = (float)pvtP.gSpeed / 1e3;
+ threadSample.track = (float)pvtP.headMot / 1e5;
+ 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);
}
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSerial.h b/src/shared/sensors/UBXGPS/UBXGPSSerial.h
index 4785778c2beab52185b2b614f06db7ab2c0e4769..d0a91a1c84f4477b0cec1651e85a0030ca3aafe0 100644
--- a/src/shared/sensors/UBXGPS/UBXGPSSerial.h
+++ b/src/shared/sensors/UBXGPS/UBXGPSSerial.h
@@ -132,6 +132,10 @@ private:
*/
bool setPVTMessageRate();
+ bool setPOSLLHMessageRate();
+
+ bool setSOLMessageRate();
+
/**
* @brief Reads a UBX frame.
*
diff --git a/src/tests/sensors/test-ubxgps-serial.cpp b/src/tests/sensors/test-ubxgps-serial.cpp
index cf7fe1c50ed6a63c56ec534207b02bbee16a6898..8c61fb72b5523c9d761a1aeafb8e042db9140757 100644
--- a/src/tests/sensors/test-ubxgps-serial.cpp
+++ b/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 selftest
+ // Perform the self test
if (gps.selfTest())
- {
- printf("Successful gps selftest\n");
- }
+ printf("Successful gps self test\n");
else
- {
- printf("Failed gps selftest\n");
- }
+ 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);
}
}