483 lines
14 KiB
C++
483 lines
14 KiB
C++
/*!
|
|
* @file Adafruit_UBX.cpp
|
|
*
|
|
* @mainpage Arduino library for UBX protocol from u-blox GPS/RTK modules
|
|
*
|
|
* @section intro_sec Introduction
|
|
*
|
|
* This is a library for parsing UBX protocol messages from u-blox GPS/RTK
|
|
* modules. It works with any Stream-based interface including UART and DDC
|
|
* (I2C).
|
|
*
|
|
* Designed specifically to work with u-blox GPS/RTK modules
|
|
* like NEO-M8P, ZED-F9P, etc.
|
|
*
|
|
* @section author Author
|
|
*
|
|
* Written by Limor Fried/Ladyada for Adafruit Industries.
|
|
*
|
|
* @section license License
|
|
*
|
|
* MIT license, all text above must be included in any redistribution
|
|
*/
|
|
|
|
#include "Adafruit_UBX.h"
|
|
|
|
/*!
|
|
* @brief Constructor
|
|
* @param stream Reference to a Stream object (Serial, Adafruit_UBloxDDC,
|
|
* etc.)
|
|
*/
|
|
Adafruit_UBX::Adafruit_UBX(Stream &stream) {
|
|
_stream = &stream;
|
|
onUBXMessage = NULL;
|
|
}
|
|
|
|
/*!
|
|
* @brief Destructor
|
|
*/
|
|
Adafruit_UBX::~Adafruit_UBX() {
|
|
if (_stream)
|
|
delete _stream;
|
|
if (onUBXMessage)
|
|
onUBXMessage = NULL;
|
|
}
|
|
|
|
/*!
|
|
* @brief Initializes the UBX parser
|
|
* @return Always returns true (initialization is trivial)
|
|
*/
|
|
bool Adafruit_UBX::begin() {
|
|
resetParser();
|
|
return true;
|
|
}
|
|
|
|
/*!
|
|
* @brief Configure the GPS module to output only UBX protocol (disables NMEA)
|
|
* @param portID Port identifier (UBX_PORT_DDC, UBX_PORT_UART1, etc.)
|
|
* @param checkAck Whether to wait for acknowledgment
|
|
* @param timeout_ms Maximum time to wait for acknowledgment in milliseconds
|
|
* @return UBXSendStatus indicating success, failure, or timeout
|
|
*/
|
|
UBXSendStatus Adafruit_UBX::setUBXOnly(UBXPortId portID, bool checkAck,
|
|
uint16_t timeout_ms) {
|
|
UBX_CFG_PRT_t cfgPrt;
|
|
|
|
// Zero out the structure
|
|
memset(&cfgPrt, 0, sizeof(cfgPrt));
|
|
|
|
// Set the port ID
|
|
cfgPrt.fields.portID = portID;
|
|
|
|
// Configure the port appropriately
|
|
switch (portID) {
|
|
case UBX_PORT_DDC: // I2C/DDC port
|
|
// Set the I2C address to 0x42 (the default)
|
|
// For DDC, the mode field contains the I2C address in bits 7:1
|
|
cfgPrt.fields.mode = 0x42 << 1; // 0x84
|
|
|
|
// Set protocol masks to UBX only
|
|
cfgPrt.fields.inProtoMask = UBX_PROTOCOL_UBX;
|
|
cfgPrt.fields.outProtoMask = UBX_PROTOCOL_UBX;
|
|
break;
|
|
|
|
case UBX_PORT_UART1: // Fall through
|
|
case UBX_PORT_UART2: // UART ports
|
|
// Keep current baud rate (baudRate = 0 keeps current setting)
|
|
// Set 8N1 mode for binary protocol
|
|
cfgPrt.fields.mode = UBX_UART_MODE_8N1;
|
|
// Set protocol masks to UBX only
|
|
cfgPrt.fields.inProtoMask = UBX_PROTOCOL_UBX;
|
|
cfgPrt.fields.outProtoMask = UBX_PROTOCOL_UBX;
|
|
break;
|
|
|
|
case UBX_PORT_USB: // USB port
|
|
// Set protocol masks to UBX only
|
|
cfgPrt.fields.inProtoMask = UBX_PROTOCOL_UBX;
|
|
cfgPrt.fields.outProtoMask = UBX_PROTOCOL_UBX;
|
|
break;
|
|
|
|
case UBX_PORT_SPI: // SPI port
|
|
// Set protocol masks to UBX only
|
|
cfgPrt.fields.outProtoMask = UBX_PROTOCOL_UBX;
|
|
break;
|
|
|
|
default:
|
|
if (verbose_debug > 0) {
|
|
Serial.println(F("UBX: Invalid port ID"));
|
|
}
|
|
return UBX_SEND_FAIL; // Invalid port ID
|
|
}
|
|
|
|
// Send the message and wait for acknowledgment if requested
|
|
if (checkAck) {
|
|
return sendMessageWithAck(UBX_CLASS_CFG, UBX_CFG_PRT, cfgPrt.raw,
|
|
sizeof(cfgPrt), timeout_ms);
|
|
} else {
|
|
if (sendMessage(UBX_CLASS_CFG, UBX_CFG_PRT, cfgPrt.raw, sizeof(cfgPrt))) {
|
|
return UBX_SEND_SUCCESS;
|
|
} else {
|
|
return UBX_SEND_FAIL;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*!
|
|
* @brief Sets the callback function for UBX messages
|
|
* @param callback Function pointer to call when a complete UBX message is
|
|
* received
|
|
*/
|
|
void Adafruit_UBX::setMessageCallback(UBXMessageCallback callback) {
|
|
onUBXMessage = callback;
|
|
}
|
|
|
|
/*!
|
|
* @brief Reset the parser state machine
|
|
*/
|
|
void Adafruit_UBX::resetParser() {
|
|
_parserState = WAIT_SYNC_1;
|
|
_payloadCounter = 0;
|
|
_payloadLength = 0;
|
|
}
|
|
|
|
/*!
|
|
* @brief Calculate UBX checksum according to protocol
|
|
* @param buffer Pointer to the data buffer
|
|
* @param len Length of data to checksum
|
|
* @param checksumA Reference to store the first checksum byte
|
|
* @param checksumB Reference to store the second checksum byte
|
|
*/
|
|
void Adafruit_UBX::calculateChecksum(uint8_t *buffer, uint16_t len,
|
|
uint8_t &checksumA, uint8_t &checksumB) {
|
|
checksumA = 0;
|
|
checksumB = 0;
|
|
|
|
for (uint16_t i = 0; i < len; i++) {
|
|
checksumA += buffer[i];
|
|
checksumB += checksumA;
|
|
}
|
|
}
|
|
|
|
/*!
|
|
* @brief Check for new UBX messages and parse them
|
|
* @return True if a complete message was parsed
|
|
*/
|
|
bool Adafruit_UBX::checkMessages() {
|
|
bool messageReceived = false;
|
|
|
|
// Process all available bytes
|
|
while (_stream->available()) {
|
|
uint8_t incomingByte = _stream->read();
|
|
|
|
// State machine for UBX protocol parsing
|
|
switch (_parserState) {
|
|
case WAIT_SYNC_1:
|
|
if (incomingByte == UBX_SYNC_CHAR_1) {
|
|
_parserState = WAIT_SYNC_2;
|
|
_buffer[0] = incomingByte; // Store for checksum calculation
|
|
}
|
|
break;
|
|
|
|
case WAIT_SYNC_2:
|
|
if (incomingByte == UBX_SYNC_CHAR_2) {
|
|
_parserState = GET_CLASS;
|
|
_buffer[1] = incomingByte; // Store for checksum calculation
|
|
} else {
|
|
resetParser(); // Invalid sync char, reset
|
|
}
|
|
break;
|
|
|
|
case GET_CLASS:
|
|
_msgClass = incomingByte;
|
|
_buffer[2] = incomingByte; // Store for checksum calculation
|
|
_parserState = GET_ID;
|
|
break;
|
|
|
|
case GET_ID:
|
|
_msgId = incomingByte;
|
|
_buffer[3] = incomingByte; // Store for checksum calculation
|
|
_parserState = GET_LENGTH_1;
|
|
break;
|
|
|
|
case GET_LENGTH_1:
|
|
_payloadLength = incomingByte;
|
|
_buffer[4] = incomingByte; // Store for checksum calculation
|
|
_parserState = GET_LENGTH_2;
|
|
break;
|
|
|
|
case GET_LENGTH_2:
|
|
_payloadLength |= (incomingByte << 8);
|
|
_buffer[5] = incomingByte; // Store for checksum calculation
|
|
|
|
if (_payloadLength > MAX_PAYLOAD_SIZE) {
|
|
resetParser(); // Payload too large, reset
|
|
} else {
|
|
_payloadCounter = 0;
|
|
_parserState = GET_PAYLOAD;
|
|
}
|
|
break;
|
|
|
|
case GET_PAYLOAD:
|
|
if (_payloadCounter < _payloadLength) {
|
|
_buffer[6 + _payloadCounter] = incomingByte;
|
|
_payloadCounter++;
|
|
|
|
if (_payloadCounter == _payloadLength) {
|
|
_parserState = GET_CHECKSUM_A;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case GET_CHECKSUM_A:
|
|
// Calculate expected checksum
|
|
calculateChecksum(_buffer + 2, _payloadLength + 4, _checksumA,
|
|
_checksumB);
|
|
|
|
if (incomingByte == _checksumA) {
|
|
_parserState = GET_CHECKSUM_B; // Checksum A matches
|
|
} else {
|
|
resetParser(); // Invalid checksum, reset
|
|
}
|
|
break;
|
|
|
|
case GET_CHECKSUM_B:
|
|
if (incomingByte == _checksumB) {
|
|
// We have a valid message!
|
|
if (onUBXMessage != NULL) {
|
|
onUBXMessage(_msgClass, _msgId, _payloadLength,
|
|
_buffer + 6); // Call the callback with the message
|
|
}
|
|
messageReceived = true;
|
|
|
|
_lastMsgClass = _msgClass;
|
|
_lastMsgId = _msgId;
|
|
_lastPayloadLength = _payloadLength;
|
|
|
|
// Store a small copy of the payload if it's within size limits
|
|
if (_payloadLength <= sizeof(_lastPayload)) {
|
|
memcpy(_lastPayload, _buffer + 6, _payloadLength);
|
|
}
|
|
|
|
if (verbose_debug > 0) {
|
|
Serial.print("UBX RX: ");
|
|
|
|
// Print header (sync chars, class, id, length)
|
|
Serial.print("HDR[B5 62 ");
|
|
if (_msgClass < 0x10)
|
|
Serial.print("0");
|
|
Serial.print(_msgClass, HEX);
|
|
Serial.print(" ");
|
|
if (_msgId < 0x10)
|
|
Serial.print("0");
|
|
Serial.print(_msgId, HEX);
|
|
Serial.print(" ");
|
|
|
|
uint8_t lenLSB = _payloadLength & 0xFF;
|
|
uint8_t lenMSB = (_payloadLength >> 8) & 0xFF;
|
|
if (lenLSB < 0x10)
|
|
Serial.print("0");
|
|
Serial.print(lenLSB, HEX);
|
|
Serial.print(" ");
|
|
if (lenMSB < 0x10)
|
|
Serial.print("0");
|
|
Serial.print(lenMSB, HEX);
|
|
Serial.print("] ");
|
|
|
|
// Print payload if verbose debug is enabled
|
|
if (verbose_debug > 1 && _payloadLength > 0) {
|
|
Serial.print("PL[");
|
|
for (uint16_t i = 0; i < _payloadLength; i++) {
|
|
if (_buffer[6 + i] < 0x10)
|
|
Serial.print("0");
|
|
Serial.print(_buffer[6 + i], HEX);
|
|
Serial.print(" ");
|
|
}
|
|
Serial.print("] ");
|
|
}
|
|
|
|
// Print checksum
|
|
Serial.print("CS[");
|
|
if (_checksumA < 0x10)
|
|
Serial.print("0");
|
|
Serial.print(_checksumA, HEX);
|
|
Serial.print(" ");
|
|
if (_checksumB < 0x10)
|
|
Serial.print("0");
|
|
Serial.print(_checksumB, HEX);
|
|
Serial.println("]");
|
|
}
|
|
}
|
|
|
|
resetParser(); // Reset for next message
|
|
break;
|
|
}
|
|
}
|
|
|
|
return messageReceived;
|
|
}
|
|
|
|
/*!
|
|
* @brief Send a UBX message and wait for acknowledgment
|
|
* @param msgClass Message class
|
|
* @param msgId Message ID
|
|
* @param payload Pointer to the payload data
|
|
* @param length Length of payload
|
|
* @param timeout_ms Maximum time to wait for acknowledgment
|
|
* @return UBXSendStatus indicating success, failure, or timeout
|
|
*/
|
|
UBXSendStatus Adafruit_UBX::sendMessageWithAck(uint8_t msgClass, uint8_t msgId,
|
|
uint8_t *payload,
|
|
uint16_t length,
|
|
uint16_t timeout_ms) {
|
|
// First send the message
|
|
if (!sendMessage(msgClass, msgId, payload, length)) {
|
|
if (verbose_debug > 0) {
|
|
Serial.println(F("UBX ACK: SEND FAIL"));
|
|
}
|
|
return UBX_SEND_FAIL;
|
|
}
|
|
|
|
uint32_t startTime = millis();
|
|
|
|
// Check for messages until timeout
|
|
while ((millis() - startTime) < timeout_ms) {
|
|
// Process incoming bytes
|
|
if (checkMessages()) {
|
|
// If we have a message handler, it will be called from checkMessages
|
|
// We need to check our last received message
|
|
if (_lastMsgClass == UBX_CLASS_ACK) {
|
|
if (_lastMsgId == UBX_ACK_ACK && _lastPayloadLength >= 2) {
|
|
// ACK-ACK message
|
|
if (_lastPayload[0] == msgClass && _lastPayload[1] == msgId) {
|
|
if (verbose_debug > 0) {
|
|
Serial.print(F("UBX ACK: SUCCESS for message class 0x"));
|
|
if (msgClass < 0x10)
|
|
Serial.print("0");
|
|
Serial.print(msgClass, HEX);
|
|
Serial.print(" ID 0x");
|
|
if (msgId < 0x10)
|
|
Serial.print("0");
|
|
Serial.println(msgId, HEX);
|
|
}
|
|
return UBX_SEND_SUCCESS;
|
|
}
|
|
} else if (_lastMsgId == UBX_ACK_NAK && _lastPayloadLength >= 2) {
|
|
// ACK-NAK message
|
|
if (_lastPayload[0] == msgClass && _lastPayload[1] == msgId) {
|
|
if (verbose_debug > 0) {
|
|
Serial.print(F("UBX ACK: NAK for message class 0x"));
|
|
if (msgClass < 0x10)
|
|
Serial.print("0");
|
|
Serial.print(msgClass, HEX);
|
|
Serial.print(" ID 0x");
|
|
if (msgId < 0x10)
|
|
Serial.print("0");
|
|
Serial.println(msgId, HEX);
|
|
}
|
|
return UBX_SEND_NAK;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Short delay
|
|
delay(1);
|
|
}
|
|
|
|
if (verbose_debug > 0) {
|
|
Serial.print(
|
|
F("UBX ACK: TIMEOUT waiting for ACK/NAK for message class 0x"));
|
|
if (msgClass < 0x10)
|
|
Serial.print("0");
|
|
Serial.print(msgClass, HEX);
|
|
Serial.print(" ID 0x");
|
|
if (msgId < 0x10)
|
|
Serial.print("0");
|
|
Serial.println(msgId, HEX);
|
|
}
|
|
|
|
return UBX_SEND_TIMEOUT;
|
|
}
|
|
|
|
/*!
|
|
* @brief Send a UBX message to the GPS module
|
|
* @param msgClass Message class
|
|
* @param msgId Message ID
|
|
* @param payload Pointer to the payload data (can be NULL for zero-length
|
|
* payload)
|
|
* @param length Length of payload
|
|
* @return True if message was sent successfully
|
|
*/
|
|
bool Adafruit_UBX::sendMessage(uint8_t msgClass, uint8_t msgId,
|
|
uint8_t *payload, uint16_t length) {
|
|
// Buffer for message (2 sync chars + class + id + 2 length bytes + payload +
|
|
// 2 checksum bytes)
|
|
uint8_t msgBuffer[length + 8];
|
|
|
|
// Sync characters
|
|
msgBuffer[0] = UBX_SYNC_CHAR_1;
|
|
msgBuffer[1] = UBX_SYNC_CHAR_2;
|
|
|
|
// Message class and ID
|
|
msgBuffer[2] = msgClass;
|
|
msgBuffer[3] = msgId;
|
|
|
|
// Length (little endian)
|
|
msgBuffer[4] = length & 0xFF;
|
|
msgBuffer[5] = (length >> 8) & 0xFF;
|
|
|
|
// Payload
|
|
if (payload != NULL && length > 0) {
|
|
memcpy(&msgBuffer[6], payload, length);
|
|
}
|
|
|
|
// Calculate checksum
|
|
uint8_t checksumA, checksumB;
|
|
calculateChecksum(&msgBuffer[2], length + 4, checksumA, checksumB);
|
|
|
|
msgBuffer[6 + length] = checksumA;
|
|
msgBuffer[7 + length] = checksumB;
|
|
|
|
// Debug output
|
|
if (verbose_debug > 0) {
|
|
Serial.print("UBX TX: ");
|
|
|
|
// Print header (sync chars, class, id, length)
|
|
Serial.print("HDR[");
|
|
for (int i = 0; i < 6; i++) {
|
|
if (msgBuffer[i] < 0x10)
|
|
Serial.print("0");
|
|
Serial.print(msgBuffer[i], HEX);
|
|
Serial.print(" ");
|
|
}
|
|
Serial.print("] ");
|
|
|
|
// Print payload if verbose debug is enabled
|
|
if (verbose_debug > 1 && length > 0) {
|
|
Serial.print("PL[");
|
|
for (uint16_t i = 0; i < length; i++) {
|
|
if (msgBuffer[6 + i] < 0x10)
|
|
Serial.print("0");
|
|
Serial.print(msgBuffer[6 + i], HEX);
|
|
Serial.print(" ");
|
|
}
|
|
Serial.print("] ");
|
|
}
|
|
|
|
// Print checksum
|
|
Serial.print("CS[");
|
|
if (checksumA < 0x10)
|
|
Serial.print("0");
|
|
Serial.print(checksumA, HEX);
|
|
Serial.print(" ");
|
|
if (checksumB < 0x10)
|
|
Serial.print("0");
|
|
Serial.print(checksumB, HEX);
|
|
Serial.println("]");
|
|
}
|
|
|
|
// Send the message
|
|
size_t written = _stream->write(msgBuffer, length + 8);
|
|
|
|
return (written == length + 8);
|
|
}
|