diff --git a/src/shared/sensors/UbloxGPS/UBXUnpackedFrame.h b/src/shared/sensors/UbloxGPS/UBXUnpackedFrame.h
deleted file mode 100644
index ff3e1c83d3499f443de147b79996735208713988..0000000000000000000000000000000000000000
--- a/src/shared/sensors/UbloxGPS/UBXUnpackedFrame.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: 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 <algorithm>
-
-namespace Boardcore
-{
-
-static constexpr uint16_t UBX_MAX_PAYLOAD_LENGTH = 92;
-static constexpr uint16_t UBX_MAX_FRAME_LENGTH = UBX_MAX_PAYLOAD_LENGTH + 8;
-static constexpr uint8_t UBX_VALID_PREAMBLE[] = {0xb5, 0x62};
-
-struct UBXUnpackedFrame
-{
- uint8_t preamble[2];
- uint8_t cls;
- uint8_t id;
- uint8_t payload[UBX_MAX_PAYLOAD_LENGTH];
- uint16_t payloadLength;
- uint8_t checksum[2];
-
- UBXUnpackedFrame() = default;
-
- UBXUnpackedFrame(uint8_t cls, uint8_t id, const uint8_t* payload,
- uint16_t payloadLength)
- : cls(cls), id(id), payloadLength(payloadLength)
- {
- memcpy(preamble, UBX_VALID_PREAMBLE, 2);
-
- memcpy(this->payload, payload,
- std::min(payloadLength, UBX_MAX_PAYLOAD_LENGTH));
-
- calcChecksum(checksum);
- }
-
- inline uint16_t getFrameLength() const { return payloadLength + 8; }
-
- inline bool isValid() const
- {
- if (memcmp(preamble, UBX_VALID_PREAMBLE, 2) != 0)
- {
- return false;
- }
-
- if (payloadLength > UBX_MAX_PAYLOAD_LENGTH)
- {
- return false;
- }
-
- uint8_t validChecksum[2];
- calcChecksum(validChecksum);
- return memcmp(checksum, validChecksum, 2) == 0;
- }
-
- inline void writePacked(uint8_t* frame) const
- {
- memcpy(frame, preamble, 2);
- frame[2] = cls;
- frame[3] = id;
- memcpy(&frame[4], &payloadLength, 2);
- memcpy(&frame[6], payload, payloadLength);
- memcpy(&frame[6 + payloadLength], checksum, 2);
- }
-
- inline void readPacked(const uint8_t* frame)
- {
- memcpy(preamble, frame, 2);
- cls = frame[2];
- id = frame[3];
- memcpy(&payloadLength, &frame[4], 2);
- memcpy(payload, &frame[6],
- std::min(payloadLength, UBX_MAX_PAYLOAD_LENGTH));
- memcpy(checksum, &frame[6 + payloadLength], 2);
- }
-
- inline void calcChecksum(uint8_t* checksum) const
- {
- uint8_t data[UBX_MAX_FRAME_LENGTH];
- data[0] = cls;
- data[1] = id;
- memcpy(&data[2], &payloadLength, 2);
- memcpy(&data[4], payload,
- std::min(payloadLength, UBX_MAX_PAYLOAD_LENGTH));
-
- uint16_t dataLength =
- std::min(payloadLength, UBX_MAX_PAYLOAD_LENGTH) + 4;
-
- checksum[0] = 0;
- checksum[1] = 0;
-
- for (uint8_t i = 0; i < dataLength; ++i)
- {
- checksum[0] += data[i];
- checksum[1] += checksum[0];
- }
- }
-};
-
-} // namespace Boardcore
diff --git a/src/shared/sensors/UbloxGPS/UbloxGPS.cpp b/src/shared/sensors/UbloxGPS/UbloxGPS.cpp
index 3bc0879ad7138d4a49b3c5860357a14f431eb3ed..dff24046dfefeca63f88c4499f1d0b2bf4c9f9ce 100644
--- a/src/shared/sensors/UbloxGPS/UbloxGPS.cpp
+++ b/src/shared/sensors/UbloxGPS/UbloxGPS.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio, Damiano Amatruda
+ * 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
@@ -28,27 +28,39 @@
#include <fcntl.h>
#include <filesystem/file_access.h>
+using namespace miosix;
+
namespace Boardcore
{
-using namespace miosix;
+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 (!setupCommunication())
+ if (!serialCommuinicationSetup())
{
return false;
}
+ Thread::sleep(10);
+
// Reset configuration to default
+ // TODO: maybe move this on serial communication setup
if (!resetConfiguration())
{
return false;
}
- Thread::sleep(100);
-
// Disable NMEA messages
if (!disableNMEAMessages())
{
@@ -73,8 +85,8 @@ bool UbloxGPS::init()
Thread::sleep(100);
- // Set sample rate
- if (!setSampleRate())
+ // Set rate
+ if (!setRate())
{
return false;
}
@@ -86,57 +98,200 @@ bool UbloxGPS::selfTest() { return true; }
UbloxGPSData UbloxGPS::sampleImpl()
{
- Lock<FastMutex> l(sampleMutex);
- return lastSample;
+ Lock<FastMutex> l(mutex);
+ return threadSample;
}
void UbloxGPS::run()
{
- UBXUnpackedFrame frame;
+ /**
+ * 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 (!readUBXFrame(frame))
+ if (!readUBXMessage(message, payloadLength))
{
LOG_DEBUG(logger, "Unable to read a UBX message");
continue;
}
// Parse the message
- if (!parseUBXFrame(frame))
+ if (!parseUBXMessage(message))
{
- LOG_DEBUG(
- logger,
- "UBX message not recognized (class: {:#02x}, id: {:#02x})",
- frame.cls, frame.id);
+ LOG_DEBUG(logger,
+ "UBX message not recognized (class:0x{02x}, id: 0x{02x})",
+ message[2], message[3]);
}
}
}
-bool UbloxGPS::resetConfiguration()
+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()
{
- static constexpr uint16_t payloadLength = 4;
+ 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;
+}
- uint8_t payload[payloadLength] = {
+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
+ 0x00, // Reserved
+ 0xff, 0xff // Checksum
};
- UBXUnpackedFrame frame{0x06, 0x04, // Message UBX-CFG-RST
- payload, payloadLength};
+ return writeUBXMessage(ubx_cfg_prt, RESET_CONFIG_MSG_LEN);
+}
- return writeUBXFrame(frame);
+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()
{
- static constexpr uint16_t payloadLength = 34;
-
- uint8_t payload[payloadLength] = {
+ 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
@@ -151,146 +306,208 @@ bool UbloxGPS::disableNMEAMessages()
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
+ 0x00, // CFG-MSGOUT-NMEA_ID_VTG_UART1 value
+ 0xff, 0xff // Checksum
};
- UBXUnpackedFrame frame{0x06, 0x8a, // Message UBX-CFG-VALSET
- payload, payloadLength};
-
- return writeUBXFrame(frame);
+ return writeUBXMessage(ubx_cfg_valset, DISABLE_NMEA_MESSAGES_MSG_LEN);
}
bool UbloxGPS::setGNSSConfiguration()
{
- static constexpr uint16_t payloadLength = 9;
-
- uint8_t payload[payloadLength] = {
+ 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
+ 0x08, // CFG-NAVSPG-DYNMODEL value
+ 0xff, 0xff // Checksum
};
- UBXUnpackedFrame frame{0x06, 0x8a, // Message UBX-CFG-VALSET
- payload, payloadLength};
-
- return writeUBXFrame(frame);
+ return writeUBXMessage(ubx_cfg_valset, SET_GNSS_CONF_LEN);
}
bool UbloxGPS::enableUBXMessages()
{
- static constexpr uint16_t payloadLength = 9;
-
- uint8_t payload[payloadLength] = {
+ 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
+ 0x01, // CFG-MSGOUT-UBX_NAV_PVT_UART1 value
+ 0xff, 0xff // Checksum
};
- UBXUnpackedFrame frame{0x06, 0x8a, // Message UBX-CFG-VALSET
- payload, payloadLength};
-
- return writeUBXFrame(frame);
+ return writeUBXMessage(ubx_cfg_valset, ENABLE_UBX_MESSAGES_MSG_LEN);
}
-bool UbloxGPS::setSampleRate()
+bool UbloxGPS::setRate()
{
- static constexpr uint16_t payloadLength = 10;
+ uint16_t rate = 1000 / sampleRate;
+ LOG_DEBUG(logger, "Rate: {}", rate);
- uint8_t payload[payloadLength] = {
+ 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 (placeholder)
+ 0xff, 0xff, // CFG-RATE-MEAS value
+ 0xff, 0xff // Checksum
};
- memcpy(&payload[8], &samplerate, 2);
- UBXUnpackedFrame frame{0x06, 0x8a, // Message UBX-CFG-VALSET
- payload, payloadLength};
+ // Prepare rate
+ ubx_cfg_valset[14] = rate;
+ ubx_cfg_valset[15] = rate >> 8;
- return writeUBXFrame(frame);
+ return writeUBXMessage(ubx_cfg_valset, SET_RATE_MSG_LEN);
}
-bool UbloxGPS::parseUBXFrame(const UBXUnpackedFrame& frame)
+bool UbloxGPS::readUBXMessage(uint8_t* message, uint16_t& payloadLength)
{
- switch (frame.cls) // Message class
+ // 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 parseUBXNAVFrame(frame);
+ return parseUBXNAVMessage(message);
case 0x05: // UBX-ACK
- return parseUBXACKFrame(frame);
+ return parseUBXACKMessage(message);
}
return false;
}
-bool UbloxGPS::parseUBXNAVFrame(const UBXUnpackedFrame& frame)
+bool UbloxGPS::parseUBXNAVMessage(uint8_t* message)
{
- switch (frame.id) // Message ID
+ switch (message[3]) // Message id
{
case 0x07: // UBX-NAV-PVT
- // Lock the lastSample variable
- Lock<FastMutex> l(sampleMutex);
+ // Lock the threadSample variable
+ Lock<FastMutex> l(mutex);
// Latitude
- int32_t rawLatitude = frame.payload[28] | frame.payload[29] << 8 |
- frame.payload[30] << 16 |
- frame.payload[31] << 24;
- lastSample.latitude = (float)rawLatitude / 1e7;
+ 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 = frame.payload[24] | frame.payload[25] << 8 |
- frame.payload[26] << 16 |
- frame.payload[27] << 24;
- lastSample.longitude = (float)rawLongitude / 1e7;
+ 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 = frame.payload[32] | frame.payload[33] << 8 |
- frame.payload[34] << 16 |
- frame.payload[35] << 24;
- lastSample.height = (float)rawHeight / 1e3;
+ 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 =
- frame.payload[48] | frame.payload[49] << 8 |
- frame.payload[50] << 16 | frame.payload[51] << 24;
- lastSample.velocityNorth = (float)rawVelocityNorth / 1e3;
+ 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 =
- frame.payload[52] | frame.payload[53] << 8 |
- frame.payload[54] << 16 | frame.payload[55] << 24;
- lastSample.velocityEast = (float)rawVelocityEast / 1e3;
+ 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 =
- frame.payload[56] | frame.payload[57] << 8 |
- frame.payload[58] << 16 | frame.payload[59] << 24;
- lastSample.velocityDown = (float)rawVelocityDown / 1e3;
+ 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 = frame.payload[60] | frame.payload[61] << 8 |
- frame.payload[62] << 16 |
- frame.payload[63] << 24;
- lastSample.speed = (float)rawSpeed / 1e3;
+ 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 = frame.payload[64] | frame.payload[65] << 8 |
- frame.payload[66] << 16 |
- frame.payload[67] << 24;
- lastSample.track = (float)rawTrack / 1e5;
+ 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
- lastSample.satellites = (uint8_t)frame.payload[23];
+ threadSample.satellites = (uint8_t)message[6 + 23];
// Fix (every type of fix accepted)
- lastSample.fix = frame.payload[20] != 0;
+ threadSample.fix = message[6 + 20] != 0;
// Timestamp
- lastSample.gpsTimestamp =
+ threadSample.gpsTimestamp =
TimestampTimer::getInstance().getTimestamp();
return true;
@@ -299,209 +516,22 @@ bool UbloxGPS::parseUBXNAVFrame(const UBXUnpackedFrame& frame)
return false;
}
-bool UbloxGPS::parseUBXACKFrame(const UBXUnpackedFrame& frame)
+bool UbloxGPS::parseUBXACKMessage(uint8_t* message)
{
- switch (frame.id) // Message ID
+ switch (message[3]) // Message id
{
case 0x00: // UBX-ACK-NAC
LOG_DEBUG(logger,
- "Received NAC for message (class: {:#02x}, id: {:#02x})",
- frame.cls, frame.id);
+ "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: {:#02x}, id: {:#02x})",
- frame.cls, frame.id);
+ "Received ACK for message (class:0x{02x}, id: 0x{02x})",
+ message[6], message[7]);
return true;
}
return false;
}
-bool UbloxGPS::writeUBXFrame(const UBXUnpackedFrame& frame)
-{
- if (!frame.isValid())
- {
- LOG_ERR(logger, "UBX frame to write is invalid");
- return false;
- }
-
- uint8_t packedFrame[UBX_MAX_FRAME_LENGTH];
- frame.writePacked(packedFrame);
-
- writeRaw(packedFrame, frame.getFrameLength());
-
- return true;
-}
-
-bool UbloxGPS::readUBXFrame(UBXUnpackedFrame& frame)
-{
- bool synchronized = false;
- while (!synchronized)
- {
- synchronized = true;
- for (uint16_t i = 0; synchronized && i < 2; ++i)
- {
- if (!readRaw(&frame.preamble[i], 1))
- return false;
-
- if (frame.preamble[i] != UBX_VALID_PREAMBLE[i])
- synchronized = false;
- }
- }
-
- if (!readRaw(&frame.cls, 1) || !readRaw(&frame.id, 1) ||
- !readRaw((uint8_t*)&frame.payloadLength, 2) ||
- !readRaw(frame.payload, frame.payloadLength) ||
- !readRaw(frame.checksum, 2))
- return false;
-
- if (!frame.isValid())
- {
- LOG_ERR(logger, "UBX frame to read is invalid");
- return false;
- }
-
- return true;
-}
-
-UbloxGPSSPI::UbloxGPSSPI(SPIBusInterface& spiBus, GpioPin spiCs,
- SPIBusConfig spiConfig, uint8_t samplerate)
- : UbloxGPS(samplerate), spiSlave(spiBus, spiCs, spiConfig)
-{
-}
-
-SPIBusConfig UbloxGPSSPI::getDefaultSPIConfig()
-{
- SPIBusConfig spiConfig{};
- spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
- spiConfig.mode = SPI::Mode::MODE_1;
- return spiConfig;
-}
-
-bool UbloxGPSSPI::writeRaw(uint8_t* data, size_t size)
-{
- SPITransaction spi{spiSlave};
- spi.write(data, size);
- return true;
-}
-
-bool UbloxGPSSPI::readRaw(uint8_t* data, size_t size)
-{
- SPITransaction spi{spiSlave};
- spi.read(data, size);
- return true;
-}
-
-UbloxGPSSerial::UbloxGPSSerial(int serialBaudrate, uint8_t samplerate,
- int serialDefaultBaudrate, int serialPortNumber,
- const char* serialPortName)
- : UbloxGPS(samplerate), serialPortNumber(serialPortNumber),
- serialPortName(serialPortName), serialBaudrate(serialBaudrate),
- serialDefaultBaudrate(serialDefaultBaudrate)
-{
- strcpy(serialFilePath, "/dev/");
- strcat(serialFilePath, serialPortName);
-}
-
-bool UbloxGPSSerial::setupCommunication()
-{
- intrusive_ref_ptr<DevFs> devFs = FilesystemManager::instance().getDevFs();
-
- // Close the serial file if already opened
- devFs->remove(serialPortName);
-
- // Change the baudrate only if it is different than the default
- if (serialBaudrate != serialDefaultBaudrate)
- {
- // Open the serial port device with the default baudrate
- if (!devFs->addDevice(serialPortName,
- intrusive_ref_ptr<Device>(new STM32Serial(
- serialPortNumber, serialDefaultBaudrate))))
- {
- LOG_ERR(logger,
- "[gps] Faild to open serial port {} with baudrate {} as "
- "file {}",
- serialPortNumber, serialDefaultBaudrate, serialPortName);
- return false;
- }
-
- // Open the serial file
- if ((serialFile = open(serialFilePath, O_RDWR)) < 0)
- {
- LOG_ERR(logger, "Failed to open serial file {}", serialFilePath);
- return false;
- }
-
- // Change baudrate
- if (!setBaudrate())
- {
- return false;
- }
-
- // Close the serial file
- if (close(serialFile) < 0)
- {
- LOG_ERR(logger, "Failed to close serial file {}", serialFilePath);
- 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, serialBaudrate))))
- {
- LOG_ERR(logger,
- "Faild to open serial port {} with baudrate {} as file {}\n",
- serialPortNumber, serialDefaultBaudrate, serialPortName);
- return false;
- }
-
- // Reopen the serial file
- if ((serialFile = open(serialFilePath, O_RDWR)) < 0)
- {
- LOG_ERR(logger, "Failed to open serial file {}", serialFilePath);
- return false;
- }
-
- return true;
-}
-
-bool UbloxGPSSerial::setBaudrate()
-{
- static constexpr uint16_t payloadLength = 12;
-
- uint8_t payload[payloadLength] = {
- 0x00, // Version
- 0xff, // All layers
- 0x00, 0x00, // Reserved
- 0x01, 0x00, 0x52, 0x40, // Configuration item key ID
- 0xff, 0xff, 0xff, 0xff // Value (placeholder)
- };
- memcpy(&payload[8], &serialBaudrate, 4);
-
- UBXUnpackedFrame frame{0x06, 0x8a, // Message UBX-CFG-VALSET
- payload, payloadLength};
-
- return writeUBXFrame(frame);
-}
-
-bool UbloxGPSSerial::writeRaw(uint8_t* data, size_t size)
-{
- return write(serialFile, data, size) >= 0;
-}
-
-bool UbloxGPSSerial::readRaw(uint8_t* data, size_t size)
-{
- return read(serialFile, data, size) >= 0;
-}
-
} // namespace Boardcore
diff --git a/src/shared/sensors/UbloxGPS/UbloxGPS.h b/src/shared/sensors/UbloxGPS/UbloxGPS.h
index 6d980d6420cf64441b46df319d8a904bdd930002..3f7a1616ef1c0c732527b91ed209964d9af3c893 100644
--- a/src/shared/sensors/UbloxGPS/UbloxGPS.h
+++ b/src/shared/sensors/UbloxGPS/UbloxGPS.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio, Damiano Amatruda
+ * 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
@@ -24,57 +24,87 @@
#include <ActiveObject.h>
#include <diagnostic/PrintLogger.h>
-#include <drivers/spi/SPIDriver.h>
+#include <miosix.h>
#include <sensors/Sensor.h>
-#include "UBXUnpackedFrame.h"
#include "UbloxGPSData.h"
namespace Boardcore
{
/**
- * @brief Sensor for Ublox GPS.
+ * @brief Driver for Ublox GPSs
*
- * This sensor handles communication and setup with Ublox GPSs. It uses the
+ * 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 sample rate,
- * resets the configuration and sets up UBX messages and GNSS parameters.
+ * 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 SPI.
+ * 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:
- explicit UbloxGPS(uint8_t samplerate) : samplerate(samplerate) {}
+ /**
+ * @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 Disables the NMEA messages, configures GNSS options and enables
- * UBX-PVT message
+ * @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
*/
- virtual bool init() override;
+ bool init() override;
/**
- * @brief Reads a single message form the GPS, waits 2 sample cycles.
+ * @brief Read a single message form the GPS, waits 2 sample cycle
*
* @return True if a message was sampled
*/
bool selfTest() override;
-protected:
+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();
- virtual bool setupCommunication() { return true; }
+ bool setBaudrate();
bool disableNMEAMessages();
@@ -82,101 +112,40 @@ protected:
bool enableUBXMessages();
- bool setSampleRate();
-
- bool parseUBXFrame(const UBXUnpackedFrame& frame);
-
- bool parseUBXNAVFrame(const UBXUnpackedFrame& frame);
-
- bool parseUBXACKFrame(const UBXUnpackedFrame& frame);
-
- bool writeUBXFrame(const UBXUnpackedFrame& frame);
-
- bool readUBXFrame(UBXUnpackedFrame& frame);
-
- virtual bool writeRaw(uint8_t* data, size_t size) = 0;
-
- virtual bool readRaw(uint8_t* data, size_t size) = 0;
-
- const uint8_t samplerate; // [Hz]
-
- mutable miosix::FastMutex sampleMutex;
- UbloxGPSData lastSample{};
-
- PrintLogger logger = Logging::getLogger("ubloxgps");
-};
+ bool setRate();
-class UbloxGPSSPI : public UbloxGPS
-{
-public:
- /**
- * @brief Constructor.
- *
- * @param bus The Spi bus.
- * @param cs The CS pin to lower when we need to sample.
- * @param config The SPI configuration.
- * @param samplerate Sample rate to communicate with the device
- */
- UbloxGPSSPI(SPIBusInterface& spiBus, miosix::GpioPin spiCs,
- SPIBusConfig spiConfig = getDefaultSPIConfig(),
- uint8_t samplerate = 250);
+ bool readUBXMessage(uint8_t *message, uint16_t &payloadLength);
- /**
- * Constructs the default config for SPI Bus.
- *
- * @returns The default SPIBusConfig.
- */
- static SPIBusConfig getDefaultSPIConfig();
+ bool parseUBXMessage(uint8_t *message);
-private:
- virtual bool writeRaw(uint8_t* data, size_t size) override;
+ bool parseUBXNAVMessage(uint8_t *message);
- virtual bool readRaw(uint8_t* data, size_t size) override;
+ bool parseUBXACKMessage(uint8_t *message);
- SPISlave spiSlave;
-};
+ const int baudrate; // [baud]
+ const uint8_t sampleRate; // [Hz]
+ const int serialPortNumber;
+ const char *serialPortName;
+ const int defaultBaudrate; // [baud]
-class UbloxGPSSerial : public UbloxGPS
-{
-public:
- /**
- * @brief Serial constructor.
- *
- * @param serialPortNumber Number of the serial port
- * @param serialPortName Name of the file for the gps device
- * @param serialBaudrate Baudrate to communicate with the device (max:
- * 921600, min: 4800 for NEO-M9N)
- * @param samplerate GPS sample rate (max: 25 for NEO-M9N)
- * @param serialDefaultBaudrate Startup baudrate (38400 for NEO-M9N)
- */
- UbloxGPSSerial(int serialBaudrate = 921600, uint8_t samplerate = 10,
- int serialDefaultBaudrate = 38400, int serialPortNumber = 2,
- const char* serialPortName = "gps");
+ char gpsFilePath[16]; ///< Allows for a filename of up to 10 characters
+ int gpsFile;
-private:
- /**
- * @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.
- */
- virtual bool setupCommunication() override;
+ mutable miosix::FastMutex mutex;
+ UbloxGPSData threadSample{};
- bool setBaudrate();
+ 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;
- virtual bool writeRaw(uint8_t* data, size_t size) override;
+ static constexpr uint8_t PREAMBLE[] = {0xb5, 0x62};
- virtual bool readRaw(uint8_t* data, size_t size) override;
+ static constexpr int UBX_MAX_PAYLOAD_LENGTH = 92;
- const int serialPortNumber;
- const char* serialPortName;
- const int serialBaudrate; // [baud]
- const int serialDefaultBaudrate; // [baud]
- char serialFilePath[16]; // Allows for a filename of up to 10 characters
- int serialFile = -1;
+ PrintLogger logger = Logging::getLogger("ubloxgps");
};
} // namespace Boardcore
diff --git a/src/shared/sensors/UbloxGPS/UbloxGPSData.h b/src/shared/sensors/UbloxGPS/UbloxGPSData.h
index f61d4fc00ab8f682c1d4afd18b3cb4d6c1ac1e0a..7f5570000b78b915a956b96e754d784e08f7611f 100644
--- a/src/shared/sensors/UbloxGPS/UbloxGPSData.h
+++ b/src/shared/sensors/UbloxGPS/UbloxGPSData.h
@@ -31,8 +31,8 @@ struct UbloxGPSData : public GPSData
{
static std::string header()
{
- return "gpsTimestamp,latitude,longitude,height,velocityNorth,"
- "velocityEast,velocityDown,speed,track,satellites,fix\n";
+ return "gps_timestamp,latitude,longitude,height,velocity_north,"
+ "velocity_east,velocity_down,speed,track,num_satellites,fix\n";
}
void print(std::ostream &os) const
diff --git a/src/tests/sensors/test-ubloxgps.cpp b/src/tests/sensors/test-ubloxgps.cpp
index 1bf036893f5cd3463252d1b6d4a06a3da582b75f..0c611daf4e7ffc2ccabcc56da70d989496e1d45f 100644
--- a/src/tests/sensors/test-ubloxgps.cpp
+++ b/src/tests/sensors/test-ubloxgps.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio, Damiano Amatruda
+ * 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
@@ -21,87 +21,69 @@
*/
#include <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
#include <sensors/UbloxGPS/UbloxGPS.h>
#include <utils/Debug.h>
-using namespace miosix;
+#include <cstdio>
+
using namespace Boardcore;
+using namespace miosix;
+
+#define RATE 4
int main()
{
- static constexpr uint8_t SAMPLE_RATE = 4;
-
- PrintLogger logger = Logging::getLogger("test-ubloxgps");
-
-#if defined(USE_SPI)
- SPIBus bus(SPI1);
- GpioPin spiSck(GPIOA_BASE, 5);
- GpioPin spiMiso(GPIOA_BASE, 6);
- GpioPin spiMosi(GPIOA_BASE, 7);
- GpioPin cs(GPIOA_BASE, 3);
-
- spiSck.mode(Mode::ALTERNATE);
- spiSck.alternateFunction(5);
- spiMiso.mode(Mode::ALTERNATE);
- spiMiso.alternateFunction(5);
- spiMosi.mode(miosix::Mode::ALTERNATE);
- spiMosi.alternateFunction(5);
- cs.mode(Mode::OUTPUT);
- cs.high();
-
- UbloxGPSSPI sensor{bus, cs, UbloxGPSSPI::getDefaultSPIConfig(),
- SAMPLE_RATE};
-#elif defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_X)
- // Keep GPS baud SAMPLE_RATE at default for easier testing
- UbloxGPSSerial sensor{2, "gps", 921600, 38400, SAMPLE_RATE};
-#else
- GpioPin tx(GPIOB_BASE, 6);
- GpioPin rx(GPIOB_BASE, 7);
+ (void)TimestampTimer::getInstance();
- tx.mode(miosix::Mode::ALTERNATE);
- rx.mode(miosix::Mode::ALTERNATE);
+ printf("Welcome to the ublox test\n");
- tx.alternateFunction(7);
- rx.alternateFunction(7);
+ // Keep GPS baud rate at default for easier testing
+ UbloxGPS gps(921600, RATE, 2, "gps", 38400);
+ UbloxGPSData dataGPS;
+ printf("Gps allocated\n");
- UbloxGPSSerial sensor{91600, SAMPLE_RATE, 38400, 1, "gps"};
-#endif
-
- LOG_INFO(logger, "Initializing sensor...\n");
-
- if (!sensor.init())
+ // Init the gps
+ if (gps.init())
{
- LOG_ERR(logger, "Initialization failed!\n");
- return -1;
+ printf("Successful gps initialization\n");
+ }
+ else
+ {
+ printf("Failed gps initialization\n");
}
- LOG_INFO(logger, "Performing self-test...\n");
-
- if (!sensor.selfTest())
+ // Perform the selftest
+ if (gps.selfTest())
+ {
+ printf("Successful gps selftest\n");
+ }
+ else
{
- LOG_ERR(logger, "Self-test failed! (code: %d)\n",
- sensor.getLastError());
- return -1;
+ printf("Failed gps selftest\n");
}
- // Start the sensor thread
- LOG_INFO(logger, "Starting sensor...\n");
- sensor.start();
+ // Start the gps thread
+ gps.start();
+ printf("Gps started\n");
while (true)
{
- long long lastTick = miosix::getTick();
+ // Give time to the thread
+ Thread::sleep(1000 / RATE);
- sensor.sample();
- GPSData sample __attribute__((unused)) = sensor.getLastSample();
+ // Sample
+ gps.sample();
+ dataGPS = gps.getLastSample();
+ // Print out the latest sample
TRACE(
- "timestamp: %4.3f, fix: %01d, lat: %f, lon: %f, height: %4.1f, "
- "nsat: %2d, speed: %3.2f, velN: %3.2f, velE: %3.2f, track %3.1f\n",
- (float)sample.gpsTimestamp / 1000000, sample.fix, sample.latitude,
- sample.longitude, sample.height, sample.satellites, sample.speed,
- sample.velocityNorth, sample.velocityEast, sample.track);
-
- Thread::sleepUntil(lastTick + 1000 / SAMPLE_RATE); // Sample period
+ "[gps] timestamp: % 4.3f, fix: %01d lat: % f lon: % f "
+ "height: %4.1f nsat: %2d speed: %3.2f velN: % 3.2f velE: % 3.2f "
+ "track %3.1f\n",
+ (float)dataGPS.gpsTimestamp / 1000000, dataGPS.fix,
+ dataGPS.latitude, dataGPS.longitude, dataGPS.height,
+ dataGPS.satellites, dataGPS.speed, dataGPS.velocityNorth,
+ dataGPS.velocityEast, dataGPS.track);
}
}