diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake index a37df344373e492d13a75e30d802312f17430004..769c32b78e1cee501fa32effb2b9f872a5f4c597 100644 --- a/cmake/boardcore.cmake +++ b/cmake/boardcore.cmake @@ -80,6 +80,8 @@ foreach(OPT_BOARD ${BOARDS}) ${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 # AeroUtils diff --git a/src/shared/sensors/SensorData.h b/src/shared/sensors/SensorData.h index a0e76f7aa0a207fa38c96edf5aee385280a1b9cf..2439fdf4bae21a224f9485c751ec939b63e860d1 100644 --- a/src/shared/sensors/SensorData.h +++ b/src/shared/sensors/SensorData.h @@ -127,16 +127,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..9f1593baaa01a1ce8b3d7be31488e9ba12cfd05d --- /dev/null +++ b/src/shared/sensors/UBXGPS/UBXFrame.h @@ -0,0 +1,308 @@ +/* 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 +}; + +/** + * @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/UBXGPS/UBXGPSData.h b/src/shared/sensors/UBXGPS/UBXGPSData.h new file mode 100644 index 0000000000000000000000000000000000000000..ed734d258bd1c0dd79206b8d392562b7009a22bf --- /dev/null +++ b/src/shared/sensors/UBXGPS/UBXGPSData.h @@ -0,0 +1,48 @@ +/* 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 + * 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 UBXGPSData : public GPSData +{ + static std::string header() + { + 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 << "," << positionDOP + << "," << (int)satellites << "," << (int)fix << "\n"; + } +}; + +} // namespace Boardcore diff --git a/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp b/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4bd60787c5a033840be60e2b64a2bb4c4e218bda --- /dev/null +++ b/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp @@ -0,0 +1,426 @@ +/* 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 (!setBaudrateAndUBX()) + { + 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::setBaudrateAndUBX(bool safe) +{ + uint8_t payload[] = { + 0x01, // UART port + 0x00, // Reserved + 0x00, 0x00, // txReady + 0xc0, 0x08, 0x00, 0x00, // 8bit, no parity, 1 stop bit + 0x00, 0x00, 0x00, 0x00, // Baudrate + 0x01, 0x00, // inProtoMask = UBX + 0x01, 0x00, // outProtoMask = UBX + 0x00, 0x00, // flags + 0x00, 0x00 // reserved2 + }; + + // Prepare baudrate + payload[8] = baudrate; + payload[9] = baudrate >> 8; + payload[10] = baudrate >> 16; + payload[11] = baudrate >> 24; + + UBXFrame frame{UBXMessage::UBX_CFG_PRT, payload, sizeof(payload)}; + + if (safe) + return safeWriteUBXFrame(frame); + else + return writeUBXFrame(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 baudrate + if (!devFs->addDevice(serialPortName, + intrusive_ref_ptr<Device>(new STM32Serial( + serialPortNumber, defaultBaudrate)))) + { + LOG_ERR(logger, + "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 baudrate + if (!setBaudrateAndUBX(false)) + { + 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 baudrate + 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::setDynamicModelToAirborne4g() +{ + uint8_t payload[] = { + 0x01, 0x00, // Parameters bitmask, apply dynamic model configuration + 0x08, // Dynamic model = airborne 4g + 0x00, // Fix mode + 0x00, 0x00, 0x00, 0x00, // Fixed altitude for 2D mode + 0x00, 0x00, 0x00, 0x00, // Fixed altitude variance for 2D mode + 0x00, // Minimum 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..4785778c2beab52185b2b614f06db7ab2c0e4769 --- /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(); + + /** + * @brief Sets the UART port baudrate and enables UBX and disables NMEA on + * the UART port. + * + * @param safe Whether to expect an ack after the command. + * @return True if the configuration received an acknowledgement. + */ + bool setBaudrateAndUBX(bool safe = true); + + /** + * @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 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/tests/catch/test-packetqueue.cpp b/src/tests/catch/test-packetqueue.cpp index 6d09548dc20e7b2cf7323c25d9b654919033c2c8..58a0a10725f85ee7be62cbe7f5664e806479677f 100644 --- a/src/tests/catch/test-packetqueue.cpp +++ b/src/tests/catch/test-packetqueue.cpp @@ -83,7 +83,7 @@ TEST_CASE("Packet tests") SECTION("Adding stuff to packet") { - REQUIRE(p.tryAppend(messageBase, 5)); + REQUIRE(p.append(messageBase, 5)); uint64_t ts = p.timestamp(); REQUIRE(miosix::getTick() - ts < 5); REQUIRE(p.dump(buf) == 5); @@ -93,20 +93,20 @@ TEST_CASE("Packet tests") REQUIRE(p.size() == 5); REQUIRE(p.getMsgCount() == 1); - REQUIRE(p.tryAppend(messageBase + 5, 3)); + REQUIRE(p.append(messageBase + 5, 3)); REQUIRE(p.dump(buf) == 8); COMPARE(buf, BUF_LEN, "01234567"); REQUIRE(p.isEmpty() == false); REQUIRE(p.size() == 8); - REQUIRE_FALSE(p.tryAppend(messageBase + 8, 3)); + REQUIRE_FALSE(p.append(messageBase + 8, 3)); REQUIRE(p.dump(buf) == 8); COMPARE(buf, BUF_LEN, "01234567"); REQUIRE(p.isEmpty() == false); REQUIRE(p.size() == 8); REQUIRE(p.getMsgCount() == 2); - REQUIRE(p.tryAppend(messageBase + 8, 2)); + REQUIRE(p.append(messageBase + 8, 2)); REQUIRE(p.dump(buf) == 10); COMPARE(buf, BUF_LEN, "0123456789"); REQUIRE(p.isEmpty() == false); @@ -130,7 +130,7 @@ TEST_CASE("Packet tests") SECTION("Edge cases") { INFO("Adding empty msg"); - REQUIRE_FALSE(p.tryAppend(messageBase, 0)); + REQUIRE_FALSE(p.append(messageBase, 0)); REQUIRE(p.isEmpty()); REQUIRE(p.isFull() == false); REQUIRE(p.isReady() == false); @@ -141,7 +141,7 @@ TEST_CASE("Packet tests") REQUIRE(p.dump(buf) == 0); INFO("Adding too big msg"); - REQUIRE_FALSE(p.tryAppend(messageBase, PKT_LEN + 1)); + REQUIRE_FALSE(p.append(messageBase, PKT_LEN + 1)); REQUIRE(p.isEmpty()); REQUIRE(p.isFull() == false); @@ -152,9 +152,9 @@ TEST_CASE("Packet tests") REQUIRE(p.getMsgCount() == 0); REQUIRE(p.dump(buf) == 0); - INFO("Adding something to full packet") - REQUIRE(p.tryAppend(messageBase, PKT_LEN)); - REQUIRE_FALSE(p.tryAppend(messageBase, 1)); + INFO("Adding something to full packet"); + REQUIRE(p.append(messageBase, PKT_LEN)); + REQUIRE_FALSE(p.append(messageBase, 1)); REQUIRE(p.isEmpty() == false); REQUIRE(p.isFull()); @@ -288,17 +288,17 @@ TEST_CASE("PacketQueue tests") SECTION("Edge cases") { - INFO("Adding too big msg") + INFO("Adding too big msg"); REQUIRE(pq.put(messageBase, PKT_LEN + 1) == -1); REQUIRE_FALSE(pq.isFull()); REQUIRE(pq.isEmpty()); REQUIRE(pq.countNotEmpty() == 0); REQUIRE(pq.countReady() == 0); - INFO("Adding empty message") + INFO("Adding empty message"); REQUIRE(pq.put(messageBase, 0) == -1); - INFO("Adding something to full queue") + INFO("Adding something to full queue"); REQUIRE(pq.put(messageBase, PKT_LEN) == 0); REQUIRE(pq.put(messageBase + 5, PKT_LEN) == 0); REQUIRE(pq.put(messageBase + 10, PKT_LEN) == 0); @@ -319,7 +319,7 @@ TEST_CASE("PacketQueue tests") REQUIRE(pq.countNotEmpty() == 3); REQUIRE(pq.countReady() == 3); - INFO("Get/Pop on empty queue") + INFO("Get/Pop on empty queue"); REQUIRE_NOTHROW(pq.pop()); REQUIRE_NOTHROW(pq.pop()); REQUIRE_NOTHROW(pq.pop());