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 d7ac92ec3282024cfc6891855abbc3b381bcff1c..26363322d4e7e677b86a8fc4fdb4e8264736d062 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -349,15 +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-ubxgps src/tests/sensors/test-ubxgps.cpp)
-sbs_target(test-ubxgps 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-serial src/tests/sensors/test-ubloxgps.cpp)
-sbs_target(test-ubloxgps-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 5c4264138caf1c3acb46abbdd7cece140b7f27ab..a38c686066fed81df16df3fd247bf487af8f56e9 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -83,8 +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/UBXGPS.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/logger/LogTypes.h b/src/shared/logger/LogTypes.h
index 6623fbf11bee706926ebb76ddb2c3d2c66aca122..741e0113bd37b467a30d2f72857caf59253db7eb 100644
--- a/src/shared/logger/LogTypes.h
+++ b/src/shared/logger/LogTypes.h
@@ -47,7 +47,6 @@
#include <sensors/MS5803/MS5803Data.h>
#include <sensors/SensorData.h>
#include <sensors/UBXGPS/UBXGPSData.h>
-#include <sensors/UbloxGPS/UbloxGPSData.h>
#include <sensors/VN100/VN100Data.h>
#include <sensors/analog/BatteryVoltageSensorData.h>
#include <sensors/analog/CurrentSensorData.h>
@@ -98,7 +97,6 @@ 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>();
diff --git a/src/shared/sensors/UBXGPS/UBXFrame.h b/src/shared/sensors/UBXGPS/UBXFrame.h
index 9f1593baaa01a1ce8b3d7be31488e9ba12cfd05d..46437171199cdad96bf4477b06d8629969f4dfda 100644
--- a/src/shared/sensors/UBXGPS/UBXFrame.h
+++ b/src/shared/sensors/UBXGPS/UBXFrame.h
@@ -35,14 +35,15 @@ 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_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
};
/**
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/UBXGPS.cpp b/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
similarity index 91%
rename from src/shared/sensors/UBXGPS/UBXGPS.cpp
rename to src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
index a3b75fa617bddfcdae0df5b6771aa0e986a57d9b..832059373b8c67d9aaff5f75850f75ad5e0e05c7 100644
--- a/src/shared/sensors/UBXGPS/UBXGPS.cpp
+++ b/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
@@ -20,7 +20,7 @@
* THE SOFTWARE.
*/
-#include "UBXGPS.h"
+#include "UBXGPSSpi.h"
#include <drivers/timer/TimestampTimer.h>
#include <interfaces/endianness.h>
@@ -33,19 +33,19 @@ constexpr uint16_t UBXFrame::MAX_FRAME_LENGTH;
constexpr uint8_t UBXFrame::PREAMBLE[];
constexpr uint8_t UBXFrame::WAIT;
-constexpr float UBXGPS::MS_TO_TICK;
+constexpr float UBXGPSSpi::MS_TO_TICK;
-constexpr unsigned int UBXGPS::RESET_SLEEP_TIME;
-constexpr unsigned int UBXGPS::READ_TIMEOUT;
-constexpr unsigned int UBXGPS::MAX_TRIES;
+constexpr unsigned int UBXGPSSpi::RESET_SLEEP_TIME;
+constexpr unsigned int UBXGPSSpi::READ_TIMEOUT;
+constexpr unsigned int UBXGPSSpi::MAX_TRIES;
-UBXGPS::UBXGPS(SPIBusInterface& spiBus, miosix::GpioPin spiCs,
- SPIBusConfig spiConfig, uint8_t sampleRate)
+UBXGPSSpi::UBXGPSSpi(SPIBusInterface& spiBus, miosix::GpioPin spiCs,
+ SPIBusConfig spiConfig, uint8_t sampleRate)
: spiSlave(spiBus, spiCs, spiConfig), sampleRate(sampleRate)
{
}
-SPIBusConfig UBXGPS::getDefaultSPIConfig()
+SPIBusConfig UBXGPSSpi::getDefaultSPIConfig()
{
/* From data sheet of series NEO-M8:
* "Minimum initialization time" (setup time): 10 us
@@ -54,9 +54,9 @@ SPIBusConfig UBXGPS::getDefaultSPIConfig()
SPI::BitOrder::MSB_FIRST, 10, 1000};
}
-uint8_t UBXGPS::getSampleRate() { return sampleRate; }
+uint8_t UBXGPSSpi::getSampleRate() { return sampleRate; }
-bool UBXGPS::init()
+bool UBXGPSSpi::init()
{
LOG_DEBUG(logger, "Resetting the device...");
@@ -106,9 +106,9 @@ bool UBXGPS::init()
return true;
}
-bool UBXGPS::selfTest() { return true; }
+bool UBXGPSSpi::selfTest() { return true; }
-UBXGPSData UBXGPS::sampleImpl()
+UBXGPSData UBXGPSSpi::sampleImpl()
{
UBXPvtFrame pvt;
@@ -134,7 +134,7 @@ UBXGPSData UBXGPS::sampleImpl()
return sample;
}
-bool UBXGPS::reset()
+bool UBXGPSSpi::reset()
{
uint8_t payload[] = {
0x00, 0x00, // Hot start
@@ -154,7 +154,7 @@ bool UBXGPS::reset()
return true;
}
-bool UBXGPS::setUBXProtocol()
+bool UBXGPSSpi::setUBXProtocol()
{
uint8_t payload[] = {
0x04, // SPI port
@@ -173,7 +173,7 @@ bool UBXGPS::setUBXProtocol()
return safeWriteUBXFrame(frame);
}
-bool UBXGPS::setDynamicModelToAirborne4g()
+bool UBXGPSSpi::setDynamicModelToAirborne4g()
{
uint8_t payload[] = {
0x01, 0x00, // Parameters bitmask, apply dynamic model configuration
@@ -202,7 +202,7 @@ bool UBXGPS::setDynamicModelToAirborne4g()
return safeWriteUBXFrame(frame);
}
-bool UBXGPS::setSampleRate()
+bool UBXGPSSpi::setSampleRate()
{
uint8_t payload[] = {
0x00, 0x00, // Measurement rate
@@ -219,7 +219,7 @@ bool UBXGPS::setSampleRate()
return safeWriteUBXFrame(frame);
}
-bool UBXGPS::setPVTMessageRate()
+bool UBXGPSSpi::setPVTMessageRate()
{
uint8_t payload[] = {
0x01, 0x07, // PVT message
@@ -231,7 +231,7 @@ bool UBXGPS::setPVTMessageRate()
return safeWriteUBXFrame(frame);
}
-bool UBXGPS::readUBXFrame(UBXFrame& frame)
+bool UBXGPSSpi::readUBXFrame(UBXFrame& frame)
{
long long start = miosix::getTick();
long long end = start + READ_TIMEOUT * MS_TO_TICK;
@@ -296,7 +296,7 @@ bool UBXGPS::readUBXFrame(UBXFrame& frame)
return true;
}
-bool UBXGPS::writeUBXFrame(const UBXFrame& frame)
+bool UBXGPSSpi::writeUBXFrame(const UBXFrame& frame)
{
if (!frame.isValid())
{
@@ -315,7 +315,7 @@ bool UBXGPS::writeUBXFrame(const UBXFrame& frame)
return true;
}
-bool UBXGPS::safeWriteUBXFrame(const UBXFrame& frame)
+bool UBXGPSSpi::safeWriteUBXFrame(const UBXFrame& frame)
{
for (unsigned int i = 0; i < MAX_TRIES; i++)
{
diff --git a/src/shared/sensors/UBXGPS/UBXGPS.h b/src/shared/sensors/UBXGPS/UBXGPSSpi.h
similarity index 92%
rename from src/shared/sensors/UBXGPS/UBXGPS.h
rename to src/shared/sensors/UBXGPS/UBXGPSSpi.h
index 8918b2ea878c8d39b76dbaf75ce78e755fffe5d1..e20bc3bfcec2c8b36e69093291be5af048d80ce6 100644
--- a/src/shared/sensors/UBXGPS/UBXGPS.h
+++ b/src/shared/sensors/UBXGPS/UBXGPSSpi.h
@@ -33,9 +33,9 @@ namespace Boardcore
{
/**
- * @brief Sensor for u-blox GPS.
+ * @brief Sensor for UBlox GPS.
*
- * This sensor handles communication and setup with u-blox GPSs. It uses the
+ * 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.
*
@@ -46,7 +46,7 @@ namespace Boardcore
*
* This sensor is compatible with series NEO-M8 and NEO-M9.
*/
-class UBXGPS : public Sensor<UBXGPSData>
+class UBXGPSSpi : public Sensor<UBXGPSData>
{
public:
/**
@@ -57,9 +57,9 @@ public:
* @param spiConfig The SPI configuration.
* @param sampleRate The GPS sample rate [kHz].
*/
- UBXGPS(SPIBusInterface& spiBus, miosix::GpioPin spiCs,
- SPIBusConfig spiConfig = getDefaultSPIConfig(),
- uint8_t sampleRate = 5);
+ UBXGPSSpi(SPIBusInterface& spiBus, miosix::GpioPin spiCs,
+ SPIBusConfig spiConfig = getDefaultSPIConfig(),
+ uint8_t sampleRate = 5);
/**
* @brief Constructs the default config for the SPI bus.
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/shared/sensors/UbloxGPS/UbloxGPSData.h b/src/shared/sensors/UbloxGPS/UbloxGPSData.h
deleted file mode 100644
index 7f5570000b78b915a956b96e754d784e08f7611f..0000000000000000000000000000000000000000
--- a/src/shared/sensors/UbloxGPS/UbloxGPSData.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/* Copyright (c) 2020 Skyward Experimental Rocketry
- * Author: Davide Bonomini
- *
- * 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 <sensors/SensorData.h>
-
-namespace Boardcore
-{
-
-struct UbloxGPSData : public GPSData
-{
- static std::string header()
- {
- return "gps_timestamp,latitude,longitude,height,velocity_north,"
- "velocity_east,velocity_down,speed,track,num_satellites,fix\n";
- }
-
- void print(std::ostream &os) const
- {
- os << gpsTimestamp << "," << latitude << "," << longitude << ","
- << height << "," << velocityNorth << "," << velocityEast << ","
- << velocityDown << "," << speed << "," << track << ","
- << (int)satellites << "," << (int)fix << "\n";
- }
-};
-
-} // 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/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.cpp b/src/tests/sensors/test-ubxgps-spi.cpp
similarity index 96%
rename from src/tests/sensors/test-ubxgps.cpp
rename to src/tests/sensors/test-ubxgps-spi.cpp
index f5ec6e4194e8d6ff006a691be49cf89819246f75..3fcecde01e161e98c8e01a1aa2021d8e4e4775dc 100644
--- a/src/tests/sensors/test-ubxgps.cpp
+++ b/src/tests/sensors/test-ubxgps-spi.cpp
@@ -20,7 +20,7 @@
* THE SOFTWARE.
*/
-#include <sensors/UBXGPS/UBXGPS.h>
+#include <sensors/UBXGPS/UBXGPSSpi.h>
#include <utils/Debug.h>
using namespace miosix;
@@ -51,9 +51,9 @@ int main()
spiMosi.mode(Mode::ALTERNATE);
spiMosi.alternateFunction(5);
- UBXGPS gps{spiBus, spiCs};
+ UBXGPSSpi gps{spiBus, spiCs};
- TRACE("Initializing UBXGPS...\n");
+ TRACE("Initializing UBXGPSSpi...\n");
while (!gps.init())
{