diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index a41cd2089e3c3d93d8ae7ebeae9a6546e2645f57..67b3b9ff613f4759e09a39a6f2d99f273dc2df33 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -51,8 +51,7 @@
"${workspaceFolder}/src/tests"
],
"limitSymbolsToIncludedHeaders": true
- },
- "compileCommands": "${workspaceFolder}/build/compile_commands.json"
+ }
},
{
"name": "stm32f429zi_stm32f4discovery",
@@ -115,7 +114,7 @@
"defines": [
"DEBUG",
"_ARCH_CORTEXM4_STM32F4",
- "_BOARD_STM32F429ZI_SKYWARD_DEATH_STACK_X",
+ "_BOARD_STM32F429ZI_SKYWARD_DEATHST_X",
"_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x",
"HSE_VALUE=8000000",
"SYSCLK_FREQ_168MHz=168000000",
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 43e514f2603ebb9bb2b90fdab6c1c80067e2f17d..e1a98af224ab9e9c71af4a5ba32200cd6d082039 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -126,6 +126,7 @@
"Baro",
"boardcore",
"Boardcorev",
+ "boudrate",
"Corigliano",
"CORTEXM",
"cpitch",
@@ -149,6 +150,7 @@
"fiprintf",
"Gatttr",
"getdetahstate",
+ "GNSS",
"Gpio",
"GPIOA",
"GPIOB",
@@ -183,6 +185,7 @@
"NDTR",
"NGPS",
"NMAG",
+ "NMEA",
"NMEKF",
"nord",
"NVIC",
@@ -215,6 +218,9 @@
"testsuite",
"TSCPP",
"Ublox",
+ "UBXACK",
+ "UBXGPS",
+ "UBXNAV",
"Unsync",
"upcounter",
"USART",
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 53273bb8481f7b3ac5c2385fd4505fd29a0f90fe..26363322d4e7e677b86a8fc4fdb4e8264736d062 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -349,12 +349,11 @@ sbs_target(test-mpu9250 stm32f429zi_parafoil)
add_executable(test-ms5803 src/tests/sensors/test-ms5803.cpp)
sbs_target(test-ms5803 stm32f429zi_skyward_death_stack_x)
-add_executable(test-ubloxgps-serial src/tests/sensors/test-ubloxgps.cpp)
-sbs_target(test-ubloxgps-serial 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)
-add_executable(test-ubloxgps-spi src/tests/sensors/test-ubloxgps.cpp)
-target_compile_definitions(test-ubloxgps-spi PRIVATE USE_SPI)
-sbs_target(test-ubloxgps-spi stm32f407vg_stm32f4discovery)
+add_executable(test-ubxgps-spi src/tests/sensors/test-ubxgps-spi.cpp)
+sbs_target(test-ubxgps-spi stm32f429zi_skyward_death_stack_x)
add_executable(test-vn100 src/tests/sensors/test-vn100.cpp)
sbs_target(test-vn100 stm32f407vg_stm32f4discovery)
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 2fdea04d4a7b2628845b1bcfc62952433eee4e2b..a38c686066fed81df16df3fd247bf487af8f56e9 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -83,7 +83,8 @@ foreach(OPT_BOARD ${BOARDS})
${SBS_BASE}/src/shared/sensors/MS5803/MS5803.cpp
${SBS_BASE}/src/shared/sensors/SensorManager.cpp
${SBS_BASE}/src/shared/sensors/SensorSampler.cpp
- ${SBS_BASE}/src/shared/sensors/UbloxGPS/UbloxGPS.cpp
+ ${SBS_BASE}/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
+ ${SBS_BASE}/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
${SBS_BASE}/src/shared/sensors/VN100/VN100.cpp
# Calibration
diff --git a/src/shared/drivers/spi/SPI.h b/src/shared/drivers/spi/SPI.h
index 07761041ff0f5079edfa690bc301a76e62068dbb..b1456fae0ff9e4965a856a121dad12b09024f3f6 100644
--- a/src/shared/drivers/spi/SPI.h
+++ b/src/shared/drivers/spi/SPI.h
@@ -485,10 +485,10 @@ inline uint16_t SPI::transfer(uint16_t data)
inline void SPI::transfer(uint8_t *data, size_t nBytes)
{
- // Cleaer the RX buffer
+ // Clear the RX buffer
(void)spi->DR;
- // Write the first data item to transmit
+ // Write the first data item to transmitted
spi->DR = data[0];
for (size_t i = 1; i < nBytes; i++)
@@ -508,26 +508,30 @@ inline void SPI::transfer(uint8_t *data, size_t nBytes)
data[i - 1] = static_cast<uint8_t>(spi->DR);
}
- // Wait until data is received
- // while ((spi->SR & SPI_SR_RXNE) == 0)
- // ;
- while (spi->SR & SPI_SR_BSY)
+ // Wait until the last data item is received
+ while ((spi->SR & SPI_SR_RXNE) == 0)
;
// Read the last received data item
data[nBytes - 1] = static_cast<uint8_t>(spi->DR);
+
+ // Wait until TXE=1 and then wait until BSY=0 before concluding
+ while ((spi->SR & SPI_SR_TXE) == 0)
+ ;
+ while (spi->SR & SPI_SR_BSY)
+ ;
}
inline void SPI::transfer(uint16_t *data, size_t nBytes)
{
- // Cleaer the RX buffer
+ // Clear the RX buffer
(void)spi->DR;
// Set 16 bit frame format
set16BitFrameFormat();
- // Write the first data item to transmit
- spi->DR = data[0];
+ // Write the first data item to transmitted
+ spi->DR = static_cast<uint16_t>(data[0]);
for (size_t i = 1; i < nBytes / 2; i++)
{
@@ -546,15 +550,19 @@ inline void SPI::transfer(uint16_t *data, size_t nBytes)
data[i - 1] = static_cast<uint16_t>(spi->DR);
}
- // Wait until data is received
- // while ((spi->SR & SPI_SR_RXNE) == 0)
- // ;
- while (spi->SR & SPI_SR_BSY)
+ // Wait until the last data item is received
+ while ((spi->SR & SPI_SR_RXNE) == 0)
;
// Read the last received data item
data[nBytes / 2 - 1] = static_cast<uint16_t>(spi->DR);
+ // Wait until TXE=1 and then wait until BSY=0 before concluding
+ while ((spi->SR & SPI_SR_TXE) == 0)
+ ;
+ while (spi->SR & SPI_SR_BSY)
+ ;
+
// Go back to 8 bit frame format
set8BitFrameFormat();
}
diff --git a/src/shared/logger/LogTypes.h b/src/shared/logger/LogTypes.h
index 4df3ac83cf0921490e029d276f9e26510322d316..741e0113bd37b467a30d2f72857caf59253db7eb 100644
--- a/src/shared/logger/LogTypes.h
+++ b/src/shared/logger/LogTypes.h
@@ -46,7 +46,7 @@
#include <sensors/MPU9250/MPU9250Data.h>
#include <sensors/MS5803/MS5803Data.h>
#include <sensors/SensorData.h>
-#include <sensors/UbloxGPS/UbloxGPSData.h>
+#include <sensors/UBXGPS/UBXGPSData.h>
#include <sensors/VN100/VN100Data.h>
#include <sensors/analog/BatteryVoltageSensorData.h>
#include <sensors/analog/CurrentSensorData.h>
@@ -97,7 +97,7 @@ void registerTypes(Deserializer& ds)
ds.registerType<MPU9250Data>();
ds.registerType<MS5803Data>();
ds.registerType<TemperatureData>();
- ds.registerType<UbloxGPSData>();
+ ds.registerType<UBXGPSData>();
ds.registerType<VN100Data>();
ds.registerType<BatteryVoltageSensorData>();
ds.registerType<CurrentSensorData>();
diff --git a/src/shared/sensors/SensorData.h b/src/shared/sensors/SensorData.h
index d40024e73622d384aacdd9bddf89b3a76307e217..2a2a2ecb08270a9b4e107871e0480b4bb7b42f79 100644
--- a/src/shared/sensors/SensorData.h
+++ b/src/shared/sensors/SensorData.h
@@ -134,16 +134,17 @@ struct MagnetometerData
struct GPSData
{
uint64_t gpsTimestamp;
- float latitude; /**< [deg] */
- float longitude; /**< [deg] */
- float height; /**< [m] */
- float velocityNorth; /**< [m/s] */
- float velocityEast; /**< [m/s] */
- float velocityDown; /**< [m/s] */
- float speed; /**< [m/s] */
- float track; /**< [deg] */
- uint8_t satellites; /**< [1] */
- bool fix;
+ float latitude; // [deg]
+ float longitude; // [deg]
+ float height; // [m]
+ float velocityNorth; // [m/s]
+ float velocityEast; // [m/s]
+ float velocityDown; // [m/s]
+ float speed; // [m/s]
+ float track; // [deg]
+ float positionDOP; // [?]
+ uint8_t satellites; // [1]
+ uint8_t fix; // 0 = no fix
};
/**
diff --git a/src/shared/sensors/UBXGPS/UBXFrame.h b/src/shared/sensors/UBXGPS/UBXFrame.h
new file mode 100644
index 0000000000000000000000000000000000000000..46437171199cdad96bf4477b06d8629969f4dfda
--- /dev/null
+++ b/src/shared/sensors/UBXGPS/UBXFrame.h
@@ -0,0 +1,309 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Damiano Amatruda, Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include <algorithm>
+#include <cstring>
+
+namespace Boardcore
+{
+
+/**
+ * @brief UBX messages enumeration.
+ */
+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_CFG_VALSET = 0x8a06, // TODO: Update to newer command
+};
+
+/**
+ * @brief Generic UBX frame.
+ */
+struct UBXFrame
+{
+ static constexpr uint16_t MAX_PAYLOAD_LENGTH = 92;
+ static constexpr uint16_t MAX_FRAME_LENGTH = MAX_PAYLOAD_LENGTH + 8;
+ static constexpr uint8_t PREAMBLE[] = {0xb5, 0x62};
+ static constexpr uint8_t WAIT = 0xff;
+
+ uint8_t preamble[2];
+ uint16_t message;
+ uint16_t payloadLength;
+ uint8_t payload[MAX_PAYLOAD_LENGTH];
+ uint8_t checksum[2];
+
+ UBXFrame() = default;
+
+ /**
+ * @brief Construct a new UBXFrame with the specified message and given
+ * payload.
+ */
+ UBXFrame(UBXMessage message, const uint8_t* payload,
+ uint16_t payloadLength);
+
+ /**
+ * @brief Return the total frame length.
+ */
+ uint16_t getLength() const;
+
+ /**
+ * @brief Return the stored payload length.
+ */
+ uint16_t getRealPayloadLength() const;
+
+ UBXMessage getMessage() const;
+
+ /**
+ * @brief Tells whether the current frame is valid or not. Checks the
+ * preamble and the checksum.
+ */
+ bool isValid() const;
+
+ /**
+ * @brief Writes the current message into the given array.
+ *
+ * @param frame Must be an array of the proper length.
+ */
+ void writePacked(uint8_t* frame) const;
+
+ /**
+ * @brief Reads a raw frame.
+ *
+ * Note that the frame bytes are assumed to start with the preamble.
+ *
+ * @param frame An array of length at least MAX_FRAME_LENGTH.
+ */
+ void readPacked(const uint8_t* frame);
+
+ /**
+ * @brief Computes the frame checksum.
+ *
+ * @param checksum Array of 2 elements where to store the checksum
+ */
+ void calcChecksum(uint8_t* checksum) const;
+};
+
+/**
+ * @brief UBX frames UBX-ACK-ACK and UBX-ACK-NAK.
+ */
+struct UBXAckFrame : public UBXFrame
+{
+ /**
+ * @brief Payload of UBX frames UBX-ACK-ACK and UBX-ACK-NAK.
+ */
+ struct __attribute__((packed)) Payload
+ {
+ uint16_t ackMessage;
+ };
+
+ Payload& getPayload() const;
+
+ UBXMessage getAckMessage() const;
+
+ /**
+ * @brief Tells whether the frame is an ack.
+ */
+ bool isAck() const;
+
+ /**
+ * @brief Tells whether the frame is a nak.
+ */
+ bool isNack() const;
+
+ /**
+ * @brief Tells whether the frame is an ack frame.
+ */
+ bool isValid() const;
+};
+
+/**
+ * @brief UBX frame UBX-NAV-PVT.
+ */
+struct UBXPvtFrame : public UBXFrame
+{
+public:
+ /**
+ * @brief Payload of UBX frame UBX-NAV-PVT.
+ */
+ struct __attribute__((packed)) Payload
+ {
+ uint32_t iTOW; // GPS time of week of the navigation epoch [ms]
+ uint16_t year; // Year (UTC) [y]
+ uint8_t month; // Month, range 1..12 (UTC) [month]
+ uint8_t day; // Day of month, range 1..31 (UTC) [d]
+ uint8_t hour; // Hour of day, range 0..23 (UTC) [h]
+ uint8_t min; // Minute of hour, range 0..59 (UTC) [min]
+ uint8_t sec; // Seconds of minute, range 0..60 (UTC) [s]
+ uint8_t valid; // Validity flags
+ uint32_t tAcc; // Time accuracy estimate (UTC) [ns]
+ int32_t nano; // Fraction of second, range -1e9 .. 1e9 (UTC) [ns]
+ uint8_t fixType; // GNSS fix Type
+ uint8_t flags; // Fix status flags
+ uint8_t flags2; // Additional flags
+ uint8_t numSV; // Number of satellites used in Nav Solution
+ 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]
+ int32_t velN; // NED north velocity [mm/s]
+ int32_t velE; // NED east velocity [mm/s]
+ int32_t velD; // NED down velocity [mm/s]
+ int32_t gSpeed; // Ground Speed (2-D) [mm/s]
+ int32_t headMot; // Heading of motion (2-D) {1e-5} [deg]
+ uint32_t sAcc; // Speed accuracy estimate [mm/s]
+ uint32_t headAcc; // Heading accuracy estimate (both motion and
+ // vehicle) {1e-5} [deg]
+ uint16_t pDOP; // Position DOP {0.01}
+ uint16_t flags3; // Additional flags
+ uint8_t reserved0[4]; // Reserved
+ int32_t headVeh; // Heading of vehicle (2-D) {1e-5} [deg]
+ int16_t magDec; // Magnetic declination {1e-2} [deg]
+ uint16_t magAcc; // Magnetic declination accuracy {1e-2} [deg]
+ };
+
+ Payload& getPayload() const;
+
+ /**
+ * @brief Tells whether the frame is an ack frame.
+ */
+ bool isValid() const;
+};
+
+inline UBXFrame::UBXFrame(UBXMessage message, const uint8_t* payload,
+ uint16_t payloadLength)
+ : message(static_cast<uint16_t>(message)), payloadLength(payloadLength)
+{
+ memcpy(preamble, PREAMBLE, 2);
+ if (payload != nullptr)
+ memcpy(this->payload, payload, getRealPayloadLength());
+ calcChecksum(checksum);
+}
+
+inline uint16_t UBXFrame::getLength() const { return payloadLength + 8; }
+
+inline uint16_t UBXFrame::getRealPayloadLength() const
+{
+ return std::min(payloadLength, MAX_PAYLOAD_LENGTH);
+}
+
+inline UBXMessage UBXFrame::getMessage() const
+{
+ return static_cast<UBXMessage>(message);
+}
+
+inline bool UBXFrame::isValid() const
+{
+ if (memcmp(preamble, PREAMBLE, 2) != 0)
+ return false;
+
+ if (payloadLength > MAX_PAYLOAD_LENGTH)
+ return false;
+
+ uint8_t validChecksum[2];
+ calcChecksum(validChecksum);
+ return memcmp(checksum, validChecksum, 2) == 0;
+}
+
+inline void UBXFrame::writePacked(uint8_t* frame) const
+{
+ memcpy(frame, preamble, 2);
+ memcpy(&frame[2], &message, 2);
+ memcpy(&frame[4], &payloadLength, 2);
+ memcpy(&frame[6], payload, getRealPayloadLength());
+ memcpy(&frame[6 + payloadLength], checksum, 2);
+}
+
+inline void UBXFrame::readPacked(const uint8_t* frame)
+{
+ memcpy(preamble, frame, 2);
+ memcpy(&message, &frame[2], 2);
+ memcpy(&payloadLength, &frame[4], 2);
+ memcpy(payload, &frame[6], getRealPayloadLength());
+ memcpy(checksum, &frame[6 + payloadLength], 2);
+}
+
+inline void UBXFrame::calcChecksum(uint8_t* checksum) const
+{
+ uint8_t data[getRealPayloadLength() + 4];
+ memcpy(data, &message, 2);
+ memcpy(&data[2], &payloadLength, 2);
+ memcpy(&data[4], payload, getRealPayloadLength());
+
+ checksum[0] = 0;
+ checksum[1] = 0;
+
+ for (size_t i = 0; i < sizeof(data); ++i)
+ {
+ checksum[0] += data[i];
+ checksum[1] += checksum[0];
+ }
+}
+
+inline UBXAckFrame::Payload& UBXAckFrame::getPayload() const
+{
+ return (Payload&)payload;
+}
+
+inline UBXMessage UBXAckFrame::getAckMessage() const
+{
+ return static_cast<UBXMessage>(getPayload().ackMessage);
+}
+
+inline bool UBXAckFrame::isAck() const
+{
+ return getMessage() == UBXMessage::UBX_ACK_ACK;
+}
+
+inline bool UBXAckFrame::isNack() const
+{
+ return getMessage() == UBXMessage::UBX_ACK_NAK;
+}
+
+inline bool UBXAckFrame::isValid() const
+{
+ return UBXFrame::isValid() && (isAck() || isNack());
+}
+
+inline UBXPvtFrame::Payload& UBXPvtFrame::getPayload() const
+{
+ return (Payload&)payload;
+}
+
+inline bool UBXPvtFrame::isValid() const
+{
+ return UBXFrame::isValid() && getMessage() == UBXMessage::UBX_NAV_PVT;
+}
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/UbloxGPS/UbloxGPSData.h b/src/shared/sensors/UBXGPS/UBXGPSData.h
similarity index 79%
rename from src/shared/sensors/UbloxGPS/UbloxGPSData.h
rename to src/shared/sensors/UBXGPS/UBXGPSData.h
index 7f5570000b78b915a956b96e754d784e08f7611f..ed734d258bd1c0dd79206b8d392562b7009a22bf 100644
--- a/src/shared/sensors/UbloxGPS/UbloxGPSData.h
+++ b/src/shared/sensors/UBXGPS/UBXGPSData.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2020 Skyward Experimental Rocketry
- * Author: Davide Bonomini
+/* Copyright (c) 2020-2022 Skyward Experimental Rocketry
+ * Authors: Davide Bonomini, Damiano Amatruda
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -27,20 +27,21 @@
namespace Boardcore
{
-struct UbloxGPSData : public GPSData
+struct UBXGPSData : public GPSData
{
static std::string header()
{
- return "gps_timestamp,latitude,longitude,height,velocity_north,"
- "velocity_east,velocity_down,speed,track,num_satellites,fix\n";
+ return "gpsTimestamp,latitude,longitude,height,velocityNorth,"
+ "velocityEast,velocityDown,speed,track,positionDOP,satellites,"
+ "fix\n";
}
void print(std::ostream &os) const
{
os << gpsTimestamp << "," << latitude << "," << longitude << ","
<< height << "," << velocityNorth << "," << velocityEast << ","
- << velocityDown << "," << speed << "," << track << ","
- << (int)satellites << "," << (int)fix << "\n";
+ << velocityDown << "," << speed << "," << track << "," << positionDOP
+ << "," << (int)satellites << "," << (int)fix << "\n";
}
};
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp b/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3c63ac353610543a18299cbfedf5739986b268ab
--- /dev/null
+++ b/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
@@ -0,0 +1,438 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "UBXGPSSerial.h"
+
+#include <diagnostic/StackLogger.h>
+#include <drivers/serial.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <fcntl.h>
+#include <filesystem/file_access.h>
+
+using namespace miosix;
+
+namespace Boardcore
+{
+
+UBXGPSSerial::UBXGPSSerial(int baudrate, uint8_t sampleRate, int serialPortNum,
+ const char* serialPortName, int defaultBaudrate)
+ : baudrate(baudrate), sampleRate(sampleRate),
+ serialPortNumber(serialPortNum), serialPortName(serialPortName),
+ defaultBaudrate(defaultBaudrate)
+{
+ // Prepare the gps file path with the specified name
+ strcpy(gpsFilePath, "/dev/");
+ strcat(gpsFilePath, serialPortName);
+}
+
+bool UBXGPSSerial::init()
+{
+ LOG_DEBUG(logger, "Changing device baudrate...");
+
+ if (!setSerialCommunication())
+ {
+ lastError = SensorErrors::INIT_FAIL;
+ LOG_ERR(logger, "Error while setting serial communication");
+ return false;
+ }
+
+ LOG_DEBUG(logger, "Resetting the device...");
+
+ if (!reset())
+ {
+ lastError = SensorErrors::INIT_FAIL;
+ LOG_ERR(logger, "Could not reset the device");
+ return false;
+ }
+
+ LOG_DEBUG(logger, "Setting the UBX protocol...");
+
+ if (!setUBXProtocol())
+ {
+ lastError = SensorErrors::INIT_FAIL;
+ LOG_ERR(logger, "Could not set the UBX protocol");
+ return false;
+ }
+
+ LOG_DEBUG(logger, "Setting the dynamic model...");
+
+ if (!setDynamicModelToAirborne4g())
+ {
+ lastError = SensorErrors::INIT_FAIL;
+ LOG_ERR(logger, "Could not set the dynamic model");
+ return false;
+ }
+
+ LOG_DEBUG(logger, "Setting the sample rate...");
+
+ if (!setSampleRate())
+ {
+ lastError = SensorErrors::INIT_FAIL;
+ LOG_ERR(logger, "Could not set the sample rate");
+ return false;
+ }
+
+ LOG_DEBUG(logger, "Setting the PVT message rate...");
+
+ if (!setPVTMessageRate())
+ {
+ lastError = SensorErrors::INIT_FAIL;
+ LOG_ERR(logger, "Could not set the PVT message rate");
+ return false;
+ }
+
+ return true;
+}
+
+bool UBXGPSSerial::selfTest() { return true; }
+
+UBXGPSData UBXGPSSerial::sampleImpl()
+{
+ Lock<FastMutex> l(mutex);
+ return threadSample;
+}
+
+bool UBXGPSSerial::reset()
+{
+ uint8_t payload[] = {
+ 0x00, 0x00, // Hot start
+ 0x00, // Hardware reset
+ 0x00 // Reserved
+ };
+
+ UBXFrame frame{UBXMessage::UBX_CFG_RST, payload, sizeof(payload)};
+
+ // The reset message will not be followed by an acknowledgment
+ if (!writeUBXFrame(frame))
+ return false;
+
+ // Do not interact with the module while it is resetting
+ miosix::Thread::sleep(RESET_SLEEP_TIME);
+
+ return true;
+}
+
+bool UBXGPSSerial::setBaudrate()
+{
+ uint8_t payload[] = {
+ 0x00, // Version
+ 0xff, // All layers
+ 0x00, 0x00, // Reserved
+ 0x01, 0x00, 0x52, 0x40, // Configuration item key ID
+ 0xff, 0xff, 0xff, 0xff, // Value
+ };
+
+ // Prepare baudrate
+ payload[8] = baudrate;
+ payload[9] = baudrate >> 8;
+ payload[10] = baudrate >> 16;
+ payload[11] = baudrate >> 24;
+
+ UBXFrame frame{UBXMessage::UBX_CFG_VALSET, payload, sizeof(payload)};
+
+ return safeWriteUBXFrame(frame);
+}
+
+bool UBXGPSSerial::setSerialCommunication()
+{
+ intrusive_ref_ptr<DevFs> devFs = FilesystemManager::instance().getDevFs();
+
+ // Change the baudrate only if it is different than the default
+ if (baudrate != defaultBaudrate)
+ {
+ // Close the gps file if already opened
+ devFs->remove(serialPortName);
+
+ // Open the serial port device with the default boudrate
+ if (!devFs->addDevice(serialPortName,
+ intrusive_ref_ptr<Device>(new STM32Serial(
+ serialPortNumber, defaultBaudrate))))
+ {
+ LOG_ERR(logger,
+ "[gps] Failed to open serial port {0} with baudrate {1} as "
+ "file {2}",
+ serialPortNumber, defaultBaudrate, serialPortName);
+ return false;
+ }
+
+ // Open the gps file
+ if ((gpsFile = open(gpsFilePath, O_RDWR)) < 0)
+ {
+ LOG_ERR(logger, "Failed to open gps file {}", gpsFilePath);
+ return false;
+ }
+
+ // Change boudrate
+ if (!setBaudrate())
+ {
+ return false;
+ };
+
+ // Close the gps file
+ if (close(gpsFile) < 0)
+ {
+ LOG_ERR(logger, "Failed to close gps file {}", gpsFilePath);
+ return false;
+ }
+
+ // Close the serial port
+ if (!devFs->remove(serialPortName))
+ {
+ LOG_ERR(logger, "Failed to close serial port {} as file {}",
+ serialPortNumber, serialPortName);
+ return false;
+ }
+ }
+
+ // Reopen the serial port with the configured boudrate
+ if (!devFs->addDevice(serialPortName,
+ intrusive_ref_ptr<Device>(
+ new STM32Serial(serialPortNumber, baudrate))))
+ {
+ LOG_ERR(logger,
+ "Failed to open serial port {} with baudrate {} as file {}\n",
+ serialPortNumber, defaultBaudrate, serialPortName);
+ return false;
+ }
+
+ // Reopen the gps file
+ if ((gpsFile = open(gpsFilePath, O_RDWR)) < 0)
+ {
+ LOG_ERR(logger, "Failed to open gps file {}", gpsFilePath);
+ return false;
+ }
+
+ return true;
+}
+
+bool UBXGPSSerial::setUBXProtocol()
+{
+ uint8_t payload[] = {
+ 0x04, // SPI port
+ 0x00, // reserved0
+ 0x00, 0x00, // txReady
+ 0x00, 0x00, 0x00, 0x00, // spiMode = 0, ffCnt = 0 (mechanism off)
+ 0x00, 0x00, 0x00, 0x00, // reserved1
+ 0x01, 0x00, // inProtoMask = UBX
+ 0x01, 0x00, // outProtoMask = UBX
+ 0x00, 0x00, // flags
+ 0x00, 0x00 // reserved2
+ };
+
+ UBXFrame frame{UBXMessage::UBX_CFG_PRT, payload, sizeof(payload)};
+
+ return safeWriteUBXFrame(frame);
+}
+
+bool UBXGPSSerial::setDynamicModelToAirborne4g()
+{
+ uint8_t payload[] = {
+ 0x01, 0x00, // Parameters bitmask, apply dynamic model configuration
+ 0x08, // Dynamic model = airbone 4g
+ 0x00, // Fix mode
+ 0x00, 0x00, 0x00, 0x00, // Fixed altitude for 2D mode
+ 0x00, 0x00, 0x00, 0x00, // Fixed altitude variance for 2D mode
+ 0x00, // Minimun elevation for a GNSS satellite to be used
+ 0x00, // Reserved
+ 0x00, 0x00, // Position DOP mask to use
+ 0x00, 0x00, // Time DOP mask to use
+ 0x00, 0x00, // Position accuracy mask
+ 0x00, 0x00, // Time accuracy mask
+ 0x00, // Static hold threshold
+ 0x00, // DGNSS timeout
+ 0x00, // C/NO threshold number SVs
+ 0x00, // C/NO threshold
+ 0x00, 0x00, // Reserved
+ 0x00, 0x00, // Static hold distance threshold
+ 0x00, // UTC standard to be used
+ 0x00, 0x00, 0x00, 0x00, 0x00 // Reserved
+ };
+
+ UBXFrame frame{UBXMessage::UBX_CFG_NAV5, payload, sizeof(payload)};
+
+ return safeWriteUBXFrame(frame);
+}
+
+bool UBXGPSSerial::setSampleRate()
+{
+ uint8_t payload[] = {
+ 0x00, 0x00, // Measurement rate
+ 0x01, 0x00, // One navigation solution per measurement
+ 0x01, 0x01 // GPS time
+ };
+
+ uint16_t sampleRateMs = 1000 / sampleRate;
+ payload[0] = sampleRateMs;
+ payload[1] = sampleRateMs >> 8;
+
+ UBXFrame frame{UBXMessage::UBX_CFG_RATE, payload, sizeof(payload)};
+
+ return safeWriteUBXFrame(frame);
+}
+
+bool UBXGPSSerial::setPVTMessageRate()
+{
+ uint8_t payload[] = {
+ 0x01, 0x07, // PVT message
+ 0x01 // Rate = 1 navigation solution update
+ };
+
+ UBXFrame frame{UBXMessage::UBX_CFG_MSG, payload, sizeof(payload)};
+
+ return safeWriteUBXFrame(frame);
+}
+
+bool UBXGPSSerial::readUBXFrame(UBXFrame& frame)
+{
+ // Search UBX frame preamble byte by byte
+ size_t i = 0;
+ while (i < 2)
+ {
+ uint8_t c;
+ if (read(gpsFile, &c, 1) <= 0) // No more data available
+ return false;
+
+ if (c == UBXFrame::PREAMBLE[i])
+ {
+ frame.preamble[i++] = c;
+ }
+ else if (c == UBXFrame::PREAMBLE[0])
+ {
+ i = 0;
+ frame.preamble[i++] = c;
+ }
+ else
+ {
+ i = 0;
+ LOG_DEBUG(logger, "Received unexpected byte: {:02x} {:#c}", c, c);
+ }
+ }
+
+ if (read(gpsFile, &frame.message, 2) <= 0 ||
+ read(gpsFile, &frame.payloadLength, 2) <= 0 ||
+ read(gpsFile, frame.payload, frame.getRealPayloadLength()) <= 0 ||
+ read(gpsFile, frame.checksum, 2) <= 0)
+ return false;
+
+ if (!frame.isValid())
+ {
+ LOG_ERR(logger, "Received invalid UBX frame");
+ return false;
+ }
+
+ return true;
+}
+
+bool UBXGPSSerial::writeUBXFrame(const UBXFrame& frame)
+{
+ if (!frame.isValid())
+ {
+ LOG_ERR(logger, "Trying to send an invalid UBX frame");
+ return false;
+ }
+
+ uint8_t packedFrame[frame.getLength()];
+ frame.writePacked(packedFrame);
+
+ if (write(gpsFile, packedFrame, frame.getLength()) < 0)
+ {
+ LOG_ERR(logger, "Failed to write ubx message");
+ return false;
+ }
+
+ return true;
+}
+
+bool UBXGPSSerial::safeWriteUBXFrame(const UBXFrame& frame)
+{
+ for (unsigned int i = 0; i < MAX_TRIES; i++)
+ {
+ if (i > 0)
+ LOG_DEBUG(logger, "Retrying (attempt {:#d} of {:#d})...", i + 1,
+ MAX_TRIES);
+
+ if (!writeUBXFrame(frame))
+ return false;
+
+ UBXAckFrame ack;
+
+ if (!readUBXFrame(ack))
+ continue;
+
+ if (ack.isNack())
+ {
+ if (ack.getAckMessage() == frame.getMessage())
+ LOG_DEBUG(logger, "Received NAK");
+ else
+ LOG_DEBUG(logger, "Received NAK for different UBX frame {:04x}",
+ static_cast<uint16_t>(ack.getPayload().ackMessage));
+ continue;
+ }
+
+ if (ack.isAck() && ack.getAckMessage() != frame.getMessage())
+ {
+ LOG_DEBUG(logger, "Received ACK for different UBX frame {:04x}",
+ static_cast<uint16_t>(ack.getPayload().ackMessage));
+ continue;
+ }
+
+ return true;
+ }
+
+ LOG_ERR(logger, "Gave up after {:#d} tries", MAX_TRIES);
+ return false;
+}
+
+void UBXGPSSerial::run()
+{
+ while (!shouldStop())
+ {
+ UBXPvtFrame pvt;
+
+ // Try to read the message
+ if (!readUBXFrame(pvt))
+ {
+ 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;
+
+ StackLogger::getInstance().updateStack(THID_GPS);
+ }
+}
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSerial.h b/src/shared/sensors/UBXGPS/UBXGPSSerial.h
new file mode 100644
index 0000000000000000000000000000000000000000..7f8bcc646cac712ac3395a15c6ff15aad9d3503f
--- /dev/null
+++ b/src/shared/sensors/UBXGPS/UBXGPSSerial.h
@@ -0,0 +1,179 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <ActiveObject.h>
+#include <diagnostic/PrintLogger.h>
+#include <miosix.h>
+#include <sensors/Sensor.h>
+
+#include "UBXFrame.h"
+#include "UBXGPSData.h"
+
+namespace Boardcore
+{
+
+/**
+ * @brief Driver for Ublox GPSs
+ *
+ * This driver handles communication and setup with Ublox GPSs. It uses the
+ * binary UBX protocol to retrieve and parse navigation data quicker than using
+ * the string based NMEA.
+ *
+ * At initialization it configures the device with the specified baudrate, reset
+ * the configuration and sets up UBX messages and GNSS parameters.
+ *
+ * Communication with the device is performed through a file, the driver opens
+ * the serial port under the filepath /dev/<serialPortName>.
+ * There is no need for the file to be setted up beforhand.
+ *
+ * This driver was written for a NEO-M9N gps with the latest version of UBX.
+ */
+class UBXGPSSerial : public Sensor<UBXGPSData>, public ActiveObject
+{
+public:
+ /**
+ * @brief Construct a new UBXGPSSerial object.
+ *
+ * @param baudrate Baudrate to communicate with the device (max: 921600,
+ * min: 4800 for NEO-M9N).
+ * @param sampleRate GPS sample rate (max: 25 for NEO-M9N).
+ * @param serialPortNumber Number of the serial port connected to the GPS.
+ * @param serialPortName Name of the file for the gps device.
+ * @param defaultBaudrate Startup baudrate (38400 for NEO-M9N).
+ */
+ UBXGPSSerial(int baudrate = 921600, uint8_t sampleRate = 10,
+ int serialPortNumber = 2, const char *serialPortName = "gps",
+ int defaultBaudrate = 38400);
+
+ /**
+ * @brief Sets up the serial port baudrate, disables the NMEA messages,
+ * configures GNSS options and enables UBX-PVT message.
+ *
+ * @return True if the operation succeeded.
+ */
+ bool init() override;
+
+ /**
+ * @brief Read a single message form the GPS, waits 2 sample cycle.
+ *
+ * @return True if a message was sampled.
+ */
+ bool selfTest() override;
+
+private:
+ UBXGPSData sampleImpl() override;
+
+ /**
+ * @brief Resets the device to its default configuration.
+ *
+ * @return True if the device reset succeeded.
+ */
+ bool reset();
+
+ bool setBaudrate();
+
+ /**
+ * @brief Sets up the serial port with the correct baudrate.
+ *
+ * Opens the serial port with the default baudrate and changes it to the
+ * value specified in the constructor, then it reopens the serial port. If
+ * the device is already using the correct baudrate this won't have effect.
+ * However if the gps is using a different baudrate the diver won't be able
+ * to communicate.
+ */
+ bool setSerialCommunication();
+
+ /**
+ * @brief Enables UBX and disables NMEA on the SPI port.
+ *
+ * @return True if the configuration received an acknowledgement.
+ */
+ bool setUBXProtocol();
+
+ /**
+ * @brief Configures the dynamic model to airborn 4g.
+ *
+ * @return True if the configuration received an acknowledgement.
+ */
+ bool setDynamicModelToAirborne4g();
+
+ /**
+ * @brief Configures the navigation solution sample rate.
+ *
+ * @return True if the configuration received an acknowledgement.
+ */
+ bool setSampleRate();
+
+ /**
+ * @brief Configures the PVT message output rate.
+ *
+ * @return True if the configuration received an acknowledgement.
+ */
+ bool setPVTMessageRate();
+
+ /**
+ * @brief Reads a UBX frame.
+ *
+ * @param frame The received frame.
+ * @return True if a valid frame was read.
+ */
+ bool readUBXFrame(UBXFrame &frame);
+
+ /**
+ * @brief Writes a UBX frame.
+ *
+ * @param frame The frame to write.
+ * @return True if the frame is valid.
+ */
+ bool writeUBXFrame(const UBXFrame &frame);
+
+ /**
+ * @brief Writes a UBX frame and waits for its acknowledgement.
+ *
+ * @param frame The frame to write.
+ * @return True if the frame is valid and acknowledged.
+ */
+ bool safeWriteUBXFrame(const UBXFrame &frame);
+
+ void run() override;
+
+ const int baudrate; // [baud]
+ const uint8_t sampleRate; // [Hz]
+ const int serialPortNumber;
+ const char *serialPortName;
+ const int defaultBaudrate; // [baud]
+
+ char gpsFilePath[16]; ///< Allows for a filename of up to 10 characters
+ int gpsFile = -1;
+
+ mutable miosix::FastMutex mutex;
+ UBXGPSData threadSample{};
+
+ PrintLogger logger = Logging::getLogger("ubloxgps");
+
+ static constexpr unsigned int RESET_SLEEP_TIME = 5000; // [ms]
+ static constexpr unsigned int MAX_TRIES = 5; // [1]
+};
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp b/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..832059373b8c67d9aaff5f75850f75ad5e0e05c7
--- /dev/null
+++ b/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
@@ -0,0 +1,358 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio, Damiano Amatruda
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "UBXGPSSpi.h"
+
+#include <drivers/timer/TimestampTimer.h>
+#include <interfaces/endianness.h>
+
+namespace Boardcore
+{
+
+constexpr uint16_t UBXFrame::MAX_PAYLOAD_LENGTH;
+constexpr uint16_t UBXFrame::MAX_FRAME_LENGTH;
+constexpr uint8_t UBXFrame::PREAMBLE[];
+constexpr uint8_t UBXFrame::WAIT;
+
+constexpr float UBXGPSSpi::MS_TO_TICK;
+
+constexpr unsigned int UBXGPSSpi::RESET_SLEEP_TIME;
+constexpr unsigned int UBXGPSSpi::READ_TIMEOUT;
+constexpr unsigned int UBXGPSSpi::MAX_TRIES;
+
+UBXGPSSpi::UBXGPSSpi(SPIBusInterface& spiBus, miosix::GpioPin spiCs,
+ SPIBusConfig spiConfig, uint8_t sampleRate)
+ : spiSlave(spiBus, spiCs, spiConfig), sampleRate(sampleRate)
+{
+}
+
+SPIBusConfig UBXGPSSpi::getDefaultSPIConfig()
+{
+ /* From data sheet of series NEO-M8:
+ * "Minimum initialization time" (setup time): 10 us
+ * "Minimum deselect time" (hold time): 1 ms */
+ return SPIBusConfig{SPI::ClockDivider::DIV_32, SPI::Mode::MODE_0,
+ SPI::BitOrder::MSB_FIRST, 10, 1000};
+}
+
+uint8_t UBXGPSSpi::getSampleRate() { return sampleRate; }
+
+bool UBXGPSSpi::init()
+{
+ LOG_DEBUG(logger, "Resetting the device...");
+
+ if (!reset())
+ {
+ lastError = SensorErrors::INIT_FAIL;
+ LOG_ERR(logger, "Could not reset the device");
+ return false;
+ }
+
+ LOG_DEBUG(logger, "Setting the UBX protocol...");
+
+ if (!setUBXProtocol())
+ {
+ lastError = SensorErrors::INIT_FAIL;
+ LOG_ERR(logger, "Could not set the UBX protocol");
+ return false;
+ }
+
+ LOG_DEBUG(logger, "Setting the dynamic model...");
+
+ if (!setDynamicModelToAirborne4g())
+ {
+ lastError = SensorErrors::INIT_FAIL;
+ LOG_ERR(logger, "Could not set the dynamic model");
+ return false;
+ }
+
+ LOG_DEBUG(logger, "Setting the sample rate...");
+
+ if (!setSampleRate())
+ {
+ lastError = SensorErrors::INIT_FAIL;
+ LOG_ERR(logger, "Could not set the sample rate");
+ return false;
+ }
+
+ LOG_DEBUG(logger, "Setting the PVT message rate...");
+
+ if (!setPVTMessageRate())
+ {
+ lastError = SensorErrors::INIT_FAIL;
+ LOG_ERR(logger, "Could not set the PVT message rate");
+ return false;
+ }
+
+ return true;
+}
+
+bool UBXGPSSpi::selfTest() { return true; }
+
+UBXGPSData UBXGPSSpi::sampleImpl()
+{
+ UBXPvtFrame pvt;
+
+ if (!readUBXFrame(pvt))
+ return lastSample;
+
+ UBXPvtFrame::Payload& pvtP = pvt.getPayload();
+
+ UBXGPSData sample;
+ sample.gpsTimestamp = TimestampTimer::getInstance().getTimestamp();
+ sample.latitude = (float)pvtP.lat / 1e7;
+ sample.longitude = (float)pvtP.lon / 1e7;
+ sample.height = (float)pvtP.height / 1e3;
+ sample.velocityNorth = (float)pvtP.velN / 1e3;
+ sample.velocityEast = (float)pvtP.velE / 1e3;
+ sample.velocityDown = (float)pvtP.velD / 1e3;
+ sample.speed = (float)pvtP.gSpeed / 1e3;
+ sample.track = (float)pvtP.headMot / 1e5;
+ sample.positionDOP = (float)pvtP.pDOP / 1e2;
+ sample.satellites = pvtP.numSV;
+ sample.fix = pvtP.fixType;
+
+ return sample;
+}
+
+bool UBXGPSSpi::reset()
+{
+ uint8_t payload[] = {
+ 0x00, 0x00, // Hot start
+ 0x00, // Hardware reset
+ 0x00 // Reserved
+ };
+
+ UBXFrame frame{UBXMessage::UBX_CFG_RST, payload, sizeof(payload)};
+
+ // The reset message will not be followed by an acknowledgment
+ if (!writeUBXFrame(frame))
+ return false;
+
+ // Do not interact with the module while it is resetting
+ miosix::Thread::sleep(RESET_SLEEP_TIME);
+
+ return true;
+}
+
+bool UBXGPSSpi::setUBXProtocol()
+{
+ uint8_t payload[] = {
+ 0x04, // SPI port
+ 0x00, // reserved0
+ 0x00, 0x00, // txReady
+ 0x00, 0x00, 0x00, 0x00, // spiMode = 0, ffCnt = 0 (mechanism off)
+ 0x00, 0x00, 0x00, 0x00, // reserved1
+ 0x01, 0x00, // inProtoMask = UBX
+ 0x01, 0x00, // outProtoMask = UBX
+ 0x00, 0x00, // flags
+ 0x00, 0x00 // reserved2
+ };
+
+ UBXFrame frame{UBXMessage::UBX_CFG_PRT, payload, sizeof(payload)};
+
+ return safeWriteUBXFrame(frame);
+}
+
+bool UBXGPSSpi::setDynamicModelToAirborne4g()
+{
+ uint8_t payload[] = {
+ 0x01, 0x00, // Parameters bitmask, apply dynamic model configuration
+ 0x08, // Dynamic model = airbone 4g
+ 0x00, // Fix mode
+ 0x00, 0x00, 0x00, 0x00, // Fixed altitude for 2D mode
+ 0x00, 0x00, 0x00, 0x00, // Fixed altitude variance for 2D mode
+ 0x00, // Minimun elevation for a GNSS satellite to be used
+ 0x00, // Reserved
+ 0x00, 0x00, // Position DOP mask to use
+ 0x00, 0x00, // Time DOP mask to use
+ 0x00, 0x00, // Position accuracy mask
+ 0x00, 0x00, // Time accuracy mask
+ 0x00, // Static hold threshold
+ 0x00, // DGNSS timeout
+ 0x00, // C/NO threshold number SVs
+ 0x00, // C/NO threshold
+ 0x00, 0x00, // Reserved
+ 0x00, 0x00, // Static hold distance threshold
+ 0x00, // UTC standard to be used
+ 0x00, 0x00, 0x00, 0x00, 0x00 // Reserved
+ };
+
+ UBXFrame frame{UBXMessage::UBX_CFG_NAV5, payload, sizeof(payload)};
+
+ return safeWriteUBXFrame(frame);
+}
+
+bool UBXGPSSpi::setSampleRate()
+{
+ uint8_t payload[] = {
+ 0x00, 0x00, // Measurement rate
+ 0x01, 0x00, // One navigation solution per measurement
+ 0x01, 0x01 // GPS time
+ };
+
+ uint16_t sampleRateMs = 1000 / sampleRate;
+ payload[0] = sampleRateMs;
+ payload[1] = sampleRateMs >> 8;
+
+ UBXFrame frame{UBXMessage::UBX_CFG_RATE, payload, sizeof(payload)};
+
+ return safeWriteUBXFrame(frame);
+}
+
+bool UBXGPSSpi::setPVTMessageRate()
+{
+ uint8_t payload[] = {
+ 0x01, 0x07, // PVT message
+ 0x01 // Rate = 1 navigation solution update
+ };
+
+ UBXFrame frame{UBXMessage::UBX_CFG_MSG, payload, sizeof(payload)};
+
+ return safeWriteUBXFrame(frame);
+}
+
+bool UBXGPSSpi::readUBXFrame(UBXFrame& frame)
+{
+ long long start = miosix::getTick();
+ long long end = start + READ_TIMEOUT * MS_TO_TICK;
+
+ {
+ SPITransaction spi{spiSlave};
+
+ // Search UBX frame preamble byte by byte
+ size_t i = 0;
+ bool waiting = false;
+ while (i < 2)
+ {
+ if (miosix::getTick() >= end)
+ {
+ LOG_ERR(logger, "Timeout for read expired");
+ return false;
+ }
+
+ uint8_t c = spi.read();
+
+ if (c == UBXFrame::PREAMBLE[i])
+ {
+ waiting = false;
+ frame.preamble[i++] = c;
+ }
+ else if (c == UBXFrame::PREAMBLE[0])
+ {
+ i = 0;
+ waiting = false;
+ frame.preamble[i++] = c;
+ }
+ else if (c == UBXFrame::WAIT)
+ {
+ i = 0;
+ if (!waiting)
+ {
+ waiting = true;
+ // LOG_DEBUG(logger, "Device is waiting...");
+ }
+ }
+ else
+ {
+ i = 0;
+ waiting = false;
+ LOG_DEBUG(logger, "Received unexpected byte: {:02x} {:#c}", c,
+ c);
+ }
+ }
+
+ frame.message = swapBytes16(spi.read16());
+ frame.payloadLength = swapBytes16(spi.read16());
+ spi.read(frame.payload, frame.getRealPayloadLength());
+ spi.read(frame.checksum, 2);
+ }
+
+ if (!frame.isValid())
+ {
+ LOG_ERR(logger, "Received invalid UBX frame");
+ return false;
+ }
+
+ return true;
+}
+
+bool UBXGPSSpi::writeUBXFrame(const UBXFrame& frame)
+{
+ if (!frame.isValid())
+ {
+ LOG_ERR(logger, "Trying to send an invalid UBX frame");
+ return false;
+ }
+
+ uint8_t packedFrame[frame.getLength()];
+ frame.writePacked(packedFrame);
+
+ {
+ SPITransaction spi{spiSlave};
+ spi.write(packedFrame, frame.getLength());
+ }
+
+ return true;
+}
+
+bool UBXGPSSpi::safeWriteUBXFrame(const UBXFrame& frame)
+{
+ for (unsigned int i = 0; i < MAX_TRIES; i++)
+ {
+ if (i > 0)
+ LOG_DEBUG(logger, "Retrying (attempt {:#d} of {:#d})...", i + 1,
+ MAX_TRIES);
+
+ if (!writeUBXFrame(frame))
+ return false;
+
+ UBXAckFrame ack;
+
+ if (!readUBXFrame(ack))
+ continue;
+
+ if (ack.isNack())
+ {
+ if (ack.getAckMessage() == frame.getMessage())
+ LOG_DEBUG(logger, "Received NAK");
+ else
+ LOG_DEBUG(logger, "Received NAK for different UBX frame {:04x}",
+ static_cast<uint16_t>(ack.getPayload().ackMessage));
+ continue;
+ }
+
+ if (ack.isAck() && ack.getAckMessage() != frame.getMessage())
+ {
+ LOG_DEBUG(logger, "Received ACK for different UBX frame {:04x}",
+ static_cast<uint16_t>(ack.getPayload().ackMessage));
+ continue;
+ }
+
+ return true;
+ }
+
+ LOG_ERR(logger, "Gave up after {:#d} tries", MAX_TRIES);
+ return false;
+}
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSpi.h b/src/shared/sensors/UBXGPS/UBXGPSSpi.h
new file mode 100644
index 0000000000000000000000000000000000000000..e20bc3bfcec2c8b36e69093291be5af048d80ce6
--- /dev/null
+++ b/src/shared/sensors/UBXGPS/UBXGPSSpi.h
@@ -0,0 +1,151 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio, Damiano Amatruda
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <diagnostic/PrintLogger.h>
+#include <drivers/spi/SPIDriver.h>
+#include <sensors/Sensor.h>
+
+#include "UBXFrame.h"
+#include "UBXGPSData.h"
+
+namespace Boardcore
+{
+
+/**
+ * @brief Sensor for UBlox GPS.
+ *
+ * This sensor handles communication and setup with UBlox GPSs. It uses the
+ * binary UBX protocol to retrieve and parse navigation data faster than using
+ * the string-based NMEA.
+ *
+ * At initialization it resets the device, sets the configuration and sets up
+ * UBX messages and GNSS parameters.
+ *
+ * Communication with the device is performed through SPI.
+ *
+ * This sensor is compatible with series NEO-M8 and NEO-M9.
+ */
+class UBXGPSSpi : public Sensor<UBXGPSData>
+{
+public:
+ /**
+ * @brief Constructor.
+ *
+ * @param spiBus The SPI bus.
+ * @param spiCs The CS pin to lower when we need to sample.
+ * @param spiConfig The SPI configuration.
+ * @param sampleRate The GPS sample rate [kHz].
+ */
+ UBXGPSSpi(SPIBusInterface& spiBus, miosix::GpioPin spiCs,
+ SPIBusConfig spiConfig = getDefaultSPIConfig(),
+ uint8_t sampleRate = 5);
+
+ /**
+ * @brief Constructs the default config for the SPI bus.
+ *
+ * @return The default SPIBusConfig object.
+ */
+ static SPIBusConfig getDefaultSPIConfig();
+
+ uint8_t getSampleRate();
+
+ bool init() override;
+
+ bool selfTest() override;
+
+private:
+ UBXGPSData sampleImpl() override;
+
+ /**
+ * @brief Resets the device to its default configuration.
+ *
+ * @return True if the device reset succeeded.
+ */
+ bool reset();
+
+ /**
+ * @brief Enables UBX and disables NMEA on the SPI port.
+ *
+ * @return True if the configuration received an acknowledgement.
+ */
+ bool setUBXProtocol();
+
+ /**
+ * @brief Configures the dynamic model to airborn 4g.
+ *
+ * @return True if the configuration received an acknowledgement.
+ */
+ bool setDynamicModelToAirborne4g();
+
+ /**
+ * @brief Configures the navigation solution sample rate.
+ *
+ * @return True if the configuration received an acknowledgement.
+ */
+ bool setSampleRate();
+
+ /**
+ * @brief Configures the PVT message output rate.
+ *
+ * @return True if the configuration received an acknowledgement.
+ */
+ bool setPVTMessageRate();
+
+ /**
+ * @brief Reads a UBX frame.
+ *
+ * @param frame The received frame.
+ * @return True if a valid frame was read.
+ */
+ bool readUBXFrame(UBXFrame& frame);
+
+ /**
+ * @brief Writes a UBX frame.
+ *
+ * @param frame The frame to write.
+ * @return True if the frame is valid.
+ */
+ bool writeUBXFrame(const UBXFrame& frame);
+
+ /**
+ * @brief Writes a UBX frame and waits for its acknowledgement.
+ *
+ * @param frame The frame to write.
+ * @return True if the frame is valid and acknowledged.
+ */
+ bool safeWriteUBXFrame(const UBXFrame& frame);
+
+ SPISlave spiSlave;
+ uint8_t sampleRate;
+
+ PrintLogger logger = Logging::getLogger("ubxgps");
+
+ static constexpr float MS_TO_TICK = miosix::TICK_FREQ / 1000.f;
+
+ static constexpr unsigned int RESET_SLEEP_TIME = 5000; // [ms]
+ static constexpr unsigned int READ_TIMEOUT = 5000; // [ms]
+ static constexpr unsigned int MAX_TRIES = 5; // [1]
+};
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/UbloxGPS/UbloxGPS.cpp b/src/shared/sensors/UbloxGPS/UbloxGPS.cpp
deleted file mode 100644
index dff24046dfefeca63f88c4499f1d0b2bf4c9f9ce..0000000000000000000000000000000000000000
--- a/src/shared/sensors/UbloxGPS/UbloxGPS.cpp
+++ /dev/null
@@ -1,537 +0,0 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#include "UbloxGPS.h"
-
-#include <diagnostic/StackLogger.h>
-#include <drivers/serial.h>
-#include <drivers/timer/TimestampTimer.h>
-#include <fcntl.h>
-#include <filesystem/file_access.h>
-
-using namespace miosix;
-
-namespace Boardcore
-{
-
-UbloxGPS::UbloxGPS(int baudrate_, uint8_t sampleRate_, int serialPortNum_,
- const char* serialPortName_, int defaultBaudrate_)
- : baudrate(baudrate_), sampleRate(sampleRate_),
- serialPortNumber(serialPortNum_), serialPortName(serialPortName_),
- defaultBaudrate(defaultBaudrate_)
-{
- // Prepare the gps file path with the specified name
- strcpy(gpsFilePath, "/dev/");
- strcat(gpsFilePath, serialPortName);
-}
-
-bool UbloxGPS::init()
-{
- // Change the baud rate from the default value
- if (!serialCommuinicationSetup())
- {
- return false;
- }
-
- Thread::sleep(10);
-
- // Reset configuration to default
- // TODO: maybe move this on serial communication setup
- if (!resetConfiguration())
- {
- return false;
- }
-
- // Disable NMEA messages
- if (!disableNMEAMessages())
- {
- return false;
- }
-
- Thread::sleep(100);
-
- // Set GNSS configuration
- if (!setGNSSConfiguration())
- {
- return false;
- }
-
- Thread::sleep(100);
-
- // Enable UBX messages
- if (!enableUBXMessages())
- {
- return false;
- }
-
- Thread::sleep(100);
-
- // Set rate
- if (!setRate())
- {
- return false;
- }
-
- return true;
-}
-
-bool UbloxGPS::selfTest() { return true; }
-
-UbloxGPSData UbloxGPS::sampleImpl()
-{
- Lock<FastMutex> l(mutex);
- return threadSample;
-}
-
-void UbloxGPS::run()
-{
- /**
- * UBX message structure:
- * - 2B: Preamble
- * - 1B: Message class
- * - 1B: Message id
- * - 2B: Payload length
- * - lB: Payload
- * - 2B: Checksum
- */
- uint8_t message[6 + UBX_MAX_PAYLOAD_LENGTH + 2];
- uint16_t payloadLength;
-
- while (!shouldStop())
- {
- StackLogger::getInstance().updateStack(THID_GPS);
-
- // Try to read the message
- if (!readUBXMessage(message, payloadLength))
- {
- LOG_DEBUG(logger, "Unable to read a UBX message");
- continue;
- }
-
- // Parse the message
- if (!parseUBXMessage(message))
- {
- LOG_DEBUG(logger,
- "UBX message not recognized (class:0x{02x}, id: 0x{02x})",
- message[2], message[3]);
- }
- }
-}
-
-void UbloxGPS::ubxChecksum(uint8_t* msg, int len)
-{
- uint8_t ck_a = 0, ck_b = 0;
-
- // The length must be valid, at least 8 bytes (preamble, msg, length,
- // checksum)
- if (len <= 8)
- {
- return;
- }
-
- // Checksum calculation from byte 2 to end of payload
- for (int i = 2; i < len - 2; i++)
- {
- ck_a = ck_a + msg[i];
- ck_b = ck_b + ck_a;
- }
-
- msg[len - 2] = ck_a;
- msg[len - 1] = ck_b;
-}
-
-bool UbloxGPS::writeUBXMessage(uint8_t* message, int length)
-{
- // Compute the checksum
- ubxChecksum(message, length);
-
- // Write configuration
- if (write(gpsFile, message, length) < 0)
- {
- LOG_ERR(logger,
- "Failed to write ubx message (class:0x{02x}, id: 0x{02x})",
- message[2], message[3]);
- return false;
- }
-
- return true;
-}
-
-bool UbloxGPS::serialCommuinicationSetup()
-{
- intrusive_ref_ptr<DevFs> devFs = FilesystemManager::instance().getDevFs();
-
- // Change the baudrate only if it is different than the default
- if (baudrate != defaultBaudrate)
- {
- // Close the gps file if already opened
- devFs->remove(serialPortName);
-
- // Open the serial port device with the default boudrate
- if (!devFs->addDevice(serialPortName,
- intrusive_ref_ptr<Device>(new STM32Serial(
- serialPortNumber, defaultBaudrate))))
- {
- LOG_ERR(logger,
- "[gps] Faild to open serial port {0} with baudrate {1} as "
- "file {2}",
- serialPortNumber, defaultBaudrate, serialPortName);
- return false;
- }
-
- // Open the gps file
- if ((gpsFile = open(gpsFilePath, O_RDWR)) < 0)
- {
- LOG_ERR(logger, "Failed to open gps file {}", gpsFilePath);
- return false;
- }
-
- // Change boudrate
- if (!setBaudrate())
- {
- return false;
- };
-
- // Close the gps file
- if (close(gpsFile) < 0)
- {
- LOG_ERR(logger, "Failed to close gps file {}", gpsFilePath);
- return false;
- }
-
- // Close the serial port
- if (!devFs->remove(serialPortName))
- {
- LOG_ERR(logger, "Failed to close serial port {} as file {}",
- serialPortNumber, serialPortName);
- return false;
- }
- }
-
- // Reopen the serial port with the configured boudrate
- if (!devFs->addDevice(serialPortName,
- intrusive_ref_ptr<Device>(
- new STM32Serial(serialPortNumber, baudrate))))
- {
- LOG_ERR(logger,
- "Faild to open serial port {} with baudrate {} as file {}\n",
- serialPortNumber, defaultBaudrate, serialPortName);
- return false;
- }
-
- // Reopen the gps file
- if ((gpsFile = open(gpsFilePath, O_RDWR)) < 0)
- {
- LOG_ERR(logger, "Failed to open gps file {}", gpsFilePath);
- return false;
- }
-
- return true;
-}
-
-bool UbloxGPS::resetConfiguration()
-{
- uint8_t ubx_cfg_prt[RESET_CONFIG_MSG_LEN] = {
- 0Xb5, 0x62, // Preamble
- 0x06, 0x04, // Message UBX-CFG-RST
- 0x04, 0x00, // Length
- 0x00, 0x00, // navBbrMask (Hot start)
- 0x00, // Hardware reset immediately
- 0x00, // Reserved
- 0xff, 0xff // Checksum
- };
-
- return writeUBXMessage(ubx_cfg_prt, RESET_CONFIG_MSG_LEN);
-}
-
-bool UbloxGPS::setBaudrate()
-{
- uint8_t ubx_cfg_prt[SET_BAUDRATE_MSG_LEN] = {
- 0Xb5, 0x62, // Preamble
- 0x06, 0x8a, // Message UBX-CFG-VALSET
- 0x0c, 0x00, // Length
- 0x00, // Version
- 0xff, // All layers
- 0x00, 0x00, // Reserved
- 0x01, 0x00, 0x52, 0x40, // Configuration item key ID
- 0xff, 0xff, 0xff, 0xff, // Value
- 0xff, 0xff // Checksum
- };
-
- // Prepare boud rate
- ubx_cfg_prt[14] = baudrate;
- ubx_cfg_prt[15] = baudrate >> 8;
- ubx_cfg_prt[16] = baudrate >> 16;
- ubx_cfg_prt[17] = baudrate >> 24;
-
- return writeUBXMessage(ubx_cfg_prt, SET_BAUDRATE_MSG_LEN);
-}
-
-bool UbloxGPS::disableNMEAMessages()
-{
- uint8_t ubx_cfg_valset[DISABLE_NMEA_MESSAGES_MSG_LEN] = {
- 0Xb5, 0x62, // Preamble
- 0x06, 0x8a, // Message UBX-CFG-VALSET
- 0x22, 0x00, // Length
- 0x00, // Version
- 0xff, // All layers
- 0x00, 0x00, // Reserved
- 0xbb, 0x00, 0x91, 0x20, // CFG-MSGOUT-NMEA_ID_GGA_UART1 key ID
- 0x00, // CFG-MSGOUT-NMEA_ID_GGA_UART1 value
- 0xca, 0x00, 0x91, 0x20, // CFG-MSGOUT-NMEA_ID_GLL_UART1 key ID
- 0x00, // CFG-MSGOUT-NMEA_ID_GLL_UART1 value
- 0xc0, 0x00, 0x91, 0x20, // CFG-MSGOUT-NMEA_ID_GSA_UART1 key ID
- 0x00, // CFG-MSGOUT-NMEA_ID_GSA_UART1 value
- 0xc5, 0x00, 0x91, 0x20, // CFG-MSGOUT-NMEA_ID_GSV_UART1 key ID
- 0x00, // CFG-MSGOUT-NMEA_ID_GSV_UART1 value
- 0xac, 0x00, 0x91, 0x20, // CFG-MSGOUT-NMEA_ID_RMC_UART1 key ID
- 0x00, // CFG-MSGOUT-NMEA_ID_RMC_UART1 value
- 0xb1, 0x00, 0x91, 0x20, // CFG-MSGOUT-NMEA_ID_VTG_UART1 key ID
- 0x00, // CFG-MSGOUT-NMEA_ID_VTG_UART1 value
- 0xff, 0xff // Checksum
- };
-
- return writeUBXMessage(ubx_cfg_valset, DISABLE_NMEA_MESSAGES_MSG_LEN);
-}
-
-bool UbloxGPS::setGNSSConfiguration()
-{
- uint8_t ubx_cfg_valset[SET_GNSS_CONF_LEN] = {
- 0Xb5, 0x62, // Preamble
- 0x06, 0x8a, // Message UBX-CFG-VALSET
- 0x09, 0x00, // Length
- 0x00, // Version
- 0x07, // All layers
- 0x00, 0x00, // Reserved
- 0x21, 0x00, 0x11, 0x20, // CFG-NAVSPG-DYNMODEL key ID
- 0x08, // CFG-NAVSPG-DYNMODEL value
- 0xff, 0xff // Checksum
- };
-
- return writeUBXMessage(ubx_cfg_valset, SET_GNSS_CONF_LEN);
-}
-
-bool UbloxGPS::enableUBXMessages()
-{
- uint8_t ubx_cfg_valset[ENABLE_UBX_MESSAGES_MSG_LEN] = {
- 0Xb5, 0x62, // Preamble
- 0x06, 0x8a, // Message UBX-CFG-VALSET
- 0x09, 0x00, // Length
- 0x00, // Version
- 0xff, // All layers
- 0x00, 0x00, // Reserved
- 0x07, 0x00, 0x91, 0x20, // CFG-MSGOUT-UBX_NAV_PVT_UART1 key ID
- 0x01, // CFG-MSGOUT-UBX_NAV_PVT_UART1 value
- 0xff, 0xff // Checksum
- };
-
- return writeUBXMessage(ubx_cfg_valset, ENABLE_UBX_MESSAGES_MSG_LEN);
-}
-
-bool UbloxGPS::setRate()
-{
- uint16_t rate = 1000 / sampleRate;
- LOG_DEBUG(logger, "Rate: {}", rate);
-
- uint8_t ubx_cfg_valset[SET_RATE_MSG_LEN] = {
- 0Xb5, 0x62, // Preamble
- 0x06, 0x8a, // Message UBX-CFG-VALSET
- 0x0a, 0x00, // Length
- 0x00, // Version
- 0x07, // All layers
- 0x00, 0x00, // Reserved
- 0x01, 0x00, 0x21, 0x30, // CFG-RATE-MEAS key ID
- 0xff, 0xff, // CFG-RATE-MEAS value
- 0xff, 0xff // Checksum
- };
-
- // Prepare rate
- ubx_cfg_valset[14] = rate;
- ubx_cfg_valset[15] = rate >> 8;
-
- return writeUBXMessage(ubx_cfg_valset, SET_RATE_MSG_LEN);
-}
-
-bool UbloxGPS::readUBXMessage(uint8_t* message, uint16_t& payloadLength)
-{
- // Read preamble
- do
- {
- // Read util the first byte of the preamble
- do
- {
- if (read(gpsFile, &message[0], 1) <= 0) // No more data available
- {
- return false;
- }
- } while (message[0] != PREAMBLE[0]);
-
- // Read the next byte
- if (read(gpsFile, &message[1], 1) <= 0) // No more data available
- {
- return false;
- }
- } while (message[1] != PREAMBLE[1]); // Continue
-
- // Read message class and ID
- if (read(gpsFile, &message[2], 1) <= 0)
- {
- return false;
- }
- if (read(gpsFile, &message[3], 1) <= 0)
- {
- return false;
- }
-
- // Read length
- if (read(gpsFile, &message[4], 2) <= 0)
- {
- return false;
- }
- payloadLength = message[4] | (message[5] << 8);
- if (payloadLength > UBX_MAX_PAYLOAD_LENGTH)
- {
- return false;
- }
-
- // Read paylaod and checksum
- for (auto i = 0; i < payloadLength + 2; i++)
- {
- if (read(gpsFile, &message[6 + i], 1) <= 0)
- {
- return false;
- }
- }
-
- // Verify the checksum
- uint8_t msgChecksum1 = message[6 + payloadLength];
- uint8_t msgChecksum2 = message[6 + payloadLength + 1];
- ubxChecksum(message, 6 + payloadLength + 2);
- if (msgChecksum1 != message[6 + payloadLength] ||
- msgChecksum2 != message[6 + payloadLength + 1])
- {
- LOG_ERR(logger, "Message checksum verification failed");
- return false;
- }
-
- return true;
-}
-
-bool UbloxGPS::parseUBXMessage(uint8_t* message)
-{
- switch (message[2]) // Message class
- {
- case 0x01: // UBX-NAV
- return parseUBXNAVMessage(message);
- case 0x05: // UBX-ACK
- return parseUBXACKMessage(message);
- }
- return false;
-}
-
-bool UbloxGPS::parseUBXNAVMessage(uint8_t* message)
-{
- switch (message[3]) // Message id
- {
- case 0x07: // UBX-NAV-PVT
- // Lock the threadSample variable
- Lock<FastMutex> l(mutex);
-
- // Latitude
- int32_t rawLatitude = message[6 + 28] | message[6 + 29] << 8 |
- message[6 + 30] << 16 | message[6 + 31] << 24;
- threadSample.latitude = (float)rawLatitude / 1e7;
-
- // Longitude
- int32_t rawLongitude = message[6 + 24] | message[6 + 25] << 8 |
- message[6 + 26] << 16 |
- message[6 + 27] << 24;
- threadSample.longitude = (float)rawLongitude / 1e7;
-
- // Height
- int32_t rawHeight = message[6 + 32] | message[6 + 33] << 8 |
- message[6 + 34] << 16 | message[6 + 35] << 24;
- threadSample.height = (float)rawHeight / 1e3;
-
- // Velocity north
- int32_t rawVelocityNorth = message[6 + 48] | message[6 + 49] << 8 |
- message[6 + 50] << 16 |
- message[6 + 51] << 24;
- threadSample.velocityNorth = (float)rawVelocityNorth / 1e3;
-
- // Velocity east
- int32_t rawVelocityEast = message[6 + 52] | message[6 + 53] << 8 |
- message[6 + 54] << 16 |
- message[6 + 55] << 24;
- threadSample.velocityEast = (float)rawVelocityEast / 1e3;
-
- // Velocity down
- int32_t rawVelocityDown = message[6 + 56] | message[6 + 57] << 8 |
- message[6 + 58] << 16 |
- message[6 + 59] << 24;
- threadSample.velocityDown = (float)rawVelocityDown / 1e3;
-
- // Speed
- int32_t rawSpeed = message[6 + 60] | message[6 + 61] << 8 |
- message[6 + 62] << 16 | message[6 + 63] << 24;
- threadSample.speed = (float)rawSpeed / 1e3;
-
- // Track (heading of motion)
- int32_t rawTrack = message[6 + 64] | message[6 + 65] << 8 |
- message[6 + 66] << 16 | message[6 + 67] << 24;
- threadSample.track = (float)rawTrack / 1e5;
-
- // Number of satellite
- threadSample.satellites = (uint8_t)message[6 + 23];
-
- // Fix (every type of fix accepted)
- threadSample.fix = message[6 + 20] != 0;
-
- // Timestamp
- threadSample.gpsTimestamp =
- TimestampTimer::getInstance().getTimestamp();
-
- return true;
- }
-
- return false;
-}
-
-bool UbloxGPS::parseUBXACKMessage(uint8_t* message)
-{
- switch (message[3]) // Message id
- {
- case 0x00: // UBX-ACK-NAC
- LOG_DEBUG(logger,
- "Received NAC for message (class:0x{02x}, id: 0x{02x})",
- message[6], message[7]);
- return true;
- case 0x01: // UBX-ACK-ACK
- LOG_DEBUG(logger,
- "Received ACK for message (class:0x{02x}, id: 0x{02x})",
- message[6], message[7]);
- return true;
- }
- return false;
-}
-
-} // namespace Boardcore
diff --git a/src/shared/sensors/UbloxGPS/UbloxGPS.h b/src/shared/sensors/UbloxGPS/UbloxGPS.h
deleted file mode 100644
index 9495bc6f5fe792774f0bb9a59c32f35c6e97e5b8..0000000000000000000000000000000000000000
--- a/src/shared/sensors/UbloxGPS/UbloxGPS.h
+++ /dev/null
@@ -1,151 +0,0 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#pragma once
-
-#include <ActiveObject.h>
-#include <diagnostic/PrintLogger.h>
-#include <miosix.h>
-#include <sensors/Sensor.h>
-
-#include "UbloxGPSData.h"
-
-namespace Boardcore
-{
-
-/**
- * @brief Driver for Ublox GPSs
- *
- * This driver handles communication and setup with Ublox GPSs. It uses the
- * binary UBX protocol to retrieve and parse navigation data quicker than using
- * the string based NMEA.
- *
- * At initialization it configures the device with the specified baudrate, reset
- * the configuration and sets up UBX messages and GNSS parameters.
- *
- * Communication with the device is performed through a file, the driver opens
- * the serial port under the filepath /dev/<serialPortName>.
- * There is no need for the file to be setted up beforhand.
- *
- * This driver was written for a NEO-M9N gps with the latest version of UBX.
- */
-class UbloxGPS : public Sensor<UbloxGPSData>, public ActiveObject
-{
-public:
- /**
- * @brief Construct a new UbloxGPS object
- *
- * @param boudrate_ Baudrate to communicate with the device (max: 921600,
- * min: 4800 for NEO-M9N)
- * @param sampleRate_ GPS sample rate (max: 25 for NEO-M9N)
- * @param serialPortName_ Name of the file for the gps device
- * @param defaultBaudrate_ Startup baudrate (38400 for NEO-M9N)
- */
- UbloxGPS(int boudrate_ = 921600, uint8_t sampleRate_ = 10,
- int serialPortNumber_ = 2, const char *serialPortName_ = "gps",
- int defaultBaudrate_ = 38400);
-
- /**
- * @brief Sets up the serial port baudrate, disables the NMEA messages,
- * configures GNSS options and enables UBX-PVT message
- *
- * @return True if the operation succeeded
- */
- bool init() override;
-
- /**
- * @brief Read a single message form the GPS, waits 2 sample cycle
- *
- * @return True if a message was sampled
- */
- bool selfTest() override;
-
-private:
- UbloxGPSData sampleImpl() override;
-
- void run() override;
-
- void ubxChecksum(uint8_t *msg, int len);
-
- /**
- * @brief Compute the checksum and write the message to the device
- */
- bool writeUBXMessage(uint8_t *message, int length);
-
- /**
- * @brief Sets up the serial port with the correct baudrate
- *
- * Opens the serial port with the defaul baudrate and changes it to
- * the value specified in the constructor, then it reopens the serial port.
- * If the device is already using the correct baudrate this won't have
- * effect. However if the gps is using a different baudrate the diver won't
- * be able to communicate.
- */
- bool serialCommuinicationSetup();
-
- bool resetConfiguration();
-
- bool setBaudrate();
-
- bool disableNMEAMessages();
-
- bool setGNSSConfiguration();
-
- bool enableUBXMessages();
-
- bool setRate();
-
- bool readUBXMessage(uint8_t *message, uint16_t &payloadLength);
-
- bool parseUBXMessage(uint8_t *message);
-
- bool parseUBXNAVMessage(uint8_t *message);
-
- bool parseUBXACKMessage(uint8_t *message);
-
- const int baudrate; // [baud]
- const uint8_t sampleRate; // [Hz]
- const int serialPortNumber;
- const char *serialPortName;
- const int defaultBaudrate; // [baud]
-
- char gpsFilePath[16]; ///< Allows for a filename of up to 10 characters
- int gpsFile = -1;
-
- mutable miosix::FastMutex mutex;
- UbloxGPSData threadSample{};
-
- static constexpr int SET_BAUDRATE_MSG_LEN = 20;
- static constexpr int RESET_CONFIG_MSG_LEN = 12;
- static constexpr int DISABLE_NMEA_MESSAGES_MSG_LEN = 42;
- static constexpr int SET_GNSS_CONF_LEN = 17;
- static constexpr int ENABLE_UBX_MESSAGES_MSG_LEN = 17;
- static constexpr int SET_RATE_MSG_LEN = 18;
-
- static constexpr uint8_t PREAMBLE[] = {0xb5, 0x62};
-
- static constexpr int UBX_MAX_PAYLOAD_LENGTH = 92;
-
- PrintLogger logger = Logging::getLogger("ubloxgps");
-};
-
-} // namespace Boardcore
diff --git a/src/tests/algorithms/NAS/test-nas-parafoil.cpp b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
index 57fd167c839ec3bdef2d24c69417ddb7f118aab1..06f5bccae190975ac036c7a61806baed7af9845c 100644
--- a/src/tests/algorithms/NAS/test-nas-parafoil.cpp
+++ b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
@@ -26,7 +26,7 @@
#include <miosix.h>
#include <sensors/MPU9250/MPU9250.h>
#include <sensors/SensorManager.h>
-#include <sensors/UbloxGPS/UbloxGPS.h>
+#include <sensors/UBXGPS/UBXGPSSerial.h>
#include <utils/AeroUtils/AeroUtils.h>
#include <utils/SkyQuaternion/SkyQuaternion.h>
@@ -48,8 +48,8 @@ Vector2f startPos = Vector2f(45.501141, 9.156281);
NAS* nas;
SPIBus spi1(SPI1);
-MPU9250* imu = nullptr;
-UbloxGPS* gps = nullptr;
+MPU9250* imu = nullptr;
+UBXGPSSerial* gps = nullptr;
int main()
{
@@ -110,7 +110,7 @@ void init()
imu = new MPU9250(spi1, sensors::mpu9250::cs::getPin());
imu->init();
- gps = new UbloxGPS(38400, 10, 2, "gps", 38400);
+ gps = new UBXGPSSerial(38400, 10, 2, "gps", 38400);
gps->init();
gps->start();
diff --git a/src/tests/algorithms/NAS/test-nas-stack.cpp b/src/tests/algorithms/NAS/test-nas-stack.cpp
index e72a1c9830b63682cffff7968cbc27106b85a61f..31980dd9195f45c444cba38d8a0bd1e927f22268 100644
--- a/src/tests/algorithms/NAS/test-nas-stack.cpp
+++ b/src/tests/algorithms/NAS/test-nas-stack.cpp
@@ -27,7 +27,7 @@
#include <sensors/BMX160/BMX160.h>
#include <sensors/MS5803/MS5803.h>
#include <sensors/SensorManager.h>
-#include <sensors/UbloxGPS/UbloxGPS.h>
+#include <sensors/UBXGPS/UBXGPSSerial.h>
#include <utils/AeroUtils/AeroUtils.h>
#include <utils/Constants.h>
#include <utils/SkyQuaternion/SkyQuaternion.h>
@@ -50,9 +50,9 @@ Vector2f startPos = Vector2f(45.501141, 9.156281);
NAS* nas;
SPIBus spi1(SPI1);
-BMX160* imu = nullptr;
-UbloxGPS* gps = nullptr;
-MS5803* baro = nullptr;
+BMX160* imu = nullptr;
+UBXGPSSerial* gps = nullptr;
+MS5803* baro = nullptr;
int main()
{
@@ -134,7 +134,7 @@ void init()
new BMX160(spi1, sensors::bmx160::cs::getPin(), bmx_config, spiConfig);
imu->init();
- gps = new UbloxGPS(921600, 10, 2, "gps", 38400);
+ gps = new UBXGPSSerial(921600, 10, 2, "gps", 38400);
gps->init();
gps->start();
diff --git a/src/tests/catch/xbee/test-xbee-parser.cpp b/src/tests/catch/xbee/test-xbee-parser.cpp
index 924d96851e8970f3729ef57d13f65de28ae78be1..26867ab7acd2037a74f063e06089dd0d52c61e2f 100644
--- a/src/tests/catch/xbee/test-xbee-parser.cpp
+++ b/src/tests/catch/xbee/test-xbee-parser.cpp
@@ -71,9 +71,7 @@ void printu64(uint64_t v)
uint8_t* p = reinterpret_cast<uint8_t*>(&v);
for (int i = 0; i < 8; i++)
- {
printf("%02X ", p[i]);
- }
printf("\n");
}
diff --git a/src/tests/sensors/test-ubloxgps.cpp b/src/tests/sensors/test-ubxgps-serial.cpp
similarity index 95%
rename from src/tests/sensors/test-ubloxgps.cpp
rename to src/tests/sensors/test-ubxgps-serial.cpp
index 24b6cc092a73e531765fbbe4897a29133a827014..5518137419f3e03ea55e72011efdab7dbae65111 100644
--- a/src/tests/sensors/test-ubloxgps.cpp
+++ b/src/tests/sensors/test-ubxgps-serial.cpp
@@ -22,7 +22,7 @@
#include <drivers/timer/TimestampTimer.h>
#include <miosix.h>
-#include <sensors/UbloxGPS/UbloxGPS.h>
+#include <sensors/UBXGPS/UBXGPSSerial.h>
#include <utils/Debug.h>
#include <cstdio>
@@ -39,8 +39,8 @@ int main()
printf("Welcome to the ublox test\n");
// Keep GPS baud rate at default for easier testing
- UbloxGPS gps(921600, RATE, 2, "gps", 38400);
- UbloxGPSData dataGPS;
+ UBXGPSSerial gps(921600, RATE, 2, "gps", 38400);
+ UBXGPSData dataGPS;
printf("Gps allocated\n");
// Init the gps
diff --git a/src/tests/sensors/test-ubxgps-spi.cpp b/src/tests/sensors/test-ubxgps-spi.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3fcecde01e161e98c8e01a1aa2021d8e4e4775dc
--- /dev/null
+++ b/src/tests/sensors/test-ubxgps-spi.cpp
@@ -0,0 +1,82 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio, Damiano Amatruda
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <sensors/UBXGPS/UBXGPSSpi.h>
+#include <utils/Debug.h>
+
+using namespace miosix;
+using namespace Boardcore;
+
+int main()
+{
+#ifdef _BOARD_STM32F429ZI_SKYWARD_DEATHST_X
+ SPIBus spiBus(SPI2);
+ GpioPin spiCs(GPIOG_BASE, 3);
+ GpioPin spiSck(GPIOB_BASE, 13);
+ GpioPin spiMiso(GPIOB_BASE, 14);
+ GpioPin spiMosi(GPIOB_BASE, 15);
+#else
+ SPIBus spiBus(SPI1);
+ GpioPin spiCs(GPIOA_BASE, 3);
+ GpioPin spiSck(GPIOA_BASE, 5);
+ GpioPin spiMiso(GPIOA_BASE, 6);
+ GpioPin spiMosi(GPIOA_BASE, 7);
+#endif
+
+ spiCs.mode(Mode::OUTPUT);
+ spiCs.high();
+ spiSck.mode(Mode::ALTERNATE);
+ spiSck.alternateFunction(5);
+ spiMiso.mode(Mode::ALTERNATE);
+ spiMiso.alternateFunction(5);
+ spiMosi.mode(Mode::ALTERNATE);
+ spiMosi.alternateFunction(5);
+
+ UBXGPSSpi gps{spiBus, spiCs};
+
+ TRACE("Initializing UBXGPSSpi...\n");
+
+ while (!gps.init())
+ {
+ TRACE("Init failed! (code: %d)\n", gps.getLastError());
+
+ TRACE("Retrying in 10 seconds...\n");
+ miosix::Thread::sleep(10000);
+ }
+
+ while (true)
+ {
+ gps.sample();
+ GPSData sample __attribute__((unused)) = gps.getLastSample();
+
+ TRACE(
+ "timestamp: %4.3f, lat: %f, lon: %f, height: %4.1f, velN: %3.2f, "
+ "velE: %3.2f, velD: %3.2f, speed: %3.2f, track %3.1f, pDOP: %f, "
+ "nsat: %2d, fix: %2d\n",
+ (float)sample.gpsTimestamp / 1000000, sample.latitude,
+ sample.longitude, sample.height, sample.velocityNorth,
+ sample.velocityEast, sample.velocityDown, sample.speed,
+ sample.track, sample.positionDOP, sample.satellites, sample.fix);
+
+ Thread::sleep(1000 / gps.getSampleRate());
+ }
+}