From 32331662b1f0a9ecfddddf71ffd7ba37b9b507ee Mon Sep 17 00:00:00 2001 From: vid553 Date: Fri, 4 Sep 2020 00:03:32 +0200 Subject: [PATCH 01/36] ported driver to zephyr --- .gitignore | 3 + examples/Example_Zephyr/CMakeLists.txt | 12 + .../nrf52840dk_nrf52840.overlay | 4 + examples/Example_Zephyr/prj.conf | 7 + examples/Example_Zephyr/src/main.c | 95 + src/SparkFun_Ublox_Zephyr_Library.cpp | 3562 +++++++++++++++++ src/SparkFun_Ublox_Zephyr_Library.h | 899 +++++ src/ublox_lib_interface.cpp | 75 + src/ublox_lib_interface.h | 21 + 9 files changed, 4678 insertions(+) create mode 100644 examples/Example_Zephyr/CMakeLists.txt create mode 100644 examples/Example_Zephyr/nrf52840dk_nrf52840.overlay create mode 100644 examples/Example_Zephyr/prj.conf create mode 100644 examples/Example_Zephyr/src/main.c create mode 100644 src/SparkFun_Ublox_Zephyr_Library.cpp create mode 100644 src/SparkFun_Ublox_Zephyr_Library.h create mode 100644 src/ublox_lib_interface.cpp create mode 100644 src/ublox_lib_interface.h diff --git a/.gitignore b/.gitignore index 484d3c2..5b0c1cd 100644 --- a/.gitignore +++ b/.gitignore @@ -53,3 +53,6 @@ Temporary Items *~ [._]*.un~ *.swp + +# Zephyr build files +examples/Example_Zephyr/build/* \ No newline at end of file diff --git a/examples/Example_Zephyr/CMakeLists.txt b/examples/Example_Zephyr/CMakeLists.txt new file mode 100644 index 0000000..cd3ef07 --- /dev/null +++ b/examples/Example_Zephyr/CMakeLists.txt @@ -0,0 +1,12 @@ +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.13.1) + +find_package(Zephyr HINTS $ENV{ZEPHYR_BASE}) +project(sparkfun_ublox_zephyr_library) + +zephyr_include_directories(../../src/) +target_sources(app PRIVATE ../../src/SparkFun_Ublox_Zephyr_Library.cpp) +target_sources(app PRIVATE ../../src/ublox_lib_interface.cpp) + +target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src/main.c) diff --git a/examples/Example_Zephyr/nrf52840dk_nrf52840.overlay b/examples/Example_Zephyr/nrf52840dk_nrf52840.overlay new file mode 100644 index 0000000..3c95572 --- /dev/null +++ b/examples/Example_Zephyr/nrf52840dk_nrf52840.overlay @@ -0,0 +1,4 @@ +&i2c0 { + status = "okay"; + compatible = "nordic,nrf-twim"; +}; \ No newline at end of file diff --git a/examples/Example_Zephyr/prj.conf b/examples/Example_Zephyr/prj.conf new file mode 100644 index 0000000..e239329 --- /dev/null +++ b/examples/Example_Zephyr/prj.conf @@ -0,0 +1,7 @@ +#turn on c++ support +CONFIG_CPLUSPLUS=y + +# turn on peripherals +CONFIG_GPIO=y +CONFIG_I2C=y +CONFIG_I2C_0=y diff --git a/examples/Example_Zephyr/src/main.c b/examples/Example_Zephyr/src/main.c new file mode 100644 index 0000000..3d376c7 --- /dev/null +++ b/examples/Example_Zephyr/src/main.c @@ -0,0 +1,95 @@ +/* + Read NMEA sentences over I2C using Ublox module SAM-M8Q, NEO-M8P, ZED-F9P, etc + By: Nathan Seidle + SparkFun Electronics + Date: August 22nd, 2018 + License: MIT. See license file for more information but you can + basically do whatever you want with this code. + + This example reads the NMEA setences from the Ublox module over I2c and outputs + them to the serial port + + Open the serial monitor at 115200 baud to see the output + I2C clock speed: 100 kHz +*/ + +#include +#include +#include +#include + +#include "ublox_lib_interface.h" + + +#define I2C_DEV "I2C_0" + +struct device *gpio_dev; +struct device *i2c_dev; +/* I2C pins used are defaults for I2C_0 on nrf52840 + SDA: 26 + SCL: 27 +*/ + +uint8_t init_gpio(void) { + const char* const gpioName = "GPIO_0"; + gpio_dev = device_get_binding(gpioName); + if (gpio_dev == NULL) { + printk("Could not get %s device\n", gpioName); + return -1; + } + int err = set_gpio_dev(gpio_dev); + if (err) { + return -1; + } + return 0; +} + +uint8_t init_i2c(void) { + i2c_dev = device_get_binding(I2C_DEV); + if (!i2c_dev) + { + printk("I2C_0 error\n"); + return -1; + } + else + { + printk("I2C_0 Init OK\n"); + return 0; + } +} + +uint8_t init_gps(void) { + if (gps_begin(i2c_dev) != 0) + { + printk("Ublox GPS init error!\n"); + return -1; + } + return 0; +} + + +void main(void) { + printk("UBlox i2c test\n"); + + int err; + err = init_gpio(); + if (err) { + return; + } + err = init_i2c(); + if (err) { + return; + } + + err = init_gps(); + if (err) { + return; + } + + while(1) { + //check_ublox(); // See if new data is available. Process bytes as they come in. + get_position(); + get_datetime(); + k_msleep(250); // Don't pound too hard on the I2C bus + } +} \ No newline at end of file diff --git a/src/SparkFun_Ublox_Zephyr_Library.cpp b/src/SparkFun_Ublox_Zephyr_Library.cpp new file mode 100644 index 0000000..5b21fa5 --- /dev/null +++ b/src/SparkFun_Ublox_Zephyr_Library.cpp @@ -0,0 +1,3562 @@ +/* + This is a library written for the Ublox ZED-F9P and NEO-M8P-2 + SparkFun sells these at its website: www.sparkfun.com + Do you like this library? Help support SparkFun. Buy a board! + https://www.sparkfun.com/products/15136 + https://www.sparkfun.com/products/15005 + https://www.sparkfun.com/products/15733 + https://www.sparkfun.com/products/15193 + https://www.sparkfun.com/products/15210 + + Written by Nathan Seidle @ SparkFun Electronics, September 6th, 2018 + + This library handles configuring and handling the responses + from a Ublox GPS module. Works with most modules from Ublox including + the Zed-F9P, NEO-M8P-2, NEO-M9N, ZOE-M8Q, SAM-M8Q, and many others. + + https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library + + Development environment specifics: + Arduino IDE 1.8.5 + + SparkFun code, firmware, and software is released under the MIT License(http://opensource.org/licenses/MIT). + The MIT License (MIT) + Copyright (c) 2016 SparkFun Electronics + 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 "SparkFun_Ublox_Zephyr_Library.h" + +#include + +SFE_UBLOX_GPS::SFE_UBLOX_GPS(void) +{ + // Constructor + currentGeofenceParams.numFences = 0; // Zero the number of geofences currently in use + moduleQueried.versionNumber = false; +} + +bool SFE_UBLOX_GPS::init_gpio_pins(struct device &gpio_dev) { + _gpio_dev = &gpio_dev; + + if (checksumFailurePin >= 0) + { + gpio_pin_configure(_gpio_dev, (uint8_t)checksumFailurePin, GPIO_OUTPUT); + gpio_pin_set(_gpio_dev, (uint8_t)checksumFailurePin, HIGH); + } + return true; +} + +//Initialize the Serial port +bool SFE_UBLOX_GPS::begin(struct device &i2c_dev, uint8_t deviceAddress) +{ + commType = COMM_TYPE_I2C; + _i2cPort = &i2c_dev; //Grab which port the user wants us to use + + //We expect caller to begin their I2C port, with the speed of their choice external to the library + //But if they forget, we start the hardware here. + + //We're moving away from the practice of starting Wire hardware in a library. This is to avoid cross platform issues. + //ie, there are some platforms that don't handle multiple starts to the wire hardware. Also, every time you start the wire + //hardware the clock speed reverts back to 100kHz regardless of previous Wire.setClocks(). + //_i2cPort->begin(); + + _gpsI2Caddress = deviceAddress; //Store the I2C address from user + + return (isConnected()); +} + +//Initialize the Serial port +/* // TODO +bool SFE_UBLOX_GPS::begin(Stream &serialPort) +{ + commType = COMM_TYPE_SERIAL; + _serialPort = &serialPort; //Grab which port the user wants us to use + + return (isConnected()); +} +*/ + +//Enable or disable the printing of sent/response HEX values. +//Use this in conjunction with 'Transport Logging' from the Universal Reader Assistant to see what they're doing that we're not +void SFE_UBLOX_GPS::enableDebugging(bool printLimitedDebug) +{ + if (printLimitedDebug == false) + { + _printDebug = true; //Should we print the commands we send? Good for debugging + } + else + { + _printLimitedDebug = true; //Should we print limited debug messages? Good for debugging high navigation rates + } +} + +void SFE_UBLOX_GPS::disableDebugging(void) +{ + _printDebug = false; //Turn off extra print statements + _printLimitedDebug = false; +} + +//Safely print messages +void SFE_UBLOX_GPS::debugPrint(char *message) +{ + if (_printDebug == true) + { + printk("%s", message); + } +} +//Safely print messages +void SFE_UBLOX_GPS::debugPrintln(char *message) +{ + if (_printDebug == true) + { + printk("%s\n", message); + } +} + +const char *SFE_UBLOX_GPS::statusString(sfe_ublox_status_e stat) +{ + switch (stat) + { + case SFE_UBLOX_STATUS_SUCCESS: + return "Success"; + break; + case SFE_UBLOX_STATUS_FAIL: + return "General Failure"; + break; + case SFE_UBLOX_STATUS_CRC_FAIL: + return "CRC Fail"; + break; + case SFE_UBLOX_STATUS_TIMEOUT: + return "Timeout"; + break; + case SFE_UBLOX_STATUS_COMMAND_NACK: + return "Command not acknowledged (NACK)"; + break; + case SFE_UBLOX_STATUS_OUT_OF_RANGE: + return "Out of range"; + break; + case SFE_UBLOX_STATUS_INVALID_ARG: + return "Invalid Arg"; + break; + case SFE_UBLOX_STATUS_INVALID_OPERATION: + return "Invalid operation"; + break; + case SFE_UBLOX_STATUS_MEM_ERR: + return "Memory Error"; + break; + case SFE_UBLOX_STATUS_HW_ERR: + return "Hardware Error"; + break; + case SFE_UBLOX_STATUS_DATA_SENT: + return "Data Sent"; + break; + case SFE_UBLOX_STATUS_DATA_RECEIVED: + return "Data Received"; + break; + case SFE_UBLOX_STATUS_I2C_COMM_FAILURE: + return "I2C Comm Failure"; + break; + case SFE_UBLOX_STATUS_DATA_OVERWRITTEN: + return "Data Packet Overwritten"; + break; + default: + return "Unknown Status"; + break; + } + return "None"; +} + +void SFE_UBLOX_GPS::factoryReset() +{ + // Copy default settings to permanent + // Note: this does not load the permanent configuration into the current configuration. Calling factoryDefault() will do that. + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_CFG; + packetCfg.len = 13; + packetCfg.startingSpot = 0; + for (uint8_t i = 0; i < 4; i++) + { + payloadCfg[0 + i] = 0xff; // clear mask: copy default config to permanent config + payloadCfg[4 + i] = 0x00; // save mask: don't save current to permanent + payloadCfg[8 + i] = 0x00; // load mask: don't copy permanent config to current + } + payloadCfg[12] = 0xff; // all forms of permanent memory + sendCommand(&packetCfg, 0); // don't expect ACK + hardReset(); // cause factory default config to actually be loaded and used cleanly +} + +void SFE_UBLOX_GPS::hardReset() +{ + // Issue hard reset + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_RST; + packetCfg.len = 4; + packetCfg.startingSpot = 0; + payloadCfg[0] = 0xff; // cold start + payloadCfg[1] = 0xff; // cold start + payloadCfg[2] = 0; // 0=HW reset + payloadCfg[3] = 0; // reserved + sendCommand(&packetCfg, 0); // don't expect ACK +} + +int SFE_UBLOX_GPS::transferWriteI2C(u8_t *buffer, u32_t num_bytes, bool stop) { + struct i2c_msg msgs[1]; + /* Send the address to write to */ + //msgs[0].buf = 0x42; + //msgs[0].len = 1; + //msgs[0].flags = I2C_MSG_WRITE; + + //printk("transferWriteI2C: %x, num_bytes: %d\n", buffer[0], num_bytes); + + /* Data to be written, and STOP or RESTART after this. */ + msgs[0].buf = buffer; + msgs[0].len = num_bytes; + if (stop) { + msgs[0].flags = I2C_MSG_WRITE | I2C_MSG_STOP; + //printk("transferWriteI2C - I2C_MSG_STOP\n"); + } + else { + msgs[0].flags = I2C_MSG_WRITE | I2C_MSG_RESTART; + //printk("transferWriteI2C - I2C_MSG_RESTART\n"); + } + return i2c_transfer(_i2cPort, &msgs[0], 1, _gpsI2Caddress); +} + +int SFE_UBLOX_GPS::transferReadI2C(u8_t *buffer, u32_t num_bytes) { + struct i2c_msg msgs[1]; + /* Send the address to write to */ + //msgs[0].buf = (u8_t)_gpsI2Caddress; + //msgs[0].len = 2U; + //msgs[0].flags = I2C_MSG_READ; + + /* Data to be read, and STOP or RESTART after this. */ + msgs[0].buf = buffer; + msgs[0].len = num_bytes; + msgs[0].flags = I2C_MSG_READ | I2C_MSG_STOP; + + int err = i2c_transfer(_i2cPort, &msgs[0], 1, _gpsI2Caddress); + return err; +} + +//Changes the serial baud rate of the Ublox module, can't return success/fail 'cause ACK from modem +//is lost due to baud rate change +void SFE_UBLOX_GPS::setSerialRate(uint32_t baudrate, uint8_t uartPort, uint16_t maxWait) +{ + //Get the current config values for the UART port + getPortSettings(uartPort, maxWait); //This will load the payloadCfg array with current port settings + + if (_printDebug == true) + { + printk("Current baud rate: "); + printk("%d\n", ((uint32_t)payloadCfg[10] << 16) | ((uint32_t)payloadCfg[9] << 8) | payloadCfg[8]); + } + + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_PRT; + packetCfg.len = 20; + packetCfg.startingSpot = 0; + + //payloadCfg is now loaded with current bytes. Change only the ones we need to + payloadCfg[8] = baudrate; + payloadCfg[9] = baudrate >> 8; + payloadCfg[10] = baudrate >> 16; + payloadCfg[11] = baudrate >> 24; + + if (_printDebug == true) + { + printk("New baud rate: "); + printk("%d\n", ((uint32_t)payloadCfg[10] << 16) | ((uint32_t)payloadCfg[9] << 8) | payloadCfg[8]); + } + + sfe_ublox_status_e retVal = sendCommand(&packetCfg, maxWait); + if (_printDebug == true) + { + printk("setSerialRate: sendCommand returned: %s\n", statusString(retVal)); + } +} + +//Changes the I2C address that the Ublox module responds to +//0x42 is the default but can be changed with this command +bool SFE_UBLOX_GPS::setI2CAddress(uint8_t deviceAddress, uint16_t maxWait) +{ + //Get the current config values for the I2C port + getPortSettings(COM_PORT_I2C, maxWait); //This will load the payloadCfg array with current port settings + + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_PRT; + packetCfg.len = 20; + packetCfg.startingSpot = 0; + + //payloadCfg is now loaded with current bytes. Change only the ones we need to + payloadCfg[4] = deviceAddress << 1; //DDC mode LSB + + if (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT) // We are only expecting an ACK + { + //Success! Now change our internal global. + _gpsI2Caddress = deviceAddress; //Store the I2C address from user + return (true); + } + return (false); +} + +//Want to see the NMEA messages on the Serial port? Here's how - ported to print to console, here only enable the flag +void SFE_UBLOX_GPS::setNMEAOutputPort() +{ + _nmeaOutputPort = true; +} + +//Called regularly to check for available bytes on the user' specified port +bool SFE_UBLOX_GPS::checkUblox(uint8_t requestedClass, uint8_t requestedID) +{ + return checkUbloxInternal(&packetCfg, requestedClass, requestedID); +} + +//Called regularly to check for available bytes on the user' specified port +bool SFE_UBLOX_GPS::checkUbloxInternal(ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID) +{ + if (commType == COMM_TYPE_I2C) + return (checkUbloxI2C(incomingUBX, requestedClass, requestedID)); + else if (commType == COMM_TYPE_SERIAL) + return (checkUbloxSerial(incomingUBX, requestedClass, requestedID)); + return false; +} + +//Polls I2C for data, passing any new bytes to process() +//Returns true if new bytes are available +bool SFE_UBLOX_GPS::checkUbloxI2C(ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID) +{ + if (k_uptime_get_32() - lastCheck >= i2cPollingWait) + { + int err; + //Get the number of bytes available from the module + uint16_t bytesAvailable = 0; + /* + _i2cPort->beginTransmission(_gpsI2Caddress); // TODO + _i2cPort->write(0xFD); //0xFD (MSB) and 0xFE (LSB) are the registers that contain number of bytes available + if (_i2cPort->endTransmission(false) != 0) //Send a restart command. Do not release bus. + return (false); //Sensor did not ACK + */ + + u8_t data_buffer[1]; + data_buffer[0] = 0xFD; + //err = i2c_write(_i2cPort, data_buffer, 1, _gpsI2Caddress); + err = transferWriteI2C(data_buffer, 1, true); + if (err) { + return (false); + } + + //_i2cPort->requestFrom((uint8_t)_gpsI2Caddress, (uint8_t)2); // TODO + //if (_i2cPort->available()) + bool data_read_ok = true; + u8_t read_buffer[2]; + //err = i2c_read(_i2cPort, read_buffer, 2, _gpsI2Caddress); + err = transferReadI2C(data_buffer, 2); + if (err) { + data_read_ok = false; + } + + if (data_read_ok) + { + //uint8_t msb = _i2cPort->read(); // TODO + //uint8_t lsb = _i2cPort->read(); + uint8_t msb = read_buffer[0]; + uint8_t lsb = read_buffer[1]; + //printk("checkUbloxI2C, data_read_ok, MSB: 0x%x LSB: 0x%x\n", msb, lsb); + + if (lsb == 0xFF) + { + //I believe this is a Ublox bug. Device should never present an 0xFF. + if ((_printDebug == true) || (_printLimitedDebug == true)) // Print this if doing limited debugging + { + printk("checkUbloxI2C: Ublox bug, length lsb is 0xFF\n"); + } + if (checksumFailurePin >= 0) + { + gpio_pin_set(_gpio_dev, (uint8_t)checksumFailurePin, LOW); + k_msleep(10); + gpio_pin_set(_gpio_dev, (uint8_t)checksumFailurePin, HIGH); + } + lastCheck = k_uptime_get_32(); //Put off checking to avoid I2C bus traffic + return (false); + } + bytesAvailable = (uint16_t)msb << 8 | lsb; + } + + if (bytesAvailable == 0) + { + if (_printDebug == true) + { + printk("checkUbloxI2C: OK, zero bytes available\n"); + } + lastCheck = k_uptime_get_32(); //Put off checking to avoid I2C bus traffic + return (false); + } + + //Check for undocumented bit error. We found this doing logic scans. + //This error is rare but if we incorrectly interpret the first bit of the two 'data available' bytes as 1 + //then we have far too many bytes to check. May be related to I2C setup time violations: https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library/issues/40 + if (bytesAvailable & ((uint16_t)1 << 15)) + { + //Clear the MSbit + bytesAvailable &= ~((uint16_t)1 << 15); + + if ((_printDebug == true) || (_printLimitedDebug == true)) // Print this if doing limited debugging + { + printk("checkUbloxI2C: Bytes available error: %d\n", bytesAvailable); + if (checksumFailurePin >= 0) + { + gpio_pin_set(_gpio_dev, (uint8_t)checksumFailurePin, LOW); + k_msleep(10); + gpio_pin_set(_gpio_dev, (uint8_t)checksumFailurePin, HIGH); + } + } + } + + if (bytesAvailable > 100) + { + if (_printDebug == true) + { + printk("checkUbloxI2C: Large packet of %d bytes received\n", bytesAvailable); + } + } + else + { + if (_printDebug == true) + { + printk("checkUbloxI2C: Reading %d bytes\n", bytesAvailable); + } + } + + while (bytesAvailable) + { + /* + _i2cPort->beginTransmission(_gpsI2Caddress); // TODO + _i2cPort->write(0xFF); //0xFF is the register to read data from + if (_i2cPort->endTransmission(false) != 0) //Send a restart command. Do not release bus. + return (false); //Sensor did not ACK + */ + u8_t data_buffer[1]; + data_buffer[0] = 0xFF; + err = transferWriteI2C(data_buffer, 1, false); + if (err) { + return (false); + } + + //Limit to 32 bytes or whatever the buffer limit is for given platform + uint16_t bytesToRead = bytesAvailable; + if (bytesToRead > I2C_BUFFER_LENGTH) + bytesToRead = I2C_BUFFER_LENGTH; + + TRY_AGAIN: + //_i2cPort->requestFrom((uint8_t)_gpsI2Caddress, (uint8_t)bytesToRead); // TODO + //if (_i2cPort->available()) + bool data_read_ok = true; + u8_t read_buffer[(uint8_t)bytesToRead]; + err = transferReadI2C(read_buffer, (uint8_t)bytesToRead); + if (err) { + data_read_ok = false; + } + + if (data_read_ok) + { + for (uint16_t x = 0; x < bytesToRead; x++) + { + //uint8_t incoming = _i2cPort->read(); //Grab the actual character // TODO + //i2c_read(_i2cPort, read_buffer, 1, _gpsI2Caddress); //Grab the actual character + uint8_t incoming = read_buffer[x]; + + //Check to see if the first read is 0x7F. If it is, the module is not ready + //to respond. Stop, wait, and try again + if (x == 0) + { + if (incoming == 0x7F) + { + if ((_printDebug == true) || (_printLimitedDebug == true)) // Print this if doing limited debugging + { + printk("checkUbloxU2C: Ublox error, module not ready with data\n"); + } + k_msleep(5); //In logic analyzation, the module starting responding after 1.48ms + if (checksumFailurePin >= 0) + { + gpio_pin_set(_gpio_dev, (uint8_t)checksumFailurePin, LOW); + k_msleep(10); + gpio_pin_set(_gpio_dev, (uint8_t)checksumFailurePin, HIGH); + } + goto TRY_AGAIN; + } + } + process(incoming, incomingUBX, requestedClass, requestedID); //Process this valid character + } + } + else + return (false); //Sensor did not respond + + bytesAvailable -= bytesToRead; + } + } + + return (true); + +} //end checkUbloxI2C() + +//Checks Serial for data, passing any new bytes to process() +bool SFE_UBLOX_GPS::checkUbloxSerial(ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID) +{ + printk("checkUbloxSerial function not ported...\n"); + /* + while (_serialPort->available()) + { + process(_serialPort->read(), incomingUBX, requestedClass, requestedID); + } + */ + return (true); + +} //end checkUbloxSerial() + +//Processes NMEA and UBX binary sentences one byte at a time +//Take a given byte and file it into the proper array +void SFE_UBLOX_GPS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID) +{ + if ((currentSentence == NONE) || (currentSentence == NMEA)) + { + if (incoming == 0xB5) //UBX binary frames start with 0xB5, aka μ + { + //This is the start of a binary sentence. Reset flags. + //We still don't know the response class + ubxFrameCounter = 0; + currentSentence = UBX; + //Reset the packetBuf.counter even though we will need to reset it again when ubxFrameCounter == 2 + packetBuf.counter = 0; + ignoreThisPayload = false; //We should not ignore this payload - yet + //Store data in packetBuf until we know if we have a requested class and ID match + activePacketBuffer = SFE_UBLOX_PACKET_PACKETBUF; + } + else if (incoming == '$') + { + currentSentence = NMEA; + } + else if (incoming == 0xD3) //RTCM frames start with 0xD3 + { + rtcmFrameCounter = 0; + currentSentence = RTCM; + } + else + { + //This character is unknown or we missed the previous start of a sentence + } + } + + //Depending on the sentence, pass the character to the individual processor + if (currentSentence == UBX) + { + //Decide what type of response this is + if ((ubxFrameCounter == 0) && (incoming != 0xB5)) //ISO 'μ' + currentSentence = NONE; //Something went wrong. Reset. + else if ((ubxFrameCounter == 1) && (incoming != 0x62)) //ASCII 'b' + currentSentence = NONE; //Something went wrong. Reset. + // Note to future self: + // There may be some duplication / redundancy in the next few lines as processUBX will also + // load information into packetBuf, but we'll do it here too for clarity + else if (ubxFrameCounter == 2) //Class + { + // Record the class in packetBuf until we know what to do with it + packetBuf.cls = incoming; // (Duplication) + rollingChecksumA = 0; //Reset our rolling checksums here (not when we receive the 0xB5) + rollingChecksumB = 0; + packetBuf.counter = 0; //Reset the packetBuf.counter (again) + packetBuf.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; // Reset the packet validity (redundant?) + packetBuf.startingSpot = incomingUBX->startingSpot; //Copy the startingSpot + } + else if (ubxFrameCounter == 3) //ID + { + // Record the ID in packetBuf until we know what to do with it + packetBuf.id = incoming; // (Duplication) + //We can now identify the type of response + //If the packet we are receiving is not an ACK then check for a class and ID match + if (packetBuf.cls != UBX_CLASS_ACK) + { + //This is not an ACK so check for a class and ID match + if ((packetBuf.cls == requestedClass) && (packetBuf.id == requestedID)) + { + //This is not an ACK and we have a class and ID match + //So start diverting data into incomingUBX (usually packetCfg) + activePacketBuffer = SFE_UBLOX_PACKET_PACKETCFG; + incomingUBX->cls = packetBuf.cls; //Copy the class and ID into incomingUBX (usually packetCfg) + incomingUBX->id = packetBuf.id; + incomingUBX->counter = packetBuf.counter; //Copy over the .counter too + } + else + { + //This is not an ACK and we do not have a class and ID match + //so we should keep diverting data into packetBuf and ignore the payload + ignoreThisPayload = true; + } + } + else + { + // This is an ACK so it is to early to do anything with it + // We need to wait until we have received the length and data bytes + // So we should keep diverting data into packetBuf + } + } + else if (ubxFrameCounter == 4) //Length LSB + { + //We should save the length in packetBuf even if activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG + packetBuf.len = incoming; // (Duplication) + } + else if (ubxFrameCounter == 5) //Length MSB + { + //We should save the length in packetBuf even if activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG + packetBuf.len |= incoming << 8; // (Duplication) + } + else if (ubxFrameCounter == 6) //This should be the first byte of the payload unless .len is zero + { + if (packetBuf.len == 0) // Check if length is zero (hopefully this is impossible!) + { + if (_printDebug == true) + { + printk("process: ZERO LENGTH packet received: Class: 0x%x ID: 0x%x\n", packetBuf.cls, packetBuf.id); + } + //If length is zero (!) this will be the first byte of the checksum so record it + packetBuf.checksumA = incoming; + } + else + { + //The length is not zero so record this byte in the payload + packetBuf.payload[0] = incoming; + } + } + else if (ubxFrameCounter == 7) //This should be the second byte of the payload unless .len is zero or one + { + if (packetBuf.len == 0) // Check if length is zero (hopefully this is impossible!) + { + //If length is zero (!) this will be the second byte of the checksum so record it + packetBuf.checksumB = incoming; + } + else if (packetBuf.len == 1) // Check if length is one + { + //The length is one so this is the first byte of the checksum + packetBuf.checksumA = incoming; + } + else // Length is >= 2 so this must be a payload byte + { + packetBuf.payload[1] = incoming; + } + // Now that we have received two payload bytes, we can check for a matching ACK/NACK + if ((activePacketBuffer == SFE_UBLOX_PACKET_PACKETBUF) // If we are not already processing a data packet + && (packetBuf.cls == UBX_CLASS_ACK) // and if this is an ACK/NACK + && (packetBuf.payload[0] == requestedClass) // and if the class matches + && (packetBuf.payload[1] == requestedID)) // and if the ID matches + { + if (packetBuf.len == 2) // Check if .len is 2 + { + // Then this is a matching ACK so copy it into packetAck + activePacketBuffer = SFE_UBLOX_PACKET_PACKETACK; + packetAck.cls = packetBuf.cls; + packetAck.id = packetBuf.id; + packetAck.len = packetBuf.len; + packetAck.counter = packetBuf.counter; + packetAck.payload[0] = packetBuf.payload[0]; + packetAck.payload[1] = packetBuf.payload[1]; + } + else // Length is not 2 (hopefully this is impossible!) + { + if (_printDebug == true) + { + printk("process: ACK received with .len != 2: Class: 0x%x ID: 0x%x, len: %d\n", packetBuf.payload[0], packetBuf.payload[1], packetBuf.len); + } + } + } + } + + //Divert incoming into the correct buffer + if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETACK) + processUBX(incoming, &packetAck, requestedClass, requestedID); + else if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG) + processUBX(incoming, incomingUBX, requestedClass, requestedID); + else // if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETBUF) + processUBX(incoming, &packetBuf, requestedClass, requestedID); + + //Finally, increment the frame counter + ubxFrameCounter++; + } + else if (currentSentence == NMEA) + { + processNMEA(incoming); //Process each NMEA character + } + else if (currentSentence == RTCM) + { + processRTCMframe(incoming); //Deal with RTCM bytes + } +} + +//This is the default or generic NMEA processor. We're only going to pipe the data to serial port so we can see it. +//User could overwrite this function to pipe characters to nmea.process(c) of tinyGPS or MicroNMEA +//Or user could pipe each character to a buffer, radio, etc. +void SFE_UBLOX_GPS::processNMEA(char incoming) +{ + //If user has assigned an output port then pipe the characters there + + if (_nmeaOutputPort) + printk("%c", incoming); //Echo this byte to the serial port +} + +//We need to be able to identify an RTCM packet and then the length +//so that we know when the RTCM message is completely received and we then start +//listening for other sentences (like NMEA or UBX) +//RTCM packet structure is very odd. I never found RTCM STANDARD 10403.2 but +//http://d1.amobbs.com/bbs_upload782111/files_39/ourdev_635123CK0HJT.pdf is good +//https://dspace.cvut.cz/bitstream/handle/10467/65205/F3-BP-2016-Shkalikava-Anastasiya-Prenos%20polohove%20informace%20prostrednictvim%20datove%20site.pdf?sequence=-1 +//Lead me to: https://forum.u-blox.com/index.php/4348/how-to-read-rtcm-messages-from-neo-m8p +//RTCM 3.2 bytes look like this: +//Byte 0: Always 0xD3 +//Byte 1: 6-bits of zero +//Byte 2: 10-bits of length of this packet including the first two-ish header bytes, + 6. +//byte 3 + 4 bits: Msg type 12 bits +//Example: D3 00 7C 43 F0 ... / 0x7C = 124+6 = 130 bytes in this packet, 0x43F = Msg type 1087 +void SFE_UBLOX_GPS::processRTCMframe(uint8_t incoming) +{ + if (rtcmFrameCounter == 1) + { + rtcmLen = (incoming & 0x03) << 8; //Get the last two bits of this byte. Bits 8&9 of 10-bit length + } + else if (rtcmFrameCounter == 2) + { + rtcmLen |= incoming; //Bits 0-7 of packet length + rtcmLen += 6; //There are 6 additional bytes of what we presume is header, msgType, CRC, and stuff + } + /*else if (rtcmFrameCounter == 3) + { + rtcmMsgType = incoming << 4; //Message Type, MS 4 bits + } + else if (rtcmFrameCounter == 4) + { + rtcmMsgType |= (incoming >> 4); //Message Type, bits 0-7 + }*/ + + rtcmFrameCounter++; + + processRTCM(incoming); //Here is where we expose this byte to the user + + if (rtcmFrameCounter == rtcmLen) + { + //We're done! + currentSentence = NONE; //Reset and start looking for next sentence type + } +} + +//This function is called for each byte of an RTCM frame +//Ths user can overwrite this function and process the RTCM frame as they please +//Bytes can be piped to Serial or other interface. The consumer could be a radio or the internet (Ntrip broadcaster) +void SFE_UBLOX_GPS::processRTCM(uint8_t incoming) +{ + //Radio.sendReliable((String)incoming); //An example of passing this byte to a radio + + //Debug printing + // printk(" "); + // if(incoming < 0x10) printk("0"); + // if(incoming < 0x10) printk("0"); + // printk("%d", incoming); + // if(rtcmFrameCounter % 16 == 0) printk("\n"); +} + +//Given a character, file it away into the uxb packet structure +//Set valid to VALID or NOT_VALID once sentence is completely received and passes or fails CRC +//The payload portion of the packet can be 100s of bytes but the max array +//size is MAX_PAYLOAD_SIZE bytes. startingSpot can be set so we only record +//a subset of bytes within a larger packet. +void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID) +{ + //Add all incoming bytes to the rolling checksum + //Stop at len+4 as this is the checksum bytes to that should not be added to the rolling checksum + if (incomingUBX->counter < incomingUBX->len + 4) + addToChecksum(incoming); + + if (incomingUBX->counter == 0) + { + incomingUBX->cls = incoming; + } + else if (incomingUBX->counter == 1) + { + incomingUBX->id = incoming; + } + else if (incomingUBX->counter == 2) //Len LSB + { + incomingUBX->len = incoming; + } + else if (incomingUBX->counter == 3) //Len MSB + { + incomingUBX->len |= incoming << 8; + } + else if (incomingUBX->counter == incomingUBX->len + 4) //ChecksumA + { + incomingUBX->checksumA = incoming; + } + else if (incomingUBX->counter == incomingUBX->len + 5) //ChecksumB + { + incomingUBX->checksumB = incoming; + + currentSentence = NONE; //We're done! Reset the sentence to being looking for a new start char + + //Validate this sentence + if ((incomingUBX->checksumA == rollingChecksumA) && (incomingUBX->checksumB == rollingChecksumB)) + { + incomingUBX->valid = SFE_UBLOX_PACKET_VALIDITY_VALID; // Flag the packet as valid + + // Let's check if the class and ID match the requestedClass and requestedID + // Remember - this could be a data packet or an ACK packet + if ((incomingUBX->cls == requestedClass) && (incomingUBX->id == requestedID)) + { + incomingUBX->classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_VALID; // If we have a match, set the classAndIDmatch flag to valid + } + + // If this is an ACK then let's check if the class and ID match the requestedClass and requestedID + else if ((incomingUBX->cls == UBX_CLASS_ACK) && (incomingUBX->id == UBX_ACK_ACK) && (incomingUBX->payload[0] == requestedClass) && (incomingUBX->payload[1] == requestedID)) + { + incomingUBX->classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_VALID; // If we have a match, set the classAndIDmatch flag to valid + } + + // If this is a NACK then let's check if the class and ID match the requestedClass and requestedID + else if ((incomingUBX->cls == UBX_CLASS_ACK) && (incomingUBX->id == UBX_ACK_NACK) && (incomingUBX->payload[0] == requestedClass) && (incomingUBX->payload[1] == requestedID)) + { + incomingUBX->classAndIDmatch = SFE_UBLOX_PACKET_NOTACKNOWLEDGED; // If we have a match, set the classAndIDmatch flag to NOTACKNOWLEDGED + if (_printDebug == true) + { + printk("processUBX: NACK received: Requested Class: 0x%x, Requested ID: 0x%x\n", incomingUBX->payload[0], incomingUBX->payload[1]); + } + } + + if (_printDebug == true) + { + printk("Incoming: Size: %d Received: \n", incomingUBX->len); + printPacket(incomingUBX); + + if (incomingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID) + { + printk("packetCfg now valid\n"); + } + if (packetAck.valid == SFE_UBLOX_PACKET_VALIDITY_VALID) + { + printk("packetAck now valid\n"); + } + if (incomingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) + { + printk("packetCfg classAndIDmatch\n"); + } + if (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) + { + printk("packetAck classAndIDmatch\n"); + } + } + + //We've got a valid packet, now do something with it but only if ignoreThisPayload is false + if (ignoreThisPayload == false) + { + processUBXpacket(incomingUBX); + } + } + else // Checksum failure + { + incomingUBX->valid = SFE_UBLOX_PACKET_VALIDITY_NOT_VALID; + + // Let's check if the class and ID match the requestedClass and requestedID. + // This is potentially risky as we are saying that we saw the requested Class and ID + // but that the packet checksum failed. Potentially it could be the class or ID bytes + // that caused the checksum error! + if ((incomingUBX->cls == requestedClass) && (incomingUBX->id == requestedID)) + { + incomingUBX->classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_VALID; // If we have a match, set the classAndIDmatch flag to not valid + } + // If this is an ACK then let's check if the class and ID match the requestedClass and requestedID + else if ((incomingUBX->cls == UBX_CLASS_ACK) && (incomingUBX->payload[0] == requestedClass) && (incomingUBX->payload[1] == requestedID)) + { + incomingUBX->classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_VALID; // If we have a match, set the classAndIDmatch flag to not valid + } + + if ((_printDebug == true) || (_printLimitedDebug == true)) // Print this if doing limited debugging + { + //Drive an external pin to allow for easier logic analyzation + if (checksumFailurePin >= 0) + { + gpio_pin_set(_gpio_dev, (uint8_t)checksumFailurePin, LOW); + k_msleep(10); + gpio_pin_set(_gpio_dev, (uint8_t)checksumFailurePin, HIGH); + } + + printk("Checksum failed: checksumA: %d checksumB: %d\n", incomingUBX->checksumA, incomingUBX->checksumB); + printk("rollingChecksumA: %d, rollingChecksumB: %d\n", rollingChecksumA, rollingChecksumB); + printk("Failed: Size: %d Received: \n", incomingUBX->len); + printPacket(incomingUBX); + } + } + } + else //Load this byte into the payload array + { + //If a UBX_NAV_PVT packet comes in asynchronously, we need to fudge the startingSpot + uint16_t startingSpot = incomingUBX->startingSpot; + if (incomingUBX->cls == UBX_CLASS_NAV && incomingUBX->id == UBX_NAV_PVT) + startingSpot = 0; + //Begin recording if counter goes past startingSpot + if ((incomingUBX->counter - 4) >= startingSpot) + { + //Check to see if we have room for this byte + if (((incomingUBX->counter - 4) - startingSpot) < MAX_PAYLOAD_SIZE) //If counter = 208, starting spot = 200, we're good to record. + { + // Check if this is payload data which should be ignored + if (ignoreThisPayload == false) + { + incomingUBX->payload[incomingUBX->counter - 4 - startingSpot] = incoming; //Store this byte into payload array + } + } + } + } + + //Increment the counter + incomingUBX->counter++; + + if (incomingUBX->counter == MAX_PAYLOAD_SIZE) + { + //Something has gone very wrong + currentSentence = NONE; //Reset the sentence to being looking for a new start char + if (_printDebug == true) + { + printk("processUBX: counter hit MAX_PAYLOAD_SIZE\n"); + } + } +} + +//Once a packet has been received and validated, identify this packet's class/id and update internal flags +//Note: if the user requests a PVT or a HPPOSLLH message using a custom packet, the data extraction will +// not work as expected beacuse extractLong etc are hardwired to packetCfg payloadCfg. Ideally +// extractLong etc should be updated so they receive a pointer to the packet buffer. +void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg) +{ + switch (msg->cls) + { + case UBX_CLASS_NAV: + if (msg->id == UBX_NAV_PVT && msg->len == 92) + { + //Parse various byte fields into global vars + constexpr int startingSpot = 0; //fixed value used in processUBX + + timeOfWeek = extractLong(0); + gpsMillisecond = extractLong(0) % 1000; //Get last three digits of iTOW + gpsYear = extractInt(4); + gpsMonth = extractByte(6); + gpsDay = extractByte(7); + gpsHour = extractByte(8); + gpsMinute = extractByte(9); + gpsSecond = extractByte(10); + gpsDateValid = extractByte(11) & 0x01; + gpsTimeValid = (extractByte(11) & 0x02) >> 1; + gpsNanosecond = extractLong(16); //Includes milliseconds + + fixType = extractByte(20 - startingSpot); + carrierSolution = extractByte(21 - startingSpot) >> 6; //Get 6th&7th bits of this byte + SIV = extractByte(23 - startingSpot); + longitude = extractLong(24 - startingSpot); + latitude = extractLong(28 - startingSpot); + altitude = extractLong(32 - startingSpot); + altitudeMSL = extractLong(36 - startingSpot); + groundSpeed = extractLong(60 - startingSpot); + headingOfMotion = extractLong(64 - startingSpot); + pDOP = extractInt(76 - startingSpot); + + //Mark all datums as fresh (not read before) + moduleQueried.gpsiTOW = true; + moduleQueried.gpsYear = true; + moduleQueried.gpsMonth = true; + moduleQueried.gpsDay = true; + moduleQueried.gpsHour = true; + moduleQueried.gpsMinute = true; + moduleQueried.gpsSecond = true; + moduleQueried.gpsDateValid = true; + moduleQueried.gpsTimeValid = true; + moduleQueried.gpsNanosecond = true; + + moduleQueried.all = true; + moduleQueried.longitude = true; + moduleQueried.latitude = true; + moduleQueried.altitude = true; + moduleQueried.altitudeMSL = true; + moduleQueried.SIV = true; + moduleQueried.fixType = true; + moduleQueried.carrierSolution = true; + moduleQueried.groundSpeed = true; + moduleQueried.headingOfMotion = true; + moduleQueried.pDOP = true; + } + else if (msg->id == UBX_NAV_HPPOSLLH && msg->len == 36) + { + timeOfWeek = extractLong(4); + highResLongitude = extractLong(8); + highResLatitude = extractLong(12); + elipsoid = extractLong(16); + meanSeaLevel = extractLong(20); + highResLongitudeHp = extractSignedChar(24); + highResLatitudeHp = extractSignedChar(25); + elipsoidHp = extractSignedChar(26); + meanSeaLevelHp = extractSignedChar(27); + horizontalAccuracy = extractLong(28); + verticalAccuracy = extractLong(32); + + highResModuleQueried.all = true; + highResModuleQueried.highResLatitude = true; + highResModuleQueried.highResLatitudeHp = true; + highResModuleQueried.highResLongitude = true; + highResModuleQueried.highResLongitudeHp = true; + highResModuleQueried.elipsoid = true; + highResModuleQueried.elipsoidHp = true; + highResModuleQueried.meanSeaLevel = true; + highResModuleQueried.meanSeaLevelHp = true; + highResModuleQueried.geoidSeparation = true; + highResModuleQueried.horizontalAccuracy = true; + highResModuleQueried.verticalAccuracy = true; + moduleQueried.gpsiTOW = true; // this can arrive via HPPOS too. + + if (_printDebug == true) // TODO check float prints + { + printk("Sec: %f", ((float)extractLong(4)) / 1000.0f); + printk("\n"); + printk("LON: %f", ((float)(int32_t)extractLong(8)) / 10000000.0f); + printk("\n"); + printk("LAT: %f", ((float)(int32_t)extractLong(12)) / 10000000.0f); + printk("\n"); + printk("ELI M: %f", ((float)(int32_t)extractLong(16)) / 1000.0f); + printk("\n"); + printk("MSL M: %f", ((float)(int32_t)extractLong(20)) / 1000.0f); + printk("\n"); + printk("LON HP: %c", extractSignedChar(24)); + printk("\n"); + printk("LAT HP: %c", extractSignedChar(25)); + printk("\n"); + printk("ELI HP: %calcChecksum", extractSignedChar(26)); + printk("\n"); + printk("MSL HP: %c", extractSignedChar(27)); + printk("\n"); + printk("HA 2D M: %f", ((float)(int32_t)extractLong(28)) / 10000.0f); + printk("\n"); + printk("VERT M: %f", ((float)(int32_t)extractLong(32)) / 10000.0f); + } + } + break; + } +} + +//Given a packet and payload, send everything including CRC bytes via I2C port +sfe_ublox_status_e SFE_UBLOX_GPS::sendCommand(ubxPacket *outgoingUBX, uint16_t maxWait) +{ + sfe_ublox_status_e retVal = SFE_UBLOX_STATUS_SUCCESS; + + calcChecksum(outgoingUBX); //Sets checksum A and B bytes of the packet + + if (_printDebug == true) + { + printk("\nSending: "); + printPacket(outgoingUBX); + } + + if (commType == COMM_TYPE_I2C) + { + retVal = sendI2cCommand(outgoingUBX, maxWait); + if (retVal != SFE_UBLOX_STATUS_SUCCESS) + { + if (_printDebug == true) + { + printk("Send I2C Command failed\n"); + } + return retVal; + } + } + else if (commType == COMM_TYPE_SERIAL) + { + sendSerialCommand(outgoingUBX); + } + + if (maxWait > 0) + { + //Depending on what we just sent, either we need to look for an ACK or not + if (outgoingUBX->cls == UBX_CLASS_CFG) + { + if (_printDebug == true) + { + printk("sendCommand: Waiting for ACK response\n"); + } + retVal = waitForACKResponse(outgoingUBX, outgoingUBX->cls, outgoingUBX->id, maxWait); //Wait for Ack response + } + else + { + if (_printDebug == true) + { + printk("sendCommand: Waiting for No ACK response\n"); + } + retVal = waitForNoACKResponse(outgoingUBX, outgoingUBX->cls, outgoingUBX->id, maxWait); //Wait for Ack response + } + } + return retVal; +} + +//Returns false if sensor fails to respond to I2C traffic +sfe_ublox_status_e SFE_UBLOX_GPS::sendI2cCommand(ubxPacket *outgoingUBX, uint16_t maxWait) +{ + int err; + //Point at 0xFF data register + /* // TODO + _i2cPort->beginTransmission((uint8_t)_gpsI2Caddress); //There is no register to write to, we just begin writing data bytes + _i2cPort->write(0xFF); + if (_i2cPort->endTransmission() != 0) //Don't release bus + return (SFE_UBLOX_STATUS_I2C_COMM_FAILURE); //Sensor did not ACK + */ + u8_t small_buffer[1]; + small_buffer[0] = 0xFF; + //err = i2c_write(_i2cPort, small_buffer, 1, _gpsI2Caddress); + err = transferWriteI2C(small_buffer, 1); + if (err) { + return (SFE_UBLOX_STATUS_I2C_COMM_FAILURE); //Sensor did not ACK + } + + //Write header bytes + /* // TODO + _i2cPort->beginTransmission((uint8_t)_gpsI2Caddress); //There is no register to write to, we just begin writing data bytes + _i2cPort->write(UBX_SYNCH_1); //μ - oh ublox, you're funny. I will call you micro-blox from now on. + _i2cPort->write(UBX_SYNCH_2); //b + _i2cPort->write(outgoingUBX->cls); + _i2cPort->write(outgoingUBX->id); + _i2cPort->write(outgoingUBX->len & 0xFF); //LSB + _i2cPort->write(outgoingUBX->len >> 8); //MSB + if (_i2cPort->endTransmission(false) != 0) //Do not release bus + return (SFE_UBLOX_STATUS_I2C_COMM_FAILURE); //Sensor did not ACK + */ + u8_t header_buffer[6]; //There is no register to write to, we just begin writing data bytes + header_buffer[0] = UBX_SYNCH_1; //μ - oh ublox, you're funny. I will call you micro-blox from now on. + header_buffer[1] = UBX_SYNCH_2; //b + header_buffer[2] = outgoingUBX->cls; + header_buffer[3] = outgoingUBX->id; + header_buffer[4] = outgoingUBX->len & 0xFF; //LSB + header_buffer[5] = outgoingUBX->len >> 8; //MSB + //err = i2c_write(_i2cPort, header_buffer, 6, _gpsI2Caddress); + + err = transferWriteI2C(header_buffer, 6, false); + k_msleep(1); // added this millisecond sleep to not progress too fast + if (err) { + return (SFE_UBLOX_STATUS_I2C_COMM_FAILURE); //Sensor did not ACK + } + + //Write payload. Limit the sends into 32 byte chunks + //This code based on ublox: https://forum.u-blox.com/index.php/20528/how-to-use-i2c-to-get-the-nmea-frames + uint16_t bytesToSend = outgoingUBX->len; + + //"The number of data bytes must be at least 2 to properly distinguish + //from the write access to set the address counter in random read accesses." + uint16_t startSpot = 0; + while (bytesToSend > 1) + { + uint8_t len = bytesToSend; + if (len > I2C_BUFFER_LENGTH) + len = I2C_BUFFER_LENGTH; + /* // TODO + _i2cPort->beginTransmission((uint8_t)_gpsI2Caddress); + //_i2cPort->write(outgoingUBX->payload, len); //Write a portion of the payload to the bus + + for (uint16_t x = 0; x < len; x++) + _i2cPort->write(outgoingUBX->payload[startSpot + x]); //Write a portion of the payload to the bus + + if (_i2cPort->endTransmission(false) != 0) //Don't release bus + return (SFE_UBLOX_STATUS_I2C_COMM_FAILURE); //Sensor did not ACK + */ + u8_t data_buffer[len]; + for (uint16_t x = 0; x < len; x++) { + data_buffer[x] = outgoingUBX->payload[startSpot + x]; + } + //err = i2c_write(_i2cPort, data_buffer, len, _gpsI2Caddress); + err = transferWriteI2C(data_buffer, len, false); + if (err) { + return (SFE_UBLOX_STATUS_I2C_COMM_FAILURE); //Sensor did not ACK + } + + //*outgoingUBX->payload += len; //Move the pointer forward + startSpot += len; //Move the pointer forward + bytesToSend -= len; + } + + //Write checksum + //_i2cPort->beginTransmission((uint8_t)_gpsI2Caddress); // TODO + if (bytesToSend == 1) { + //_i2cPort->write(outgoingUBX->payload, 1); + + //i2c_write(_i2cPort, outgoingUBX->payload, 1, _gpsI2Caddress); + u8_t checksum_buffer[3]; + uint8_t payload = *outgoingUBX->payload; + checksum_buffer[0] = payload; + checksum_buffer[1] = outgoingUBX->checksumA; + checksum_buffer[2] = outgoingUBX->checksumB; + err = transferWriteI2C(checksum_buffer, 3, true); + } + else { + u8_t checksum_buffer[2]; + checksum_buffer[0] = outgoingUBX->checksumA; + checksum_buffer[1] = outgoingUBX->checksumB; + err = transferWriteI2C(checksum_buffer, 2, true); + } + //_i2cPort->write(outgoingUBX->checksumA); // TODO + //_i2cPort->write(outgoingUBX->checksumB); + + //err = i2c_write(_i2cPort, checksum_buffer, 3, _gpsI2Caddress); + if (err) { + return (SFE_UBLOX_STATUS_I2C_COMM_FAILURE); //Sensor did not ACK + } + + return (SFE_UBLOX_STATUS_SUCCESS); +} + +//Given a packet and payload, send everything including CRC bytesA via Serial port +void SFE_UBLOX_GPS::sendSerialCommand(ubxPacket *outgoingUBX) +{ + printk("sendSerialCommand function not ported...\n"); + /* + //Write header bytes + _serialPort->write(UBX_SYNCH_1); //μ - oh ublox, you're funny. I will call you micro-blox from now on. + _serialPort->write(UBX_SYNCH_2); //b + _serialPort->write(outgoingUBX->cls); + _serialPort->write(outgoingUBX->id); + _serialPort->write(outgoingUBX->len & 0xFF); //LSB + _serialPort->write(outgoingUBX->len >> 8); //MSB + + //Write payload. + for (int i = 0; i < outgoingUBX->len; i++) + { + _serialPort->write(outgoingUBX->payload[i]); + } + + //Write checksum + _serialPort->write(outgoingUBX->checksumA); + _serialPort->write(outgoingUBX->checksumB); + */ +} + +//Returns true if I2C device ack's +bool SFE_UBLOX_GPS::isConnected(uint16_t maxWait) +{ + if (commType == COMM_TYPE_I2C) + { + /* // TODO + _i2cPort->beginTransmission((uint8_t)_gpsI2Caddress); + if (_i2cPort->endTransmission() != 0) + return false; //Sensor did not ack + */ + + u8_t buff[1]; + buff[0] = 0x00; + //int err = i2c_write(_i2cPort, buff, 1, _gpsI2Caddress); + int err = transferWriteI2C(buff, 1); + if (err) { + return false; //Sensor did not ack + } + + } + + // Query navigation rate to see whether we get a meaningful response + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_RATE; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + //return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_RECEIVED); // We are polling the RATE so we expect data and an ACK + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // data send is returned not data received - TODO library bug? +} + +//Given a message, calc and store the two byte "8-Bit Fletcher" checksum over the entirety of the message +//This is called before we send a command message +void SFE_UBLOX_GPS::calcChecksum(ubxPacket *msg) +{ + msg->checksumA = 0; + msg->checksumB = 0; + + msg->checksumA += msg->cls; + msg->checksumB += msg->checksumA; + + msg->checksumA += msg->id; + msg->checksumB += msg->checksumA; + + msg->checksumA += (msg->len & 0xFF); + msg->checksumB += msg->checksumA; + + msg->checksumA += (msg->len >> 8); + msg->checksumB += msg->checksumA; + + for (uint16_t i = 0; i < msg->len; i++) + { + msg->checksumA += msg->payload[i]; + msg->checksumB += msg->checksumA; + } +} + +//Given a message and a byte, add to rolling "8-Bit Fletcher" checksum +//This is used when receiving messages from module +void SFE_UBLOX_GPS::addToChecksum(uint8_t incoming) +{ + rollingChecksumA += incoming; + rollingChecksumB += rollingChecksumA; +} + +//Pretty prints the current ubxPacket +void SFE_UBLOX_GPS::printPacket(ubxPacket *packet) +{ + if (_printDebug == true) + { + printk("CLS:"); + if (packet->cls == UBX_CLASS_NAV) //1 + printk("NAV"); + else if (packet->cls == UBX_CLASS_ACK) //5 + printk("ACK"); + else if (packet->cls == UBX_CLASS_CFG) //6 + printk("CFG"); + else if (packet->cls == UBX_CLASS_MON) //0x0A + printk("MON"); + else + { + printk("0x%x", packet->cls); + } + + printk(" ID:"); + if (packet->cls == UBX_CLASS_NAV && packet->id == UBX_NAV_PVT) + printk("PVT"); + else if (packet->cls == UBX_CLASS_CFG && packet->id == UBX_CFG_RATE) + printk("RATE"); + else if (packet->cls == UBX_CLASS_CFG && packet->id == UBX_CFG_CFG) + printk("SAVE"); + else + { + printk("0x%x", packet->id); + } + + printk(" Len: 0x%x", packet->len); + + // Only print the payload is ignoreThisPayload is false otherwise + // we could be printing gibberish from beyond the end of packetBuf + if (ignoreThisPayload == false) + { + printk(" Payload:"); + + for (int x = 0; x < packet->len; x++) + { + printk(" %x", packet->payload[x]); + } + } + else + { + printk(" Payload: IGNORED"); + } + printk("\n"); + } +} + +//=-=-=-=-=-=-=-= Specific commands =-=-=-=-=-=-=-==-=-=-=-=-=-=-= +//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= + +//When messages from the class CFG are sent to the receiver, the receiver will send an "acknowledge"(UBX - ACK - ACK) or a +//"not acknowledge"(UBX-ACK-NAK) message back to the sender, depending on whether or not the message was processed correctly. +//Some messages from other classes also use the same acknowledgement mechanism. + +//When we poll or get a setting, we will receive _both_ a config packet and an ACK +//If the poll or get request is not valid, we will receive _only_ a NACK + +//If we are trying to get or poll a setting, then packetCfg.len will be 0 or 1 when the packetCfg is _sent_. +//If we poll the setting for a particular port using UBX-CFG-PRT then .len will be 1 initially +//For all other gets or polls, .len will be 0 initially +//(It would be possible for .len to be 2 _if_ we were using UBX-CFG-MSG to poll the settings for a particular message - but we don't use that (currently)) + +//If the get or poll _fails_, i.e. is NACK'd, then packetCfg.len could still be 0 or 1 after the NACK is received +//But if the get or poll is ACK'd, then packetCfg.len will have been updated by the incoming data and will always be at least 2 + +//If we are going to set the value for a setting, then packetCfg.len will be at least 3 when the packetCfg is _sent_. +//(UBX-CFG-MSG appears to have the shortest set length of 3 bytes) + +//We need to think carefully about how interleaved PVT packets affect things. +//It is entirely possible that our packetCfg and packetAck were received successfully +//but while we are still in the "if (checkUblox() == true)" loop a PVT packet is processed +//or _starts_ to arrive (remember that Serial data can arrive very slowly). + +//Returns SFE_UBLOX_STATUS_DATA_RECEIVED if we got an ACK and a valid packetCfg (module is responding with register content) +//Returns SFE_UBLOX_STATUS_DATA_SENT if we got an ACK and no packetCfg (no valid packetCfg needed, module absorbs new register data) +//Returns SFE_UBLOX_STATUS_FAIL if something very bad happens (e.g. a double checksum failure) +//Returns SFE_UBLOX_STATUS_COMMAND_NACK if the packet was not-acknowledged (NACK) +//Returns SFE_UBLOX_STATUS_CRC_FAIL if we had a checksum failure +//Returns SFE_UBLOX_STATUS_TIMEOUT if we timed out +//Returns SFE_UBLOX_STATUS_DATA_OVERWRITTEN if we got an ACK and a valid packetCfg but that the packetCfg has been +// or is currently being overwritten (remember that Serial data can arrive very slowly) +sfe_ublox_status_e SFE_UBLOX_GPS::waitForACKResponse(ubxPacket *outgoingUBX, uint8_t requestedClass, uint8_t requestedID, uint16_t maxTime) +{ + outgoingUBX->valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; //This will go VALID (or NOT_VALID) when we receive a response to the packet we sent + packetAck.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; + packetBuf.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; + outgoingUBX->classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; // This will go VALID (or NOT_VALID) when we receive a packet that matches the requested class and ID + packetAck.classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; + packetBuf.classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; + + unsigned long startTime = k_uptime_get_32(); + while (k_uptime_get_32() - startTime < maxTime) + { + if (checkUbloxInternal(outgoingUBX, requestedClass, requestedID) == true) //See if new data is available. Process bytes as they come in. + { + // If both the outgoingUBX->classAndIDmatch and packetAck.classAndIDmatch are VALID + // and outgoingUBX->valid is _still_ VALID and the class and ID _still_ match + // then we can be confident that the data in outgoingUBX is valid + if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID) && (outgoingUBX->cls == requestedClass) && (outgoingUBX->id == requestedID)) + { + if (_printDebug == true) + { + printk("waitForACKResponse: valid data and valid ACK received after %lu msec\n", k_uptime_get_32() - startTime); + } + return (SFE_UBLOX_STATUS_DATA_RECEIVED); //We received valid data and a correct ACK! + } + + // We can be confident that the data packet (if we are going to get one) will always arrive + // before the matching ACK. So if we sent a config packet which only produces an ACK + // then outgoingUBX->classAndIDmatch will be NOT_DEFINED and the packetAck.classAndIDmatch will VALID. + // We should not check outgoingUBX->valid, outgoingUBX->cls or outgoingUBX->id + // as these may have been changed by a PVT packet. + else if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED) && (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID)) + { + if (_printDebug == true) + { + printk("waitForACKResponse: no data and valid ACK after %lu msec\n", k_uptime_get_32() - startTime); + } + return (SFE_UBLOX_STATUS_DATA_SENT); //We got an ACK but no data... + } + + // If both the outgoingUBX->classAndIDmatch and packetAck.classAndIDmatch are VALID + // but the outgoingUBX->cls or ID no longer match then we can be confident that we had + // valid data but it has been or is currently being overwritten by another packet (e.g. PVT). + // If (e.g.) a PVT packet is _being_ received: outgoingUBX->valid will be NOT_DEFINED + // If (e.g.) a PVT packet _has been_ received: outgoingUBX->valid will be VALID (or just possibly NOT_VALID) + // So we cannot use outgoingUBX->valid as part of this check. + // Note: the addition of packetBuf should make this check redundant! + else if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && !((outgoingUBX->cls != requestedClass) || (outgoingUBX->id != requestedID))) + { + if (_printDebug == true) + { + printk("waitForACKResponse: data being OVERWRITTEN after %lu msec\n", k_uptime_get_32() - startTime); + } + return (SFE_UBLOX_STATUS_DATA_OVERWRITTEN); // Data was valid but has been or is being overwritten + } + + // If packetAck.classAndIDmatch is VALID but both outgoingUBX->valid and outgoingUBX->classAndIDmatch + // are NOT_VALID then we can be confident we have had a checksum failure on the data packet + else if ((packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID)) + { + if (_printDebug == true) + { + printk("waitForACKResponse: CRC failed after %lu msec\n", k_uptime_get_32() - startTime); + } + return (SFE_UBLOX_STATUS_CRC_FAIL); //Checksum fail + } + + // If our packet was not-acknowledged (NACK) we do not receive a data packet - we only get the NACK. + // So you would expect outgoingUBX->valid and outgoingUBX->classAndIDmatch to still be NOT_DEFINED + // But if a full PVT packet arrives afterwards outgoingUBX->valid could be VALID (or just possibly NOT_VALID) + // but outgoingUBX->cls and outgoingUBX->id would not match... + // So I think this is telling us we need a special state for packetAck.classAndIDmatch to tell us + // the packet was definitely NACK'd otherwise we are possibly just guessing... + // Note: the addition of packetBuf changes the logic of this, but we'll leave the code as is for now. + else if (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_NOTACKNOWLEDGED) + { + if (_printDebug == true) + { + printk("waitForACKResponse: data was NOTACKNOWLEDGED (NACK) after %lu msec\n", k_uptime_get_32() - startTime); + } + return (SFE_UBLOX_STATUS_COMMAND_NACK); //We received a NACK! + } + + // If the outgoingUBX->classAndIDmatch is VALID but the packetAck.classAndIDmatch is NOT_VALID + // then the ack probably had a checksum error. We will take a gamble and return DATA_RECEIVED. + // If we were playing safe, we should return FAIL instead + else if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID) && (outgoingUBX->cls == requestedClass) && (outgoingUBX->id == requestedID)) + { + if (_printDebug == true) + { + printk("waitForACKResponse: VALID data and INVALID ACK received after %lu msec\n", k_uptime_get_32() - startTime); + } + return (SFE_UBLOX_STATUS_DATA_RECEIVED); //We received valid data and an invalid ACK! + } + + // If the outgoingUBX->classAndIDmatch is NOT_VALID and the packetAck.classAndIDmatch is NOT_VALID + // then we return a FAIL. This must be a double checksum failure? + else if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID) && (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID)) + { + if (_printDebug == true) + { + printk("waitForACKResponse: INVALID data and INVALID ACK received after %lu msec\n", k_uptime_get_32() - startTime); + } + return (SFE_UBLOX_STATUS_FAIL); //We received invalid data and an invalid ACK! + } + + // If the outgoingUBX->classAndIDmatch is VALID and the packetAck.classAndIDmatch is NOT_DEFINED + // then the ACK has not yet been received and we should keep waiting for it + else if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED)) + { + if (_printDebug == true) + { + printk("waitForACKResponse: valid data after %lu msec. Waiting for ACK\n", k_uptime_get_32() - startTime); + } + } + + } //checkUbloxInternal == true + + k_usleep(500); + } //while (k_uptime_get_32() - startTime < maxTime) + + // We have timed out... + // If the outgoingUBX->classAndIDmatch is VALID then we can take a gamble and return DATA_RECEIVED + // even though we did not get an ACK + if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID) && (outgoingUBX->cls == requestedClass) && (outgoingUBX->id == requestedID)) + { + if (_printDebug == true) + { + printk("waitForACKResponse: TIMEOUT with valid data after %lu msec.\n", k_uptime_get_32() - startTime); + } + return (SFE_UBLOX_STATUS_DATA_RECEIVED); //We received valid data... But no ACK! + } + + if (_printDebug == true) + { + printk("waitForACKResponse: TIMEOUT after %lu msec.\n", k_uptime_get_32() - startTime); + } + + return (SFE_UBLOX_STATUS_TIMEOUT); +} + +//For non-CFG queries no ACK is sent so we use this function +//Returns SFE_UBLOX_STATUS_DATA_RECEIVED if we got a config packet full of response data that has CLS/ID match to our query packet +//Returns SFE_UBLOX_STATUS_CRC_FAIL if we got a corrupt config packet that has CLS/ID match to our query packet +//Returns SFE_UBLOX_STATUS_TIMEOUT if we timed out +//Returns SFE_UBLOX_STATUS_DATA_OVERWRITTEN if we got an a valid packetCfg but that the packetCfg has been +// or is currently being overwritten (remember that Serial data can arrive very slowly) +sfe_ublox_status_e SFE_UBLOX_GPS::waitForNoACKResponse(ubxPacket *outgoingUBX, uint8_t requestedClass, uint8_t requestedID, uint16_t maxTime) +{ + outgoingUBX->valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; //This will go VALID (or NOT_VALID) when we receive a response to the packet we sent + packetAck.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; + packetBuf.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; + outgoingUBX->classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; // This will go VALID (or NOT_VALID) when we receive a packet that matches the requested class and ID + packetAck.classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; + packetBuf.classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; + + unsigned long startTime = k_uptime_get_32(); + while (k_uptime_get_32() - startTime < maxTime) + { + if (checkUbloxInternal(outgoingUBX, requestedClass, requestedID) == true) //See if new data is available. Process bytes as they come in. + { + + // If outgoingUBX->classAndIDmatch is VALID + // and outgoingUBX->valid is _still_ VALID and the class and ID _still_ match + // then we can be confident that the data in outgoingUBX is valid + if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID) && (outgoingUBX->cls == requestedClass) && (outgoingUBX->id == requestedID)) + { + if (_printDebug == true) + { + printk("waitForNoACKResponse: valid data with CLS/ID match after %lu msec\n", k_uptime_get_32() - startTime); + } + return (SFE_UBLOX_STATUS_DATA_RECEIVED); //We received valid data! + } + + // If the outgoingUBX->classAndIDmatch is VALID + // but the outgoingUBX->cls or ID no longer match then we can be confident that we had + // valid data but it has been or is currently being overwritten by another packet (e.g. PVT). + // If (e.g.) a PVT packet is _being_ received: outgoingUBX->valid will be NOT_DEFINED + // If (e.g.) a PVT packet _has been_ received: outgoingUBX->valid will be VALID (or just possibly NOT_VALID) + // So we cannot use outgoingUBX->valid as part of this check. + // Note: the addition of packetBuf should make this check redundant! + else if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && !((outgoingUBX->cls != requestedClass) || (outgoingUBX->id != requestedID))) + { + if (_printDebug == true) + { + printk("waitForNoACKResponse: data being OVERWRITTEN after %lu msec\n", k_uptime_get_32() - startTime); + } + return (SFE_UBLOX_STATUS_DATA_OVERWRITTEN); // Data was valid but has been or is being overwritten + } + + // If outgoingUBX->classAndIDmatch is NOT_DEFINED + // and outgoingUBX->valid is VALID then this must be (e.g.) a PVT packet + else if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID)) + { + if (_printDebug == true) + { + printk("waitForNoACKResponse: valid but UNWANTED data after %lu msec. Class: %d ID: %d\n", k_uptime_get_32() - startTime, outgoingUBX->cls, outgoingUBX->id); + } + } + + // If the outgoingUBX->classAndIDmatch is NOT_VALID then we return CRC failure + else if (outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID) + { + if (_printDebug == true) + { + printk("waitForNoACKResponse: CLS/ID match but failed CRC after %lu msec\n", k_uptime_get_32() - startTime); + } + return (SFE_UBLOX_STATUS_CRC_FAIL); //We received invalid data + } + } + + k_usleep(500); + } + + if (_printDebug == true) + { + printk("waitForNoACKResponse: TIMEOUT after %lu msec. No packet received.\n", k_uptime_get_32() - startTime); + } + + return (SFE_UBLOX_STATUS_TIMEOUT); +} + +//Save current configuration to flash and BBR (battery backed RAM) +//This still works but it is the old way of configuring ublox modules. See getVal and setVal for the new methods +bool SFE_UBLOX_GPS::saveConfiguration(uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_CFG; + packetCfg.len = 12; + packetCfg.startingSpot = 0; + + //Clear packet payload + for (uint8_t x = 0; x < packetCfg.len; x++) + packetCfg.payload[x] = 0; + + packetCfg.payload[4] = 0xFF; //Set any bit in the saveMask field to save current config to Flash and BBR + packetCfg.payload[5] = 0xFF; + + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Save the selected configuration sub-sections to flash and BBR (battery backed RAM) +//This still works but it is the old way of configuring ublox modules. See getVal and setVal for the new methods +bool SFE_UBLOX_GPS::saveConfigSelective(uint32_t configMask, uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_CFG; + packetCfg.len = 12; + packetCfg.startingSpot = 0; + + //Clear packet payload + for (uint8_t x = 0; x < packetCfg.len; x++) + packetCfg.payload[x] = 0; + + packetCfg.payload[4] = configMask & 0xFF; //Set the appropriate bits in the saveMask field to save current config to Flash and BBR + packetCfg.payload[5] = (configMask >> 8) & 0xFF; + packetCfg.payload[6] = (configMask >> 16) & 0xFF; + packetCfg.payload[7] = (configMask >> 24) & 0xFF; + + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Reset module to factory defaults +//This still works but it is the old way of configuring ublox modules. See getVal and setVal for the new methods +bool SFE_UBLOX_GPS::factoryDefault(uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_CFG; + packetCfg.len = 12; + packetCfg.startingSpot = 0; + + //Clear packet payload + for (uint8_t x = 0; x < packetCfg.len; x++) + packetCfg.payload[x] = 0; + + packetCfg.payload[0] = 0xFF; //Set any bit in the clearMask field to clear saved config + packetCfg.payload[1] = 0xFF; + packetCfg.payload[8] = 0xFF; //Set any bit in the loadMask field to discard current config and rebuild from lower non-volatile memory layers + packetCfg.payload[9] = 0xFF; + + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Given a group, ID and size, return the value of this config spot +//The 32-bit key is put together from group/ID/size. See other getVal to send key directly. +//Configuration of modern Ublox modules is now done via getVal/setVal/delVal, ie protocol v27 and above found on ZED-F9P +uint8_t SFE_UBLOX_GPS::getVal8(uint16_t group, uint16_t id, uint8_t size, uint8_t layer, uint16_t maxWait) +{ + //Create key + uint32_t key = 0; + key |= (uint32_t)id; + key |= (uint32_t)group << 16; + key |= (uint32_t)size << 28; + + if (_printDebug == true) + { + printk("key: 0x%x\n", key); + } + + return getVal8(key, layer, maxWait); +} + +//Given a key, return its value +//This function takes a full 32-bit key +//Default layer is BBR +//Configuration of modern Ublox modules is now done via getVal/setVal/delVal, ie protocol v27 and above found on ZED-F9P +uint8_t SFE_UBLOX_GPS::getVal8(uint32_t key, uint8_t layer, uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_VALGET; + packetCfg.len = 4 + 4 * 1; //While multiple keys are allowed, we will send only one key at a time + packetCfg.startingSpot = 0; + + //Clear packet payload + for (uint8_t x = 0; x < packetCfg.len; x++) + packetCfg.payload[x] = 0; + + //VALGET uses different memory layer definitions to VALSET + //because it can only return the value for one layer. + //So we need to fiddle the layer here. + //And just to complicate things further, the ZED-F9P only responds + //correctly to layer 0 (RAM) and layer 7 (Default)! + uint8_t getLayer = 7; // 7 is the "Default Layer" + if ((layer & VAL_LAYER_RAM) == VAL_LAYER_RAM) // Did the user request the RAM layer? + { + getLayer = 0; // Layer 0 is RAM + } + + payloadCfg[0] = 0; //Message Version - set to 0 + payloadCfg[1] = getLayer; //Layer + + //Load key into outgoing payload + payloadCfg[4] = key >> 8 * 0; //Key LSB + payloadCfg[5] = key >> 8 * 1; + payloadCfg[6] = key >> 8 * 2; + payloadCfg[7] = key >> 8 * 3; + + if (_printDebug == true) + { + printk("key: 0x%x\n", key); + } + + //Send VALGET command with this key + + sfe_ublox_status_e retVal = sendCommand(&packetCfg, maxWait); + if (_printDebug == true) + { + printk("getVal8: sendCommand returned: %s\n", statusString(retVal)); + } + if (retVal != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK + return (0); //If command send fails then bail + + //Verify the response is the correct length as compared to what the user called (did the module respond with 8-bits but the user called getVal32?) + //Response is 8 bytes plus cfg data + //if(packet->len > 8+1) + + //Pull the requested value from the response + //Response starts at 4+1*N with the 32-bit key so the actual data we're looking for is at 8+1*N + return (extractByte(8)); +} + +//Given a key, set a 16-bit value +//This function takes a full 32-bit key +//Default layer is BBR +//Configuration of modern Ublox modules is now done via getVal/setVal/delVal, ie protocol v27 and above found on ZED-F9P +uint8_t SFE_UBLOX_GPS::setVal(uint32_t key, uint16_t value, uint8_t layer, uint16_t maxWait) +{ + return setVal16(key, value, layer, maxWait); +} + +//Given a key, set a 16-bit value +//This function takes a full 32-bit key +//Default layer is BBR +//Configuration of modern Ublox modules is now done via getVal/setVal/delVal, ie protocol v27 and above found on ZED-F9P +uint8_t SFE_UBLOX_GPS::setVal16(uint32_t key, uint16_t value, uint8_t layer, uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_VALSET; + packetCfg.len = 4 + 4 + 2; //4 byte header, 4 byte key ID, 2 bytes of value + packetCfg.startingSpot = 0; + + //Clear packet payload + for (uint16_t x = 0; x < packetCfg.len; x++) + packetCfg.payload[x] = 0; + + payloadCfg[0] = 0; //Message Version - set to 0 + payloadCfg[1] = layer; //By default we ask for the BBR layer + + //Load key into outgoing payload + payloadCfg[4] = key >> 8 * 0; //Key LSB + payloadCfg[5] = key >> 8 * 1; + payloadCfg[6] = key >> 8 * 2; + payloadCfg[7] = key >> 8 * 3; + + //Load user's value + payloadCfg[8] = value >> 8 * 0; //Value LSB + payloadCfg[9] = value >> 8 * 1; + + //Send VALSET command with this key and value + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Given a key, set an 8-bit value +//This function takes a full 32-bit key +//Default layer is BBR +//Configuration of modern Ublox modules is now done via getVal/setVal/delVal, ie protocol v27 and above found on ZED-F9P +uint8_t SFE_UBLOX_GPS::setVal8(uint32_t key, uint8_t value, uint8_t layer, uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_VALSET; + packetCfg.len = 4 + 4 + 1; //4 byte header, 4 byte key ID, 1 byte value + packetCfg.startingSpot = 0; + + //Clear packet payload + for (uint16_t x = 0; x < packetCfg.len; x++) + packetCfg.payload[x] = 0; + + payloadCfg[0] = 0; //Message Version - set to 0 + payloadCfg[1] = layer; //By default we ask for the BBR layer + + //Load key into outgoing payload + payloadCfg[4] = key >> 8 * 0; //Key LSB + payloadCfg[5] = key >> 8 * 1; + payloadCfg[6] = key >> 8 * 2; + payloadCfg[7] = key >> 8 * 3; + + //Load user's value + payloadCfg[8] = value; //Value + + //Send VALSET command with this key and value + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Given a key, set a 32-bit value +//This function takes a full 32-bit key +//Default layer is BBR +//Configuration of modern Ublox modules is now done via getVal/setVal/delVal, ie protocol v27 and above found on ZED-F9P +uint8_t SFE_UBLOX_GPS::setVal32(uint32_t key, uint32_t value, uint8_t layer, uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_VALSET; + packetCfg.len = 4 + 4 + 4; //4 byte header, 4 byte key ID, 4 bytes of value + packetCfg.startingSpot = 0; + + //Clear packet payload + for (uint16_t x = 0; x < packetCfg.len; x++) + packetCfg.payload[x] = 0; + + payloadCfg[0] = 0; //Message Version - set to 0 + payloadCfg[1] = layer; //By default we ask for the BBR layer + + //Load key into outgoing payload + payloadCfg[4] = key >> 8 * 0; //Key LSB + payloadCfg[5] = key >> 8 * 1; + payloadCfg[6] = key >> 8 * 2; + payloadCfg[7] = key >> 8 * 3; + + //Load user's value + payloadCfg[8] = value >> 8 * 0; //Value LSB + payloadCfg[9] = value >> 8 * 1; + payloadCfg[10] = value >> 8 * 2; + payloadCfg[11] = value >> 8 * 3; + + //Send VALSET command with this key and value + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Start defining a new UBX-CFG-VALSET ubxPacket +//This function takes a full 32-bit key and 32-bit value +//Default layer is BBR +//Configuration of modern Ublox modules is now done via getVal/setVal/delVal, ie protocol v27 and above found on ZED-F9P +uint8_t SFE_UBLOX_GPS::newCfgValset32(uint32_t key, uint32_t value, uint8_t layer) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_VALSET; + packetCfg.len = 4 + 4 + 4; //4 byte header, 4 byte key ID, 4 bytes of value + packetCfg.startingSpot = 0; + + //Clear packet payload + for (uint16_t x = 0; x < MAX_PAYLOAD_SIZE; x++) + packetCfg.payload[x] = 0; + + payloadCfg[0] = 0; //Message Version - set to 0 + payloadCfg[1] = layer; //By default we ask for the BBR layer + + //Load key into outgoing payload + payloadCfg[4] = key >> 8 * 0; //Key LSB + payloadCfg[5] = key >> 8 * 1; + payloadCfg[6] = key >> 8 * 2; + payloadCfg[7] = key >> 8 * 3; + + //Load user's value + payloadCfg[8] = value >> 8 * 0; //Value LSB + payloadCfg[9] = value >> 8 * 1; + payloadCfg[10] = value >> 8 * 2; + payloadCfg[11] = value >> 8 * 3; + + //All done + return (true); +} + +//Start defining a new UBX-CFG-VALSET ubxPacket +//This function takes a full 32-bit key and 16-bit value +//Default layer is BBR +//Configuration of modern Ublox modules is now done via getVal/setVal/delVal, ie protocol v27 and above found on ZED-F9P +uint8_t SFE_UBLOX_GPS::newCfgValset16(uint32_t key, uint16_t value, uint8_t layer) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_VALSET; + packetCfg.len = 4 + 4 + 2; //4 byte header, 4 byte key ID, 2 bytes of value + packetCfg.startingSpot = 0; + + //Clear packet payload + for (uint16_t x = 0; x < MAX_PAYLOAD_SIZE; x++) + packetCfg.payload[x] = 0; + + payloadCfg[0] = 0; //Message Version - set to 0 + payloadCfg[1] = layer; //By default we ask for the BBR layer + + //Load key into outgoing payload + payloadCfg[4] = key >> 8 * 0; //Key LSB + payloadCfg[5] = key >> 8 * 1; + payloadCfg[6] = key >> 8 * 2; + payloadCfg[7] = key >> 8 * 3; + + //Load user's value + payloadCfg[8] = value >> 8 * 0; //Value LSB + payloadCfg[9] = value >> 8 * 1; + + //All done + return (true); +} + +//Start defining a new UBX-CFG-VALSET ubxPacket +//This function takes a full 32-bit key and 8-bit value +//Default layer is BBR +//Configuration of modern Ublox modules is now done via getVal/setVal/delVal, ie protocol v27 and above found on ZED-F9P +uint8_t SFE_UBLOX_GPS::newCfgValset8(uint32_t key, uint8_t value, uint8_t layer) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_VALSET; + packetCfg.len = 4 + 4 + 1; //4 byte header, 4 byte key ID, 1 byte value + packetCfg.startingSpot = 0; + + //Clear packet payload + for (uint16_t x = 0; x < MAX_PAYLOAD_SIZE; x++) + packetCfg.payload[x] = 0; + + payloadCfg[0] = 0; //Message Version - set to 0 + payloadCfg[1] = layer; //By default we ask for the BBR layer + + //Load key into outgoing payload + payloadCfg[4] = key >> 8 * 0; //Key LSB + payloadCfg[5] = key >> 8 * 1; + payloadCfg[6] = key >> 8 * 2; + payloadCfg[7] = key >> 8 * 3; + + //Load user's value + payloadCfg[8] = value; //Value + + //All done + return (true); +} + +//Add another keyID and value to an existing UBX-CFG-VALSET ubxPacket +//This function takes a full 32-bit key and 32-bit value +uint8_t SFE_UBLOX_GPS::addCfgValset32(uint32_t key, uint32_t value) +{ + //Load key into outgoing payload + payloadCfg[packetCfg.len + 0] = key >> 8 * 0; //Key LSB + payloadCfg[packetCfg.len + 1] = key >> 8 * 1; + payloadCfg[packetCfg.len + 2] = key >> 8 * 2; + payloadCfg[packetCfg.len + 3] = key >> 8 * 3; + + //Load user's value + payloadCfg[packetCfg.len + 4] = value >> 8 * 0; //Value LSB + payloadCfg[packetCfg.len + 5] = value >> 8 * 1; + payloadCfg[packetCfg.len + 6] = value >> 8 * 2; + payloadCfg[packetCfg.len + 7] = value >> 8 * 3; + + //Update packet length: 4 byte key ID, 4 bytes of value + packetCfg.len = packetCfg.len + 4 + 4; + + //All done + return (true); +} + +//Add another keyID and value to an existing UBX-CFG-VALSET ubxPacket +//This function takes a full 32-bit key and 16-bit value +uint8_t SFE_UBLOX_GPS::addCfgValset16(uint32_t key, uint16_t value) +{ + //Load key into outgoing payload + payloadCfg[packetCfg.len + 0] = key >> 8 * 0; //Key LSB + payloadCfg[packetCfg.len + 1] = key >> 8 * 1; + payloadCfg[packetCfg.len + 2] = key >> 8 * 2; + payloadCfg[packetCfg.len + 3] = key >> 8 * 3; + + //Load user's value + payloadCfg[packetCfg.len + 4] = value >> 8 * 0; //Value LSB + payloadCfg[packetCfg.len + 5] = value >> 8 * 1; + + //Update packet length: 4 byte key ID, 2 bytes of value + packetCfg.len = packetCfg.len + 4 + 2; + + //All done + return (true); +} + +//Add another keyID and value to an existing UBX-CFG-VALSET ubxPacket +//This function takes a full 32-bit key and 8-bit value +uint8_t SFE_UBLOX_GPS::addCfgValset8(uint32_t key, uint8_t value) +{ + //Load key into outgoing payload + payloadCfg[packetCfg.len + 0] = key >> 8 * 0; //Key LSB + payloadCfg[packetCfg.len + 1] = key >> 8 * 1; + payloadCfg[packetCfg.len + 2] = key >> 8 * 2; + payloadCfg[packetCfg.len + 3] = key >> 8 * 3; + + //Load user's value + payloadCfg[packetCfg.len + 4] = value; //Value + + //Update packet length: 4 byte key ID, 1 byte value + packetCfg.len = packetCfg.len + 4 + 1; + + //All done + return (true); +} + +//Add a final keyID and value to an existing UBX-CFG-VALSET ubxPacket and send it +//This function takes a full 32-bit key and 32-bit value +uint8_t SFE_UBLOX_GPS::sendCfgValset32(uint32_t key, uint32_t value, uint16_t maxWait) +{ + //Load keyID and value into outgoing payload + addCfgValset32(key, value); + + //Send VALSET command with this key and value + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Add a final keyID and value to an existing UBX-CFG-VALSET ubxPacket and send it +//This function takes a full 32-bit key and 16-bit value +uint8_t SFE_UBLOX_GPS::sendCfgValset16(uint32_t key, uint16_t value, uint16_t maxWait) +{ + //Load keyID and value into outgoing payload + addCfgValset16(key, value); + + //Send VALSET command with this key and value + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Add a final keyID and value to an existing UBX-CFG-VALSET ubxPacket and send it +//This function takes a full 32-bit key and 8-bit value +uint8_t SFE_UBLOX_GPS::sendCfgValset8(uint32_t key, uint8_t value, uint16_t maxWait) +{ + //Load keyID and value into outgoing payload + addCfgValset8(key, value); + + //Send VALSET command with this key and value + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Get the current TimeMode3 settings - these contain survey in statuses +bool SFE_UBLOX_GPS::getSurveyMode(uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_TMODE3; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_RECEIVED); // We are expecting data and an ACK +} + +//Control Survey-In for NEO-M8P +bool SFE_UBLOX_GPS::setSurveyMode(uint8_t mode, uint16_t observationTime, float requiredAccuracy, uint16_t maxWait) +{ + if (getSurveyMode(maxWait) == false) //Ask module for the current TimeMode3 settings. Loads into payloadCfg. + return (false); + + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_TMODE3; + packetCfg.len = 40; + packetCfg.startingSpot = 0; + + //Clear packet payload + for (uint8_t x = 0; x < packetCfg.len; x++) + packetCfg.payload[x] = 0; + + //payloadCfg should be loaded with poll response. Now modify only the bits we care about + payloadCfg[2] = mode; //Set mode. Survey-In and Disabled are most common. Use ECEF (not LAT/LON/ALT). + + //svinMinDur is U4 (uint32_t) but we'll only use a uint16_t (waiting more than 65535 seconds seems excessive!) + payloadCfg[24] = observationTime & 0xFF; //svinMinDur in seconds + payloadCfg[25] = observationTime >> 8; //svinMinDur in seconds + payloadCfg[26] = 0; //Truncate to 16 bits + payloadCfg[27] = 0; //Truncate to 16 bits + + //svinAccLimit is U4 (uint32_t) in 0.1mm. + uint32_t svinAccLimit = (uint32_t)(requiredAccuracy * 10000.0); //Convert m to 0.1mm + payloadCfg[28] = svinAccLimit & 0xFF; //svinAccLimit in 0.1mm increments + payloadCfg[29] = svinAccLimit >> 8; + payloadCfg[30] = svinAccLimit >> 16; + payloadCfg[31] = svinAccLimit >> 24; + + return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Begin Survey-In for NEO-M8P +bool SFE_UBLOX_GPS::enableSurveyMode(uint16_t observationTime, float requiredAccuracy, uint16_t maxWait) +{ + return (setSurveyMode(SVIN_MODE_ENABLE, observationTime, requiredAccuracy, maxWait)); +} + +//Stop Survey-In for NEO-M8P +bool SFE_UBLOX_GPS::disableSurveyMode(uint16_t maxWait) +{ + return (setSurveyMode(SVIN_MODE_DISABLE, 0, 0, maxWait)); +} + +//Reads survey in status and sets the global variables +//for status, position valid, observation time, and mean 3D StdDev +//Returns true if commands was successful +bool SFE_UBLOX_GPS::getSurveyStatus(uint16_t maxWait) +{ + //Reset variables + svin.active = false; + svin.valid = false; + svin.observationTime = 0; + svin.meanAccuracy = 0; + + packetCfg.cls = UBX_CLASS_NAV; + packetCfg.id = UBX_NAV_SVIN; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + if ((sendCommand(&packetCfg, maxWait)) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK + return (false); //If command send fails then bail + + //We got a response, now parse the bits into the svin structure + + //dur (Passed survey-in observation time) is U4 (uint32_t) seconds. We truncate to 16 bits + //(waiting more than 65535 seconds (18.2 hours) seems excessive!) + uint32_t tmpObsTime = extractLong(8); + if (tmpObsTime <= 0xFFFF) + { + svin.observationTime = (uint16_t)tmpObsTime; + } + else + { + svin.observationTime = 0xFFFF; + } + + // meanAcc is U4 (uint32_t) in 0.1mm. We convert this to float. + uint32_t tempFloat = extractLong(28); + svin.meanAccuracy = ((float)tempFloat) / 10000.0; //Convert 0.1mm to m + + svin.valid = payloadCfg[36]; //1 if survey-in position is valid, 0 otherwise + svin.active = payloadCfg[37]; //1 if survey-in in progress, 0 otherwise + + return (true); +} + +//Loads the payloadCfg array with the current protocol bits located the UBX-CFG-PRT register for a given port +bool SFE_UBLOX_GPS::getPortSettings(uint8_t portID, uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_PRT; + packetCfg.len = 1; + packetCfg.startingSpot = 0; + + payloadCfg[0] = portID; + + return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_RECEIVED); // We are expecting data and an ACK +} + +//Configure a given port to output UBX, NMEA, RTCM3 or a combination thereof +//Port 0=I2c, 1=UART1, 2=UART2, 3=USB, 4=SPI +//Bit:0 = UBX, :1=NMEA, :5=RTCM3 +bool SFE_UBLOX_GPS::setPortOutput(uint8_t portID, uint8_t outStreamSettings, uint16_t maxWait) +{ + //Get the current config values for this port ID + if (getPortSettings(portID, maxWait) == false) + return (false); //Something went wrong. Bail. + + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_PRT; + packetCfg.len = 20; + packetCfg.startingSpot = 0; + + //payloadCfg is now loaded with current bytes. Change only the ones we need to + payloadCfg[14] = outStreamSettings; //OutProtocolMask LSB - Set outStream bits + + return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Configure a given port to input UBX, NMEA, RTCM3 or a combination thereof +//Port 0=I2c, 1=UART1, 2=UART2, 3=USB, 4=SPI +//Bit:0 = UBX, :1=NMEA, :5=RTCM3 +bool SFE_UBLOX_GPS::setPortInput(uint8_t portID, uint8_t inStreamSettings, uint16_t maxWait) +{ + //Get the current config values for this port ID + //This will load the payloadCfg array with current port settings + if (getPortSettings(portID, maxWait) == false) + return (false); //Something went wrong. Bail. + + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_PRT; + packetCfg.len = 20; + packetCfg.startingSpot = 0; + + //payloadCfg is now loaded with current bytes. Change only the ones we need to + payloadCfg[12] = inStreamSettings; //InProtocolMask LSB - Set inStream bits + + return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Configure a port to output UBX, NMEA, RTCM3 or a combination thereof +bool SFE_UBLOX_GPS::setI2COutput(uint8_t comSettings, uint16_t maxWait) +{ + return (setPortOutput(COM_PORT_I2C, comSettings, maxWait)); +} +bool SFE_UBLOX_GPS::setUART1Output(uint8_t comSettings, uint16_t maxWait) +{ + return (setPortOutput(COM_PORT_UART1, comSettings, maxWait)); +} +bool SFE_UBLOX_GPS::setUART2Output(uint8_t comSettings, uint16_t maxWait) +{ + return (setPortOutput(COM_PORT_UART2, comSettings, maxWait)); +} +bool SFE_UBLOX_GPS::setUSBOutput(uint8_t comSettings, uint16_t maxWait) +{ + return (setPortOutput(COM_PORT_USB, comSettings, maxWait)); +} +bool SFE_UBLOX_GPS::setSPIOutput(uint8_t comSettings, uint16_t maxWait) +{ + return (setPortOutput(COM_PORT_SPI, comSettings, maxWait)); +} + +//Set the rate at which the module will give us an updated navigation solution +//Expects a number that is the updates per second. For example 1 = 1Hz, 2 = 2Hz, etc. +//Max is 40Hz(?!) +bool SFE_UBLOX_GPS::setNavigationFrequency(uint8_t navFreq, uint16_t maxWait) +{ + //if(updateRate > 40) updateRate = 40; //Not needed: module will correct out of bounds values + + //Adjust the I2C polling timeout based on update rate + i2cPollingWait = 1000 / (navFreq * 4); //This is the number of ms to wait between checks for new I2C data + + //Query the module for the latest lat/long + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_RATE; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + //This will load the payloadCfg array with current settings of the given register + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK + return (false); //If command send fails then bail + + uint16_t measurementRate = 1000 / navFreq; + + //payloadCfg is now loaded with current bytes. Change only the ones we need to + payloadCfg[0] = measurementRate & 0xFF; //measRate LSB + payloadCfg[1] = measurementRate >> 8; //measRate MSB + + return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Get the rate at which the module is outputting nav solutions +uint8_t SFE_UBLOX_GPS::getNavigationFrequency(uint16_t maxWait) +{ + //Query the module for the latest lat/long + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_RATE; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + //This will load the payloadCfg array with current settings of the given register + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK + return (false); //If command send fails then bail + + uint16_t measurementRate = 0; + + //payloadCfg is now loaded with current bytes. Get what we need + measurementRate = extractInt(0); //Pull from payloadCfg at measRate LSB + + measurementRate = 1000 / measurementRate; //This may return an int when it's a float, but I'd rather not return 4 bytes + return (measurementRate); +} + +//In case no config access to the GPS is possible and PVT is send cyclically already +//set config to suitable parameters +bool SFE_UBLOX_GPS::assumeAutoPVT(bool enabled, bool implicitUpdate) +{ + bool changes = autoPVT != enabled || autoPVTImplicitUpdate != implicitUpdate; + if (changes) + { + autoPVT = enabled; + autoPVTImplicitUpdate = implicitUpdate; + } + return changes; +} + +//Enable or disable automatic navigation message generation by the GPS. This changes the way getPVT +//works. +bool SFE_UBLOX_GPS::setAutoPVT(bool enable, uint16_t maxWait) +{ + return setAutoPVT(enable, true, maxWait); +} + +//Enable or disable automatic navigation message generation by the GPS. This changes the way getPVT +//works. +bool SFE_UBLOX_GPS::setAutoPVT(bool enable, bool implicitUpdate, uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_MSG; + packetCfg.len = 3; + packetCfg.startingSpot = 0; + payloadCfg[0] = UBX_CLASS_NAV; + payloadCfg[1] = UBX_NAV_PVT; + payloadCfg[2] = enable ? 1 : 0; // rate relative to navigation freq. + + bool ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK + if (ok) + { + autoPVT = enable; + autoPVTImplicitUpdate = implicitUpdate; + } + moduleQueried.all = false; + return ok; +} + +//Configure a given message type for a given port (UART1, I2C, SPI, etc) +bool SFE_UBLOX_GPS::configureMessage(uint8_t msgClass, uint8_t msgID, uint8_t portID, uint8_t sendRate, uint16_t maxWait) +{ + //Poll for the current settings for a given message + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_MSG; + packetCfg.len = 2; + packetCfg.startingSpot = 0; + + payloadCfg[0] = msgClass; + payloadCfg[1] = msgID; + + //This will load the payloadCfg array with current settings of the given register + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK + return (false); //If command send fails then bail + + //Now send it back with new mods + packetCfg.len = 8; + + //payloadCfg is now loaded with current bytes. Change only the ones we need to + payloadCfg[2 + portID] = sendRate; //Send rate is relative to the event a message is registered on. For example, if the rate of a navigation message is set to 2, the message is sent every 2nd navigation solution. + + return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Enable a given message type, default of 1 per update rate (usually 1 per second) +bool SFE_UBLOX_GPS::enableMessage(uint8_t msgClass, uint8_t msgID, uint8_t portID, uint8_t rate, uint16_t maxWait) +{ + return (configureMessage(msgClass, msgID, portID, rate, maxWait)); +} +//Disable a given message type on a given port +bool SFE_UBLOX_GPS::disableMessage(uint8_t msgClass, uint8_t msgID, uint8_t portID, uint16_t maxWait) +{ + return (configureMessage(msgClass, msgID, portID, 0, maxWait)); +} + +bool SFE_UBLOX_GPS::enableNMEAMessage(uint8_t msgID, uint8_t portID, uint8_t rate, uint16_t maxWait) +{ + return (configureMessage(UBX_CLASS_NMEA, msgID, portID, rate, maxWait)); +} +bool SFE_UBLOX_GPS::disableNMEAMessage(uint8_t msgID, uint8_t portID, uint16_t maxWait) +{ + return (enableNMEAMessage(msgID, portID, 0, maxWait)); +} + +//Given a message number turns on a message ID for output over a given portID (UART, I2C, SPI, USB, etc) +//To disable a message, set secondsBetween messages to 0 +//Note: This function will return false if the message is already enabled +//For base station RTK output we need to enable various sentences + +//NEO-M8P has four: +//1005 = 0xF5 0x05 - Stationary RTK reference ARP +//1077 = 0xF5 0x4D - GPS MSM7 +//1087 = 0xF5 0x57 - GLONASS MSM7 +//1230 = 0xF5 0xE6 - GLONASS code-phase biases, set to once every 10 seconds + +//ZED-F9P has six: +//1005, 1074, 1084, 1094, 1124, 1230 + +//Much of this configuration is not documented and instead discerned from u-center binary console +bool SFE_UBLOX_GPS::enableRTCMmessage(uint8_t messageNumber, uint8_t portID, uint8_t sendRate, uint16_t maxWait) +{ + return (configureMessage(UBX_RTCM_MSB, messageNumber, portID, sendRate, maxWait)); +} + +//Disable a given message on a given port by setting secondsBetweenMessages to zero +bool SFE_UBLOX_GPS::disableRTCMmessage(uint8_t messageNumber, uint8_t portID, uint16_t maxWait) +{ + return (enableRTCMmessage(messageNumber, portID, 0, maxWait)); +} + +//Add a new geofence using UBX-CFG-GEOFENCE +bool SFE_UBLOX_GPS::addGeofence(int32_t latitude, int32_t longitude, uint32_t radius, uint8_t confidence, uint8_t pinPolarity, uint8_t pin, uint16_t maxWait) +{ + if (currentGeofenceParams.numFences >= 4) + return (false); // Quit if we already have four geofences defined + + // Store the new geofence parameters + currentGeofenceParams.lats[currentGeofenceParams.numFences] = latitude; + currentGeofenceParams.longs[currentGeofenceParams.numFences] = longitude; + currentGeofenceParams.rads[currentGeofenceParams.numFences] = radius; + currentGeofenceParams.numFences = currentGeofenceParams.numFences + 1; // Increment the number of fences + + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_GEOFENCE; + packetCfg.len = (currentGeofenceParams.numFences * 12) + 8; + packetCfg.startingSpot = 0; + + payloadCfg[0] = 0; // Message version = 0x00 + payloadCfg[1] = currentGeofenceParams.numFences; // numFences + payloadCfg[2] = confidence; // confLvl = Confidence level 0-4 (none, 68%, 95%, 99.7%, 99.99%) + payloadCfg[3] = 0; // reserved1 + if (pin > 0) + { + payloadCfg[4] = 1; // enable PIO combined fence state + } + else + { + payloadCfg[4] = 0; // disable PIO combined fence state + } + payloadCfg[5] = pinPolarity; // PIO pin polarity (0 = low means inside, 1 = low means outside (or unknown)) + payloadCfg[6] = pin; // PIO pin + payloadCfg[7] = 0; //reserved2 + payloadCfg[8] = currentGeofenceParams.lats[0] & 0xFF; + payloadCfg[9] = currentGeofenceParams.lats[0] >> 8; + payloadCfg[10] = currentGeofenceParams.lats[0] >> 16; + payloadCfg[11] = currentGeofenceParams.lats[0] >> 24; + payloadCfg[12] = currentGeofenceParams.longs[0] & 0xFF; + payloadCfg[13] = currentGeofenceParams.longs[0] >> 8; + payloadCfg[14] = currentGeofenceParams.longs[0] >> 16; + payloadCfg[15] = currentGeofenceParams.longs[0] >> 24; + payloadCfg[16] = currentGeofenceParams.rads[0] & 0xFF; + payloadCfg[17] = currentGeofenceParams.rads[0] >> 8; + payloadCfg[18] = currentGeofenceParams.rads[0] >> 16; + payloadCfg[19] = currentGeofenceParams.rads[0] >> 24; + if (currentGeofenceParams.numFences >= 2) + { + payloadCfg[20] = currentGeofenceParams.lats[1] & 0xFF; + payloadCfg[21] = currentGeofenceParams.lats[1] >> 8; + payloadCfg[22] = currentGeofenceParams.lats[1] >> 16; + payloadCfg[23] = currentGeofenceParams.lats[1] >> 24; + payloadCfg[24] = currentGeofenceParams.longs[1] & 0xFF; + payloadCfg[25] = currentGeofenceParams.longs[1] >> 8; + payloadCfg[26] = currentGeofenceParams.longs[1] >> 16; + payloadCfg[27] = currentGeofenceParams.longs[1] >> 24; + payloadCfg[28] = currentGeofenceParams.rads[1] & 0xFF; + payloadCfg[29] = currentGeofenceParams.rads[1] >> 8; + payloadCfg[30] = currentGeofenceParams.rads[1] >> 16; + payloadCfg[31] = currentGeofenceParams.rads[1] >> 24; + } + if (currentGeofenceParams.numFences >= 3) + { + payloadCfg[32] = currentGeofenceParams.lats[2] & 0xFF; + payloadCfg[33] = currentGeofenceParams.lats[2] >> 8; + payloadCfg[34] = currentGeofenceParams.lats[2] >> 16; + payloadCfg[35] = currentGeofenceParams.lats[2] >> 24; + payloadCfg[36] = currentGeofenceParams.longs[2] & 0xFF; + payloadCfg[37] = currentGeofenceParams.longs[2] >> 8; + payloadCfg[38] = currentGeofenceParams.longs[2] >> 16; + payloadCfg[39] = currentGeofenceParams.longs[2] >> 24; + payloadCfg[40] = currentGeofenceParams.rads[2] & 0xFF; + payloadCfg[41] = currentGeofenceParams.rads[2] >> 8; + payloadCfg[42] = currentGeofenceParams.rads[2] >> 16; + payloadCfg[43] = currentGeofenceParams.rads[2] >> 24; + } + if (currentGeofenceParams.numFences >= 4) + { + payloadCfg[44] = currentGeofenceParams.lats[3] & 0xFF; + payloadCfg[45] = currentGeofenceParams.lats[3] >> 8; + payloadCfg[46] = currentGeofenceParams.lats[3] >> 16; + payloadCfg[47] = currentGeofenceParams.lats[3] >> 24; + payloadCfg[48] = currentGeofenceParams.longs[3] & 0xFF; + payloadCfg[49] = currentGeofenceParams.longs[3] >> 8; + payloadCfg[50] = currentGeofenceParams.longs[3] >> 16; + payloadCfg[51] = currentGeofenceParams.longs[3] >> 24; + payloadCfg[52] = currentGeofenceParams.rads[3] & 0xFF; + payloadCfg[53] = currentGeofenceParams.rads[3] >> 8; + payloadCfg[54] = currentGeofenceParams.rads[3] >> 16; + payloadCfg[55] = currentGeofenceParams.rads[3] >> 24; + } + return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Clear all geofences using UBX-CFG-GEOFENCE +bool SFE_UBLOX_GPS::clearGeofences(uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_GEOFENCE; + packetCfg.len = 8; + packetCfg.startingSpot = 0; + + payloadCfg[0] = 0; // Message version = 0x00 + payloadCfg[1] = 0; // numFences + payloadCfg[2] = 0; // confLvl + payloadCfg[3] = 0; // reserved1 + payloadCfg[4] = 0; // disable PIO combined fence state + payloadCfg[5] = 0; // PIO pin polarity (0 = low means inside, 1 = low means outside (or unknown)) + payloadCfg[6] = 0; // PIO pin + payloadCfg[7] = 0; //reserved2 + + currentGeofenceParams.numFences = 0; // Zero the number of geofences currently in use + + return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Clear the antenna control settings using UBX-CFG-ANT +//This function is hopefully redundant but may be needed to release +//any PIO pins pre-allocated for antenna functions +bool SFE_UBLOX_GPS::clearAntPIO(uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_ANT; + packetCfg.len = 4; + packetCfg.startingSpot = 0; + + payloadCfg[0] = 0x10; // Antenna flag mask: set the recovery bit + payloadCfg[1] = 0; + payloadCfg[2] = 0xFF; // Antenna pin configuration: set pinSwitch and pinSCD to 31 + payloadCfg[3] = 0xFF; // Antenna pin configuration: set pinOCD to 31, set reconfig bit + + return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Returns the combined geofence state using UBX-NAV-GEOFENCE +bool SFE_UBLOX_GPS::getGeofenceState(geofenceState ¤tGeofenceState, uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_NAV; + packetCfg.id = UBX_NAV_GEOFENCE; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + //Ask module for the geofence status. Loads into payloadCfg. + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK + return (false); + + currentGeofenceState.status = payloadCfg[5]; // Extract the status + currentGeofenceState.numFences = payloadCfg[6]; // Extract the number of geofences + currentGeofenceState.combState = payloadCfg[7]; // Extract the combined state of all geofences + if (currentGeofenceState.numFences > 0) + currentGeofenceState.states[0] = payloadCfg[8]; // Extract geofence 1 state + if (currentGeofenceState.numFences > 1) + currentGeofenceState.states[1] = payloadCfg[10]; // Extract geofence 2 state + if (currentGeofenceState.numFences > 2) + currentGeofenceState.states[2] = payloadCfg[12]; // Extract geofence 3 state + if (currentGeofenceState.numFences > 3) + currentGeofenceState.states[3] = payloadCfg[14]; // Extract geofence 4 state + + return (true); +} + +//Power Save Mode +//Enables/Disables Low Power Mode using UBX-CFG-RXM +bool SFE_UBLOX_GPS::powerSaveMode(bool power_save, uint16_t maxWait) +{ + // Let's begin by checking the Protocol Version as UBX_CFG_RXM is not supported on the ZED (protocol >= 27) + uint8_t protVer = getProtocolVersionHigh(maxWait); + /* + if (_printDebug == true) + { + printk"(Protocol version is %d\n", protVer); + } + */ + if (protVer >= 27) + { + if (_printDebug == true) + { + printk("powerSaveMode (UBX-CFG-RXM) is not supported by this protocol version\n"); + } + return (false); + } + + // Now let's change the power setting using UBX-CFG-RXM + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_RXM; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + //Ask module for the current power management settings. Loads into payloadCfg. + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK + return (false); + + if (power_save) + { + payloadCfg[1] = 1; // Power Save Mode + } + else + { + payloadCfg[1] = 0; // Continuous Mode + } + + packetCfg.len = 2; + packetCfg.startingSpot = 0; + + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +// Get Power Save Mode +// Returns the current Low Power Mode using UBX-CFG-RXM +// Returns 255 if the sendCommand fails +uint8_t SFE_UBLOX_GPS::getPowerSaveMode(uint16_t maxWait) +{ + // Let's begin by checking the Protocol Version as UBX_CFG_RXM is not supported on the ZED (protocol >= 27) + uint8_t protVer = getProtocolVersionHigh(maxWait); + /* + if (_printDebug == true) + { + printk("Protocol version is %d\n", protVer); + } + */ + if (protVer >= 27) + { + if (_printDebug == true) + { + printk("powerSaveMode (UBX-CFG-RXM) is not supported by this protocol version\n"); + } + return (255); + } + + // Now let's read the power setting using UBX-CFG-RXM + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_RXM; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + //Ask module for the current power management settings. Loads into payloadCfg. + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK + return (255); + + return (payloadCfg[1]); // Return the low power mode +} + +// Powers off the GPS device for a given duration to reduce power consumption. +// NOTE: Querying the device before the duration is complete, for example by "getLatitude()" will wake it up! +// Returns true if command has not been not acknowledged. +// Returns false if command has not been acknowledged or maxWait = 0. +bool SFE_UBLOX_GPS::powerOff(uint32_t durationInMs, uint16_t maxWait) +{ + // use durationInMs = 0 for infinite duration + if (_printDebug == true) + { + printk("Powering off for %d ms\n", durationInMs); + } + + // Power off device using UBX-RXM-PMREQ + packetCfg.cls = UBX_CLASS_RXM; // 0x02 + packetCfg.id = UBX_RXM_PMREQ; // 0x41 + packetCfg.len = 8; + packetCfg.startingSpot = 0; + + // duration + // big endian to little endian, switch byte order + payloadCfg[0] = (durationInMs >> (8 * 0)) & 0xff; + payloadCfg[1] = (durationInMs >> (8 * 1)) & 0xff; + payloadCfg[2] = (durationInMs >> (8 * 2)) & 0xff; + payloadCfg[3] = (durationInMs >> (8 * 3)) & 0xff; + + payloadCfg[4] = 0x02; //Flags : set the backup bit + payloadCfg[5] = 0x00; //Flags + payloadCfg[6] = 0x00; //Flags + payloadCfg[7] = 0x00; //Flags + + if (maxWait != 0) + { + // check for "not acknowledged" command + return (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_COMMAND_NACK); + } + else + { + sendCommand(&packetCfg, maxWait); + return false; // can't tell if command not acknowledged if maxWait = 0 + } +} + +// Powers off the GPS device for a given duration to reduce power consumption. +// While powered off it can be woken up by creating a falling or rising voltage edge on the specified pin. +// NOTE: The GPS seems to be sensitve to signals on the pins while powered off. Works best when Microcontroller is in deepsleep. +// NOTE: Querying the device before the duration is complete, for example by "getLatitude()" will wake it up! +// Returns true if command has not been not acknowledged. +// Returns false if command has not been acknowledged or maxWait = 0. +bool SFE_UBLOX_GPS::powerOffWithInterrupt(uint32_t durationInMs, uint32_t wakeupSources, bool forceWhileUsb, uint16_t maxWait) +{ + // use durationInMs = 0 for infinite duration + if (_printDebug == true) + { + printk("Powering off for %d ms\n", durationInMs); + } + + // Power off device using UBX-RXM-PMREQ + packetCfg.cls = UBX_CLASS_RXM; // 0x02 + packetCfg.id = UBX_RXM_PMREQ; // 0x41 + packetCfg.len = 16; + packetCfg.startingSpot = 0; + + payloadCfg[0] = 0x00; // message version + + // bytes 1-3 are reserved - and must be set to zero + payloadCfg[1] = 0x00; + payloadCfg[2] = 0x00; + payloadCfg[3] = 0x00; + + // duration + // big endian to little endian, switch byte order + payloadCfg[4] = (durationInMs >> (8 * 0)) & 0xff; + payloadCfg[5] = (durationInMs >> (8 * 1)) & 0xff; + payloadCfg[6] = (durationInMs >> (8 * 2)) & 0xff; + payloadCfg[7] = (durationInMs >> (8 * 3)) & 0xff; + + // flags + + // disables USB interface when powering off, defaults to true + if (forceWhileUsb) + { + payloadCfg[8] = 0x06; // force | backup + } + else + { + payloadCfg[8] = 0x02; // backup only (leave the force bit clear - module will stay on if USB is connected) + } + + payloadCfg[9] = 0x00; + payloadCfg[10] = 0x00; + payloadCfg[11] = 0x00; + + // wakeUpSources + + // wakeupPin mapping, defaults to VAL_RXM_PMREQ_WAKEUPSOURCE_EXTINT0 + + // Possible values are: + // VAL_RXM_PMREQ_WAKEUPSOURCE_UARTRX + // VAL_RXM_PMREQ_WAKEUPSOURCE_EXTINT0 + // VAL_RXM_PMREQ_WAKEUPSOURCE_EXTINT1 + // VAL_RXM_PMREQ_WAKEUPSOURCE_SPICS + + payloadCfg[12] = (wakeupSources >> (8 * 0)) & 0xff; + payloadCfg[13] = (wakeupSources >> (8 * 1)) & 0xff; + payloadCfg[14] = (wakeupSources >> (8 * 2)) & 0xff; + payloadCfg[15] = (wakeupSources >> (8 * 3)) & 0xff; + + if (maxWait != 0) + { + // check for "not acknowledged" command + return (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_COMMAND_NACK); + } + else + { + sendCommand(&packetCfg, maxWait); + return false; // can't tell if command not acknowledged if maxWait = 0 + } +} + +//Change the dynamic platform model using UBX-CFG-NAV5 +//Possible values are: +//PORTABLE,STATIONARY,PEDESTRIAN,AUTOMOTIVE,SEA, +//AIRBORNE1g,AIRBORNE2g,AIRBORNE4g,WRIST,BIKE +//WRIST is not supported in protocol versions less than 18 +//BIKE is supported in protocol versions 19.2 +bool SFE_UBLOX_GPS::setDynamicModel(dynModel newDynamicModel, uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_NAV5; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + //Ask module for the current navigation model settings. Loads into payloadCfg. + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK + return (false); + + payloadCfg[0] = 0x01; // mask: set only the dyn bit (0) + payloadCfg[1] = 0x00; // mask + payloadCfg[2] = newDynamicModel; // dynModel + + packetCfg.len = 36; + packetCfg.startingSpot = 0; + + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + +//Get the dynamic platform model using UBX-CFG-NAV5 +//Returns 255 if the sendCommand fails +uint8_t SFE_UBLOX_GPS::getDynamicModel(uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_NAV5; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + //Ask module for the current navigation model settings. Loads into payloadCfg. + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK + return (255); + + return (payloadCfg[2]); // Return the dynamic model +} + +//Given a spot in the payload array, extract four bytes and build a long +uint32_t SFE_UBLOX_GPS::extractLong(uint8_t spotToStart) +{ + uint32_t val = 0; + val |= (uint32_t)payloadCfg[spotToStart + 0] << 8 * 0; + val |= (uint32_t)payloadCfg[spotToStart + 1] << 8 * 1; + val |= (uint32_t)payloadCfg[spotToStart + 2] << 8 * 2; + val |= (uint32_t)payloadCfg[spotToStart + 3] << 8 * 3; + return (val); +} + +//Given a spot in the payload array, extract two bytes and build an int +uint16_t SFE_UBLOX_GPS::extractInt(uint8_t spotToStart) +{ + uint16_t val = 0; + val |= (uint16_t)payloadCfg[spotToStart + 0] << 8 * 0; + val |= (uint16_t)payloadCfg[spotToStart + 1] << 8 * 1; + return (val); +} + +//Given a spot, extract a byte from the payload +uint8_t SFE_UBLOX_GPS::extractByte(uint8_t spotToStart) +{ + return (payloadCfg[spotToStart]); +} + +//Given a spot, extract a signed 8-bit value from the payload +int8_t SFE_UBLOX_GPS::extractSignedChar(uint8_t spotToStart) +{ + return ((int8_t)payloadCfg[spotToStart]); +} + +//Get the current year +uint16_t SFE_UBLOX_GPS::getYear(uint16_t maxWait) +{ + if (moduleQueried.gpsYear == false) + getPVT(maxWait); + moduleQueried.gpsYear = false; //Since we are about to give this to user, mark this data as stale + return (gpsYear); +} + +//Get the current month +uint8_t SFE_UBLOX_GPS::getMonth(uint16_t maxWait) +{ + if (moduleQueried.gpsMonth == false) + getPVT(maxWait); + moduleQueried.gpsMonth = false; //Since we are about to give this to user, mark this data as stale + return (gpsMonth); +} + +//Get the current day +uint8_t SFE_UBLOX_GPS::getDay(uint16_t maxWait) +{ + if (moduleQueried.gpsDay == false) + getPVT(maxWait); + moduleQueried.gpsDay = false; //Since we are about to give this to user, mark this data as stale + return (gpsDay); +} + +//Get the current hour +uint8_t SFE_UBLOX_GPS::getHour(uint16_t maxWait) +{ + if (moduleQueried.gpsHour == false) + getPVT(maxWait); + moduleQueried.gpsHour = false; //Since we are about to give this to user, mark this data as stale + return (gpsHour); +} + +//Get the current minute +uint8_t SFE_UBLOX_GPS::getMinute(uint16_t maxWait) +{ + if (moduleQueried.gpsMinute == false) + getPVT(maxWait); + moduleQueried.gpsMinute = false; //Since we are about to give this to user, mark this data as stale + return (gpsMinute); +} + +//Get the current second +uint8_t SFE_UBLOX_GPS::getSecond(uint16_t maxWait) +{ + if (moduleQueried.gpsSecond == false) + getPVT(maxWait); + moduleQueried.gpsSecond = false; //Since we are about to give this to user, mark this data as stale + return (gpsSecond); +} + +//Get the current date validity +bool SFE_UBLOX_GPS::getDateValid(uint16_t maxWait) +{ + if (moduleQueried.gpsDateValid == false) + getPVT(maxWait); + moduleQueried.gpsDateValid = false; //Since we are about to give this to user, mark this data as stale + return (gpsDateValid); +} + +//Get the current time validity +bool SFE_UBLOX_GPS::getTimeValid(uint16_t maxWait) +{ + if (moduleQueried.gpsTimeValid == false) + getPVT(maxWait); + moduleQueried.gpsTimeValid = false; //Since we are about to give this to user, mark this data as stale + return (gpsTimeValid); +} + +//Get the current millisecond +uint16_t SFE_UBLOX_GPS::getMillisecond(uint16_t maxWait) +{ + if (moduleQueried.gpsiTOW == false) + getPVT(maxWait); + moduleQueried.gpsiTOW = false; //Since we are about to give this to user, mark this data as stale + return (gpsMillisecond); +} + +//Get the current nanoseconds - includes milliseconds +int32_t SFE_UBLOX_GPS::getNanosecond(uint16_t maxWait) +{ + if (moduleQueried.gpsNanosecond == false) + getPVT(maxWait); + moduleQueried.gpsNanosecond = false; //Since we are about to give this to user, mark this data as stale + return (gpsNanosecond); +} + +//Get the latest Position/Velocity/Time solution and fill all global variables +bool SFE_UBLOX_GPS::getPVT(uint16_t maxWait) +{ + if (autoPVT && autoPVTImplicitUpdate) + { + //The GPS is automatically reporting, we just check whether we got unread data + if (_printDebug == true) + { + printk("getPVT: Autoreporting\n"); + } + checkUbloxInternal(&packetCfg, UBX_CLASS_NAV, UBX_NAV_PVT); + return moduleQueried.all; + } + else if (autoPVT && !autoPVTImplicitUpdate) + { + //Someone else has to call checkUblox for us... + if (_printDebug == true) + { + printk("getPVT: Exit immediately\n"); + } + return (false); + } + else + { + if (_printDebug == true) + { + printk("getPVT: Polling\n"); + } + + //The GPS is not automatically reporting navigation position so we have to poll explicitly + packetCfg.cls = UBX_CLASS_NAV; + packetCfg.id = UBX_NAV_PVT; + packetCfg.len = 0; + //packetCfg.startingSpot = 20; //Begin listening at spot 20 so we can record up to 20+MAX_PAYLOAD_SIZE = 84 bytes Note:now hard-coded in processUBX + + //The data is parsed as part of processing the response + sfe_ublox_status_e retVal = sendCommand(&packetCfg, maxWait); + + if (retVal == SFE_UBLOX_STATUS_DATA_RECEIVED) + return (true); + + if (_printDebug == true) + { + printk("getPVT retVal: %s\n", statusString(retVal)); + } + return (false); + } +} + +uint32_t SFE_UBLOX_GPS::getTimeOfWeek(uint16_t maxWait /* = 250*/) +{ + if (moduleQueried.gpsiTOW == false) + getPVT(maxWait); + moduleQueried.gpsiTOW = false; //Since we are about to give this to user, mark this data as stale + return (timeOfWeek); +} + +int32_t SFE_UBLOX_GPS::getHighResLatitude(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.highResLatitude == false) + getHPPOSLLH(maxWait); + highResModuleQueried.highResLatitude = false; //Since we are about to give this to user, mark this data as stale + return (highResLatitude); +} + +int8_t SFE_UBLOX_GPS::getHighResLatitudeHp(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.highResLatitudeHp == false) + getHPPOSLLH(maxWait); + highResModuleQueried.highResLatitudeHp = false; //Since we are about to give this to user, mark this data as stale + return (highResLatitudeHp); +} + +int32_t SFE_UBLOX_GPS::getHighResLongitude(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.highResLongitude == false) + getHPPOSLLH(maxWait); + highResModuleQueried.highResLongitude = false; //Since we are about to give this to user, mark this data as stale + return (highResLongitude); +} + +int8_t SFE_UBLOX_GPS::getHighResLongitudeHp(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.highResLongitudeHp == false) + getHPPOSLLH(maxWait); + highResModuleQueried.highResLongitudeHp = false; //Since we are about to give this to user, mark this data as stale + return (highResLongitudeHp); +} + +int32_t SFE_UBLOX_GPS::getElipsoid(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.elipsoid == false) + getHPPOSLLH(maxWait); + highResModuleQueried.elipsoid = false; //Since we are about to give this to user, mark this data as stale + return (elipsoid); +} + +int8_t SFE_UBLOX_GPS::getElipsoidHp(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.elipsoidHp == false) + getHPPOSLLH(maxWait); + highResModuleQueried.elipsoidHp = false; //Since we are about to give this to user, mark this data as stale + return (elipsoidHp); +} + +int32_t SFE_UBLOX_GPS::getMeanSeaLevel(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.meanSeaLevel == false) + getHPPOSLLH(maxWait); + highResModuleQueried.meanSeaLevel = false; //Since we are about to give this to user, mark this data as stale + return (meanSeaLevel); +} + +int8_t SFE_UBLOX_GPS::getMeanSeaLevelHp(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.meanSeaLevelHp == false) + getHPPOSLLH(maxWait); + highResModuleQueried.meanSeaLevelHp = false; //Since we are about to give this to user, mark this data as stale + return (meanSeaLevelHp); +} + +// getGeoidSeparation is currently redundant. The geoid separation seems to only be provided in NMEA GGA and GNS messages. +int32_t SFE_UBLOX_GPS::getGeoidSeparation(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.geoidSeparation == false) + getHPPOSLLH(maxWait); + highResModuleQueried.geoidSeparation = false; //Since we are about to give this to user, mark this data as stale + return (geoidSeparation); +} + +uint32_t SFE_UBLOX_GPS::getHorizontalAccuracy(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.horizontalAccuracy == false) + getHPPOSLLH(maxWait); + highResModuleQueried.horizontalAccuracy = false; //Since we are about to give this to user, mark this data as stale + return (horizontalAccuracy); +} + +uint32_t SFE_UBLOX_GPS::getVerticalAccuracy(uint16_t maxWait /* = 250*/) +{ + if (highResModuleQueried.verticalAccuracy == false) + getHPPOSLLH(maxWait); + highResModuleQueried.verticalAccuracy = false; //Since we are about to give this to user, mark this data as stale + return (verticalAccuracy); +} + +bool SFE_UBLOX_GPS::getHPPOSLLH(uint16_t maxWait) +{ + //The GPS is not automatically reporting navigation position so we have to poll explicitly + packetCfg.cls = UBX_CLASS_NAV; + packetCfg.id = UBX_NAV_HPPOSLLH; + packetCfg.len = 0; + + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_RECEIVED); // We are only expecting data (no ACK) +} + +//Get the current 3D high precision positional accuracy - a fun thing to watch +//Returns a long representing the 3D accuracy in millimeters +uint32_t SFE_UBLOX_GPS::getPositionAccuracy(uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_NAV; + packetCfg.id = UBX_NAV_HPPOSECEF; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are only expecting data (no ACK) + return (0); //If command send fails then bail + + uint32_t tempAccuracy = extractLong(24); //We got a response, now extract a long beginning at a given position + + if ((tempAccuracy % 10) >= 5) + tempAccuracy += 5; //Round fraction of mm up to next mm if .5 or above + tempAccuracy /= 10; //Convert 0.1mm units to mm + + return (tempAccuracy); +} + +//Get the current latitude in degrees +//Returns a long representing the number of degrees *10^-7 +int32_t SFE_UBLOX_GPS::getLatitude(uint16_t maxWait) +{ + if (moduleQueried.latitude == false) + getPVT(maxWait); + moduleQueried.latitude = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (latitude); +} + +//Get the current longitude in degrees +//Returns a long representing the number of degrees *10^-7 +int32_t SFE_UBLOX_GPS::getLongitude(uint16_t maxWait) +{ + if (moduleQueried.longitude == false) + getPVT(maxWait); + moduleQueried.longitude = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (longitude); +} + +//Get the current altitude in mm according to ellipsoid model +int32_t SFE_UBLOX_GPS::getAltitude(uint16_t maxWait) +{ + if (moduleQueried.altitude == false) + getPVT(maxWait); + moduleQueried.altitude = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (altitude); +} + +//Get the current altitude in mm according to mean sea level +//Ellipsoid model: https://www.esri.com/news/arcuser/0703/geoid1of3.html +//Difference between Ellipsoid Model and Mean Sea Level: https://eos-gnss.com/elevation-for-beginners/ +int32_t SFE_UBLOX_GPS::getAltitudeMSL(uint16_t maxWait) +{ + if (moduleQueried.altitudeMSL == false) + getPVT(maxWait); + moduleQueried.altitudeMSL = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (altitudeMSL); +} + +//Get the number of satellites used in fix +uint8_t SFE_UBLOX_GPS::getSIV(uint16_t maxWait) +{ + if (moduleQueried.SIV == false) + getPVT(maxWait); + moduleQueried.SIV = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (SIV); +} + +//Get the current fix type +//0=no fix, 1=dead reckoning, 2=2D, 3=3D, 4=GNSS, 5=Time fix +uint8_t SFE_UBLOX_GPS::getFixType(uint16_t maxWait) +{ + if (moduleQueried.fixType == false) + { + getPVT(maxWait); + } + moduleQueried.fixType = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (fixType); +} + +//Get the carrier phase range solution status +//Useful when querying module to see if it has high-precision RTK fix +//0=No solution, 1=Float solution, 2=Fixed solution +uint8_t SFE_UBLOX_GPS::getCarrierSolutionType(uint16_t maxWait) +{ + if (moduleQueried.carrierSolution == false) + getPVT(maxWait); + moduleQueried.carrierSolution = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (carrierSolution); +} + +//Get the ground speed in mm/s +int32_t SFE_UBLOX_GPS::getGroundSpeed(uint16_t maxWait) +{ + if (moduleQueried.groundSpeed == false) + getPVT(maxWait); + moduleQueried.groundSpeed = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (groundSpeed); +} + +//Get the heading of motion (as opposed to heading of car) in degrees * 10^-5 +int32_t SFE_UBLOX_GPS::getHeading(uint16_t maxWait) +{ + if (moduleQueried.headingOfMotion == false) + getPVT(maxWait); + moduleQueried.headingOfMotion = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (headingOfMotion); +} + +//Get the positional dillution of precision * 10^-2 +uint16_t SFE_UBLOX_GPS::getPDOP(uint16_t maxWait) +{ + if (moduleQueried.pDOP == false) + getPVT(maxWait); + moduleQueried.pDOP = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (pDOP); +} + +//Get the current protocol version of the Ublox module we're communicating with +//This is helpful when deciding if we should call the high-precision Lat/Long (HPPOSLLH) or the regular (POSLLH) +uint8_t SFE_UBLOX_GPS::getProtocolVersionHigh(uint16_t maxWait) +{ + if (moduleQueried.versionNumber == false) + getProtocolVersion(maxWait); + return (versionHigh); +} + +//Get the current protocol version of the Ublox module we're communicating with +//This is helpful when deciding if we should call the high-precision Lat/Long (HPPOSLLH) or the regular (POSLLH) +uint8_t SFE_UBLOX_GPS::getProtocolVersionLow(uint16_t maxWait) +{ + if (moduleQueried.versionNumber == false) + getProtocolVersion(maxWait); + return (versionLow); +} + +//Get the current protocol version of the Ublox module we're communicating with +//This is helpful when deciding if we should call the high-precision Lat/Long (HPPOSLLH) or the regular (POSLLH) +bool SFE_UBLOX_GPS::getProtocolVersion(uint16_t maxWait) +{ + //Send packet with only CLS and ID, length of zero. This will cause the module to respond with the contents of that CLS/ID. + packetCfg.cls = UBX_CLASS_MON; + packetCfg.id = UBX_MON_VER; + + packetCfg.len = 0; + packetCfg.startingSpot = 40; //Start at first "extended software information" string + + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are only expecting data (no ACK) + return (false); //If command send fails then bail + + //Payload should now contain ~220 characters (depends on module type) + + // if (_printDebug == true) + // { + // printk("MON VER Payload:")); + // for (int location = 0; location < packetCfg.len; location++) + // { + // if (location % 30 == 0) + // printk("\n"); + // printk("%d", payloadCfg[location]); + // } + // printk("\n"); + // } + + //We will step through the payload looking at each extension field of 30 bytes + for (uint8_t extensionNumber = 0; extensionNumber < 10; extensionNumber++) + { + //Now we need to find "PROTVER=18.00" in the incoming byte stream + if (payloadCfg[(30 * extensionNumber) + 0] == 'P' && payloadCfg[(30 * extensionNumber) + 6] == 'R') + { + versionHigh = (payloadCfg[(30 * extensionNumber) + 8] - '0') * 10 + (payloadCfg[(30 * extensionNumber) + 9] - '0'); //Convert '18' to 18 + versionLow = (payloadCfg[(30 * extensionNumber) + 11] - '0') * 10 + (payloadCfg[(30 * extensionNumber) + 12] - '0'); //Convert '00' to 00 + moduleQueried.versionNumber = true; //Mark this data as new + + if (_printDebug == true) + { + printk("Protocol version: %d.%d\n", versionHigh, versionLow); + } + return (true); //Success! + } + } + + return (false); //We failed +} + +//Mark all the PVT data as read/stale. This is handy to get data alignment after CRC failure +void SFE_UBLOX_GPS::flushPVT() +{ + //Mark all datums as stale (read before) + moduleQueried.gpsiTOW = false; + moduleQueried.gpsYear = false; + moduleQueried.gpsMonth = false; + moduleQueried.gpsDay = false; + moduleQueried.gpsHour = false; + moduleQueried.gpsMinute = false; + moduleQueried.gpsSecond = false; + moduleQueried.gpsDateValid = false; + moduleQueried.gpsTimeValid = false; + moduleQueried.gpsNanosecond = false; + + moduleQueried.all = false; + moduleQueried.longitude = false; + moduleQueried.latitude = false; + moduleQueried.altitude = false; + moduleQueried.altitudeMSL = false; + moduleQueried.SIV = false; + moduleQueried.fixType = false; + moduleQueried.carrierSolution = false; + moduleQueried.groundSpeed = false; + moduleQueried.headingOfMotion = false; + moduleQueried.pDOP = false; +} + +//Relative Positioning Information in NED frame +//Returns true if commands was successful +bool SFE_UBLOX_GPS::getRELPOSNED(uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_NAV; + packetCfg.id = UBX_NAV_RELPOSNED; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are only expecting data (no ACK) + return (false); //If command send fails then bail + + //We got a response, now parse the bits + + uint16_t refStationID = extractInt(2); + //printk("refStationID: %d\n", refStationID); + + int32_t tempRelPos; + + tempRelPos = extractLong(8); + relPosInfo.relPosN = tempRelPos / 100.0; //Convert cm to m + + tempRelPos = extractLong(12); + relPosInfo.relPosE = tempRelPos / 100.0; //Convert cm to m + + tempRelPos = extractLong(16); + relPosInfo.relPosD = tempRelPos / 100.0; //Convert cm to m + + relPosInfo.relPosLength = extractLong(20); + relPosInfo.relPosHeading = extractLong(24); + + relPosInfo.relPosHPN = payloadCfg[32]; + relPosInfo.relPosHPE = payloadCfg[33]; + relPosInfo.relPosHPD = payloadCfg[34]; + relPosInfo.relPosHPLength = payloadCfg[35]; + + uint32_t tempAcc; + + tempAcc = extractLong(36); + relPosInfo.accN = tempAcc / 10000.0; //Convert 0.1 mm to m + + tempAcc = extractLong(40); + relPosInfo.accE = tempAcc / 10000.0; //Convert 0.1 mm to m + + tempAcc = extractLong(44); + relPosInfo.accD = tempAcc / 10000.0; //Convert 0.1 mm to m + + uint8_t flags = payloadCfg[60]; + + relPosInfo.gnssFixOk = flags & (1 << 0); + relPosInfo.diffSoln = flags & (1 << 1); + relPosInfo.relPosValid = flags & (1 << 2); + relPosInfo.carrSoln = (flags & (0b11 << 3)) >> 3; + relPosInfo.isMoving = flags & (1 << 5); + relPosInfo.refPosMiss = flags & (1 << 6); + relPosInfo.refObsMiss = flags & (1 << 7); + + return (true); +} +bool SFE_UBLOX_GPS::getEsfInfo(uint16_t maxWait) +{ + // Requesting Data from the receiver + packetCfg.cls = UBX_CLASS_ESF; + packetCfg.id = UBX_ESF_STATUS; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) + return (false); //If command send fails then bail + + checkUblox(); + + // payload should be loaded. + imuMeas.version = extractByte(4); + imuMeas.fusionMode = extractByte(12); + ubloxSen.numSens = extractByte(15); + + // Individual Status Sensor in different function + return (true); +} + +// +bool SFE_UBLOX_GPS::getEsfIns(uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_ESF; + packetCfg.id = UBX_ESF_INS; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) + return (false); //If command send fails then bail + + checkUblox(); + + // Validity of each sensor value below + uint32_t validity = extractLong(0); + + imuMeas.xAngRateVald = (validity && 0x0080) >> 8; + imuMeas.yAngRateVald = (validity && 0x0100) >> 9; + imuMeas.zAngRateVald = (validity && 0x0200) >> 10; + imuMeas.xAccelVald = (validity && 0x0400) >> 11; + imuMeas.yAccelVald = (validity && 0x0800) >> 12; + imuMeas.zAccelVald = (validity && 0x1000) >> 13; + + imuMeas.xAngRate = extractLong(12); // deg/s + imuMeas.yAngRate = extractLong(16); // deg/s + imuMeas.zAngRate = extractLong(20); // deg/s + + imuMeas.xAccel = extractLong(24); // m/s + imuMeas.yAccel = extractLong(28); // m/s + imuMeas.zAccel = extractLong(32); // m/s + + return (true); +} + +// +bool SFE_UBLOX_GPS::getEsfDataInfo(uint16_t maxWait) +{ + + packetCfg.cls = UBX_CLASS_ESF; + packetCfg.id = UBX_ESF_MEAS; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) + return (false); //If command send fails then bail + + checkUblox(); + + uint32_t timeStamp = extractLong(0); + uint32_t flags = extractInt(4); + + uint8_t timeSent = (flags && 0x01) >> 1; + uint8_t timeEdge = (flags && 0x02) >> 2; + uint8_t tagValid = (flags && 0x04) >> 3; + uint8_t numMeas = (flags && 0x1000) >> 15; + + if (numMeas > DEF_NUM_SENS) + numMeas = DEF_NUM_SENS; + + uint8_t byteOffset = 4; + + for (uint8_t i = 0; i < numMeas; i++) + { + + uint32_t bitField = extractLong(4 + byteOffset * i); + imuMeas.dataType[i] = (bitField && 0xFF000000) >> 23; + imuMeas.data[i] = (bitField && 0xFFFFFF); + imuMeas.dataTStamp[i] = extractLong(8 + byteOffset * i); + } + + return (true); +} + +bool SFE_UBLOX_GPS::getEsfRawDataInfo(uint16_t maxWait) +{ + + // Need to know the number of sensor to get the correct data + // Rate selected in UBX-CFG-MSG is not respected + packetCfg.cls = UBX_CLASS_ESF; + packetCfg.id = UBX_ESF_RAW; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) + return (false); //If command send fails then bail + + checkUblox(); + + uint32_t bitField = extractLong(4); + imuMeas.rawDataType = (bitField && 0xFF000000) >> 23; + imuMeas.rawData = (bitField && 0xFFFFFF); + imuMeas.rawTStamp = extractLong(8); + + return (true); +} + +sfe_ublox_status_e SFE_UBLOX_GPS::getSensState(uint8_t sensor, uint16_t maxWait) +{ + + packetCfg.cls = UBX_CLASS_ESF; + packetCfg.id = UBX_ESF_STATUS; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) + return (SFE_UBLOX_STATUS_FAIL); //If command send fails then bail + + ubloxSen.numSens = extractByte(15); + + if (sensor > ubloxSen.numSens) + return (SFE_UBLOX_STATUS_OUT_OF_RANGE); + + checkUblox(); + + uint8_t offset = 4; + + // Only the last sensor value checked will remain. + for (uint8_t i = 0; i < sensor; i++) + { + + uint8_t sensorFieldOne = extractByte(16 + offset * i); + uint8_t sensorFieldTwo = extractByte(17 + offset * i); + ubloxSen.freq = extractByte(18 + offset * i); + uint8_t sensorFieldThr = extractByte(19 + offset * i); + + ubloxSen.senType = (sensorFieldOne && 0x10) >> 5; + ubloxSen.isUsed = (sensorFieldOne && 0x20) >> 6; + ubloxSen.isReady = (sensorFieldOne && 0x30) >> 7; + + ubloxSen.calibStatus = sensorFieldTwo && 0x03; + ubloxSen.timeStatus = (sensorFieldTwo && 0xC) >> 2; + + ubloxSen.badMeas = (sensorFieldThr && 0x01); + ubloxSen.badTag = (sensorFieldThr && 0x02) >> 1; + ubloxSen.missMeas = (sensorFieldThr && 0x04) >> 2; + ubloxSen.noisyMeas = (sensorFieldThr && 0x08) >> 3; + } + + return (SFE_UBLOX_STATUS_SUCCESS); +} + +bool SFE_UBLOX_GPS::getVehAtt(uint16_t maxWait) +{ + + packetCfg.cls = UBX_CLASS_NAV; + packetCfg.id = UBX_NAV_ATT; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) + return (SFE_UBLOX_STATUS_FAIL); //If command send fails then bail + + checkUblox(); + + vehAtt.roll = extractLong(8); + vehAtt.pitch = extractLong(12); + vehAtt.heading = extractLong(16); + + vehAtt.accRoll = extractLong(20); + vehAtt.accPitch = extractLong(24); + vehAtt.accHeading = extractLong(28); + + return (true); +} diff --git a/src/SparkFun_Ublox_Zephyr_Library.h b/src/SparkFun_Ublox_Zephyr_Library.h new file mode 100644 index 0000000..76c6abb --- /dev/null +++ b/src/SparkFun_Ublox_Zephyr_Library.h @@ -0,0 +1,899 @@ +/* + This is a library written for the Ublox ZED-F9P and NEO-M8P-2 + SparkFun sells these at its website: www.sparkfun.com + Do you like this library? Help support SparkFun. Buy a board! + https://www.sparkfun.com/products/15136 + https://www.sparkfun.com/products/15005 + https://www.sparkfun.com/products/15733 + https://www.sparkfun.com/products/15193 + https://www.sparkfun.com/products/15210 + + Written by Nathan Seidle @ SparkFun Electronics, September 6th, 2018 + + This library handles configuring and handling the responses + from a Ublox GPS module. Works with most modules from Ublox including + the Zed-F9P, NEO-M8P-2, NEO-M9N, ZOE-M8Q, SAM-M8Q, and many others. + + https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library + + Development environment specifics: + Arduino IDE 1.8.5 + + SparkFun code, firmware, and software is released under the MIT License(http://opensource.org/licenses/MIT). + The MIT License (MIT) + Copyright (c) 2016 SparkFun Electronics + 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. +*/ + +#ifndef SPARKFUN_UBLOX_ZEPHYR_LIBRARY_H +#define SPARKFUN_UBLOX_ZEPHYR_LIBRARY_H + +#include +#include + +//The catch-all default is 32 +#define I2C_BUFFER_LENGTH 32 + +//Define pin states +#define LOW 0 +#define HIGH 1 + +//Define a digital pin to aid checksum failure capture and analysis +//Leave set to -1 if not needed +const int checksumFailurePin = -1; + +// Global Status Returns +typedef enum +{ + SFE_UBLOX_STATUS_SUCCESS, + SFE_UBLOX_STATUS_FAIL, + SFE_UBLOX_STATUS_CRC_FAIL, + SFE_UBLOX_STATUS_TIMEOUT, + SFE_UBLOX_STATUS_COMMAND_NACK, // Indicates that the command was unrecognised, invalid or that the module is too busy to respond + SFE_UBLOX_STATUS_OUT_OF_RANGE, + SFE_UBLOX_STATUS_INVALID_ARG, + SFE_UBLOX_STATUS_INVALID_OPERATION, + SFE_UBLOX_STATUS_MEM_ERR, + SFE_UBLOX_STATUS_HW_ERR, + SFE_UBLOX_STATUS_DATA_SENT, // This indicates that a 'set' was successful + SFE_UBLOX_STATUS_DATA_RECEIVED, // This indicates that a 'get' (poll) was successful + SFE_UBLOX_STATUS_I2C_COMM_FAILURE, + SFE_UBLOX_STATUS_DATA_OVERWRITTEN // This is an error - the data was valid but has been or _is being_ overwritten by another packet +} sfe_ublox_status_e; + +// ubxPacket validity +typedef enum +{ + SFE_UBLOX_PACKET_VALIDITY_NOT_VALID, + SFE_UBLOX_PACKET_VALIDITY_VALID, + SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, + SFE_UBLOX_PACKET_NOTACKNOWLEDGED // This indicates that we received a NACK +} sfe_ublox_packet_validity_e; + +// Identify which packet buffer is in use: +// packetCfg (or a custom packet), packetAck or packetBuf +typedef enum +{ + SFE_UBLOX_PACKET_PACKETCFG, + SFE_UBLOX_PACKET_PACKETACK, + SFE_UBLOX_PACKET_PACKETBUF +} sfe_ublox_packet_buffer_e; + +//Registers +const uint8_t UBX_SYNCH_1 = 0xB5; +const uint8_t UBX_SYNCH_2 = 0x62; + +//The following are UBX Class IDs. Descriptions taken from ZED-F9P Interface Description Document page 32, NEO-M8P Interface Description page 145 +const uint8_t UBX_CLASS_NAV = 0x01; //Navigation Results Messages: Position, Speed, Time, Acceleration, Heading, DOP, SVs used +const uint8_t UBX_CLASS_RXM = 0x02; //Receiver Manager Messages: Satellite Status, RTC Status +const uint8_t UBX_CLASS_INF = 0x04; //Information Messages: Printf-Style Messages, with IDs such as Error, Warning, Notice +const uint8_t UBX_CLASS_ACK = 0x05; //Ack/Nak Messages: Acknowledge or Reject messages to UBX-CFG input messages +const uint8_t UBX_CLASS_CFG = 0x06; //Configuration Input Messages: Configure the receiver. +const uint8_t UBX_CLASS_UPD = 0x09; //Firmware Update Messages: Memory/Flash erase/write, Reboot, Flash identification, etc. +const uint8_t UBX_CLASS_MON = 0x0A; //Monitoring Messages: Communication Status, CPU Load, Stack Usage, Task Status +const uint8_t UBX_CLASS_AID = 0x0B; //(NEO-M8P ONLY!!!) AssistNow Aiding Messages: Ephemeris, Almanac, other A-GPS data input +const uint8_t UBX_CLASS_TIM = 0x0D; //Timing Messages: Time Pulse Output, Time Mark Results +const uint8_t UBX_CLASS_ESF = 0x10; //(NEO-M8P ONLY!!!) External Sensor Fusion Messages: External Sensor Measurements and Status Information +const uint8_t UBX_CLASS_MGA = 0x13; //Multiple GNSS Assistance Messages: Assistance data for various GNSS +const uint8_t UBX_CLASS_LOG = 0x21; //Logging Messages: Log creation, deletion, info and retrieval +const uint8_t UBX_CLASS_SEC = 0x27; //Security Feature Messages +const uint8_t UBX_CLASS_HNR = 0x28; //(NEO-M8P ONLY!!!) High Rate Navigation Results Messages: High rate time, position speed, heading +const uint8_t UBX_CLASS_NMEA = 0xF0; //NMEA Strings: standard NMEA strings + +//The following are used for configuration. Descriptions are from the ZED-F9P Interface Description pg 33-34 and NEO-M9N Interface Description pg 47-48 +const uint8_t UBX_CFG_ANT = 0x13; //Antenna Control Settings. Used to configure the antenna control settings +const uint8_t UBX_CFG_BATCH = 0x93; //Get/set data batching configuration. +const uint8_t UBX_CFG_CFG = 0x09; //Clear, Save, and Load Configurations. Used to save current configuration +const uint8_t UBX_CFG_DAT = 0x06; //Set User-defined Datum or The currently defined Datum +const uint8_t UBX_CFG_DGNSS = 0x70; //DGNSS configuration +const uint8_t UBX_CFG_GEOFENCE = 0x69; //Geofencing configuration. Used to configure a geofence +const uint8_t UBX_CFG_GNSS = 0x3E; //GNSS system configuration +const uint8_t UBX_CFG_INF = 0x02; //Depending on packet length, either: poll configuration for one protocol, or information message configuration +const uint8_t UBX_CFG_ITFM = 0x39; //Jamming/Interference Monitor configuration +const uint8_t UBX_CFG_LOGFILTER = 0x47; //Data Logger Configuration +const uint8_t UBX_CFG_MSG = 0x01; //Poll a message configuration, or Set Message Rate(s), or Set Message Rate +const uint8_t UBX_CFG_NAV5 = 0x24; //Navigation Engine Settings. Used to configure the navigation engine including the dynamic model. +const uint8_t UBX_CFG_NAVX5 = 0x23; //Navigation Engine Expert Settings +const uint8_t UBX_CFG_NMEA = 0x17; //Extended NMEA protocol configuration V1 +const uint8_t UBX_CFG_ODO = 0x1E; //Odometer, Low-speed COG Engine Settings +const uint8_t UBX_CFG_PM2 = 0x3B; //Extended power management configuration +const uint8_t UBX_CFG_PMS = 0x86; //Power mode setup +const uint8_t UBX_CFG_PRT = 0x00; //Used to configure port specifics. Polls the configuration for one I/O Port, or Port configuration for UART ports, or Port configuration for USB port, or Port configuration for SPI port, or Port configuration for DDC port +const uint8_t UBX_CFG_PWR = 0x57; //Put receiver in a defined power state +const uint8_t UBX_CFG_RATE = 0x08; //Navigation/Measurement Rate Settings. Used to set port baud rates. +const uint8_t UBX_CFG_RINV = 0x34; //Contents of Remote Inventory +const uint8_t UBX_CFG_RST = 0x04; //Reset Receiver / Clear Backup Data Structures. Used to reset device. +const uint8_t UBX_CFG_RXM = 0x11; //RXM configuration +const uint8_t UBX_CFG_SBAS = 0x16; //SBAS configuration +const uint8_t UBX_CFG_TMODE3 = 0x71; //Time Mode Settings 3. Used to enable Survey In Mode +const uint8_t UBX_CFG_TP5 = 0x31; //Time Pulse Parameters +const uint8_t UBX_CFG_USB = 0x1B; //USB Configuration +const uint8_t UBX_CFG_VALDEL = 0x8C; //Used for config of higher version Ublox modules (ie protocol v27 and above). Deletes values corresponding to provided keys/ provided keys with a transaction +const uint8_t UBX_CFG_VALGET = 0x8B; //Used for config of higher version Ublox modules (ie protocol v27 and above). Configuration Items +const uint8_t UBX_CFG_VALSET = 0x8A; //Used for config of higher version Ublox modules (ie protocol v27 and above). Sets values corresponding to provided key-value pairs/ provided key-value pairs within a transaction. + +//The following are used to enable NMEA messages. Descriptions come from the NMEA messages overview in the ZED-F9P Interface Description +const uint8_t UBX_NMEA_MSB = 0xF0; //All NMEA enable commands have 0xF0 as MSB +const uint8_t UBX_NMEA_DTM = 0x0A; //GxDTM (datum reference) +const uint8_t UBX_NMEA_GAQ = 0x45; //GxGAQ (poll a standard message (if the current talker ID is GA)) +const uint8_t UBX_NMEA_GBQ = 0x44; //GxGBQ (poll a standard message (if the current Talker ID is GB)) +const uint8_t UBX_NMEA_GBS = 0x09; //GxGBS (GNSS satellite fault detection) +const uint8_t UBX_NMEA_GGA = 0x00; //GxGGA (Global positioning system fix data) +const uint8_t UBX_NMEA_GLL = 0x01; //GxGLL (latitude and long, whith time of position fix and status) +const uint8_t UBX_NMEA_GLQ = 0x43; //GxGLQ (poll a standard message (if the current Talker ID is GL)) +const uint8_t UBX_NMEA_GNQ = 0x42; //GxGNQ (poll a standard message (if the current Talker ID is GN)) +const uint8_t UBX_NMEA_GNS = 0x0D; //GxGNS (GNSS fix data) +const uint8_t UBX_NMEA_GPQ = 0x040; //GxGPQ (poll a standard message (if the current Talker ID is GP)) +const uint8_t UBX_NMEA_GRS = 0x06; //GxGRS (GNSS range residuals) +const uint8_t UBX_NMEA_GSA = 0x02; //GxGSA (GNSS DOP and Active satellites) +const uint8_t UBX_NMEA_GST = 0x07; //GxGST (GNSS Pseudo Range Error Statistics) +const uint8_t UBX_NMEA_GSV = 0x03; //GxGSV (GNSS satellites in view) +const uint8_t UBX_NMEA_RMC = 0x04; //GxRMC (Recommended minimum data) +const uint8_t UBX_NMEA_TXT = 0x41; //GxTXT (text transmission) +const uint8_t UBX_NMEA_VLW = 0x0F; //GxVLW (dual ground/water distance) +const uint8_t UBX_NMEA_VTG = 0x05; //GxVTG (course over ground and Ground speed) +const uint8_t UBX_NMEA_ZDA = 0x08; //GxZDA (Time and Date) + +//The following are used to configure the NMEA protocol main talker ID and GSV talker ID +const uint8_t UBX_NMEA_MAINTALKERID_NOTOVERRIDDEN = 0x00; //main talker ID is system dependent +const uint8_t UBX_NMEA_MAINTALKERID_GP = 0x01; //main talker ID is GPS +const uint8_t UBX_NMEA_MAINTALKERID_GL = 0x02; //main talker ID is GLONASS +const uint8_t UBX_NMEA_MAINTALKERID_GN = 0x03; //main talker ID is combined receiver +const uint8_t UBX_NMEA_MAINTALKERID_GA = 0x04; //main talker ID is Galileo +const uint8_t UBX_NMEA_MAINTALKERID_GB = 0x05; //main talker ID is BeiDou +const uint8_t UBX_NMEA_GSVTALKERID_GNSS = 0x00; //GNSS specific Talker ID (as defined by NMEA) +const uint8_t UBX_NMEA_GSVTALKERID_MAIN = 0x01; //use the main Talker ID + +//The following are used to configure INF UBX messages (information messages). Descriptions from UBX messages overview (ZED_F9P Interface Description Document page 34) +const uint8_t UBX_INF_CLASS = 0x04; //All INF messages have 0x04 as the class +const uint8_t UBX_INF_DEBUG = 0x04; //ASCII output with debug contents +const uint8_t UBX_INF_ERROR = 0x00; //ASCII output with error contents +const uint8_t UBX_INF_NOTICE = 0x02; //ASCII output with informational contents +const uint8_t UBX_INF_TEST = 0x03; //ASCII output with test contents +const uint8_t UBX_INF_WARNING = 0x01; //ASCII output with warning contents + +//The following are used to configure LOG UBX messages (loggings messages). Descriptions from UBX messages overview (ZED_F9P Interface Description Document page 34) +const uint8_t UBX_LOG_CREATE = 0x07; //Create Log File +const uint8_t UBX_LOG_ERASE = 0x03; //Erase Logged Data +const uint8_t UBX_LOG_FINDTIME = 0x0E; //Find index of a log entry based on a given time, or response to FINDTIME requested +const uint8_t UBX_LOG_INFO = 0x08; //Poll for log information, or Log information +const uint8_t UBX_LOG_RETRIEVEPOSEXTRA = 0x0F; //Odometer log entry +const uint8_t UBX_LOG_RETRIEVEPOS = 0x0B; //Position fix log entry +const uint8_t UBX_LOG_RETRIEVESTRING = 0x0D; //Byte string log entry +const uint8_t UBX_LOG_RETRIEVE = 0x09; //Request log data +const uint8_t UBX_LOG_STRING = 0x04; //Store arbitrary string on on-board flash + +//The following are used to configure MGA UBX messages (Multiple GNSS Assistance Messages). Descriptions from UBX messages overview (ZED_F9P Interface Description Document page 34) +const uint8_t UBX_MGA_ACK_DATA0 = 0x60; //Multiple GNSS Acknowledge message +const uint8_t UBX_MGA_BDS_EPH = 0x03; //BDS Ephemeris Assistance +const uint8_t UBX_MGA_BDS_ALM = 0x03; //BDS Almanac Assistance +const uint8_t UBX_MGA_BDS_HEALTH = 0x03; //BDS Health Assistance +const uint8_t UBX_MGA_BDS_UTC = 0x03; //BDS UTC Assistance +const uint8_t UBX_MGA_BDS_IONO = 0x03; //BDS Ionospheric Assistance +const uint8_t UBX_MGA_DBD = 0x80; //Either: Poll the Navigation Database, or Navigation Database Dump Entry +const uint8_t UBX_MGA_GAL_EPH = 0x02; //Galileo Ephemeris Assistance +const uint8_t UBX_MGA_GAL_ALM = 0x02; //Galileo Almanac Assitance +const uint8_t UBX_MGA_GAL_TIMOFFSET = 0x02; //Galileo GPS time offset assistance +const uint8_t UBX_MGA_GAL_UTC = 0x02; //Galileo UTC Assistance +const uint8_t UBX_MGA_GLO_EPH = 0x06; //GLONASS Ephemeris Assistance +const uint8_t UBX_MGA_GLO_ALM = 0x06; //GLONASS Almanac Assistance +const uint8_t UBX_MGA_GLO_TIMEOFFSET = 0x06; //GLONASS Auxiliary Time Offset Assistance +const uint8_t UBX_MGA_GPS_EPH = 0x00; //GPS Ephemeris Assistance +const uint8_t UBX_MGA_GPS_ALM = 0x00; //GPS Almanac Assistance +const uint8_t UBX_MGA_GPS_HEALTH = 0x00; //GPS Health Assistance +const uint8_t UBX_MGA_GPS_UTC = 0x00; //GPS UTC Assistance +const uint8_t UBX_MGA_GPS_IONO = 0x00; //GPS Ionosphere Assistance +const uint8_t UBX_MGA_INI_POS_XYZ = 0x40; //Initial Position Assistance +const uint8_t UBX_MGA_INI_POS_LLH = 0x40; //Initial Position Assitance +const uint8_t UBX_MGA_INI_TIME_UTC = 0x40; //Initial Time Assistance +const uint8_t UBX_MGA_INI_TIME_GNSS = 0x40; //Initial Time Assistance +const uint8_t UBX_MGA_INI_CLKD = 0x40; //Initial Clock Drift Assitance +const uint8_t UBX_MGA_INI_FREQ = 0x40; //Initial Frequency Assistance +const uint8_t UBX_MGA_INI_EOP = 0x40; //Earth Orientation Parameters Assistance +const uint8_t UBX_MGA_QZSS_EPH = 0x05; //QZSS Ephemeris Assistance +const uint8_t UBX_MGA_QZSS_ALM = 0x05; //QZSS Almanac Assistance +const uint8_t UBX_MGA_QZAA_HEALTH = 0x05; //QZSS Health Assistance + +//The following are used to configure the MON UBX messages (monitoring messages). Descriptions from UBX messages overview (ZED_F9P Interface Description Document page 35) +const uint8_t UBX_MON_COMMS = 0x36; //Comm port information +const uint8_t UBX_MON_GNSS = 0x28; //Information message major GNSS selection +const uint8_t UBX_MON_HW2 = 0x0B; //Extended Hardware Status +const uint8_t UBX_MON_HW3 = 0x37; //HW I/O pin information +const uint8_t UBX_MON_HW = 0x09; //Hardware Status +const uint8_t UBX_MON_IO = 0x02; //I/O Subsystem Status +const uint8_t UBX_MON_MSGPP = 0x06; //Message Parse and Process Status +const uint8_t UBX_MON_PATCH = 0x27; //Output information about installed patches +const uint8_t UBX_MON_RF = 0x38; //RF information +const uint8_t UBX_MON_RXBUF = 0x07; //Receiver Buffer Status +const uint8_t UBX_MON_RXR = 0x21; //Receiver Status Information +const uint8_t UBX_MON_TXBUF = 0x08; //Transmitter Buffer Status. Used for query tx buffer size/state. +const uint8_t UBX_MON_VER = 0x04; //Receiver/Software Version. Used for obtaining Protocol Version. + +//The following are used to configure the NAV UBX messages (navigation results messages). Descriptions from UBX messages overview (ZED_F9P Interface Description Document page 35-36) +const uint8_t UBX_NAV_ATT = 0x05; //Vehicle "Attitude" Solution +const uint8_t UBX_NAV_CLOCK = 0x22; //Clock Solution +const uint8_t UBX_NAV_DOP = 0x04; //Dilution of precision +const uint8_t UBX_NAV_EOE = 0x61; //End of Epoch +const uint8_t UBX_NAV_GEOFENCE = 0x39; //Geofencing status. Used to poll the geofence status +const uint8_t UBX_NAV_HPPOSECEF = 0x13; //High Precision Position Solution in ECEF. Used to find our positional accuracy (high precision). +const uint8_t UBX_NAV_HPPOSLLH = 0x14; //High Precision Geodetic Position Solution. Used for obtaining lat/long/alt in high precision +const uint8_t UBX_NAV_ODO = 0x09; //Odometer Solution +const uint8_t UBX_NAV_ORB = 0x34; //GNSS Orbit Database Info +const uint8_t UBX_NAV_POSECEF = 0x01; //Position Solution in ECEF +const uint8_t UBX_NAV_POSLLH = 0x02; //Geodetic Position Solution +const uint8_t UBX_NAV_PVT = 0x07; //All the things! Position, velocity, time, PDOP, height, h/v accuracies, number of satellites. Navigation Position Velocity Time Solution. +const uint8_t UBX_NAV_RELPOSNED = 0x3C; //Relative Positioning Information in NED frame +const uint8_t UBX_NAV_RESETODO = 0x10; //Reset odometer +const uint8_t UBX_NAV_SAT = 0x35; //Satellite Information +const uint8_t UBX_NAV_SIG = 0x43; //Signal Information +const uint8_t UBX_NAV_STATUS = 0x03; //Receiver Navigation Status +const uint8_t UBX_NAV_SVIN = 0x3B; //Survey-in data. Used for checking Survey In status +const uint8_t UBX_NAV_TIMEBDS = 0x24; //BDS Time Solution +const uint8_t UBX_NAV_TIMEGAL = 0x25; //Galileo Time Solution +const uint8_t UBX_NAV_TIMEGLO = 0x23; //GLO Time Solution +const uint8_t UBX_NAV_TIMEGPS = 0x20; //GPS Time Solution +const uint8_t UBX_NAV_TIMELS = 0x26; //Leap second event information +const uint8_t UBX_NAV_TIMEUTC = 0x21; //UTC Time Solution +const uint8_t UBX_NAV_VELECEF = 0x11; //Velocity Solution in ECEF +const uint8_t UBX_NAV_VELNED = 0x12; //Velocity Solution in NED + +//The following are used to configure the RXM UBX messages (receiver manager messages). Descriptions from UBX messages overview (ZED_F9P Interface Description Document page 36) +const uint8_t UBX_RXM_MEASX = 0x14; //Satellite Measurements for RRLP +const uint8_t UBX_RXM_PMREQ = 0x41; //Requests a Power Management task (two differenent packet sizes) +const uint8_t UBX_RXM_RAWX = 0x15; //Multi-GNSS Raw Measurement Data +const uint8_t UBX_RXM_RLM = 0x59; //Galileo SAR Short-RLM report (two different packet sizes) +const uint8_t UBX_RXM_RTCM = 0x32; //RTCM input status +const uint8_t UBX_RXM_SFRBX = 0x13; //Boradcast Navigation Data Subframe + +//The following are used to configure the SEC UBX messages (security feature messages). Descriptions from UBX messages overview (ZED_F9P Interface Description Document page 36) +const uint8_t UBX_SEC_UNIQID = 0x03; //Unique chip ID + +//The following are used to configure the TIM UBX messages (timing messages). Descriptions from UBX messages overview (ZED_F9P Interface Description Document page 36) +const uint8_t UBX_TIM_TM2 = 0x03; //Time mark data +const uint8_t UBX_TIM_TP = 0x01; //Time Pulse Timedata +const uint8_t UBX_TIM_VRFY = 0x06; //Sourced Time Verification + +//The following are used to configure the UPD UBX messages (firmware update messages). Descriptions from UBX messages overview (ZED-F9P Interface Description Document page 36) +const uint8_t UBX_UPD_SOS = 0x14; //Poll Backup Fil Restore Status, Create Backup File in Flash, Clear Backup File in Flash, Backup File Creation Acknowledge, System Restored from Backup + +//The following are used to enable RTCM messages +const uint8_t UBX_RTCM_MSB = 0xF5; //All RTCM enable commands have 0xF5 as MSB +const uint8_t UBX_RTCM_1005 = 0x05; //Stationary RTK reference ARP +const uint8_t UBX_RTCM_1074 = 0x4A; //GPS MSM4 +const uint8_t UBX_RTCM_1077 = 0x4D; //GPS MSM7 +const uint8_t UBX_RTCM_1084 = 0x54; //GLONASS MSM4 +const uint8_t UBX_RTCM_1087 = 0x57; //GLONASS MSM7 +const uint8_t UBX_RTCM_1094 = 0x5E; //Galileo MSM4 +const uint8_t UBX_RTCM_1097 = 0x61; //Galileo MSM7 +const uint8_t UBX_RTCM_1124 = 0x7C; //BeiDou MSM4 +const uint8_t UBX_RTCM_1127 = 0x7F; //BeiDou MSM7 +const uint8_t UBX_RTCM_1230 = 0xE6; //GLONASS code-phase biases, set to once every 10 seconds +const uint8_t UBX_RTCM_4072_0 = 0xFE; //Reference station PVT (ublox proprietary RTCM message) +const uint8_t UBX_RTCM_4072_1 = 0xFD; //Additional reference station information (ublox proprietary RTCM message) + +const uint8_t UBX_ACK_NACK = 0x00; +const uint8_t UBX_ACK_ACK = 0x01; +const uint8_t UBX_ACK_NONE = 0x02; //Not a real value + +// The following constants are used to get External Sensor Measurements and Status +// Information. +const uint8_t UBX_ESF_MEAS = 0x02; +const uint8_t UBX_ESF_RAW = 0x03; +const uint8_t UBX_ESF_STATUS = 0x10; +const uint8_t UBX_ESF_INS = 0x15; //36 bytes + +const uint8_t SVIN_MODE_DISABLE = 0x00; +const uint8_t SVIN_MODE_ENABLE = 0x01; + +//The following consts are used to configure the various ports and streams for those ports. See -CFG-PRT. +const uint8_t COM_PORT_I2C = 0; +const uint8_t COM_PORT_UART1 = 1; +const uint8_t COM_PORT_UART2 = 2; +const uint8_t COM_PORT_USB = 3; +const uint8_t COM_PORT_SPI = 4; + +const uint8_t COM_TYPE_UBX = (1 << 0); +const uint8_t COM_TYPE_NMEA = (1 << 1); +const uint8_t COM_TYPE_RTCM3 = (1 << 5); + +//The following consts are used to generate KEY values for the advanced protocol functions of VELGET/SET/DEL +const uint8_t VAL_SIZE_1 = 0x01; //One bit +const uint8_t VAL_SIZE_8 = 0x02; //One byte +const uint8_t VAL_SIZE_16 = 0x03; //Two bytes +const uint8_t VAL_SIZE_32 = 0x04; //Four bytes +const uint8_t VAL_SIZE_64 = 0x05; //Eight bytes + +//These are the Bitfield layers definitions for the UBX-CFG-VALSET message (not to be confused with Bitfield deviceMask in UBX-CFG-CFG) +const uint8_t VAL_LAYER_RAM = (1 << 0); +const uint8_t VAL_LAYER_BBR = (1 << 1); +const uint8_t VAL_LAYER_FLASH = (1 << 2); + +//Below are various Groups, IDs, and sizes for various settings +//These can be used to call getVal/setVal/delVal +const uint8_t VAL_GROUP_I2COUTPROT = 0x72; +const uint8_t VAL_GROUP_I2COUTPROT_SIZE = VAL_SIZE_1; //All fields in I2C group are currently 1 bit + +const uint8_t VAL_ID_I2COUTPROT_UBX = 0x01; +const uint8_t VAL_ID_I2COUTPROT_NMEA = 0x02; +const uint8_t VAL_ID_I2COUTPROT_RTCM3 = 0x03; + +const uint8_t VAL_GROUP_I2C = 0x51; +const uint8_t VAL_GROUP_I2C_SIZE = VAL_SIZE_8; //All fields in I2C group are currently 1 byte + +const uint8_t VAL_ID_I2C_ADDRESS = 0x01; + +// Configuration Sub-Section mask definitions for saveConfigSelective (UBX-CFG-CFG) +const uint32_t VAL_CFG_SUBSEC_IOPORT = 0x00000001; // ioPort - communications port settings (causes IO system reset!) +const uint32_t VAL_CFG_SUBSEC_MSGCONF = 0x00000002; // msgConf - message configuration +const uint32_t VAL_CFG_SUBSEC_INFMSG = 0x00000004; // infMsg - INF message configuration +const uint32_t VAL_CFG_SUBSEC_NAVCONF = 0x00000008; // navConf - navigation configuration +const uint32_t VAL_CFG_SUBSEC_RXMCONF = 0x00000010; // rxmConf - receiver manager configuration +const uint32_t VAL_CFG_SUBSEC_SENCONF = 0x00000100; // senConf - sensor interface configuration (requires protocol 19+) +const uint32_t VAL_CFG_SUBSEC_RINVCONF = 0x00000200; // rinvConf - remove inventory configuration +const uint32_t VAL_CFG_SUBSEC_ANTCONF = 0x00000400; // antConf - antenna configuration +const uint32_t VAL_CFG_SUBSEC_LOGCONF = 0x00000800; // logConf - logging configuration +const uint32_t VAL_CFG_SUBSEC_FTSCONF = 0x00001000; // ftsConf - FTS configuration (FTS products only) + +// Bitfield wakeupSources for UBX_RXM_PMREQ +const uint32_t VAL_RXM_PMREQ_WAKEUPSOURCE_UARTRX = 0x00000008; // uartrx +const uint32_t VAL_RXM_PMREQ_WAKEUPSOURCE_EXTINT0 = 0x00000020; // extint0 +const uint32_t VAL_RXM_PMREQ_WAKEUPSOURCE_EXTINT1 = 0x00000040; // extint1 +const uint32_t VAL_RXM_PMREQ_WAKEUPSOURCE_SPICS = 0x00000080; // spics + +enum dynModel // Possible values for the dynamic platform model, which provide more accuract position output for the situation. Description extracted from ZED-F9P Integration Manual +{ + DYN_MODEL_PORTABLE = 0, //Applications with low acceleration, e.g. portable devices. Suitable for most situations. + // 1 is not defined + DYN_MODEL_STATIONARY = 2, //Used in timing applications (antenna must be stationary) or other stationary applications. Velocity restricted to 0 m/s. Zero dynamics assumed. + DYN_MODEL_PEDESTRIAN, //Applications with low acceleration and speed, e.g. how a pedestrian would move. Low acceleration assumed. + DYN_MODEL_AUTOMOTIVE, //Used for applications with equivalent dynamics to those of a passenger car. Low vertical acceleration assumed + DYN_MODEL_SEA, //Recommended for applications at sea, with zero vertical velocity. Zero vertical velocity assumed. Sea level assumed. + DYN_MODEL_AIRBORNE1g, //Airborne <1g acceleration. Used for applications with a higher dynamic range and greater vertical acceleration than a passenger car. No 2D position fixes supported. + DYN_MODEL_AIRBORNE2g, //Airborne <2g acceleration. Recommended for typical airborne environments. No 2D position fixes supported. + DYN_MODEL_AIRBORNE4g, //Airborne <4g acceleration. Only recommended for extremely dynamic environments. No 2D position fixes supported. + DYN_MODEL_WRIST, // Not supported in protocol versions less than 18. Only recommended for wrist worn applications. Receiver will filter out arm motion. + DYN_MODEL_BIKE, // Supported in protocol versions 19.2 +}; + +#ifndef MAX_PAYLOAD_SIZE + +#define MAX_PAYLOAD_SIZE 256 //We need ~220 bytes for getProtocolVersion on most ublox modules +//#define MAX_PAYLOAD_SIZE 768 //Worst case: UBX_CFG_VALSET packet with 64 keyIDs each with 64 bit values + +#endif + +//-=-=-=-=- UBX binary specific variables +typedef struct +{ + uint8_t cls; + uint8_t id; + uint16_t len; //Length of the payload. Does not include cls, id, or checksum bytes + uint16_t counter; //Keeps track of number of overall bytes received. Some responses are larger than 255 bytes. + uint16_t startingSpot; //The counter value needed to go past before we begin recording into payload array + uint8_t *payload; + uint8_t checksumA; //Given to us from module. Checked against the rolling calculated A/B checksums. + uint8_t checksumB; + sfe_ublox_packet_validity_e valid; //Goes from NOT_DEFINED to VALID or NOT_VALID when checksum is checked + sfe_ublox_packet_validity_e classAndIDmatch; // Goes from NOT_DEFINED to VALID or NOT_VALID when the Class and ID match the requestedClass and requestedID +} ubxPacket; + +// Struct to hold the results returned by getGeofenceState (returned by UBX-NAV-GEOFENCE) +typedef struct +{ + uint8_t status; // Geofencing status: 0 - Geofencing not available or not reliable; 1 - Geofencing active + uint8_t numFences; // Number of geofences + uint8_t combState; // Combined (logical OR) state of all geofences: 0 - Unknown; 1 - Inside; 2 - Outside + uint8_t states[4]; // Geofence states: 0 - Unknown; 1 - Inside; 2 - Outside +} geofenceState; + +// Struct to hold the current geofence parameters +typedef struct +{ + uint8_t numFences; // Number of active geofences + int32_t lats[4]; // Latitudes of geofences (in degrees * 10^-7) + int32_t longs[4]; // Longitudes of geofences (in degrees * 10^-7) + uint32_t rads[4]; // Radii of geofences (in m * 10^-2) +} geofenceParams; + +class SFE_UBLOX_GPS +{ +public: + SFE_UBLOX_GPS(void); + +// A default of 250ms for maxWait seems fine for I2C but is not enough for SerialUSB. +// If you know you are only going to be using I2C / Qwiic communication, you can +// safely reduce defaultMaxWait to 250. +#ifndef defaultMaxWait // Let's allow the user to define their own value if they want to +#define defaultMaxWait 1100 // TODO +#endif + + // Set gpio device, used by checksumFailurePin + bool init_gpio_pins(struct device &gpio_dev); + + //By default use the default I2C address, and use Wire port + bool begin(struct device &i2c_dev, uint8_t deviceAddress = 0x42); //Returns true if module is detected + //serialPort needs to be perviously initialized to correct baud rate + //bool begin(Stream &serialPort); //Returns true if module is detected // TODO + + //Returns true if device answers on _gpsI2Caddress address or via Serial + //maxWait is only used for Serial + bool isConnected(uint16_t maxWait = 1100); + + //Changed in V1.8.1: provides backward compatibility for the examples that call checkUblox directly + //Will default to using packetCfg to look for explicit autoPVT packets so they get processed correctly by processUBX + bool checkUblox(uint8_t requestedClass = UBX_CLASS_NAV, uint8_t requestedID = UBX_NAV_PVT); //Checks module with user selected commType + + bool checkUbloxI2C(ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID); //Method for I2C polling of data, passing any new bytes to process() + bool checkUbloxSerial(ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID); //Method for serial polling of data, passing any new bytes to process() + + void process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID); //Processes NMEA and UBX binary sentences one byte at a time + void processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID); //Given a character, file it away into the uxb packet structure + void processRTCMframe(uint8_t incoming); //Monitor the incoming bytes for start and length bytes + void processRTCM(uint8_t incoming) __attribute__((weak)); //Given rtcm byte, do something with it. User can overwrite if desired to pipe bytes to radio, internet, etc. + + void processUBXpacket(ubxPacket *msg); //Once a packet has been received and validated, identify this packet's class/id and update internal flags + void processNMEA(char incoming) __attribute__((weak)); //Given a NMEA character, do something with it. User can overwrite if desired to use something like tinyGPS or MicroNMEA libraries + + void calcChecksum(ubxPacket *msg); //Sets the checksumA and checksumB of a given messages + sfe_ublox_status_e sendCommand(ubxPacket *outgoingUBX, uint16_t maxWait = defaultMaxWait); //Given a packet and payload, send everything including CRC bytes, return true if we got a response + sfe_ublox_status_e sendI2cCommand(ubxPacket *outgoingUBX, uint16_t maxWait = 250); + void sendSerialCommand(ubxPacket *outgoingUBX); + + void printPacket(ubxPacket *packet); //Useful for debugging + + void factoryReset(); //Send factory reset sequence (i.e. load "default" configuration and perform hardReset) + void hardReset(); //Perform a reset leading to a cold start (zero info start-up) + + int transferWriteI2C(u8_t *buf, u32_t num_bytes, bool stop = true); // Port to Zephyr, i2c function to actualy WRITE data + int transferReadI2C(u8_t *buf, u32_t num_bytes); // Port to Zephyr, i2c function to actualy READ data + + bool setI2CAddress(uint8_t deviceAddress, uint16_t maxTime = 250); //Changes the I2C address of the Ublox module + void setSerialRate(uint32_t baudrate, uint8_t uartPort = COM_PORT_UART1, uint16_t maxTime = defaultMaxWait); //Changes the serial baud rate of the Ublox module, uartPort should be COM_PORT_UART1/2 + void setNMEAOutputPort(); //Sets the internal variable for the port to direct NMEA characters to + + bool setNavigationFrequency(uint8_t navFreq, uint16_t maxWait = defaultMaxWait); //Set the number of nav solutions sent per second + uint8_t getNavigationFrequency(uint16_t maxWait = defaultMaxWait); //Get the number of nav solutions sent per second currently being output by module + bool saveConfiguration(uint16_t maxWait = defaultMaxWait); //Save current configuration to flash and BBR (battery backed RAM) + bool factoryDefault(uint16_t maxWait = defaultMaxWait); //Reset module to factory defaults + bool saveConfigSelective(uint32_t configMask, uint16_t maxWait = defaultMaxWait); //Save the selected configuration sub-sections to flash and BBR (battery backed RAM) + + sfe_ublox_status_e waitForACKResponse(ubxPacket *outgoingUBX, uint8_t requestedClass, uint8_t requestedID, uint16_t maxTime = defaultMaxWait); //Poll the module until a config packet and an ACK is received + sfe_ublox_status_e waitForNoACKResponse(ubxPacket *outgoingUBX, uint8_t requestedClass, uint8_t requestedID, uint16_t maxTime = defaultMaxWait); //Poll the module until a config packet is received + +// getPVT will only return data once in each navigation cycle. By default, that is once per second. +// Therefore we should set getPVTmaxWait to slightly longer than that. +// If you change the navigation frequency to (e.g.) 4Hz using setNavigationFrequency(4) +// then you should use a shorter maxWait for getPVT. 300msec would be about right: getPVT(300) +// The same is true for getHPPOSLLH. +#define getPVTmaxWait 1100 // Default maxWait for getPVT and all functions which call it +#define getHPPOSLLHmaxWait 1100 // Default maxWait for getHPPOSLLH and all functions which call it + + bool assumeAutoPVT(bool enabled, bool implicitUpdate = true); //In case no config access to the GPS is possible and PVT is send cyclically already + bool setAutoPVT(bool enabled, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic PVT reports at the navigation frequency + bool getPVT(uint16_t maxWait = getPVTmaxWait); //Query module for latest group of datums and load global vars: lat, long, alt, speed, SIV, accuracies, etc. If autoPVT is disabled, performs an explicit poll and waits, if enabled does not block. Retruns true if new PVT is available. + bool setAutoPVT(bool enabled, bool implicitUpdate, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic PVT reports at the navigation frequency, with implicitUpdate == false accessing stale data will not issue parsing of data in the rxbuffer of your interface, instead you have to call checkUblox when you want to perform an update + bool getHPPOSLLH(uint16_t maxWait = getHPPOSLLHmaxWait); //Query module for latest group of datums and load global vars: lat, long, alt, speed, SIV, accuracies, etc. If autoPVT is disabled, performs an explicit poll and waits, if enabled does not block. Retruns true if new PVT is available. + void flushPVT(); //Mark all the PVT data as read/stale. This is handy to get data alignment after CRC failure + + int32_t getLatitude(uint16_t maxWait = getPVTmaxWait); //Returns the current latitude in degrees * 10^-7. Auto selects between HighPrecision and Regular depending on ability of module. + int32_t getLongitude(uint16_t maxWait = getPVTmaxWait); //Returns the current longitude in degrees * 10-7. Auto selects between HighPrecision and Regular depending on ability of module. + int32_t getAltitude(uint16_t maxWait = getPVTmaxWait); //Returns the current altitude in mm above ellipsoid + int32_t getAltitudeMSL(uint16_t maxWait = getPVTmaxWait); //Returns number of sats used in fix + uint8_t getSIV(uint16_t maxWait = getPVTmaxWait); //Returns number of sats used in fix + uint8_t getFixType(uint16_t maxWait = getPVTmaxWait); //Returns the type of fix: 0=no, 3=3D, 4=GNSS+Deadreckoning + uint8_t getCarrierSolutionType(uint16_t maxWait = getPVTmaxWait); //Returns RTK solution: 0=no, 1=float solution, 2=fixed solution + int32_t getGroundSpeed(uint16_t maxWait = getPVTmaxWait); //Returns speed in mm/s + int32_t getHeading(uint16_t maxWait = getPVTmaxWait); //Returns heading in degrees * 10^-7 + uint16_t getPDOP(uint16_t maxWait = getPVTmaxWait); //Returns positional dillution of precision * 10^-2 + uint16_t getYear(uint16_t maxWait = getPVTmaxWait); + uint8_t getMonth(uint16_t maxWait = getPVTmaxWait); + uint8_t getDay(uint16_t maxWait = getPVTmaxWait); + uint8_t getHour(uint16_t maxWait = getPVTmaxWait); + uint8_t getMinute(uint16_t maxWait = getPVTmaxWait); + uint8_t getSecond(uint16_t maxWait = getPVTmaxWait); + uint16_t getMillisecond(uint16_t maxWait = getPVTmaxWait); + int32_t getNanosecond(uint16_t maxWait = getPVTmaxWait); + uint32_t getTimeOfWeek(uint16_t maxWait = getPVTmaxWait); + bool getDateValid(uint16_t maxWait = getPVTmaxWait); + bool getTimeValid(uint16_t maxWait = getPVTmaxWait); + + int32_t getHighResLatitude(uint16_t maxWait = getHPPOSLLHmaxWait); + int8_t getHighResLatitudeHp(uint16_t maxWait = getHPPOSLLHmaxWait); + int32_t getHighResLongitude(uint16_t maxWait = getHPPOSLLHmaxWait); + int8_t getHighResLongitudeHp(uint16_t maxWait = getHPPOSLLHmaxWait); + int32_t getElipsoid(uint16_t maxWait = getHPPOSLLHmaxWait); + int8_t getElipsoidHp(uint16_t maxWait = getHPPOSLLHmaxWait); + int32_t getMeanSeaLevel(uint16_t maxWait = getHPPOSLLHmaxWait); + int8_t getMeanSeaLevelHp(uint16_t maxWait = getHPPOSLLHmaxWait); + int32_t getGeoidSeparation(uint16_t maxWait = getHPPOSLLHmaxWait); + uint32_t getHorizontalAccuracy(uint16_t maxWait = getHPPOSLLHmaxWait); + uint32_t getVerticalAccuracy(uint16_t maxWait = getHPPOSLLHmaxWait); + + //Port configurations + bool setPortOutput(uint8_t portID, uint8_t comSettings, uint16_t maxWait = defaultMaxWait); //Configure a given port to output UBX, NMEA, RTCM3 or a combination thereof + bool setPortInput(uint8_t portID, uint8_t comSettings, uint16_t maxWait = defaultMaxWait); //Configure a given port to input UBX, NMEA, RTCM3 or a combination thereof + bool getPortSettings(uint8_t portID, uint16_t maxWait = defaultMaxWait); //Returns the current protocol bits in the UBX-CFG-PRT command for a given port + + bool setI2COutput(uint8_t comSettings, uint16_t maxWait = 250); //Configure I2C port to output UBX, NMEA, RTCM3 or a combination thereof + bool setUART1Output(uint8_t comSettings, uint16_t maxWait = defaultMaxWait); //Configure UART1 port to output UBX, NMEA, RTCM3 or a combination thereof + bool setUART2Output(uint8_t comSettings, uint16_t maxWait = defaultMaxWait); //Configure UART2 port to output UBX, NMEA, RTCM3 or a combination thereof + bool setUSBOutput(uint8_t comSettings, uint16_t maxWait = 250); //Configure USB port to output UBX, NMEA, RTCM3 or a combination thereof + bool setSPIOutput(uint8_t comSettings, uint16_t maxWait = 250); //Configure SPI port to output UBX, NMEA, RTCM3 or a combination thereof + + //Functions to turn on/off message types for a given port ID (see COM_PORT_I2C, etc above) + bool configureMessage(uint8_t msgClass, uint8_t msgID, uint8_t portID, uint8_t sendRate, uint16_t maxWait = defaultMaxWait); + bool enableMessage(uint8_t msgClass, uint8_t msgID, uint8_t portID, uint8_t sendRate = 1, uint16_t maxWait = defaultMaxWait); + bool disableMessage(uint8_t msgClass, uint8_t msgID, uint8_t portID, uint16_t maxWait = defaultMaxWait); + bool enableNMEAMessage(uint8_t msgID, uint8_t portID, uint8_t sendRate = 1, uint16_t maxWait = defaultMaxWait); + bool disableNMEAMessage(uint8_t msgID, uint8_t portID, uint16_t maxWait = defaultMaxWait); + bool enableRTCMmessage(uint8_t messageNumber, uint8_t portID, uint8_t sendRate, uint16_t maxWait = defaultMaxWait); //Given a message number turns on a message ID for output over given PortID + bool disableRTCMmessage(uint8_t messageNumber, uint8_t portID, uint16_t maxWait = defaultMaxWait); //Turn off given RTCM message from a given port + + //General configuration (used only on protocol v27 and higher - ie, ZED-F9P) + //It is probably safe to assume that users ofovaj the ZED-F9P will be using I2C / Qwiic. + //If they are using Serial then the higher baud rate will also help. So let's leave maxWait set to 250ms. + uint8_t getVal8(uint16_t group, uint16_t id, uint8_t size, uint8_t layer = VAL_LAYER_BBR, uint16_t maxWait = 250); //Returns the value at a given group/id/size location + uint8_t getVal8(uint32_t keyID, uint8_t layer = VAL_LAYER_BBR, uint16_t maxWait = 250); //Returns the value at a given group/id/size location + uint8_t setVal(uint32_t keyID, uint16_t value, uint8_t layer = VAL_LAYER_BBR, uint16_t maxWait = 250); //Sets the 16-bit value at a given group/id/size location + uint8_t setVal8(uint32_t keyID, uint8_t value, uint8_t layer = VAL_LAYER_BBR, uint16_t maxWait = 250); //Sets the 8-bit value at a given group/id/size location + uint8_t setVal16(uint32_t keyID, uint16_t value, uint8_t layer = VAL_LAYER_BBR, uint16_t maxWait = 250); //Sets the 16-bit value at a given group/id/size location + uint8_t setVal32(uint32_t keyID, uint32_t value, uint8_t layer = VAL_LAYER_BBR, uint16_t maxWait = 250); //Sets the 32-bit value at a given group/id/size location + uint8_t newCfgValset8(uint32_t keyID, uint8_t value, uint8_t layer = VAL_LAYER_BBR); //Define a new UBX-CFG-VALSET with the given KeyID and 8-bit value + uint8_t newCfgValset16(uint32_t keyID, uint16_t value, uint8_t layer = VAL_LAYER_BBR); //Define a new UBX-CFG-VALSET with the given KeyID and 16-bit value + uint8_t newCfgValset32(uint32_t keyID, uint32_t value, uint8_t layer = VAL_LAYER_BBR); //Define a new UBX-CFG-VALSET with the given KeyID and 32-bit value + uint8_t addCfgValset8(uint32_t keyID, uint8_t value); //Add a new KeyID and 8-bit value to an existing UBX-CFG-VALSET ubxPacket + uint8_t addCfgValset16(uint32_t keyID, uint16_t value); //Add a new KeyID and 16-bit value to an existing UBX-CFG-VALSET ubxPacket + uint8_t addCfgValset32(uint32_t keyID, uint32_t value); //Add a new KeyID and 32-bit value to an existing UBX-CFG-VALSET ubxPacket + uint8_t sendCfgValset8(uint32_t keyID, uint8_t value, uint16_t maxWait = 250); //Add the final KeyID and 8-bit value to an existing UBX-CFG-VALSET ubxPacket and send it + uint8_t sendCfgValset16(uint32_t keyID, uint16_t value, uint16_t maxWait = 250); //Add the final KeyID and 16-bit value to an existing UBX-CFG-VALSET ubxPacket and send it + uint8_t sendCfgValset32(uint32_t keyID, uint32_t value, uint16_t maxWait = 250); //Add the final KeyID and 32-bit value to an existing UBX-CFG-VALSET ubxPacket and send it + + //Functions used for RTK and base station setup + //It is probably safe to assume that users of the RTK will be using I2C / Qwiic. So let's leave maxWait set to 250ms. + bool getSurveyMode(uint16_t maxWait = 250); //Get the current TimeMode3 settings + bool setSurveyMode(uint8_t mode, uint16_t ovajobservationTime, float requiredAccuracy, uint16_t maxWait = 250); //Control survey in mode + bool enableSurveyMode(uint16_t observationTime, float requiredAccuracy, uint16_t maxWait = 250); //Begin Survey-In for NEO-M8P + bool disableSurveyMode(uint16_t maxWait = 250); //Stop Survey-In mode + + bool getSurveyStatus(uint16_t maxWait); //Reads survey in status and sets the global variables + + uint32_t getPositionAccuracy(uint16_t maxWait = 1100); //Returns the 3D accuracy of the current high-precision fix, in mm. Supported on NEO-M8P, ZED-F9P, + + uint8_t getProtocolVersionHigh(uint16_t maxWait = 500); //Returns the PROTVER XX.00 from UBX-MON-VER register + uint8_t getProtocolVersionLow(uint16_t maxWait = 500); //Returns the PROTVER 00.XX from UBX-MON-VER register + bool getProtocolVersion(uint16_t maxWait = 500); //Queries module, loads low/high bytes + + bool getRELPOSNED(uint16_t maxWait = 1100); //Get Relative Positioning Information of the NED frame + + void enableDebugging(bool printLimitedDebug = false); //Given a port to print to, enable debug messages. Default to all, not limited. + void disableDebugging(void); //Turn off debug statements + void debugPrint(char *message); //Safely print debug statements + void debugPrintln(char *message); //Safely print debug statements + const char *statusString(sfe_ublox_status_e stat); //Pretty print the return value + + //Support for geofences + bool addGeofence(int32_t latitude, int32_t longitude, uint32_t radius, uint8_t confidence = 0, uint8_t pinPolarity = 0, uint8_t pin = 0, uint16_t maxWait = 1100); // Add a new geofence + bool clearGeofences(uint16_t maxWait = 1100); //Clears all geofences + bool getGeofenceState(geofenceState ¤tGeofenceState, uint16_t maxWait = 1100); //Returns the combined geofence state + bool clearAntPIO(uint16_t maxWait = 1100); //Clears the antenna control pin settings to release the PIOs + geofenceParams currentGeofenceParams; // Global to store the geofence parameters + + bool powerSaveMode(bool power_save = true, uint16_t maxWait = 1100); + uint8_t getPowerSaveMode(uint16_t maxWait = 1100); // Returns 255 if the sendCommand fails + bool powerOff(uint32_t durationInMs, uint16_t maxWait = 1100); + bool powerOffWithInterrupt(uint32_t durationInMs, uint32_t wakeupSources = VAL_RXM_PMREQ_WAKEUPSOURCE_EXTINT0, bool forceWhileUsb = true, uint16_t maxWait = 1100); + + //Change the dynamic platform model using UBX-CFG-NAV5 + bool setDynamicModel(dynModel newDynamicModel = DYN_MODEL_PORTABLE, uint16_t maxWait = 1100); + uint8_t getDynamicModel(uint16_t maxWait = 1100); // Get the dynamic model - returns 255 if the sendCommand fails + + bool getEsfInfo(uint16_t maxWait = 1100); + bool getEsfIns(uint16_t maxWait = 1100); + bool getEsfDataInfo(uint16_t maxWait = 1100); + bool getEsfRawDataInfo(uint16_t maxWait = 1100); + sfe_ublox_status_e getSensState(uint8_t sensor, uint16_t maxWait = 1100); + bool getVehAtt(uint16_t maxWait = 1100); + + //Survey-in specific controls + struct svinStructure + { + bool active; + bool valid; + uint16_t observationTime; + float meanAccuracy; + } svin; + + //Relative Positioning Info in NED frame specific controls + struct frelPosInfoStructure + { + uint16_t refStationID; + + float relPosN; + float relPosE; + float relPosD; + + long relPosLength; + long relPosHeading; + + int8_t relPosHPN; + int8_t relPosHPE; + int8_t relPosHPD; + int8_t relPosHPLength; + + float accN; + float accE; + float accD; + + bool gnssFixOk; + bool diffSoln; + bool relPosValid; + uint8_t carrSoln; + bool isMoving; + bool refPosMiss; + bool refObsMiss; + } relPosInfo; + + //The major datums we want to globally store + uint16_t gpsYear; + uint8_t gpsMonth; + uint8_t gpsDay; + uint8_t gpsHour; + uint8_t gpsMinute; + uint8_t gpsSecond; + uint16_t gpsMillisecond; + int32_t gpsNanosecond; + bool gpsDateValid; + bool gpsTimeValid; + + int32_t latitude; //Degrees * 10^-7 (more accurate than floats) + int32_t longitude; //Degrees * 10^-7 (more accurate than floats) + int32_t altitude; //Number of mm above ellipsoid + int32_t altitudeMSL; //Number of mm above Mean Sea Level + uint8_t SIV; //Number of satellites used in position solution + uint8_t fixType; //Tells us when we have a solution aka lock + uint8_t carrierSolution; //Tells us when we have an RTK float/fixed solution + int32_t groundSpeed; //mm/s + int32_t headingOfMotion; //degrees * 10^-5 + uint16_t pDOP; //Positional dilution of precision + uint8_t versionLow; //Loaded from getProtocolVersion(). + uint8_t versionHigh; + + uint32_t timeOfWeek; // ms + int32_t highResLatitude; // Degrees * 10^-7 + int32_t highResLongitude; // Degrees * 10^-7 + int32_t elipsoid; // Height above ellipsoid in mm (Typo! Should be eLLipsoid! **Uncorrected for backward-compatibility.**) + int32_t meanSeaLevel; // Height above mean sea level in mm + int32_t geoidSeparation; // This seems to only be provided in NMEA GGA and GNS messages + uint32_t horizontalAccuracy; // mm * 10^-1 (i.e. 0.1mm) + uint32_t verticalAccuracy; // mm * 10^-1 (i.e. 0.1mm) + int8_t elipsoidHp; // High precision component of the height above ellipsoid in mm * 10^-1 (Deliberate typo! Should be eLLipsoidHp!) + int8_t meanSeaLevelHp; // High precision component of Height above mean sea level in mm * 10^-1 + int8_t highResLatitudeHp; // High precision component of latitude: Degrees * 10^-9 + int8_t highResLongitudeHp; // High precision component of longitude: Degrees * 10^-9 + + uint16_t rtcmFrameCounter = 0; //Tracks the type of incoming byte inside RTCM frame + +#define DEF_NUM_SENS 7 + struct deadReckData + { + uint8_t version; + uint8_t fusionMode; + + uint8_t xAngRateVald; + uint8_t yAngRateVald; + uint8_t zAngRateVald; + uint8_t xAccelVald; + uint8_t yAccelVald; + uint8_t zAccelVald; + + int32_t xAngRate; + int32_t yAngRate; + int32_t zAngRate; + + int32_t xAccel; + int32_t yAccel; + int32_t zAccel; + + // The array size is based on testing directly on M8U and F9R + uint32_t rawData; + uint32_t rawDataType; + uint32_t rawTStamp; + + uint32_t data[DEF_NUM_SENS]; + uint32_t dataType[DEF_NUM_SENS]; + uint32_t dataTStamp[DEF_NUM_SENS]; + } imuMeas; + + struct indivImuData + { + + uint8_t numSens; + + uint8_t senType; + bool isUsed; + bool isReady; + uint8_t calibStatus; + uint8_t timeStatus; + + uint8_t freq; // Hz + + bool badMeas; + bool badTag; + bool missMeas; + bool noisyMeas; + } ubloxSen; + + struct vehicleAttitude + { + // All values in degrees + int32_t roll; + int32_t pitch; + int32_t heading; + uint32_t accRoll; + uint32_t accPitch; + uint32_t accHeading; + } vehAtt; + +private: + //Depending on the sentence type the processor will load characters into different arrays + enum SentenceTypes + { + NONE = 0, + NMEA, + UBX, + RTCM + } currentSentence = NONE; + + //Depending on the ubx binary response class, store binary responses into different places + enum classTypes + { + CLASS_NONE = 0, + CLASS_ACK, + CLASS_NOT_AN_ACK + } ubxFrameClass = CLASS_NONE; + + enum commTypes + { + COMM_TYPE_I2C = 0, + COMM_TYPE_SERIAL, + COMM_TYPE_SPI + } commType = COMM_TYPE_I2C; //Controls which port we look to for incoming bytes + + //Functions + bool checkUbloxInternal(ubxPacket *incomingUBX, uint8_t requestedClass = 255, uint8_t requestedID = 255); //Checks module with user selected commType + uint32_t extractLong(uint8_t spotToStart); //Combine four bytes from payload into long + uint16_t extractInt(uint8_t spotToStart); //Combine two bytes from payload into int + uint8_t extractByte(uint8_t spotToStart); //Get byte from payload + int8_t extractSignedChar(uint8_t spotToStart); //Get signed 8-bit value from payload + void addToChecksum(uint8_t incoming); //Given an incoming byte, adjust rollingChecksumA/B + + //Variables + struct device *_gpio_dev; // GPIO device + struct device *_i2cPort; //The generic connection to user's chosen I2C hardware + //Stream *_serialPort; //The generic connection to user's chosen Serial hardware + bool _nmeaOutputPort = false; //The user can assign an output port to print NMEA sentences if they wish - ported to print to console, converted to flag + //Stream *_debugSerial; //The stream to send debug messages to if enabled + + uint8_t _gpsI2Caddress = 0x42; //Default 7-bit unshifted address of the ublox 6/7/8/M8/F9 series + //This can be changed using the ublox configuration software + + bool _printDebug = false; //Flag to print the serial commands we are sending to the Serial port for debug + bool _printLimitedDebug = false; //Flag to print limited debug messages. Useful for I2C debugging or high navigation rates + + //The packet buffers + //These are pointed at from within the ubxPacket + uint8_t payloadAck[2]; // Holds the requested ACK/NACK + uint8_t payloadCfg[MAX_PAYLOAD_SIZE]; // Holds the requested data packet + uint8_t payloadBuf[2]; // Temporary buffer used to screen incoming packets or dump unrequested packets + + //Init the packet structures and init them with pointers to the payloadAck, payloadCfg and payloadBuf arrays + ubxPacket packetAck = {0, 0, 0, 0, 0, payloadAck, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED}; + ubxPacket packetCfg = {0, 0, 0, 0, 0, payloadCfg, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED}; + ubxPacket packetBuf = {0, 0, 0, 0, 0, payloadBuf, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED}; + + //Flag if this packet is unrequested (and so should be ignored and not copied into packetCfg or packetAck) + bool ignoreThisPayload = false; + + //Identify which buffer is in use + //Data is stored in packetBuf until the requested class and ID can be validated + //If a match is seen, data is diverted into packetAck or packetCfg + sfe_ublox_packet_buffer_e activePacketBuffer = SFE_UBLOX_PACKET_PACKETBUF; + + //Limit checking of new data to every X ms + //If we are expecting an update every X Hz then we should check every half that amount of time + //Otherwise we may block ourselves from seeing new data + uint8_t i2cPollingWait = 100; //Default to 100ms. Adjusted when user calls setNavigationFrequency() + + unsigned long lastCheck = 0; + bool autoPVT = false; //Whether autoPVT is enabled or not + bool autoPVTImplicitUpdate = true; // Whether autoPVT is triggered by accessing stale data (=true) or by a call to checkUblox (=false) + uint16_t ubxFrameCounter; //It counts all UBX frame. [Fixed header(2bytes), CLS(1byte), ID(1byte), length(2bytes), payload(x bytes), checksums(2bytes)] + + uint8_t rollingChecksumA; //Rolls forward as we receive incoming bytes. Checked against the last two A/B checksum bytes + uint8_t rollingChecksumB; //Rolls forward as we receive incoming bytes. Checked against the last two A/B checksum bytes + + //Create bit field for staleness of each datum in PVT we want to monitor + //moduleQueried.latitude goes true each time we call getPVT() + //This reduces the number of times we have to call getPVT as this can take up to ~1s per read + //depending on update rate + struct + { + uint32_t gpsiTOW : 1; + uint32_t gpsYear : 1; + uint32_t gpsMonth : 1; + uint32_t gpsDay : 1; + uint32_t gpsHour : 1; + uint32_t gpsMinute : 1; + uint32_t gpsSecond : 1; + uint32_t gpsDateValid : 1; + uint32_t gpsTimeValid : 1; + uint32_t gpsNanosecond : 1; + + uint32_t all : 1; + uint32_t longitude : 1; + uint32_t latitude : 1; + uint32_t altitude : 1; + uint32_t altitudeMSL : 1; + uint32_t SIV : 1; + uint32_t fixType : 1; + uint32_t carrierSolution : 1; + uint32_t groundSpeed : 1; + uint32_t headingOfMotion : 1; + uint32_t pDOP : 1; + uint32_t versionNumber : 1; + } moduleQueried; + + struct + { + uint16_t all : 1; + uint16_t timeOfWeek : 1; + uint16_t highResLatitude : 1; + uint16_t highResLongitude : 1; + uint16_t elipsoid : 1; + uint16_t meanSeaLevel : 1; + uint16_t geoidSeparation : 1; // Redundant but kept for backward-compatibility + uint16_t horizontalAccuracy : 1; + uint16_t verticalAccuracy : 1; + uint16_t elipsoidHp : 1; + uint16_t meanSeaLevelHp : 1; + uint16_t highResLatitudeHp : 1; + uint16_t highResLongitudeHp : 1; + } highResModuleQueried; + + uint16_t rtcmLen = 0; +}; + +#endif diff --git a/src/ublox_lib_interface.cpp b/src/ublox_lib_interface.cpp new file mode 100644 index 0000000..62b1019 --- /dev/null +++ b/src/ublox_lib_interface.cpp @@ -0,0 +1,75 @@ +#include "ublox_lib_interface.h" + +#include "SparkFun_Ublox_Zephyr_Library.h" + + +SFE_UBLOX_GPS myGPS; +long lastTime = 0; //Simple local timer. Limits amount if I2C traffic to Ublox module. + +uint8_t set_gpio_dev(struct device *gpio_dev) { + if (myGPS.init_gpio_pins(*gpio_dev) == false) { + return -1; + } + + // This will pipe all NMEA sentences to the serial port so we can see them + //myGPS.setNMEAOutputPort(); // uncomment this to see raw prints + // turn on debugging + myGPS.enableDebugging(); + return 0; +} + +uint8_t gps_begin(struct device *i2c_dev) { + if (myGPS.begin(*i2c_dev) == false) { + return -1; + } + + myGPS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise) + myGPS.saveConfiguration(); //Save the current settings to flash and BBR + + return 0; +} + +void check_ublox(void) { + myGPS.checkUblox(); +} + +void get_position(void) +{ + //Query module only every second. Doing it more often will just cause I2C traffic. + //The module only responds when a new position is available + if (k_uptime_get_32() - lastTime > 1000) + { + lastTime = k_uptime_get_32(); //Update the timer + + long latitude = myGPS.getLatitude(); + long longitude = myGPS.getLongitude(); + long altitude = myGPS.getAltitude(); + + uint8_t SIV = myGPS.getSIV(); + + printk("Position: Lat: %ld, Lon: %ld, Alt: %ld, SIV: %d\n", latitude, longitude, altitude, SIV); + } +} + +void get_datetime(void) { + int year = myGPS.getYear(); + int month = myGPS.getMonth(); + int day = myGPS.getDay(); + int hour = myGPS.getHour(); + int minute = myGPS.getMinute(); + int second = myGPS.getSecond(); + + printk("DateTime: %d-%d-%d %d:%d:%d\n", year, month, day, hour, minute, second); + + printk("Time is "); + if (myGPS.getTimeValid() == false) + { + printk("not "); + } + printk("valid. Date is "); + if (myGPS.getDateValid() == false) + { + printk("not "); + } + printk("valid.\n"); +} \ No newline at end of file diff --git a/src/ublox_lib_interface.h b/src/ublox_lib_interface.h new file mode 100644 index 0000000..3f1935b --- /dev/null +++ b/src/ublox_lib_interface.h @@ -0,0 +1,21 @@ +#include + +#ifndef _UBLOX_LIB_INTERFACE_H_ +#define _UBLOX_LIB_INTERFACE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + uint8_t set_gpio_dev(struct device *gpio_dev); + + uint8_t gps_begin(struct device *i2c_dev); + void check_ublox(void); + void get_position(void); + void get_datetime(void); + +#ifdef __cplusplus +} +#endif + +#endif // _UBLOX_LIB_INTERFACE_H_ \ No newline at end of file From 1fe56ab6c074d186d4a655e541d024a4621ad364 Mon Sep 17 00:00:00 2001 From: vid553 Date: Fri, 4 Sep 2020 00:12:48 +0200 Subject: [PATCH 02/36] add comment --- examples/Example_Zephyr/src/main.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/Example_Zephyr/src/main.c b/examples/Example_Zephyr/src/main.c index 3d376c7..74ff49f 100644 --- a/examples/Example_Zephyr/src/main.c +++ b/examples/Example_Zephyr/src/main.c @@ -11,6 +11,8 @@ Open the serial monitor at 115200 baud to see the output I2C clock speed: 100 kHz + + Ported to Zephyr by Vid Rajtmajer , IRNAS d.o.o. */ #include From e25cfca1abe23a27f31f081db3d20314c9ad94db Mon Sep 17 00:00:00 2001 From: PaulZC Date: Wed, 28 Oct 2020 09:13:27 +0000 Subject: [PATCH 03/36] Adding all the keys... See description for full details. This commit adds _all_ the keys from the ZED-F9P Interface Description. The ones Morten extracted plus all the others. They're included as const uint32_t. I was puzzled about why there were no duplications with the existing keys. And the reason is that the existing keys are all prefixed with UBLOX_. So, we have a decision to make. Leave them as-is? Or add UBLOX_ to all of them? Or remove UBLOX_ from all of them? --- src/u-blox_config_keys.h | 368 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 367 insertions(+), 1 deletion(-) diff --git a/src/u-blox_config_keys.h b/src/u-blox_config_keys.h index 3b1c0c6..76fd049 100644 --- a/src/u-blox_config_keys.h +++ b/src/u-blox_config_keys.h @@ -158,7 +158,103 @@ const uint32_t UBLOX_CFG_USBOUTPROT_UBX = 0x10780001; const uint32_t UBLOX_CFG_USBOUTPROT_NMEA = 0x10780002; const uint32_t UBLOX_CFG_USBOUTPROT_RTCM3X = 0x10780004; +//CFG-BDS: BeiDou system configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_BDS_USE_PRN_1_TO_5 = 0x10340014; // Use BeiDou geostationary satellites (PRN 1-5) + +//CFG-GEOFENCE: Geofencing configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_GEOFENCE_CONFLVL = 0x20240011; // Required confidence level for state evaluation +const uint32_t CFG_GEOFENCE_USE_PIO = 0x10240012; // Use PIO combined fence state output +const uint32_t CFG_GEOFENCE_PINPOL = 0x20240013; // PIO pin polarity +const uint32_t CFG_GEOFENCE_PIN = 0x20240014; // PIO pin number +const uint32_t CFG_GEOFENCE_USE_FENCE1 = 0x10240020; // Use frst geofence +const uint32_t CFG_GEOFENCE_FENCE1_LAT = 0x40240021; // Latitude of the first geofence circle center +const uint32_t CFG_GEOFENCE_FENCE1_LON = 0x40240022; // Longitude of the first geofence circle center +const uint32_t CFG_GEOFENCE_FENCE1_RAD = 0x40240023; // Radius of the first geofence circle +const uint32_t CFG_GEOFENCE_USE_FENCE2 = 0x10240030; // Use second geofence +const uint32_t CFG_GEOFENCE_FENCE2_LAT = 0x40240031; // Latitude of the second geofence circle center +const uint32_t CFG_GEOFENCE_FENCE2_LON = 0x40240032; // Longitude of the second geofence circle center +const uint32_t CFG_GEOFENCE_FENCE2_RAD = 0x40240033; // Radius of the second geofence circle +const uint32_t CFG_GEOFENCE_USE_FENCE3 = 0x10240040; // Use third geofence +const uint32_t CFG_GEOFENCE_FENCE3_LAT = 0x40240041; // Latitude of the third geofence circle center +const uint32_t CFG_GEOFENCE_FENCE3_LON = 0x40240042; // Longitude of the third geofence circle center +const uint32_t CFG_GEOFENCE_FENCE3_RAD = 0x40240043; // Radius of the third geofence circle +const uint32_t CFG_GEOFENCE_USE_FENCE4 = 0x10240050; // Use fourth geofence +const uint32_t CFG_GEOFENCE_FENCE4_LAT = 0x40240051; // Latitude of the fourth geofence circle center +const uint32_t CFG_GEOFENCE_FENCE4_LON = 0x40240052; // Longitude of the fourth geofence circle center +const uint32_t CFG_GEOFENCE_FENCE4_RAD = 0x40240053; // Radius of the fourth geofence circle + +//CFG-HW: Hardware configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_HW_ANT_CFG_VOLTCTRL = 0x10a3002e; // Active antenna voltage control flag +const uint32_t CFG_HW_ANT_CFG_SHORTDET = 0x10a3002f; // Short antenna detection flag +const uint32_t CFG_HW_ANT_CFG_SHORTDET_POL = 0x10a30030; // Short antenna detection polarity +const uint32_t CFG_HW_ANT_CFG_OPENDET = 0x10a30031; // Open antenna detection flag +const uint32_t CFG_HW_ANT_CFG_OPENDET_POL = 0x10a30032; // Open antenna detection polarity +const uint32_t CFG_HW_ANT_CFG_PWRDOWN = 0x10a30033; // Power down antenna flag +const uint32_t CFG_HW_ANT_CFG_PWRDOWN_POL = 0x10a30034; // Power down antenna logic polarity +const uint32_t CFG_HW_ANT_CFG_RECOVER = 0x10a30035; // Automatic recovery from short state flag +const uint32_t CFG_HW_ANT_SUP_SWITCH_PIN = 0x20a30036; // ANT1 PIO number +const uint32_t CFG_HW_ANT_SUP_SHORT_PIN = 0x20a30037; // ANT0 PIO number +const uint32_t CFG_HW_ANT_SUP_OPEN_PIN = 0x20a30038; // ANT2 PIO number +const uint32_t CFG_HW_ANT_SUP_ENGINE = 0x20a30054; // Antenna supervisor engine selection +const uint32_t CFG_HW_ANT_SUP_SHORT_THR = 0x20a30055; // Antenna supervisor MADC engine short detection threshold +const uint32_t CFG_HW_ANT_SUP_OPEN_THR = 0x20a30056; // Antenna supervisor MADC engine open detection threshold + +//CFG-I2C: Configuration of the I2C interface +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_I2C_ADDRESS = 0x20510001; // I2C slave address of the receiver (7 bits) +const uint32_t CFG_I2C_EXTENDEDTIMEOUT = 0x10510002; // Flag to disable timeouting the interface after 1.5 s +const uint32_t CFG_I2C_ENABLED = 0x10510003; // Flag to indicate if the I2C interface should be enabled + +//CFG-I2CINPROT: Input protocol configuration of the I2C interface +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_I2CINPROT_UBX = 0x10710001; // Flag to indicate if UBX should be an input protocol on I2C +const uint32_t CFG_I2CINPROT_NMEA = 0x10710002; // Flag to indicate if NMEA should be an input protocol on I2C +const uint32_t CFG_I2CINPROT_RTCM3X = 0x10710004; // Flag to indicate if RTCM3X should be an input protocol on I2C + +//CFG-I2COUTPROT: Output protocol configuration of the I2C interface +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_I2COUTPROT_UBX = 0x10720001; // Flag to indicate if UBX should be an output protocol on I2C +const uint32_t CFG_I2COUTPROT_NMEA = 0x10720002; // Flag to indicate if NMEA should be an output protocol on I2C +const uint32_t CFG_I2COUTPROT_RTCM3X = 0x10720004; // Flag to indicate if RTCM3X should be an output protocol on I2C + +//CFG-INFMSG: Information message configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_INFMSG_UBX_I2C = 0x20920001; // Information message enable flags for the UBX protocol on the I2C interface +const uint32_t CFG_INFMSG_UBX_UART1 = 0x20920002; // Information message enable flags for the UBX protocol on the UART1 interface +const uint32_t CFG_INFMSG_UBX_UART2 = 0x20920003; // Information message enable flags for the UBX protocol on the UART2 interface +const uint32_t CFG_INFMSG_UBX_USB = 0x20920004; // Information message enable flags for the UBX protocol on the USB interface +const uint32_t CFG_INFMSG_UBX_SPI = 0x20920005; // Information message enable flags for the UBX protocol on the SPI interface +const uint32_t CFG_INFMSG_NMEA_I2C = 0x20920006; // Information message enable flags for the NMEA protocol on the I2C interface +const uint32_t CFG_INFMSG_NMEA_UART1 = 0x20920007; // Information message enable flags for the NMEA protocol on the UART1 interface +const uint32_t CFG_INFMSG_NMEA_UART2 = 0x20920008; // Information message enable flags for the NMEA protocol on the UART2 interface +const uint32_t CFG_INFMSG_NMEA_USB = 0x20920009; // Information message enable flags for the NMEA protocol on the USB interface +const uint32_t CFG_INFMSG_NMEA_SPI = 0x2092000a; // Information message enable flags for the NMEA protocol on the SPI interface + +//CFG-ITFM: Jamming and interference monitor configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_ITFM_BBTHRESHOLD = 0x20410001; // Broadband jamming detection threshold +const uint32_t CFG_ITFM_CWTHRESHOLD = 0x20410002; // CW jamming detection threshold +const uint32_t CFG_ITFM_ENABLE = 0x1041000d; // Enable interference detection +const uint32_t CFG_ITFM_ANTSETTING = 0x20410010; // Antenna setting +const uint32_t CFG_ITFM_ENABLE_AUX = 0x10410013; // Scan auxiliary bands + +//CFG-LOGFILTER: Data logger configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_LOGFILTER_RECORD_ENA = 0x10de0002; // Recording enabled +const uint32_t CFG_LOGFILTER_ONCE_PER_WAKE_UP_ENA = 0x10de0003; // Once per wake up +const uint32_t CFG_LOGFILTER_APPLY_ALL_FILTERS = 0x10de0004; // Apply all filter settings +const uint32_t CFG_LOGFILTER_MIN_INTERVAL = 0x30de0005; // Minimum time interval between loggedpositions +const uint32_t CFG_LOGFILTER_TIME_THRS = 0x30de0006; // Time threshold +const uint32_t CFG_LOGFILTER_SPEED_THRS = 0x30de0007; // Speed threshold +const uint32_t CFG_LOGFILTER_POSITION_THRS = 0x40de0008; // Position threshold + +//CFG-MOT: Motion detector configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_MOT_GNSSSPEED_THRS = 0x20250038; // GNSS speed threshold below which platform is considered as stationary (a.k.a. static hold threshold) +const uint32_t CFG_MOT_GNSSDIST_THRS = 0x3025003b; // Distance above which GNSS-based stationary motion is exit (a.k.a. static hold distance threshold) // CFG-MSGOUT: Message output configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- @@ -543,5 +639,275 @@ const uint32_t CFG_MSGOUT_UBX_TIM_VRFY_SPI = 0x20910096; // Output rate of the const uint32_t CFG_MSGOUT_UBX_TIM_VRFY_UART1 = 0x20910093; // Output rate of the UBX-TIM-VRFY message on port UART1 const uint32_t CFG_MSGOUT_UBX_TIM_VRFY_UART2 = 0x20910094; // Output rate of the UBX-TIM-VRFY message on port UART2 const uint32_t CFG_MSGOUT_UBX_TIM_VRFY_USB = 0x20910095; // Output rate of the UBX-TIM-VRFY message on port USB + +//CFG-NAVHPG: High precision navigation configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_NAVHPG_DGNSSMODE = 0x20140011; // Differential corrections mode + +//CFG-NAVSPG: Standard precision navigation configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_NAVSPG_FIXMODE = 0x20110011; // Position fix mode +const uint32_t CFG_NAVSPG_INIFIX3D = 0x10110013; // Initial fix must be a 3D fix +const uint32_t CFG_NAVSPG_WKNROLLOVER = 0x30110017; // GPS week rollover number +const uint32_t CFG_NAVSPG_UTCSTANDARD = 0x2011001c; // UTC standard to be used +const uint32_t CFG_NAVSPG_DYNMODEL = 0x20110021; // Dynamic platform model +const uint32_t CFG_NAVSPG_ACKAIDING = 0x10110025; // Acknowledge assistance input messages +const uint32_t CFG_NAVSPG_USE_USRDAT = 0x10110061; // Use user geodetic datum parameters +const uint32_t CFG_NAVSPG_USRDAT_MAJA = 0x50110062; // Geodetic datum semi-major axis +const uint32_t CFG_NAVSPG_USRDAT_FLAT = 0x50110063; // Geodetic datum 1.0 flattening +const uint32_t CFG_NAVSPG_USRDAT_DX = 0x40110064; // Geodetic datum X axis shift at the origin +const uint32_t CFG_NAVSPG_USRDAT_DY = 0x40110065; // Geodetic datum Y axis shift at the origin +const uint32_t CFG_NAVSPG_USRDAT_DZ = 0x40110066; // Geodetic datum Z axis shift at the origin +const uint32_t CFG_NAVSPG_USRDAT_ROTX = 0x40110067; // arcsec Geodetic datum rotation about the X axis +const uint32_t CFG_NAVSPG_USRDAT_ROTY = 0x40110068; // arcsec Geodetic datum rotation about the Y axis +const uint32_t CFG_NAVSPG_USRDAT_ROTZ = 0x40110069; // arcsec Geodetic datum rotation about the Z axis +const uint32_t CFG_NAVSPG_USRDAT_SCALE = 0x4011006a; // ppm Geodetic datum scale factor +const uint32_t CFG_NAVSPG_INFIL_MINSVS = 0x201100a1; // Minimum number of satellites for navigation +const uint32_t CFG_NAVSPG_INFIL_MAXSVS = 0x201100a2; // Maximum number of satellites for navigation +const uint32_t CFG_NAVSPG_INFIL_MINCNO = 0x201100a3; // Minimum satellite signal level for navigation +const uint32_t CFG_NAVSPG_INFIL_MINELEV = 0x201100a4; // Minimum elevation for a GNSS satellite to be used in navigation +const uint32_t CFG_NAVSPG_INFIL_NCNOTHRS = 0x201100aa; // Number of satellites required to have C/N0 above const uint32_t CFG_NAVSPG-INFIL_CNOTHRS for a fix to be attempted +const uint32_t CFG_NAVSPG_INFIL_CNOTHRS = 0x201100ab; // C/N0 threshold for deciding whether to attempt a fix +const uint32_t CFG_NAVSPG_OUTFIL_PDOP = 0x301100b1; // Output filter position DOP mask (threshold) +const uint32_t CFG_NAVSPG_OUTFIL_TDOP = 0x301100b2; // Output filter time DOP mask (threshold) +const uint32_t CFG_NAVSPG_OUTFIL_PACC = 0x301100b3; // Output filter position accuracy mask (threshold) +const uint32_t CFG_NAVSPG_OUTFIL_TACC = 0x301100b4; // Output filter time accuracy mask (threshold) +const uint32_t CFG_NAVSPG_OUTFIL_FACC = 0x301100b5; // Output filter frequency accuracy mask (threshold) +const uint32_t CFG_NAVSPG_CONSTR_ALT = 0x401100c1; // Fixed altitude (mean sea level) for 2D fix mode +const uint32_t CFG_NAVSPG_CONSTR_ALTVAR = 0x401100c2; // Fixed altitude variance for 2D mode +const uint32_t CFG_NAVSPG_CONSTR_DGNSSTO = 0x201100c4; // DGNSS timeout + +//CFG-NMEA: NMEA protocol configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_NMEA_PROTVER = 0x20930001; // NMEA protocol version +const uint32_t CFG_NMEA_MAXSVS = 0x20930002; // Maximum number of SVs to report per Talker ID +const uint32_t CFG_NMEA_COMPAT = 0x10930003; // Enable compatibility mode +const uint32_t CFG_NMEA_CONSIDER = 0x10930004; // Enable considering mode +const uint32_t CFG_NMEA_LIMIT82 = 0x10930005; // Enable strict limit to 82 characters maximum NMEA message length +const uint32_t CFG_NMEA_HIGHPREC = 0x10930006; // Enable high precision mode +const uint32_t CFG_NMEA_SVNUMBERING = 0x20930007; // Display configuration for SVs that do not have value defined in NMEA +const uint32_t CFG_NMEA_FILT_GPS = 0x10930011; // Disable reporting of GPS satellites +const uint32_t CFG_NMEA_FILT_SBAS = 0x10930012; // Disable reporting of SBAS satellites +const uint32_t CFG_NMEA_FILT_GAL = 0x10930013; // Disable reporting of Galileo satellites +const uint32_t CFG_NMEA_FILT_QZSS = 0x10930015; // Disable reporting of QZSS satellites +const uint32_t CFG_NMEA_FILT_GLO = 0x10930016; // Disable reporting of GLONASS satellites +const uint32_t CFG_NMEA_FILT_BDS = 0x10930017; // Disable reporting of BeiDou satellites +const uint32_t CFG_NMEA_OUT_INVFIX = 0x10930021; // Enable position output for failed or invalid fixes +const uint32_t CFG_NMEA_OUT_MSKFIX = 0x10930022; // Enable position output for invalid fixes +const uint32_t CFG_NMEA_OUT_INVTIME = 0x10930023; // Enable time output for invalid times +const uint32_t CFG_NMEA_OUT_INVDATE = 0x10930024; // Enable date output for invalid dates +const uint32_t CFG_NMEA_OUT_ONLYGPS = 0x10930025; // Restrict output to GPS satellites only +const uint32_t CFG_NMEA_OUT_FROZENCOG = 0x10930026; // Enable course over ground output even if it is frozen +const uint32_t CFG_NMEA_MAINTALKERID = 0x20930031; // Main Talker ID +const uint32_t CFG_NMEA_GSVTALKERID = 0x20930032; // Talker ID for GSV NMEA messages +const uint32_t CFG_NMEA_BDSTALKERID = 0x30930033; // BeiDou Talker ID + +//CFG-ODO: Odometer and low-speed course over ground filter +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_ODO_USE_ODO = 0x10220001; // Use odometer +const uint32_t CFG_ODO_USE_COG = 0x10220002; // Use low-speed course over ground filter +const uint32_t CFG_ODO_OUTLPVEL = 0x10220003; // Output low-pass filtered velocity +const uint32_t CFG_ODO_OUTLPCOG = 0x10220004; // Output low-pass filtered course over ground (heading) +const uint32_t CFG_ODO_PROFILE = 0x20220005; // Odometer profile configuration +const uint32_t CFG_ODO_COGMAXSPEED = 0x20220021; // Upper speed limit for low-speed course over ground filter +const uint32_t CFG_ODO_COGMAXPOSACC = 0x20220022; // Maximum acceptable position accuracy for computing low-speed filtered course over ground +const uint32_t CFG_ODO_VELLPGAIN = 0x20220031; // Velocity low-pass filter level +const uint32_t CFG_ODO_COGLPGAIN = 0x20220032; // Course over ground low-pass filter level (at speed < 8 m/s) + +//CFG-QZSS: QZSS system configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_QZSS_USE_SLAS_DGNSS = 0x10370005; // Apply QZSS SLAS DGNSS corrections +const uint32_t CFG_QZSS_USE_SLAS_TESTMODE = 0x10370006; // Use QZSS SLAS data when it is in test mode (SLAS msg 0) +const uint32_t CFG_QZSS_USE_SLAS_RAIM_UNCORR = 0x10370007; // Raim out measurements that are not corrected by QZSS SLAS, if at least 5 measurements are corrected + +//CFG-RATE: Navigation and measurement rate configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_RATE_MEAS = 0x30210001; // Nominal time between GNSS measurements +const uint32_t CFG_RATE_NAV = 0x30210002; // Ratio of number of measurements to number of navigation solutions +const uint32_t CFG_RATE_TIMEREF = 0x20210003; // Time system to which measurements are aligned + +//CFG-RINV: Remote inventory +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_RINV_DUMP = 0x10c70001; // Dump data at startup +const uint32_t CFG_RINV_BINARY = 0x10c70002; // Data is binary +const uint32_t CFG_RINV_DATA_SIZE = 0x20c70003; // Size of data +const uint32_t CFG_RINV_CHUNK0 = 0x50c70004; // Data bytes 1-8 (LSB) +const uint32_t CFG_RINV_CHUNK1 = 0x50c70005; // Data bytes 9-16 +const uint32_t CFG_RINV_CHUNK2 = 0x50c70006; // Data bytes 17-240x44434241. +const uint32_t CFG_RINV_CHUNK3 = 0x50c70007; // Data bytes 25-30 (MSB) + +//CFG-RTCM: RTCM protocol configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_RTCM_DF003_OUT = 0x30090001; // RTCM DF003 (Reference station ID) output value +const uint32_t CFG_RTCM_DF003_IN = 0x30090008; // RTCM DF003 (Reference station ID) input value +const uint32_t CFG_RTCM_DF003_IN_FILTER = 0x20090009; // RTCM input filter configuration based on RTCM DF003 (Reference station ID) value + +//CFG-SBAS: SBAS configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_SBAS_USE_TESTMODE = 0x10360002; // Use SBAS data when it is in test mode (SBAS msg 0) +const uint32_t CFG_SBAS_USE_RANGING = 0x10360003; // Use SBAS GEOs as a ranging source (for navigation) +const uint32_t CFG_SBAS_USE_DIFFCORR = 0x10360004; // Use SBAS differential corrections +const uint32_t CFG_SBAS_USE_INTEGRITY = 0x10360005; // Use SBAS integrity information +const uint32_t CFG_SBAS_PRNSCANMASK = 0x50360006; // SBAS PRN search configuration + +//CFG-SIGNAL: Satellite systems (GNSS) signal configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_SIGNAL_GPS_ENA = 0x1031001f; // GPS enable +const uint32_t CFG_SIGNAL_GPS_L1CA_ENA = 0x10310001; // GPS L1C/A +const uint32_t CFG_SIGNAL_GPS_L2C_ENA = 0x10310003; // GPS L2C (only on u-blox F9 platform products) +const uint32_t CFG_SIGNAL_SBAS_ENA = 0x10310020; // SBAS enable +const uint32_t CFG_SIGNAL_SBAS_L1CA_ENA = 0x10310005; // SBAS L1C/A +const uint32_t CFG_SIGNAL_GAL_ENA = 0x10310021; // Galileo enable +const uint32_t CFG_SIGNAL_GAL_E1_ENA = 0x10310007; // Galileo E1 +const uint32_t CFG_SIGNAL_GAL_E5B_ENA = 0x1031000a; // Galileo E5b (only on u-blox F9 platform products) +const uint32_t CFG_SIGNAL_BDS_ENA = 0x10310022; // BeiDou Enable +const uint32_t CFG_SIGNAL_BDS_B1_ENA = 0x1031000d; // BeiDou B1I +const uint32_t CFG_SIGNAL_BDS_B2_ENA = 0x1031000e; // BeiDou B2I (only on u-blox F9 platform products) +const uint32_t CFG_SIGNAL_QZSS_ENA = 0x10310024; // QZSS enable +const uint32_t CFG_SIGNAL_QZSS_L1CA_ENA = 0x10310012; // QZSS L1C/A +const uint32_t CFG_SIGNAL_QZSS_L1S_ENA = 0x10310014; // QZSS L1S +const uint32_t CFG_SIGNAL_QZSS_L2C_ENA = 0x10310015; // QZSS L2C (only on u-blox F9 platform products) +const uint32_t CFG_SIGNAL_GLO_ENA = 0x10310025; // GLONASS enable +const uint32_t CFG_SIGNAL_GLO_L1_ENA = 0x10310018; // GLONASS L1 +const uint32_t CFG_SIGNAL_GLO_L2_ENA = 0x1031001a; // GLONASS L2 (only on u-blox F9 platform products) + +//CFG-SPI: Configuration of the SPI interface +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_SPI_MAXFF = 0x20640001; // Number of bytes containing 0xFF to receive before switching off reception. Range: 0 (mechanism off) - 63 +const uint32_t CFG_SPI_CPOLARITY = 0x10640002; // Clock polarity select: 0: Active Hight Clock, SCLK idles low, 1: Active Low Clock, SCLK idles high +const uint32_t CFG_SPI_CPHASE = 0x10640003; // Clock phase select: 0: Data captured on first edge of SCLK, 1: Data captured on second edge of SCLK +const uint32_t CFG_SPI_EXTENDEDTIMEOUT = 0x10640005; // Flag to disable timeouting the interface after 1.5s +const uint32_t CFG_SPI_ENABLED = 0x10640006; // Flag to indicate if the SPI interface should be enabled + +//CFG-SPIINPROT: Input protocol configuration of the SPI interface +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_SPIINPROT_UBX = 0x10790001; // Flag to indicate if UBX should be an input protocol on SPI +const uint32_t CFG_SPIINPROT_NMEA = 0x10790002; // Flag to indicate if NMEA should be an input protocol on SPI +const uint32_t CFG_SPIINPROT_RTCM3X = 0x10790004; // Flag to indicate if RTCM3X should be an input protocol on SPI + +//CFG-SPIOUTPROT: Output protocol configuration of the SPI interface +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_SPIOUTPROT_UBX = 0x107a0001; // Flag to indicate if UBX should be an output protocol on SPI +const uint32_t CFG_SPIOUTPROT_NMEA = 0x107a0002; // Flag to indicate if NMEA should be an output protocol on SPI +const uint32_t CFG_SPIOUTPROT_RTCM3X = 0x107a0004; // Flag to indicate if RTCM3X should be an output protocol on SPI + +//CFG-TMODE: Time mode configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_TMODE_MODE = 0x20030001; // Receiver mode +const uint32_t CFG_TMODE_POS_TYPE = 0x20030002; // Determines whether the ARP position is given in ECEF or LAT/LON/HEIGHT? +const uint32_t CFG_TMODE_ECEF_X = 0x40030003; // ECEF X coordinate of the ARP position. +const uint32_t CFG_TMODE_ECEF_Y = 0x40030004; // ECEF Y coordinate of the ARP position. +const uint32_t CFG_TMODE_ECEF_Z = 0x40030005; // ECEF Z coordinate of the ARP position. +const uint32_t CFG_TMODE_ECEF_X_HP = 0x20030006; // High-precision ECEF X coordinate of the ARP position. +const uint32_t CFG_TMODE_ECEF_Y_HP = 0x20030007; // High-precision ECEF Y coordinate of the ARP position. +const uint32_t CFG_TMODE_ECEF_Z_HP = 0x20030008; // High-precision ECEF Z coordinate of the ARP position. +const uint32_t CFG_TMODE_LAT = 0x40030009; // Latitude of the ARP position. +const uint32_t CFG_TMODE_LON = 0x4003000a; // Longitude of the ARP position. +const uint32_t CFG_TMODE_HEIGHT = 0x4003000b; // Height of the ARP position. +const uint32_t CFG_TMODE_LAT_HP = 0x2003000c; // High-precision latitude of the ARP position +const uint32_t CFG_TMODE_LON_HP = 0x2003000d; // High-precision longitude of the ARP position. +const uint32_t CFG_TMODE_HEIGHT_HP = 0x2003000e; // High-precision height of the ARP position. +const uint32_t CFG_TMODE_FIXED_POS_ACC = 0x4003000f; // Fixed position 3D accuracy +const uint32_t CFG_TMODE_SVIN_MIN_DUR = 0x40030010; // Survey-in minimum duration +const uint32_t CFG_TMODE_SVIN_ACC_LIMIT = 0x40030011; // Survey-in position accuracy limit + +//CFG-TP: Timepulse configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_TP_PULSE_DEF = 0x20050023; // Determines whether the time pulse is interpreted as frequency or period +const uint32_t CFG_TP_PULSE_LENGTH_DEF = 0x20050030; // Determines whether the time pulse length is interpreted as length[us] or pulse ratio[%] +const uint32_t CFG_TP_FREQ_TP1 = 0x40050024; // Time pulse frequency (TP1) +const uint32_t CFG_TP_FREQ_LOCK_TP1 = 0x40050025; // Time pulse frequency when locked to GNSS time (TP1) +const uint32_t CFG_TP_LEN_TP1 = 0x40050004; // Time pulse length (TP1) +const uint32_t CFG_TP_LEN_LOCK_TP1 = 0x40050005; // Time pulse length when locked to GNSS time (TP1) +const uint32_t CFG_TP_DUTY_TP1 = 0x5005002a; // Time pulse duty cycle (TP1) +const uint32_t CFG_TP_DUTY_LOCK_TP1 = 0x5005002b; // Time pulse duty cycle when locked to GNSS time (TP1) +const uint32_t CFG_TP_USER_DELAY_TP1 = 0x40050006; // User-configurable time pulse delay (TP1) +const uint32_t CFG_TP_TP1_ENA = 0x10050007; // Enable the first timepulse +const uint32_t CFG_TP_SYNC_GNSS_TP1 = 0x10050008; // Sync time pulse to GNSS time or local clock (TP1) +const uint32_t CFG_TP_USE_LOCKED_TP1 = 0x10050009; // Use locked parameters when possible (TP1) +const uint32_t CFG_TP_ALIGN_TO_TOW_TP1 = 0x1005000a; // Align time pulse to top of second (TP1) +const uint32_t CFG_TP_POL_TP1 = 0x1005000b; // Set time pulse polarity (TP1) +const uint32_t CFG_TP_TIMEGRID_TP1 = 0x2005000c; // Time grid to use (TP1) + +//CFG-TXREADY: TX ready configuration +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_TXREADY_ENABLED = 0x10a20001; // Flag to indicate if TX ready pin mechanism should be enabled +const uint32_t CFG_TXREADY_POLARITY = 0x10a20002; // The polarity of the TX ready pin: false:high- active, true:low-active +const uint32_t CFG_TXREADY_PIN = 0x20a20003; // Pin number to use for the TX ready functionality +const uint32_t CFG_TXREADY_THRESHOLD = 0x30a20004; // Amount of data that should be ready on the interface before triggering the TX ready pin +const uint32_t CFG_TXREADY_INTERFACE = 0x20a20005; // Interface where the TX ready feature should be linked to + +//CFG-UART1: Configuration of the UART1 interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -#endif \ No newline at end of file +const uint32_t CFG_UART1_BAUDRATE = 0x40520001; // The baud rate that should be configured on the UART1 +const uint32_t CFG_UART1_STOPBITS = 0x20520002; // Number of stopbits that should be used on UART1 +const uint32_t CFG_UART1_DATABITS = 0x20520003; // Number of databits that should be used on UART1 +const uint32_t CFG_UART1_PARITY = 0x20520004; // Parity mode that should be used on UART1 +const uint32_t CFG_UART1_ENABLED = 0x10520005; // Flag to indicate if the UART1 should be enabled + +//CFG-UART1INPROT: Input protocol configuration of the UART1 interface +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_UART1INPROT_UBX = 0x10730001; // Flag to indicate if UBX should be an input protocol on UART1 +const uint32_t CFG_UART1INPROT_NMEA = 0x10730002; // Flag to indicate if NMEA should be an input protocol on UART1 +const uint32_t CFG_UART1INPROT_RTCM3X = 0x10730004; // Flag to indicate if RTCM3X should be an input protocol on UART1 + +//CFG-UART1OUTPROT: Output protocol configuration of the UART1 interface +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_UART1OUTPROT_UBX = 0x10740001; // Flag to indicate if UBX should be an output protocol on UART1 +const uint32_t CFG_UART1OUTPROT_NMEA = 0x10740002; // Flag to indicate if NMEA should be an output protocol on UART1 +const uint32_t CFG_UART1OUTPROT_RTCM3X = 0x10740004; // Flag to indicate if RTCM3X should be an output protocol on UART1 + +//CFG-UART2: Configuration of the UART2 interface +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_UART2_BAUDRATE = 0x40530001; // The baud rate that should be configured on the UART2 +const uint32_t CFG_UART2_STOPBITS = 0x20530002; // Number of stopbits that should be used on UART2 +const uint32_t CFG_UART2_DATABITS = 0x20530003; // Number of databits that should be used on UART2 +const uint32_t CFG_UART2_PARITY = 0x20530004; // Parity mode that should be used on UART2 +const uint32_t CFG_UART2_ENABLED = 0x10530005; // Flag to indicate if the UART2 should be enabled +const uint32_t CFG_UART2_REMAP = 0x10530006; // UART2 Remapping + +//CFG-UART2INPROT: Input protocol configuration of the UART2 interface +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_UART2INPROT_UBX = 0x10750001; // Flag to indicate if UBX should be an input protocol on UART2 +const uint32_t CFG_UART2INPROT_NMEA = 0x10750002; // Flag to indicate if NMEA should be an input protocol on UART2 +const uint32_t CFG_UART2INPROT_RTCM3X = 0x10750004; // Flag to indicate if RTCM3X should be an input protocol on UART2 + +//CFG-UART2OUTPROT: Output protocol configuration of the UART2 interface +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_UART2OUTPROT_UBX = 0x10760001; // Flag to indicate if UBX should be an output protocol on UART2 +const uint32_t CFG_UART2OUTPROT_NMEA = 0x10760002; // Flag to indicate if NMEA should be an output protocol on UART2 +const uint32_t CFG_UART2OUTPROT_RTCM3X = 0x10760004; // Flag to indicate if RTCM3X should be an output protocol on UART2 + +//CFG-USB: Configuration of the USB interface +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_USB_ENABLED = 0x10650001; // Flag to indicate if the USB interface should be enabled +const uint32_t CFG_USB_SELFPOW = 0x10650002; // Self-powered device +const uint32_t CFG_USB_VENDOR_ID = 0x3065000a; // Vendor ID +const uint32_t CFG_USB_PRODUCT_ID = 0x3065000b; // Vendor ID +const uint32_t CFG_USB_POWER = 0x3065000c; // Power consumption +const uint32_t CFG_USB_VENDOR_STR0 = 0x5065000d; // Vendor string characters 0-7 +const uint32_t CFG_USB_VENDOR_STR1 = 0x5065000e; // Vendor string characters 8-15 +const uint32_t CFG_USB_VENDOR_STR2 = 0x5065000f; // Vendor string characters 16-23 +const uint32_t CFG_USB_VENDOR_STR3 = 0x50650010; // Vendor string characters 24-31 +const uint32_t CFG_USB_PRODUCT_STR0 = 0x50650011; // Product string characters 0-7 +const uint32_t CFG_USB_PRODUCT_STR1 = 0x50650012; // Product string characters 8-15 +const uint32_t CFG_USB_PRODUCT_STR2 = 0x50650013; // Product string characters 16-23 +const uint32_t CFG_USB_PRODUCT_STR3 = 0x50650014; // Product string characters 24-31 +const uint32_t CFG_USB_SERIAL_NO_STR0 = 0x50650015; // Serial number string characters 0-7 +const uint32_t CFG_USB_SERIAL_NO_STR1 = 0x50650016; // Serial number string characters 8-15 +const uint32_t CFG_USB_SERIAL_NO_STR2 = 0x50650017; // Serial number string characters 16-23 +const uint32_t CFG_USB_SERIAL_NO_STR3 = 0x50650018; // Serial number string characters 24-31 + +//CFG-USBINPROT: Input protocol configuration of the USB interface +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_USBINPROT_UBX = 0x10770001; // Flag to indicate if UBX should be an input protocol on USB +const uint32_t CFG_USBINPROT_NMEA = 0x10770002; // Flag to indicate if NMEA should be an input protocol on USB +const uint32_t CFG_USBINPROT_RTCM3X = 0x10770004; // Flag to indicate if RTCM3X should be an input protocol on USB + +//CFG-USBOUTPROT: Output protocol configuration of the USB interface +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- +const uint32_t CFG_USBOUTPROT_UBX = 0x10780001; // Flag to indicate if UBX should be an output protocol on USB +const uint32_t CFG_USBOUTPROT_NMEA = 0x10780002; // Flag to indicate if NMEA should be an output protocol on USB +const uint32_t CFG_USBOUTPROT_RTCM3X = 0x10780004; // Flag to indicate if RTCM3X should be an output protocol on USB + +#endif From fc33d0bc9330c6d26e25fce5463a10d33b40b743 Mon Sep 17 00:00:00 2001 From: Morten Nielsen Date: Thu, 29 Oct 2020 13:28:32 -0700 Subject: [PATCH 04/36] Adds support for auto-reporting DOP values --- src/SparkFun_Ublox_Arduino_Library.cpp | 243 ++++++++++++++++++++++++- src/SparkFun_Ublox_Arduino_Library.h | 46 ++++- 2 files changed, 277 insertions(+), 12 deletions(-) diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp index 81994a1..061151d 100644 --- a/src/SparkFun_Ublox_Arduino_Library.cpp +++ b/src/SparkFun_Ublox_Arduino_Library.cpp @@ -553,8 +553,9 @@ void SFE_UBLOX_GPS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t re //This is not an ACK and we do not have a complete class and ID match //So let's check for an HPPOSLLH message arriving when we were expecting PVT and vice versa else if ((packetBuf.cls == requestedClass) && - (((packetBuf.id == UBX_NAV_PVT) && (requestedID == UBX_NAV_HPPOSLLH)) || - ((packetBuf.id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT)))) + (((packetBuf.id == UBX_NAV_PVT) && (requestedID == UBX_NAV_HPPOSLLH || requestedID == UBX_NAV_DOP)) || + ((packetBuf.id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_DOP)) || + ((packetBuf.id == UBX_NAV_DOP) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_HPPOSLLH)))) { //This is not the message we were expecting but we start diverting data into incomingUBX (usually packetCfg) and process it anyway activePacketBuffer = SFE_UBLOX_PACKET_PACKETCFG; @@ -563,7 +564,7 @@ void SFE_UBLOX_GPS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t re incomingUBX->counter = packetBuf.counter; //Copy over the .counter too if (_printDebug == true) { - _debugSerial->print(F("process: auto PVT/HPPOSLLH collision: Requested ID: 0x")); + _debugSerial->print(F("process: auto PVT/HPPOSLLH/DOP collision: Requested ID: 0x")); _debugSerial->print(requestedID, HEX); _debugSerial->print(F(" Message ID: 0x")); _debugSerial->println(packetBuf.id, HEX); @@ -825,14 +826,15 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t //This is not an ACK and we do not have a complete class and ID match //So let's check for an HPPOSLLH message arriving when we were expecting PVT and vice versa else if ((incomingUBX->cls == requestedClass) && - (((incomingUBX->id == UBX_NAV_PVT) && (requestedID == UBX_NAV_HPPOSLLH)) || - ((incomingUBX->id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT)))) + (((incomingUBX->id == UBX_NAV_PVT) && (requestedID == UBX_NAV_HPPOSLLH || requestedID == UBX_NAV_DOP)) || + ((incomingUBX->id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_DOP)) || + ((incomingUBX->id == UBX_NAV_DOP) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_HPPOSLLH)))) { // This isn't the message we are looking for... // Let's say so and leave incomingUBX->classAndIDmatch _unchanged_ if (_printDebug == true) { - _debugSerial->print(F("processUBX: auto PVT/HPPOSLLH collision: Requested ID: 0x")); + _debugSerial->print(F("processUBX: auto PVT/HPPOSLLH/DOP collision: Requested ID: 0x")); _debugSerial->print(requestedID, HEX); _debugSerial->print(F(" Message ID: 0x")); _debugSerial->println(incomingUBX->id, HEX); @@ -1080,6 +1082,24 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg) } */ } + else if (msg->id == UBX_NAV_DOP && msg->len == 18) + { + geometricDOP = extractInt(4); + positionDOP = extractInt(6); + timeDOP = extractInt(8); + verticalDOP = extractInt(10); + horizontalDOP = extractInt(12); + northingDOP = extractInt(14); + eastingDOP = extractInt(16); + dopModuleQueried.all = true; + dopModuleQueried.geometricDOP = true; + dopModuleQueried.positionDOP = true; + dopModuleQueried.timeDOP = true; + dopModuleQueried.verticalDOP = true; + dopModuleQueried.horizontalDOP = true; + dopModuleQueried.northingDOP = true; + dopModuleQueried.eastingDOP = true; + } break; } } @@ -2403,6 +2423,49 @@ boolean SFE_UBLOX_GPS::setAutoHPPOSLLH(boolean enable, boolean implicitUpdate, u return ok; } + +//In case no config access to the GPS is possible and DOP is send cyclically already +//set config to suitable parameters +boolean SFE_UBLOX_GPS::assumeAutoDOP(boolean enabled, boolean implicitUpdate) +{ + boolean changes = autoDOP != enabled || autoDOPImplicitUpdate != implicitUpdate; + if (changes) + { + autoDOP = enabled; + autoDOPImplicitUpdate = implicitUpdate; + } + return changes; +} + +//Enable or disable automatic navigation message generation by the GPS. This changes the way getDOP +//works. +boolean SFE_UBLOX_GPS::setAutoDOP(boolean enable, uint16_t maxWait) +{ + return setAutoDOP(enable, true, maxWait); +} + +//Enable or disable automatic navigation message generation by the GPS. This changes the way getDOP +//works. +boolean SFE_UBLOX_GPS::setAutoDOP(boolean enable, boolean implicitUpdate, uint16_t maxWait) +{ + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_MSG; + packetCfg.len = 3; + packetCfg.startingSpot = 0; + payloadCfg[0] = UBX_CLASS_NAV; + payloadCfg[1] = UBX_NAV_DOP; + payloadCfg[2] = enable ? 1 : 0; // rate relative to navigation freq. + + boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK + if (ok) + { + autoDOP = enable; + autoDOPImplicitUpdate = implicitUpdate; + } + dopModuleQueried.all = false; + return ok; +} + //Configure a given message type for a given port (UART1, I2C, SPI, etc) boolean SFE_UBLOX_GPS::configureMessage(uint8_t msgClass, uint8_t msgID, uint8_t portID, uint8_t sendRate, uint16_t maxWait) { @@ -3054,6 +3117,15 @@ boolean SFE_UBLOX_GPS::getPVT(uint16_t maxWait) return (true); } + if ((retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN) && (packetCfg.id == UBX_NAV_DOP)) + { + if (_printDebug == true) + { + _debugSerial->println(F("getPVT: data was OVERWRITTEN by DOP (but that's OK)")); + } + return (true); + } + if (_printDebug == true) { _debugSerial->print(F("getPVT retVal: ")); @@ -3229,6 +3301,14 @@ boolean SFE_UBLOX_GPS::getHPPOSLLH(uint16_t maxWait) } return (true); } + if ((retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN) && (packetCfg.id == UBX_NAV_DOP)) + { + if (_printDebug == true) + { + _debugSerial->println(F("getHPPOSLLH: data was OVERWRITTEN by DOP (but that's OK)")); + } + return (true); + } if (_printDebug == true) { @@ -3239,6 +3319,141 @@ boolean SFE_UBLOX_GPS::getHPPOSLLH(uint16_t maxWait) } } +uint16_t SFE_UBLOX_GPS::getGeometricDOP(uint16_t maxWait /* = 250*/) +{ + if (dopModuleQueried.geometricDOP == false) + getDOP(maxWait); + dopModuleQueried.geometricDOP = false; //Since we are about to give this to user, mark this data as stale + dopModuleQueried.all = false; + + return (geometricDOP); +} + +uint16_t SFE_UBLOX_GPS::getPositionDOP(uint16_t maxWait /* = 250*/) +{ + if (dopModuleQueried.positionDOP == false) + getDOP(maxWait); + dopModuleQueried.positionDOP = false; //Since we are about to give this to user, mark this data as stale + dopModuleQueried.all = false; + + return (positionDOP); +} + +uint16_t SFE_UBLOX_GPS::getTimeDOP(uint16_t maxWait /* = 250*/) +{ + if (dopModuleQueried.timeDOP == false) + getDOP(maxWait); + dopModuleQueried.timeDOP = false; //Since we are about to give this to user, mark this data as stale + dopModuleQueried.all = false; + + return (timeDOP); +} + +uint16_t SFE_UBLOX_GPS::getVerticalDOP(uint16_t maxWait /* = 250*/) +{ + if (dopModuleQueried.verticalDOP == false) + getDOP(maxWait); + dopModuleQueried.verticalDOP = false; //Since we are about to give this to user, mark this data as stale + dopModuleQueried.all = false; + + return (verticalDOP); +} + +uint16_t SFE_UBLOX_GPS::getHorizontalDOP(uint16_t maxWait /* = 250*/) +{ + if (dopModuleQueried.horizontalDOP == false) + getDOP(maxWait); + dopModuleQueried.horizontalDOP = false; //Since we are about to give this to user, mark this data as stale + dopModuleQueried.all = false; + + return (horizontalDOP); +} + +uint16_t SFE_UBLOX_GPS::getNorthingDOP(uint16_t maxWait /* = 250*/) +{ + if (dopModuleQueried.northingDOP == false) + getDOP(maxWait); + dopModuleQueried.northingDOP = false; //Since we are about to give this to user, mark this data as stale + dopModuleQueried.all = false; + + return (northingDOP); +} + +uint16_t SFE_UBLOX_GPS::getEastingDOP(uint16_t maxWait /* = 250*/) +{ + if (dopModuleQueried.eastingDOP == false) + getDOP(maxWait); + dopModuleQueried.eastingDOP = false; //Since we are about to give this to user, mark this data as stale + dopModuleQueried.all = false; + + return (eastingDOP); +} + +boolean SFE_UBLOX_GPS::getDOP(uint16_t maxWait) +{ + if (autoDOP && autoDOPImplicitUpdate) + { + //The GPS is automatically reporting, we just check whether we got unread data + if (_printDebug == true) + { + _debugSerial->println(F("getDOP: Autoreporting")); + } + checkUbloxInternal(&packetCfg, UBX_CLASS_NAV, UBX_NAV_DOP); + return dopModuleQueried.all; + } + else if (autoDOP && !autoDOPImplicitUpdate) + { + //Someone else has to call checkUblox for us... + if (_printDebug == true) + { + _debugSerial->println(F("getDOP: Exit immediately")); + } + return (false); + } + else + { + if (_printDebug == true) + { + _debugSerial->println(F("getDOP: Polling")); + } + + //The GPS is not automatically reporting navigation position so we have to poll explicitly + packetCfg.cls = UBX_CLASS_NAV; + packetCfg.id = UBX_NAV_DOP; + packetCfg.len = 0; + + //The data is parsed as part of processing the response + sfe_ublox_status_e retVal = sendCommand(&packetCfg, maxWait); + + if (retVal == SFE_UBLOX_STATUS_DATA_RECEIVED) + return (true); + + if ((retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN) && (packetCfg.id == UBX_NAV_PVT)) + { + if (_printDebug == true) + { + _debugSerial->println(F("getHPPOSLLH: data was OVERWRITTEN by PVT (but that's OK)")); + } + return (true); + } + + if ((retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN) && (packetCfg.id == UBX_NAV_HPPOSLLH)) + { + if (_printDebug == true) + { + _debugSerial->println(F("getPVT: data was OVERWRITTEN by HPPOSLLH (but that's OK)")); + } + return (true); + } + + if (_printDebug == true) + { + _debugSerial->print(F("getDOP retVal: ")); + _debugSerial->println(statusString(retVal)); + } + return (false); + } +} //Get the current 3D high precision positional accuracy - a fun thing to watch //Returns a long representing the 3D accuracy in millimeters uint32_t SFE_UBLOX_GPS::getPositionAccuracy(uint16_t maxWait) @@ -3496,6 +3711,20 @@ void SFE_UBLOX_GPS::flushHPPOSLLH() //moduleQueried.gpsiTOW = false; // this can arrive via HPPOS too. } +//Mark all the DOP data as read/stale. This is handy to get data alignment after CRC failure +void SFE_UBLOX_GPS::flushDOP() +{ + //Mark all DOPs as stale (read before) + dopModuleQueried.all = false; + dopModuleQueried.geometricDOP = false; + dopModuleQueried.positionDOP = false; + dopModuleQueried.timeDOP = false; + dopModuleQueried.verticalDOP = false; + dopModuleQueried.horizontalDOP = false; + dopModuleQueried.northingDOP = false; + dopModuleQueried.eastingDOP = false; +} + //Relative Positioning Information in NED frame //Returns true if commands was successful boolean SFE_UBLOX_GPS::getRELPOSNED(uint16_t maxWait) @@ -3800,4 +4029,4 @@ bool SFE_UBLOX_GPS::setStaticPosition(int32_t ecefXOrLat, int8_t ecefXOrLatHP, i bool SFE_UBLOX_GPS::setStaticPosition(int32_t ecefXOrLat, int32_t ecefYOrLon, int32_t ecefZOrAlt, bool latlong, uint16_t maxWait) { return (setStaticPosition(ecefXOrLat, 0, ecefYOrLon, 0, ecefZOrAlt, 0, latlong, maxWait)); -} \ No newline at end of file +} diff --git a/src/SparkFun_Ublox_Arduino_Library.h b/src/SparkFun_Ublox_Arduino_Library.h index 8feadfc..d3d48f0 100644 --- a/src/SparkFun_Ublox_Arduino_Library.h +++ b/src/SparkFun_Ublox_Arduino_Library.h @@ -56,16 +56,15 @@ // Boards like the RedBoard Turbo use SerialUSB (not Serial). // But other boards like the SAMD51 Thing Plus use Serial (not SerialUSB). // The next nine lines let the code compile cleanly on as many SAMD boards as possible. -#if defined(ARDUINO_ARCH_SAMD) // Is this a SAMD board? -#if defined(USB_VID) // Is the USB Vendor ID defined? -#if (USB_VID == 0x1B4F) // Is this a SparkFun board? +#if defined(ARDUINO_ARCH_SAMD) // Is this a SAMD board? +#if defined(USB_VID) // Is the USB Vendor ID defined? +#if (USB_VID == 0x1B4F) // Is this a SparkFun board? #if !defined(ARDUINO_SAMD51_THING_PLUS) & !defined(ARDUINO_SAMD51_MICROMOD) // If it is not a SAMD51 Thing Plus or SAMD51 MicroMod -#define Serial SerialUSB // Define Serial as SerialUSB +#define Serial SerialUSB // Define Serial as SerialUSB #endif #endif #endif #endif - //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //Define a digital pin to aid checksum failure capture and analysis @@ -492,6 +491,7 @@ class SFE_UBLOX_GPS // The same is true for getHPPOSLLH. #define getPVTmaxWait 1100 // Default maxWait for getPVT and all functions which call it #define getHPPOSLLHmaxWait 1100 // Default maxWait for getHPPOSLLH and all functions which call it +#define getDOPmaxWait 1100 // Default maxWait for getDOP and all functions which all it boolean assumeAutoPVT(boolean enabled, boolean implicitUpdate = true); //In case no config access to the GPS is possible and PVT is send cyclically already boolean setAutoPVT(boolean enabled, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic PVT reports at the navigation frequency @@ -501,8 +501,13 @@ class SFE_UBLOX_GPS boolean setAutoHPPOSLLH(boolean enabled, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic HPPOSLLH reports at the navigation frequency boolean setAutoHPPOSLLH(boolean enabled, boolean implicitUpdate, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic HPPOSLLH reports at the navigation frequency, with implicitUpdate == false accessing stale data will not issue parsing of data in the rxbuffer of your interface, instead you have to call checkUblox when you want to perform an update boolean getHPPOSLLH(uint16_t maxWait = getHPPOSLLHmaxWait); //Query module for latest group of datums and load global vars: lat, long, alt, speed, SIV, accuracies, etc. If autoPVT is disabled, performs an explicit poll and waits, if enabled does not block. Returns true if new HPPOSLLH is available. + boolean assumeAutoDOP(boolean enabled, boolean implicitUpdate = true); //In case no config access to the GPS is possible and DOP is send cyclically already + boolean setAutoDOP(boolean enabled, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic DOP reports at the navigation frequency + boolean setAutoDOP(boolean enabled, boolean implicitUpdate, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic DOP reports at the navigation frequency, with implicitUpdate == false accessing stale data will not issue parsing of data in the rxbuffer of your interface, instead you have to call checkUblox when you want to perform an update + boolean getDOP(uint16_t maxWait = getDOPmaxWait); //Query module for latest dilution of precision values and load global vars:. If autoDOP is disabled, performs an explicit poll and waits, if enabled does not block. Returns true if new DOP is available. void flushPVT(); //Mark all the PVT data as read/stale. This is handy to get data alignment after CRC failure void flushHPPOSLLH(); //Mark all the PVT data as read/stale. This is handy to get data alignment after CRC failure + void flushDOP(); //Mark all the DOP data as read/stale. This is handy to get data alignment after CRC failure int32_t getLatitude(uint16_t maxWait = getPVTmaxWait); //Returns the current latitude in degrees * 10^-7. Auto selects between HighPrecision and Regular depending on ability of module. int32_t getLongitude(uint16_t maxWait = getPVTmaxWait); //Returns the current longitude in degrees * 10-7. Auto selects between HighPrecision and Regular depending on ability of module. @@ -538,6 +543,14 @@ class SFE_UBLOX_GPS uint32_t getHorizontalAccuracy(uint16_t maxWait = getHPPOSLLHmaxWait); uint32_t getVerticalAccuracy(uint16_t maxWait = getHPPOSLLHmaxWait); + uint16_t getGeometricDOP(uint16_t maxWait = getDOPmaxWait); + uint16_t getPositionDOP(uint16_t maxWait = getDOPmaxWait); + uint16_t getTimeDOP(uint16_t maxWait = getDOPmaxWait); + uint16_t getVerticalDOP(uint16_t maxWait = getDOPmaxWait); + uint16_t getHorizontalDOP(uint16_t maxWait = getDOPmaxWait); + uint16_t getNorthingDOP(uint16_t maxWait = getDOPmaxWait); + uint16_t getEastingDOP(uint16_t maxWait = getDOPmaxWait); + //Port configurations boolean setPortOutput(uint8_t portID, uint8_t comSettings, uint16_t maxWait = defaultMaxWait); //Configure a given port to output UBX, NMEA, RTCM3 or a combination thereof boolean setPortInput(uint8_t portID, uint8_t comSettings, uint16_t maxWait = defaultMaxWait); //Configure a given port to input UBX, NMEA, RTCM3 or a combination thereof @@ -715,6 +728,14 @@ class SFE_UBLOX_GPS uint16_t rtcmFrameCounter = 0; //Tracks the type of incoming byte inside RTCM frame +uint16_t geometricDOP; // Geometric dilution of precision * 10^-2 +uint16_t positionDOP; // Posoition dilution of precision * 10^-2 +uint16_t timeDOP; // Time dilution of precision * 10^-2 +uint16_t verticalDOP; // Vertical dilution of precision * 10^-2 +uint16_t horizontalDOP; // Horizontal dilution of precision * 10^-2 +uint16_t northingDOP; // Northing dilution of precision * 10^-2 +uint16_t eastingDOP; // Easting dilution of precision * 10^-2 + #define DEF_NUM_SENS 7 struct deadReckData { @@ -850,6 +871,9 @@ class SFE_UBLOX_GPS boolean autoPVTImplicitUpdate = true; // Whether autoPVT is triggered by accessing stale data (=true) or by a call to checkUblox (=false) boolean autoHPPOSLLH = false; //Whether autoHPPOSLLH is enabled or not boolean autoHPPOSLLHImplicitUpdate = true; // Whether autoHPPOSLLH is triggered by accessing stale data (=true) or by a call to checkUblox (=false) + boolean autoDOP = false; //Whether autoDOP is enabled or not + boolean autoDOPImplicitUpdate = true; // Whether autoDOP is triggered by accessing stale data (=true) or by a call to checkUblox (=false) + uint16_t ubxFrameCounter; //It counts all UBX frame. [Fixed header(2bytes), CLS(1byte), ID(1byte), length(2bytes), payload(x bytes), checksums(2bytes)] uint8_t rollingChecksumA; //Rolls forward as we receive incoming bytes. Checked against the last two A/B checksum bytes @@ -903,6 +927,18 @@ class SFE_UBLOX_GPS uint16_t highResLongitudeHp : 1; } highResModuleQueried; + struct + { + uint16_t all : 1; + uint16_t geometricDOP : 1; + uint16_t positionDOP : 1; + uint16_t timeDOP : 1; + uint16_t verticalDOP : 1; + uint16_t horizontalDOP : 1; + uint16_t northingDOP : 1; + uint16_t eastingDOP : 1; + } dopModuleQueried; + uint16_t rtcmLen = 0; }; From e36415a153363f55677bc014d2671774a2f5bbf6 Mon Sep 17 00:00:00 2001 From: Morten Nielsen Date: Thu, 29 Oct 2020 15:30:51 -0700 Subject: [PATCH 05/36] Add gnssfix valid status and whether differential has been applied --- src/SparkFun_Ublox_Arduino_Library.cpp | 28 ++++++++++++++++++++++++++ src/SparkFun_Ublox_Arduino_Library.h | 7 +++++++ 2 files changed, 35 insertions(+) diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp index 81994a1..e286540 100644 --- a/src/SparkFun_Ublox_Arduino_Library.cpp +++ b/src/SparkFun_Ublox_Arduino_Library.cpp @@ -980,6 +980,8 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg) gpsNanosecond = extractLong(16); //Includes milliseconds fixType = extractByte(20 - startingSpot); + gnssFixOk = extractByte(21 - startingSpot) & 0x1; //Get the 1st bit + diffSoln = extractByte(21 - startingSpot) >> 1 & 0x1; //Get the 2nd bit carrierSolution = extractByte(21 - startingSpot) >> 6; //Get 6th&7th bits of this byte SIV = extractByte(23 - startingSpot); longitude = extractLong(24 - startingSpot); @@ -1003,6 +1005,8 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg) moduleQueried.gpsNanosecond = true; moduleQueried.all = true; + moduleQueried.gnssFixOk = true; + moduleQueried.diffSoln = true; moduleQueried.longitude = true; moduleQueried.latitude = true; moduleQueried.altitude = true; @@ -3333,6 +3337,28 @@ uint8_t SFE_UBLOX_GPS::getFixType(uint16_t maxWait) return (fixType); } +//Get whether we have a valid fix (i.e within DOP & accuracy masks) +bool SFE_UBLOX_GPS::getGnssFixOk(uint16_t maxWait) +{ + if (moduleQueried.gnssFixOk == false) + getPVT(maxWait); + moduleQueried.gnssFixOk = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (gnssFixOk); +} + +//Get whether differential corrections were applied +bool SFE_UBLOX_GPS::getDiffSoln(uint16_t maxWait) +{ + if (moduleQueried.diffSoln == false) + getPVT(maxWait); + moduleQueried.diffSoln = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (diffSoln); +} + //Get the carrier phase range solution status //Useful when querying module to see if it has high-precision RTK fix //0=No solution, 1=Float solution, 2=Fixed solution @@ -3465,6 +3491,8 @@ void SFE_UBLOX_GPS::flushPVT() moduleQueried.gpsNanosecond = false; moduleQueried.all = false; + moduleQueried.gnssFixOk = false; + moduleQueried.diffSoln = false; moduleQueried.longitude = false; moduleQueried.latitude = false; moduleQueried.altitude = false; diff --git a/src/SparkFun_Ublox_Arduino_Library.h b/src/SparkFun_Ublox_Arduino_Library.h index 8feadfc..bc81953 100644 --- a/src/SparkFun_Ublox_Arduino_Library.h +++ b/src/SparkFun_Ublox_Arduino_Library.h @@ -504,6 +504,8 @@ class SFE_UBLOX_GPS void flushPVT(); //Mark all the PVT data as read/stale. This is handy to get data alignment after CRC failure void flushHPPOSLLH(); //Mark all the PVT data as read/stale. This is handy to get data alignment after CRC failure + bool getGnssFixOk(uint16_t maxWait = getPVTmaxWait); //Get whether we have a valid fix (i.e within DOP & accuracy masks) + bool getDiffSoln(uint16_t maxWait = getPVTmaxWait); //Get whether differential corrections were applied int32_t getLatitude(uint16_t maxWait = getPVTmaxWait); //Returns the current latitude in degrees * 10^-7. Auto selects between HighPrecision and Regular depending on ability of module. int32_t getLongitude(uint16_t maxWait = getPVTmaxWait); //Returns the current longitude in degrees * 10-7. Auto selects between HighPrecision and Regular depending on ability of module. int32_t getAltitude(uint16_t maxWait = getPVTmaxWait); //Returns the current altitude in mm above ellipsoid @@ -687,6 +689,9 @@ class SFE_UBLOX_GPS bool gpsDateValid; bool gpsTimeValid; + + bool gnssFixOk; //valid fix (i.e within DOP & accuracy masks) + bool diffSoln; //Differential corrections were applied int32_t latitude; //Degrees * 10^-7 (more accurate than floats) int32_t longitude; //Degrees * 10^-7 (more accurate than floats) int32_t altitude; //Number of mm above ellipsoid @@ -873,6 +878,8 @@ class SFE_UBLOX_GPS uint32_t gpsNanosecond : 1; uint32_t all : 1; + uint32_t gnssFixOk : 1; + uint32_t diffSoln : 1; uint32_t longitude : 1; uint32_t latitude : 1; uint32_t altitude : 1; From bba88b1dc6f073021cd1822bf798998f87a9db25 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Fri, 30 Oct 2020 07:31:28 +0000 Subject: [PATCH 06/36] Update u-blox_config_keys.h to include all the keys. **Note:** they are all prefixed with **UBLOX_** --- src/u-blox_config_keys.h | 1366 ++++++++++++++++++-------------------- 1 file changed, 643 insertions(+), 723 deletions(-) diff --git a/src/u-blox_config_keys.h b/src/u-blox_config_keys.h index 76fd049..1c6a667 100644 --- a/src/u-blox_config_keys.h +++ b/src/u-blox_config_keys.h @@ -78,836 +78,756 @@ const uint8_t VAL_ID_I2C_ADDRESS = 0x01; //Below are the key values for a given configuration setting -//CFG-NMEA -//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t UBLOX_CFG_NMEA_PROTVER = 0x10930001; -const uint32_t UBLOX_CFG_NMEA_MAXSVS = 0x10930002; -const uint32_t UBLOX_CFG_NMEA_COMPAT = 0x10930003; -const uint32_t UBLOX_CFG_NMEA_CONSIDER = 0x10930004; -const uint32_t UBLOX_CFG_NMEA_LIMIT82 = 0x10930005; -const uint32_t UBLOX_CFG_NMEA_HIGHPREC = 0x10930006; -const uint32_t UBLOX_CFG_NMEA_SVNUMBERING = 0x20930007; -const uint32_t UBLOX_CFG_NMEA_FILT_GPS = 0x10930011; -const uint32_t UBLOX_CFG_NMEA_FILT_SBAS = 0x10930012; -const uint32_t UBLOX_CFG_NMEA_FILT_GAL = 0x10930013; -const uint32_t UBLOX_CFG_NMEA_FILT_QZSS = 0x10930015; -const uint32_t UBLOX_CFG_NMEA_FILT_GLO = 0x10930016; -const uint32_t UBLOX_CFG_NMEA_FILT_BDS = 0x10930017; -const uint32_t UBLOX_CFG_NMEA_OUT_INVFIX = 0x10930021; -const uint32_t UBLOX_CFG_NMEA_OUT_MSKFIX = 0x10930022; -const uint32_t UBLOX_CFG_NMEA_OUT_INVTIME = 0x10930023; -const uint32_t UBLOX_CFG_NMEA_OUT_INVDATE = 0x10930024; -const uint32_t UBLOX_CFG_NMEA_OUT_ONLYGPS = 0x10930025; -const uint32_t UBLOX_CFG_NMEA_OUT_FROZENCOG = 0x10930026; -const uint32_t UBLOX_CFG_NMEA_MAINTALKERID = 0x20930031; -const uint32_t UBLOX_CFG_NMEA_GSVTALKERID = 0x20930032; -const uint32_t UBLOX_CFG_NMEA_BDSTALKERID = 0x30930033; - -//CFG-RATE -//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t UBLOX_CFG_RATE_MEAS = 0x30210001; -const uint32_t UBLOX_CFG_RATE_NAV = 0x30210002; -const uint32_t UBLOX_CFG_RATE_TIMEREF = 0x20210003; - -//CFG-I2C -//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t UBLOX_CFG_I2C_ADDRESS = 0x20510001; -const uint32_t UBLOX_CFG_I2C_ENABLED = 0x10510003; - -const uint32_t UBLOX_CFG_I2CINPROT_UBX = 0x10710001; -const uint32_t UBLOX_CFG_I2CINPROT_NMEA = 0x10710002; -const uint32_t UBLOX_CFG_I2CINPROT_RTCM3X = 0x10710004; - -const uint32_t UBLOX_CFG_I2COUTPROT_UBX = 0x10720001; -const uint32_t UBLOX_CFG_I2COUTPROT_NMEA = 0x10720002; -const uint32_t UBLOX_CFG_I2COUTPROT_RTCM3X = 0x10720004; - -//CFG-UART1 -//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t UBLOX_CFG_UART1_BAUDRATE = 0x40520001; -const uint32_t UBLOX_CFG_UART1_ENABLED = 0x10520005; - -const uint32_t UBLOX_CFG_UART1INPROT_UBX = 0x10730001; -const uint32_t UBLOX_CFG_UART1INPROT_NMEA = 0x10730002; -const uint32_t UBLOX_CFG_UART1INPROT_RTCM3X = 0x10730004; - -const uint32_t UBLOX_CFG_UART1OUTPROT_UBX = 0x10740001; -const uint32_t UBLOX_CFG_UART1OUTPROT_NMEA = 0x10740002; -const uint32_t UBLOX_CFG_UART1OUTPROT_RTCM3X = 0x10740004; - -//CFG-UART2 -//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t UBLOX_CFG_UART2_BAUDRATE = 0x40530001; -const uint32_t UBLOX_CFG_UART2_ENABLED = 0x10530005; - -const uint32_t UBLOX_CFG_UART2INPROT_UBX = 0x10750001; -const uint32_t UBLOX_CFG_UART2INPROT_NMEA = 0x10750002; -const uint32_t UBLOX_CFG_UART2INPROT_RTCM3X = 0x10750004; - -const uint32_t UBLOX_CFG_UART2OUTPROT_UBX = 0x10760001; -const uint32_t UBLOX_CFG_UART2OUTPROT_NMEA = 0x10760002; -const uint32_t UBLOX_CFG_UART2OUTPROT_RTCM3X = 0x10760004; - -//CFG-USB -//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t UBLOX_CFG_USBINPROT_UBX = 0x10770001; -const uint32_t UBLOX_CFG_USBINPROT_NMEA = 0x10770002; -const uint32_t UBLOX_CFG_USBINPROT_RTCM3X = 0x10770004; - -const uint32_t UBLOX_CFG_USBOUTPROT_UBX = 0x10780001; -const uint32_t UBLOX_CFG_USBOUTPROT_NMEA = 0x10780002; -const uint32_t UBLOX_CFG_USBOUTPROT_RTCM3X = 0x10780004; - //CFG-BDS: BeiDou system configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_BDS_USE_PRN_1_TO_5 = 0x10340014; // Use BeiDou geostationary satellites (PRN 1-5) +const uint32_t UBLOX_CFG_BDS_USE_PRN_1_TO_5 = 0x10340014; // Use BeiDou geostationary satellites (PRN 1-5) //CFG-GEOFENCE: Geofencing configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_GEOFENCE_CONFLVL = 0x20240011; // Required confidence level for state evaluation -const uint32_t CFG_GEOFENCE_USE_PIO = 0x10240012; // Use PIO combined fence state output -const uint32_t CFG_GEOFENCE_PINPOL = 0x20240013; // PIO pin polarity -const uint32_t CFG_GEOFENCE_PIN = 0x20240014; // PIO pin number -const uint32_t CFG_GEOFENCE_USE_FENCE1 = 0x10240020; // Use frst geofence -const uint32_t CFG_GEOFENCE_FENCE1_LAT = 0x40240021; // Latitude of the first geofence circle center -const uint32_t CFG_GEOFENCE_FENCE1_LON = 0x40240022; // Longitude of the first geofence circle center -const uint32_t CFG_GEOFENCE_FENCE1_RAD = 0x40240023; // Radius of the first geofence circle -const uint32_t CFG_GEOFENCE_USE_FENCE2 = 0x10240030; // Use second geofence -const uint32_t CFG_GEOFENCE_FENCE2_LAT = 0x40240031; // Latitude of the second geofence circle center -const uint32_t CFG_GEOFENCE_FENCE2_LON = 0x40240032; // Longitude of the second geofence circle center -const uint32_t CFG_GEOFENCE_FENCE2_RAD = 0x40240033; // Radius of the second geofence circle -const uint32_t CFG_GEOFENCE_USE_FENCE3 = 0x10240040; // Use third geofence -const uint32_t CFG_GEOFENCE_FENCE3_LAT = 0x40240041; // Latitude of the third geofence circle center -const uint32_t CFG_GEOFENCE_FENCE3_LON = 0x40240042; // Longitude of the third geofence circle center -const uint32_t CFG_GEOFENCE_FENCE3_RAD = 0x40240043; // Radius of the third geofence circle -const uint32_t CFG_GEOFENCE_USE_FENCE4 = 0x10240050; // Use fourth geofence -const uint32_t CFG_GEOFENCE_FENCE4_LAT = 0x40240051; // Latitude of the fourth geofence circle center -const uint32_t CFG_GEOFENCE_FENCE4_LON = 0x40240052; // Longitude of the fourth geofence circle center -const uint32_t CFG_GEOFENCE_FENCE4_RAD = 0x40240053; // Radius of the fourth geofence circle +const uint32_t UBLOX_CFG_GEOFENCE_CONFLVL = 0x20240011; // Required confidence level for state evaluation +const uint32_t UBLOX_CFG_GEOFENCE_USE_PIO = 0x10240012; // Use PIO combined fence state output +const uint32_t UBLOX_CFG_GEOFENCE_PINPOL = 0x20240013; // PIO pin polarity +const uint32_t UBLOX_CFG_GEOFENCE_PIN = 0x20240014; // PIO pin number +const uint32_t UBLOX_CFG_GEOFENCE_USE_FENCE1 = 0x10240020; // Use frst geofence +const uint32_t UBLOX_CFG_GEOFENCE_FENCE1_LAT = 0x40240021; // Latitude of the first geofence circle center +const uint32_t UBLOX_CFG_GEOFENCE_FENCE1_LON = 0x40240022; // Longitude of the first geofence circle center +const uint32_t UBLOX_CFG_GEOFENCE_FENCE1_RAD = 0x40240023; // Radius of the first geofence circle +const uint32_t UBLOX_CFG_GEOFENCE_USE_FENCE2 = 0x10240030; // Use second geofence +const uint32_t UBLOX_CFG_GEOFENCE_FENCE2_LAT = 0x40240031; // Latitude of the second geofence circle center +const uint32_t UBLOX_CFG_GEOFENCE_FENCE2_LON = 0x40240032; // Longitude of the second geofence circle center +const uint32_t UBLOX_CFG_GEOFENCE_FENCE2_RAD = 0x40240033; // Radius of the second geofence circle +const uint32_t UBLOX_CFG_GEOFENCE_USE_FENCE3 = 0x10240040; // Use third geofence +const uint32_t UBLOX_CFG_GEOFENCE_FENCE3_LAT = 0x40240041; // Latitude of the third geofence circle center +const uint32_t UBLOX_CFG_GEOFENCE_FENCE3_LON = 0x40240042; // Longitude of the third geofence circle center +const uint32_t UBLOX_CFG_GEOFENCE_FENCE3_RAD = 0x40240043; // Radius of the third geofence circle +const uint32_t UBLOX_CFG_GEOFENCE_USE_FENCE4 = 0x10240050; // Use fourth geofence +const uint32_t UBLOX_CFG_GEOFENCE_FENCE4_LAT = 0x40240051; // Latitude of the fourth geofence circle center +const uint32_t UBLOX_CFG_GEOFENCE_FENCE4_LON = 0x40240052; // Longitude of the fourth geofence circle center +const uint32_t UBLOX_CFG_GEOFENCE_FENCE4_RAD = 0x40240053; // Radius of the fourth geofence circle //CFG-HW: Hardware configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_HW_ANT_CFG_VOLTCTRL = 0x10a3002e; // Active antenna voltage control flag -const uint32_t CFG_HW_ANT_CFG_SHORTDET = 0x10a3002f; // Short antenna detection flag -const uint32_t CFG_HW_ANT_CFG_SHORTDET_POL = 0x10a30030; // Short antenna detection polarity -const uint32_t CFG_HW_ANT_CFG_OPENDET = 0x10a30031; // Open antenna detection flag -const uint32_t CFG_HW_ANT_CFG_OPENDET_POL = 0x10a30032; // Open antenna detection polarity -const uint32_t CFG_HW_ANT_CFG_PWRDOWN = 0x10a30033; // Power down antenna flag -const uint32_t CFG_HW_ANT_CFG_PWRDOWN_POL = 0x10a30034; // Power down antenna logic polarity -const uint32_t CFG_HW_ANT_CFG_RECOVER = 0x10a30035; // Automatic recovery from short state flag -const uint32_t CFG_HW_ANT_SUP_SWITCH_PIN = 0x20a30036; // ANT1 PIO number -const uint32_t CFG_HW_ANT_SUP_SHORT_PIN = 0x20a30037; // ANT0 PIO number -const uint32_t CFG_HW_ANT_SUP_OPEN_PIN = 0x20a30038; // ANT2 PIO number -const uint32_t CFG_HW_ANT_SUP_ENGINE = 0x20a30054; // Antenna supervisor engine selection -const uint32_t CFG_HW_ANT_SUP_SHORT_THR = 0x20a30055; // Antenna supervisor MADC engine short detection threshold -const uint32_t CFG_HW_ANT_SUP_OPEN_THR = 0x20a30056; // Antenna supervisor MADC engine open detection threshold +const uint32_t UBLOX_CFG_HW_ANT_CFG_VOLTCTRL = 0x10a3002e; // Active antenna voltage control flag +const uint32_t UBLOX_CFG_HW_ANT_CFG_SHORTDET = 0x10a3002f; // Short antenna detection flag +const uint32_t UBLOX_CFG_HW_ANT_CFG_SHORTDET_POL = 0x10a30030; // Short antenna detection polarity +const uint32_t UBLOX_CFG_HW_ANT_CFG_OPENDET = 0x10a30031; // Open antenna detection flag +const uint32_t UBLOX_CFG_HW_ANT_CFG_OPENDET_POL = 0x10a30032; // Open antenna detection polarity +const uint32_t UBLOX_CFG_HW_ANT_CFG_PWRDOWN = 0x10a30033; // Power down antenna flag +const uint32_t UBLOX_CFG_HW_ANT_CFG_PWRDOWN_POL = 0x10a30034; // Power down antenna logic polarity +const uint32_t UBLOX_CFG_HW_ANT_CFG_RECOVER = 0x10a30035; // Automatic recovery from short state flag +const uint32_t UBLOX_CFG_HW_ANT_SUP_SWITCH_PIN = 0x20a30036; // ANT1 PIO number +const uint32_t UBLOX_CFG_HW_ANT_SUP_SHORT_PIN = 0x20a30037; // ANT0 PIO number +const uint32_t UBLOX_CFG_HW_ANT_SUP_OPEN_PIN = 0x20a30038; // ANT2 PIO number +const uint32_t UBLOX_CFG_HW_ANT_SUP_ENGINE = 0x20a30054; // Antenna supervisor engine selection +const uint32_t UBLOX_CFG_HW_ANT_SUP_SHORT_THR = 0x20a30055; // Antenna supervisor MADC engine short detection threshold +const uint32_t UBLOX_CFG_HW_ANT_SUP_OPEN_THR = 0x20a30056; // Antenna supervisor MADC engine open detection threshold //CFG-I2C: Configuration of the I2C interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_I2C_ADDRESS = 0x20510001; // I2C slave address of the receiver (7 bits) -const uint32_t CFG_I2C_EXTENDEDTIMEOUT = 0x10510002; // Flag to disable timeouting the interface after 1.5 s -const uint32_t CFG_I2C_ENABLED = 0x10510003; // Flag to indicate if the I2C interface should be enabled +const uint32_t UBLOX_CFG_I2C_ADDRESS = 0x20510001; // I2C slave address of the receiver (7 bits) +const uint32_t UBLOX_CFG_I2C_EXTENDEDTIMEOUT = 0x10510002; // Flag to disable timeouting the interface after 1.5 s +const uint32_t UBLOX_CFG_I2C_ENABLED = 0x10510003; // Flag to indicate if the I2C interface should be enabled //CFG-I2CINPROT: Input protocol configuration of the I2C interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_I2CINPROT_UBX = 0x10710001; // Flag to indicate if UBX should be an input protocol on I2C -const uint32_t CFG_I2CINPROT_NMEA = 0x10710002; // Flag to indicate if NMEA should be an input protocol on I2C -const uint32_t CFG_I2CINPROT_RTCM3X = 0x10710004; // Flag to indicate if RTCM3X should be an input protocol on I2C +const uint32_t UBLOX_CFG_I2CINPROT_UBX = 0x10710001; // Flag to indicate if UBX should be an input protocol on I2C +const uint32_t UBLOX_CFG_I2CINPROT_NMEA = 0x10710002; // Flag to indicate if NMEA should be an input protocol on I2C +const uint32_t UBLOX_CFG_I2CINPROT_RTCM3X = 0x10710004; // Flag to indicate if RTCM3X should be an input protocol on I2C //CFG-I2COUTPROT: Output protocol configuration of the I2C interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_I2COUTPROT_UBX = 0x10720001; // Flag to indicate if UBX should be an output protocol on I2C -const uint32_t CFG_I2COUTPROT_NMEA = 0x10720002; // Flag to indicate if NMEA should be an output protocol on I2C -const uint32_t CFG_I2COUTPROT_RTCM3X = 0x10720004; // Flag to indicate if RTCM3X should be an output protocol on I2C +const uint32_t UBLOX_CFG_I2COUTPROT_UBX = 0x10720001; // Flag to indicate if UBX should be an output protocol on I2C +const uint32_t UBLOX_CFG_I2COUTPROT_NMEA = 0x10720002; // Flag to indicate if NMEA should be an output protocol on I2C +const uint32_t UBLOX_CFG_I2COUTPROT_RTCM3X = 0x10720004; // Flag to indicate if RTCM3X should be an output protocol on I2C //CFG-INFMSG: Information message configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_INFMSG_UBX_I2C = 0x20920001; // Information message enable flags for the UBX protocol on the I2C interface -const uint32_t CFG_INFMSG_UBX_UART1 = 0x20920002; // Information message enable flags for the UBX protocol on the UART1 interface -const uint32_t CFG_INFMSG_UBX_UART2 = 0x20920003; // Information message enable flags for the UBX protocol on the UART2 interface -const uint32_t CFG_INFMSG_UBX_USB = 0x20920004; // Information message enable flags for the UBX protocol on the USB interface -const uint32_t CFG_INFMSG_UBX_SPI = 0x20920005; // Information message enable flags for the UBX protocol on the SPI interface -const uint32_t CFG_INFMSG_NMEA_I2C = 0x20920006; // Information message enable flags for the NMEA protocol on the I2C interface -const uint32_t CFG_INFMSG_NMEA_UART1 = 0x20920007; // Information message enable flags for the NMEA protocol on the UART1 interface -const uint32_t CFG_INFMSG_NMEA_UART2 = 0x20920008; // Information message enable flags for the NMEA protocol on the UART2 interface -const uint32_t CFG_INFMSG_NMEA_USB = 0x20920009; // Information message enable flags for the NMEA protocol on the USB interface -const uint32_t CFG_INFMSG_NMEA_SPI = 0x2092000a; // Information message enable flags for the NMEA protocol on the SPI interface +const uint32_t UBLOX_CFG_INFMSG_UBX_I2C = 0x20920001; // Information message enable flags for the UBX protocol on the I2C interface +const uint32_t UBLOX_CFG_INFMSG_UBX_UART1 = 0x20920002; // Information message enable flags for the UBX protocol on the UART1 interface +const uint32_t UBLOX_CFG_INFMSG_UBX_UART2 = 0x20920003; // Information message enable flags for the UBX protocol on the UART2 interface +const uint32_t UBLOX_CFG_INFMSG_UBX_USB = 0x20920004; // Information message enable flags for the UBX protocol on the USB interface +const uint32_t UBLOX_CFG_INFMSG_UBX_SPI = 0x20920005; // Information message enable flags for the UBX protocol on the SPI interface +const uint32_t UBLOX_CFG_INFMSG_NMEA_I2C = 0x20920006; // Information message enable flags for the NMEA protocol on the I2C interface +const uint32_t UBLOX_CFG_INFMSG_NMEA_UART1 = 0x20920007; // Information message enable flags for the NMEA protocol on the UART1 interface +const uint32_t UBLOX_CFG_INFMSG_NMEA_UART2 = 0x20920008; // Information message enable flags for the NMEA protocol on the UART2 interface +const uint32_t UBLOX_CFG_INFMSG_NMEA_USB = 0x20920009; // Information message enable flags for the NMEA protocol on the USB interface +const uint32_t UBLOX_CFG_INFMSG_NMEA_SPI = 0x2092000a; // Information message enable flags for the NMEA protocol on the SPI interface //CFG-ITFM: Jamming and interference monitor configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_ITFM_BBTHRESHOLD = 0x20410001; // Broadband jamming detection threshold -const uint32_t CFG_ITFM_CWTHRESHOLD = 0x20410002; // CW jamming detection threshold -const uint32_t CFG_ITFM_ENABLE = 0x1041000d; // Enable interference detection -const uint32_t CFG_ITFM_ANTSETTING = 0x20410010; // Antenna setting -const uint32_t CFG_ITFM_ENABLE_AUX = 0x10410013; // Scan auxiliary bands +const uint32_t UBLOX_CFG_ITFM_BBTHRESHOLD = 0x20410001; // Broadband jamming detection threshold +const uint32_t UBLOX_CFG_ITFM_CWTHRESHOLD = 0x20410002; // CW jamming detection threshold +const uint32_t UBLOX_CFG_ITFM_ENABLE = 0x1041000d; // Enable interference detection +const uint32_t UBLOX_CFG_ITFM_ANTSETTING = 0x20410010; // Antenna setting +const uint32_t UBLOX_CFG_ITFM_ENABLE_AUX = 0x10410013; // Scan auxiliary bands //CFG-LOGFILTER: Data logger configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_LOGFILTER_RECORD_ENA = 0x10de0002; // Recording enabled -const uint32_t CFG_LOGFILTER_ONCE_PER_WAKE_UP_ENA = 0x10de0003; // Once per wake up -const uint32_t CFG_LOGFILTER_APPLY_ALL_FILTERS = 0x10de0004; // Apply all filter settings -const uint32_t CFG_LOGFILTER_MIN_INTERVAL = 0x30de0005; // Minimum time interval between loggedpositions -const uint32_t CFG_LOGFILTER_TIME_THRS = 0x30de0006; // Time threshold -const uint32_t CFG_LOGFILTER_SPEED_THRS = 0x30de0007; // Speed threshold -const uint32_t CFG_LOGFILTER_POSITION_THRS = 0x40de0008; // Position threshold +const uint32_t UBLOX_CFG_LOGFILTER_RECORD_ENA = 0x10de0002; // Recording enabled +const uint32_t UBLOX_CFG_LOGFILTER_ONCE_PER_WAKE_UP_ENA = 0x10de0003; // Once per wake up +const uint32_t UBLOX_CFG_LOGFILTER_APPLY_ALL_FILTERS = 0x10de0004; // Apply all filter settings +const uint32_t UBLOX_CFG_LOGFILTER_MIN_INTERVAL = 0x30de0005; // Minimum time interval between loggedpositions +const uint32_t UBLOX_CFG_LOGFILTER_TIME_THRS = 0x30de0006; // Time threshold +const uint32_t UBLOX_CFG_LOGFILTER_SPEED_THRS = 0x30de0007; // Speed threshold +const uint32_t UBLOX_CFG_LOGFILTER_POSITION_THRS = 0x40de0008; // Position threshold //CFG-MOT: Motion detector configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_MOT_GNSSSPEED_THRS = 0x20250038; // GNSS speed threshold below which platform is considered as stationary (a.k.a. static hold threshold) -const uint32_t CFG_MOT_GNSSDIST_THRS = 0x3025003b; // Distance above which GNSS-based stationary motion is exit (a.k.a. static hold distance threshold) +const uint32_t UBLOX_CFG_MOT_GNSSSPEED_THRS = 0x20250038; // GNSS speed threshold below which platform is considered as stationary (a.k.a. static hold threshold) +const uint32_t UBLOX_CFG_MOT_GNSSDIST_THRS = 0x3025003b; // Distance above which GNSS-based stationary motion is exit (a.k.a. static hold distance threshold) // CFG-MSGOUT: Message output configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- // For each message and port a separate output rate (per second, per epoch) can be configured. -const uint32_t CFG_MSGOUT_NMEA_ID_DTM_I2C = 0x209100a6; //Output rate of the NMEA-GX-DTM message on port I2C -const uint32_t CFG_MSGOUT_NMEA_ID_DTM_SPI = 0x209100aa; //Output rate of the NMEA-GX-DTM message on port SPI -const uint32_t CFG_MSGOUT_NMEA_ID_DTM_UART1 = 0x209100a7; //Output rate of the NMEA-GX-DTM message on port UART1 -const uint32_t CFG_MSGOUT_NMEA_ID_DTM_UART2 = 0x209100a8; //Output rate of the NMEA-GX-DTM message on port UART2 -const uint32_t CFG_MSGOUT_NMEA_ID_DTM_USB = 0x209100a9; //Output rate of the NMEA-GX-DTM message on port USB -const uint32_t CFG_MSGOUT_NMEA_ID_GBS_I2C = 0x209100dd; //Output rate of the NMEA-GX-GBS message on port I2C -const uint32_t CFG_MSGOUT_NMEA_ID_GBS_SPI = 0x209100e1; //Output rate of the NMEA-GX-GBS message on port SPI -const uint32_t CFG_MSGOUT_NMEA_ID_GBS_UART1 = 0x209100de; //Output rate of the NMEA-GX-GBS message on port UART1 -const uint32_t CFG_MSGOUT_NMEA_ID_GBS_UART2 = 0x209100df; //Output rate of the NMEA-GX-GBS message on port UART2 -const uint32_t CFG_MSGOUT_NMEA_ID_GBS_USB = 0x209100e0; //Output rate of the NMEA-GX-GBS message on port USB -const uint32_t CFG_MSGOUT_NMEA_ID_GGA_I2C = 0x209100ba; //Output rate of the NMEA-GX-GGA message on port I2C -const uint32_t CFG_MSGOUT_NMEA_ID_GGA_SPI = 0x209100be; //Output rate of the NMEA-GX-GGA message on port SPI -const uint32_t CFG_MSGOUT_NMEA_ID_GGA_UART1 = 0x209100bb; //Output rate of the NMEA-GX-GGA message on port UART1 -const uint32_t CFG_MSGOUT_NMEA_ID_GGA_UART2 = 0x209100bc; //Output rate of the NMEA-GX-GGA message on port UART2 -const uint32_t CFG_MSGOUT_NMEA_ID_GGA_USB = 0x209100bd; //Output rate of the NMEA-GX-GGA message on port USB -const uint32_t CFG_MSGOUT_NMEA_ID_GLL_I2C = 0x209100c9; //Output rate of the NMEA-GX-GLL message on port I2C -const uint32_t CFG_MSGOUT_NMEA_ID_GLL_SPI = 0x209100cd; //Output rate of the NMEA-GX-GLL message on port SPI -const uint32_t CFG_MSGOUT_NMEA_ID_GLL_UART1 = 0x209100ca; //Output rate of the NMEA-GX-GLL message on port UART1 -const uint32_t CFG_MSGOUT_NMEA_ID_GLL_UART2 = 0x209100cb; //Output rate of the NMEA-GX-GLL message on port UART2 -const uint32_t CFG_MSGOUT_NMEA_ID_GLL_USB = 0x209100cc; //Output rate of the NMEA-GX-GLL message on port USB -const uint32_t CFG_MSGOUT_NMEA_ID_GNS_I2C = 0x209100b5; //Output rate of the NMEA-GX-GNS message on port I2C -const uint32_t CFG_MSGOUT_NMEA_ID_GNS_SPI = 0x209100b9; //Output rate of the NMEA-GX-GNS message on port SPI -const uint32_t CFG_MSGOUT_NMEA_ID_GNS_UART1 = 0x209100b6; //Output rate of the NMEA-GX-GNS message on port UART1 -const uint32_t CFG_MSGOUT_NMEA_ID_GNS_UART2 = 0x209100b7; //Output rate of the NMEA-GX-GNS message on port UART2 -const uint32_t CFG_MSGOUT_NMEA_ID_GNS_USB = 0x209100b8; //Output rate of the NMEA-GX-GNS message on port USB -const uint32_t CFG_MSGOUT_NMEA_ID_GRS_I2C = 0x209100ce; //Output rate of the NMEA-GX-GRS message on port I2C -const uint32_t CFG_MSGOUT_NMEA_ID_GRS_SPI = 0x209100d2; //Output rate of the NMEA-GX-GRS message on port SPI -const uint32_t CFG_MSGOUT_NMEA_ID_GRS_UART1 = 0x209100cf; //Output rate of the NMEA-GX-GRS message on port UART1 -const uint32_t CFG_MSGOUT_NMEA_ID_GRS_UART2 = 0x209100d0; //Output rate of the NMEA-GX-GRS message on port UART2 -const uint32_t CFG_MSGOUT_NMEA_ID_GRS_USB = 0x209100d1; //Output rate of the NMEA-GX-GRS message on port USB -const uint32_t CFG_MSGOUT_NMEA_ID_GSA_I2C = 0x209100bf; //Output rate of the NMEA-GX-GSA message on port I2C -const uint32_t CFG_MSGOUT_NMEA_ID_GSA_SPI = 0x209100c3; //Output rate of the NMEA-GX-GSA message on port SPI -const uint32_t CFG_MSGOUT_NMEA_ID_GSA_UART1 = 0x209100c0; //Output rate of the NMEA-GX-GSA message on port UART1 -const uint32_t CFG_MSGOUT_NMEA_ID_GSA_UART2 = 0x209100c1; //Output rate of the NMEA-GX-GSA message on port UART2 -const uint32_t CFG_MSGOUT_NMEA_ID_GSA_USB = 0x209100c2; //Output rate of the NMEA-GX-GSA message on port USB -const uint32_t CFG_MSGOUT_NMEA_ID_GST_I2C = 0x209100d3; //Output rate of the NMEA-GX-GST message on port I2C -const uint32_t CFG_MSGOUT_NMEA_ID_GST_SPI = 0x209100d7; //Output rate of the NMEA-GX-GST message on port SPI -const uint32_t CFG_MSGOUT_NMEA_ID_GST_UART1 = 0x209100d4; //Output rate of the NMEA-GX-GST message on port UART1 -const uint32_t CFG_MSGOUT_NMEA_ID_GST_UART2 = 0x209100d5; //Output rate of the NMEA-GX-GST message on port UART2 -const uint32_t CFG_MSGOUT_NMEA_ID_GST_USB = 0x209100d6; //Output rate of the NMEA-GX-GST message on port USB -const uint32_t CFG_MSGOUT_NMEA_ID_GSV_I2C = 0x209100c4; //Output rate of the NMEA-GX-GSV message on port I2C -const uint32_t CFG_MSGOUT_NMEA_ID_GSV_SPI = 0x209100c8; //Output rate of the NMEA-GX-GSV message on port SPI -const uint32_t CFG_MSGOUT_NMEA_ID_GSV_UART1 = 0x209100c5; //Output rate of the NMEA-GX-GSV message on port UART1 -const uint32_t CFG_MSGOUT_NMEA_ID_GSV_UART2 = 0x209100c6; //Output rate of the NMEA-GX-GSV message on port UART2 -const uint32_t CFG_MSGOUT_NMEA_ID_GSV_USB = 0x209100c7; //Output rate of the NMEA-GX-GSV message on port USB -const uint32_t CFG_MSGOUT_NMEA_ID_RLM_I2C = 0x20910400; //Output rate of the NMEA-GX-RLM message on port I2C -const uint32_t CFG_MSGOUT_NMEA_ID_RLM_SPI = 0x20910404; //Output rate of the NMEA-GX-RLM message on port SPI -const uint32_t CFG_MSGOUT_NMEA_ID_RLM_UART1 = 0x20910401; //Output rate of the NMEA-GX-RLM message on port UART1 -const uint32_t CFG_MSGOUT_NMEA_ID_RLM_UART2 = 0x20910402; //Output rate of the NMEA-GX-RLM message on port UART2 -const uint32_t CFG_MSGOUT_NMEA_ID_RLM_USB = 0x20910403; //Output rate of the NMEA-GX-RLM message on port USB -const uint32_t CFG_MSGOUT_NMEA_ID_RMC_I2C = 0x209100ab; //Output rate of the NMEA-GX-RMC message on port I2C -const uint32_t CFG_MSGOUT_NMEA_ID_RMC_SPI = 0x209100af; //Output rate of the NMEA-GX-RMC message on port SPI -const uint32_t CFG_MSGOUT_NMEA_ID_RMC_UART1 = 0x209100ac; //Output rate of the NMEA-GX-RMC message on port UART1 -const uint32_t CFG_MSGOUT_NMEA_ID_RMC_UART2 = 0x209100ad; //Output rate of the NMEA-GX-RMC message on port UART2 -const uint32_t CFG_MSGOUT_NMEA_ID_RMC_USB = 0x209100ae; //Output rate of the NMEA-GX-RMC message on port USB -const uint32_t CFG_MSGOUT_NMEA_ID_VLW_I2C = 0x209100e7; //Output rate of the NMEA-GX-VLW message on port I2C -const uint32_t CFG_MSGOUT_NMEA_ID_VLW_SPI = 0x209100eb; //Output rate of the NMEA-GX-VLW message on port SPI -const uint32_t CFG_MSGOUT_NMEA_ID_VLW_UART1 = 0x209100e8; //Output rate of the NMEA-GX-VLW message on port UART1 -const uint32_t CFG_MSGOUT_NMEA_ID_VLW_UART2 = 0x209100e9; //Output rate of the NMEA-GX-VLW message on port UART2 -const uint32_t CFG_MSGOUT_NMEA_ID_VLW_USB = 0x209100ea; //Output rate of the NMEA-GX-VLW message on port USB -const uint32_t CFG_MSGOUT_NMEA_ID_VTG_I2C = 0x209100b0; //Output rate of the NMEA-GX-VTG message on port I2C -const uint32_t CFG_MSGOUT_NMEA_ID_VTG_SPI = 0x209100b4; //Output rate of the NMEA-GX-VTG message on port SPI -const uint32_t CFG_MSGOUT_NMEA_ID_VTG_UART1 = 0x209100b1; //Output rate of the NMEA-GX-VTG message on port UART1 -const uint32_t CFG_MSGOUT_NMEA_ID_VTG_UART2 = 0x209100b2; //Output rate of the NMEA-GX-VTG message on port UART2 -const uint32_t CFG_MSGOUT_NMEA_ID_VTG_USB = 0x209100b3; //Output rate of the NMEA-GX-VTG message on port USB -const uint32_t CFG_MSGOUT_NMEA_ID_ZDA_I2C = 0x209100d8; //Output rate of the NMEA-GX-ZDA message on port I2C -const uint32_t CFG_MSGOUT_NMEA_ID_ZDA_SPI = 0x209100dc; //Output rate of the NMEA-GX-ZDA message on port SPI -const uint32_t CFG_MSGOUT_NMEA_ID_ZDA_UART1 = 0x209100d9; //Output rate of the NMEA-GX-ZDA message on port UART1 -const uint32_t CFG_MSGOUT_NMEA_ID_ZDA_UART2 = 0x209100da; //Output rate of the NMEA-GX-ZDA message on port UART2 -const uint32_t CFG_MSGOUT_NMEA_ID_ZDA_USB = 0x209100db; //Output rate of the NMEA-GX-ZDA message on port USB -const uint32_t CFG_MSGOUT_PUBX_ID_POLYP_I2C = 0x209100ec; //Output rate of the NMEA-GX-PUBX00 message on port I2C -const uint32_t CFG_MSGOUT_PUBX_ID_POLYP_SPI = 0x209100f0; //Output rate of the NMEA-GX-PUBX00 message on port SPI -const uint32_t CFG_MSGOUT_PUBX_ID_POLYP_UART1 = 0x209100ed; //Output rate of the NMEA-GX-PUBX00 message on port UART1 -const uint32_t CFG_MSGOUT_PUBX_ID_POLYP_UART2 = 0x209100ee; //Output rate of the NMEA-GX-PUBX00 message on port UART2 -const uint32_t CFG_MSGOUT_PUBX_ID_POLYP_USB = 0x209100ef; //Output rate of the NMEA-GX-PUBX00 message on port USB -const uint32_t CFG_MSGOUT_PUBX_ID_POLYS_I2C = 0x209100f1; //Output rate of the NMEA-GX-PUBX03 message on port I2C -const uint32_t CFG_MSGOUT_PUBX_ID_POLYS_SPI = 0x209100f5; //Output rate of the NMEA-GX-PUBX03 message on port SPI -const uint32_t CFG_MSGOUT_PUBX_ID_POLYS_UART1 = 0x209100f2; //Output rate of the NMEA-GX-PUBX03 message on port UART1 -const uint32_t CFG_MSGOUT_PUBX_ID_POLYS_UART2 = 0x209100f3; //Output rate of the NMEA-GX-PUBX03 message on port UART2 -const uint32_t CFG_MSGOUT_PUBX_ID_POLYS_USB = 0x209100f4; //Output rate of the NMEA-GX-PUBX03 message on port USB -const uint32_t CFG_MSGOUT_PUBX_ID_POLYT_I2C = 0x209100f6; //Output rate of the NMEA-GX-PUBX04 message on port I2C -const uint32_t CFG_MSGOUT_PUBX_ID_POLYT_SPI = 0x209100fa; //Output rate of the NMEA-GX-PUBX04 message on port SPI -const uint32_t CFG_MSGOUT_PUBX_ID_POLYT_UART1 = 0x209100f7; //Output rate of the NMEA-GX-PUBX04 message on port UART1 -const uint32_t CFG_MSGOUT_PUBX_ID_POLYT_UART2 = 0x209100f8; //Output rate of the NMEA-GX-PUBX04 message on port UART2 -const uint32_t CFG_MSGOUT_PUBX_ID_POLYT_USB = 0x209100f9; //Output rate of the NMEA-GX-PUBX04 message on port USB -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1005_I2C = 0x209102bd; //Output rate of the RTCM-3X-TYPE1005 message on port I2C -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1005_SPI = 0x209102c1; //Output rate of the RTCM-3X-TYPE1005 message on port SPI -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1005_UART1 = 0x209102be;//Output rate of the RTCM-3X-TYPE1005 message on port UART1 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1005_UART2 = 0x209102bf;//Output rate of the RTCM-3X-TYPE1005 message on port UART2 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1005_USB = 0x209102c0; //Output rate of the RTCM-3X-TYPE1005 message on port USB -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1074_I2C = 0x2091035e; //Output rate of the RTCM-3X-TYPE1074 message on port I2C -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1074_SPI = 0x20910362; //Output rate of the RTCM-3X-TYPE1074 message on port SPI -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1074_UART1 = 0x2091035f;//Output rate of the RTCM-3X-TYPE1074 message on port UART1 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1074_UART2 = 0x20910360;//Output rate of the RTCM-3X-TYPE1074 message on port UART2 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1074_USB = 0x20910361; //Output rate of the RTCM-3X-TYPE1074 message on port USB -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1077_I2C = 0x209102cc; //Output rate of the RTCM-3X-TYPE1077 message on port I2C -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1077_SPI = 0x209102d0; //Output rate of the RTCM-3X-TYPE1077 message on port SPI -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1077_UART1 = 0x209102cd;//Output rate of the RTCM-3X-TYPE1077 message on port UART1 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1077_UART2 = 0x209102ce;//Output rate of the RTCM-3X-TYPE1077 message on port UART2 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1077_USB = 0x209102cf; //Output rate of the RTCM-3X-TYPE1077 message on port USB -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1084_I2C = 0x20910363; //Output rate of the RTCM-3X-TYPE1084 message on port I2C -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1084_SPI = 0x20910367; //Output rate of the RTCM-3X-TYPE1084 message on port SPI -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1084_UART1 = 0x20910364;//Output rate of the RTCM-3X-TYPE1084 message on port UART1 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1084_UART2 = 0x20910365;//Output rate of the RTCM-3X-TYPE1084 message on port UART2 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1084_USB = 0x20910366; //Output rate of the RTCM-3X-TYPE1084 message on port USB -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1087_I2C = 0x209102d1; //Output rate of the RTCM-3X-TYPE1087 message on port I2C -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1087_SPI = 0x209102d5; //Output rate of the RTCM-3X-TYPE1087 message on port SPI -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1087_UART1 = 0x209102d2;//Output rate of the RTCM-3X-TYPE1087 message on port UART1 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1087_UART2 = 0x209102d3;//Output rate of the RTCM-3X-TYPE1087 message on port UART2 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1087_USB = 0x209102d4; //Output rate of the RTCM-3X-TYPE1087 message on port USB -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1094_I2C = 0x20910368; //Output rate of the RTCM-3X-TYPE1094 message on port I2C -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1094_SPI = 0x2091036c; //Output rate of the RTCM-3X-TYPE1094 message on port SPI -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1094_UART1 = 0x20910369;//Output rate of the RTCM-3X-TYPE1094 message on port UART1 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1094_UART2 = 0x2091036a;//Output rate of the RTCM-3X-TYPE1094 message on port UART2 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1094_USB = 0x2091036b; //Output rate of the RTCM-3X-TYPE1094 message on port USB -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1097_I2C = 0x20910318; //Output rate of the RTCM-3X-TYPE1097 message on port I2C -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1097_SPI = 0x2091031c; //Output rate of the RTCM-3X-TYPE1097 message on port SPI -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1097_UART1 = 0x20910319;//Output rate of the RTCM-3X-TYPE1097 message on port UART1 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1097_UART2 = 0x2091031a;//Output rate of the RTCM-3X-TYPE1097 message on port UART2 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1097_USB = 0x2091031b; //Output rate of the RTCM-3X-TYPE1097 message on port USB -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1124_I2C = 0x2091036d; //Output rate of the RTCM-3X-TYPE1124 message on port I2C -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1124_SPI = 0x20910371; //Output rate of the RTCM-3X-TYPE1124 message on port SPI -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1124_UART1 = 0x2091036e;//Output rate of the RTCM-3X-TYPE1124 message on port UART1 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1124_UART2 = 0x2091036f;//Output rate of the RTCM-3X-TYPE1124 message on port UART2 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1124_USB = 0x20910370; //Output rate of the RTCM-3X-TYPE1124 message on port USB -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1127_I2C = 0x209102d6; //Output rate of the RTCM-3X-TYPE1127 message on port I2C -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1127_SPI = 0x209102da; //Output rate of the RTCM-3X-TYPE1127 message on port SPI -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1127_UART1 = 0x209102d7;//Output rate of the RTCM-3X-TYPE1127 message on port UART1 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1127_UART2 = 0x209102d8;//Output rate of the RTCM-3X-TYPE1127 message on port UART2 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1127_USB = 0x209102d9; //Output rate of the RTCM-3X-TYPE1127 message on port USB -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1230_I2C = 0x20910303; //Output rate of the RTCM-3X-TYPE1230 message on port I2C -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1230_SPI = 0x20910307; //Output rate of the RTCM-3X-TYPE1230 message on port SPI -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1230_UART1 = 0x20910304;//Output rate of the RTCM-3X-TYPE1230 message on port UART1 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1230_UART2 = 0x20910305;//Output rate of the RTCM-3X-TYPE1230 message on port UART2 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE1230_USB = 0x20910306; //Output rate of the RTCM-3X-TYPE1230 message on port USB -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE4072_0_I2C = 0x209102fe;//Output rate of the RTCM-3X-TYPE4072_0 message on port I2C -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE4072_0_SPI = 0x20910302;//Output rate of the RTCM-3X-TYPE4072_0 message on port SPI -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE4072_0_UART1 = 0x209102ff; //Output rate of the RTCM-3X-TYPE4072_0 message on port UART1 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE4072_0_UART2 = 0x20910300; //Output rate of the RTCM-3X-TYPE4072_0 message on port UART2 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE4072_0_USB = 0x20910301;//Output rate of the RTCM-3X-TYPE4072_0 message on port USB -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE4072_1_I2C = 0x20910381;//Output rate of the RTCM-3X-TYPE4072_1 message on port I2C -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE4072_1_SPI = 0x20910385;//Output rate of the RTCM-3X-TYPE4072_1 message on port SPI -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE4072_1_UART1 = 0x20910382; //Output rate of the RTCM-3X-TYPE4072_1 message on port UART1 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE4072_1_UART2 = 0x20910383; //Output rate of the RTCM-3X-TYPE4072_1 message on port UART2 -const uint32_t CFG_MSGOUT_RTCM_3X_TYPE4072_1_USB = 0x20910384;//Output rate of the RTCM-3X-TYPE4072_1 message on port USB -const uint32_t CFG_MSGOUT_UBX_LOG_INFO_I2C = 0x20910259; //Output rate of the UBX-LOG-INFO message on port I2C -const uint32_t CFG_MSGOUT_UBX_LOG_INFO_SPI = 0x2091025d; //Output rate of the UBX-LOG-INFO message on port SPI -const uint32_t CFG_MSGOUT_UBX_LOG_INFO_UART1 = 0x2091025a; //Output rate of the UBX-LOG-INFO message on port UART1 -const uint32_t CFG_MSGOUT_UBX_LOG_INFO_UART2 = 0x2091025b; //Output rate of the UBX-LOG-INFO message on port UART2 -const uint32_t CFG_MSGOUT_UBX_LOG_INFO_USB = 0x2091025c; //Output rate of the UBX-LOG-INFO message on port USB -const uint32_t CFG_MSGOUT_UBX_MON_COMMS_I2C = 0x2091034f; //Output rate of the UBX-MON-COMMS message on port I2C -const uint32_t CFG_MSGOUT_UBX_MON_COMMS_SPI = 0x20910353; //Output rate of the UBX-MON-COMMS message on port SPI -const uint32_t CFG_MSGOUT_UBX_MON_COMMS_UART1 = 0x20910350; //Output rate of the UBX-MON-COMMS message on port UART1 -const uint32_t CFG_MSGOUT_UBX_MON_COMMS_UART2 = 0x20910351; //Output rate of the UBX-MON-COMMS message on port UART2 -const uint32_t CFG_MSGOUT_UBX_MON_COMMS_USB = 0x20910352; //Output rate of the UBX-MON-COMMS message on port USB -const uint32_t CFG_MSGOUT_UBX_MON_HW2_I2C = 0x209101b9; //Output rate of the UBX-MON-HW2 message on port I2C -const uint32_t CFG_MSGOUT_UBX_MON_HW2_SPI = 0x209101bd; //Output rate of the UBX-MON-HW2 message on port SPI -const uint32_t CFG_MSGOUT_UBX_MON_HW2_UART1 = 0x209101ba; //Output rate of the UBX-MON-HW2 message on port UART1 -const uint32_t CFG_MSGOUT_UBX_MON_HW2_UART2 = 0x209101bb; //Output rate of the UBX-MON-HW2 message on port UART2 -const uint32_t CFG_MSGOUT_UBX_MON_HW2_USB = 0x209101bc; //Output rate of the UBX-MON-HW2 message on port USB -const uint32_t CFG_MSGOUT_UBX_MON_HW3_I2C = 0x20910354; //Output rate of the UBX-MON-HW3 message on port I2C -const uint32_t CFG_MSGOUT_UBX_MON_HW3_SPI = 0x20910358; //Output rate of the UBX-MON-HW3 message on port SPI -const uint32_t CFG_MSGOUT_UBX_MON_HW3_UART1 = 0x20910355; //Output rate of the UBX-MON-HW3 message on port UART1 -const uint32_t CFG_MSGOUT_UBX_MON_HW3_UART2 = 0x20910356; //Output rate of the UBX-MON-HW3 message on port UART2 -const uint32_t CFG_MSGOUT_UBX_MON_HW3_USB = 0x20910357; //Output rate of the UBX-MON-HW3 message on port USB -const uint32_t CFG_MSGOUT_UBX_MON_HW_I2C = 0x209101b4; //Output rate of the UBX-MON-HW message on port I2C -const uint32_t CFG_MSGOUT_UBX_MON_HW_SPI = 0x209101b8; //Output rate of the UBX-MON-HW message on port SPI -const uint32_t CFG_MSGOUT_UBX_MON_HW_UART1 = 0x209101b5; //Output rate of the UBX-MON-HW message on port UART1 -const uint32_t CFG_MSGOUT_UBX_MON_HW_UART2 = 0x209101b6; //Output rate of the UBX-MON-HW message on port UART2 -const uint32_t CFG_MSGOUT_UBX_MON_HW_USB = 0x209101b7; //Output rate of the UBX-MON-HW message on port USB -const uint32_t CFG_MSGOUT_UBX_MON_IO_I2C = 0x209101a5; //Output rate of the UBX-MON-IO message on port I2C -const uint32_t CFG_MSGOUT_UBX_MON_IO_SPI = 0x209101a9; //Output rate of the UBX-MON-IO message on port SPI -const uint32_t CFG_MSGOUT_UBX_MON_IO_UART1 = 0x209101a6; //Output rate of the UBX-MON-IO message on port UART1 -const uint32_t CFG_MSGOUT_UBX_MON_IO_UART2 = 0x209101a7; //Output rate of the UBX-MON-IO message on port UART2 -const uint32_t CFG_MSGOUT_UBX_MON_IO_USB = 0x209101a8; //Output rate of the UBX-MON-IO message on port USB -const uint32_t CFG_MSGOUT_UBX_MON_MSGPP_I2C = 0x20910196; //Output rate of the UBX-MON-MSGPP message on port I2C -const uint32_t CFG_MSGOUT_UBX_MON_MSGPP_SPI = 0x2091019a; //Output rate of the UBX-MON-MSGPP message on port SPI -const uint32_t CFG_MSGOUT_UBX_MON_MSGPP_UART1 = 0x20910197; //Output rate of the UBX-MON-MSGPP message on port UART1 -const uint32_t CFG_MSGOUT_UBX_MON_MSGPP_UART2 = 0x20910198; //Output rate of the UBX-MON-MSGPP message on port UART2 -const uint32_t CFG_MSGOUT_UBX_MON_MSGPP_USB = 0x20910199; //Output rate of the UBX-MON-MSGPP message on port USB -const uint32_t CFG_MSGOUT_UBX_MON_RF_I2C = 0x20910359; //Output rate of the UBX-MON-RF message on port I2C -const uint32_t CFG_MSGOUT_UBX_MON_RF_SPI = 0x2091035d; //Output rate of the UBX-MON-RF message on port SPI -const uint32_t CFG_MSGOUT_UBX_MON_RF_UART1 = 0x2091035a; //Output rate of the UBX-MON-RF message on port UART1 -const uint32_t CFG_MSGOUT_UBX_MON_RF_UART2 = 0x2091035b; //Output rate of the UBX-MON-RF message on port UART2 -const uint32_t CFG_MSGOUT_UBX_MON_RF_USB = 0x2091035c; // Output rate of the UBX-MON-RF message on port USB -const uint32_t CFG_MSGOUT_UBX_MON_RXBUF_I2C = 0x209101a0; // Output rate of the UBX-MON-RXBUF message on port I2C -const uint32_t CFG_MSGOUT_UBX_MON_RXBUF_SPI = 0x209101a4; // Output rate of the UBX-MON-RXBUF message on port SPI -const uint32_t CFG_MSGOUT_UBX_MON_RXBUF_UART1 = 0x209101a1; // Output rate of the UBX-MON-RXBUF message on port UART1 -const uint32_t CFG_MSGOUT_UBX_MON_RXBUF_UART2 = 0x209101a2; // Output rate of the UBX-MON-RXBUF message on port UART2 -const uint32_t CFG_MSGOUT_UBX_MON_RXBUF_USB = 0x209101a3; // Output rate of the UBX-MON-RXBUF message on port USB -const uint32_t CFG_MSGOUT_UBX_MON_RXR_I2C = 0x20910187; // Output rate of the UBX-MON-RXR message on port I2C -const uint32_t CFG_MSGOUT_UBX_MON_RXR_SPI = 0x2091018b; // Output rate of the UBX-MON-RXR message on port SPI -const uint32_t CFG_MSGOUT_UBX_MON_RXR_UART1 = 0x20910188; // Output rate of the UBX-MON-RXR message on port UART1 -const uint32_t CFG_MSGOUT_UBX_MON_RXR_UART2 = 0x20910189; // Output rate of the UBX-MON-RXR message on port UART2 -const uint32_t CFG_MSGOUT_UBX_MON_RXR_USB = 0x2091018a; // Output rate of the UBX-MON-RXR message on port USB -const uint32_t CFG_MSGOUT_UBX_MON_SPAN_I2C = 0x2091038b; // Output rate of the UBX-MON-SPAN message on port I2C -const uint32_t CFG_MSGOUT_UBX_MON_SPAN_SPI = 0x2091038f; // Output rate of the UBX-MON-SPAN message on port SPI -const uint32_t CFG_MSGOUT_UBX_MON_SPAN_UART1 = 0x2091038c; // Output rate of the UBX-MON-SPAN message on port UART1 -const uint32_t CFG_MSGOUT_UBX_MON_SPAN_UART2 = 0x2091038d; // Output rate of the UBX-MON-SPAN message on port UART2 -const uint32_t CFG_MSGOUT_UBX_MON_SPAN_USB = 0x2091038e; // Output rate of the UBX-MON-SPAN message on port USB -const uint32_t CFG_MSGOUT_UBX_MON_TXBUF_I2C = 0x2091019b; // Output rate of the UBX-MON-TXBUF message on port I2C -const uint32_t CFG_MSGOUT_UBX_MON_TXBUF_SPI = 0x2091019f; // Output rate of the UBX-MON-TXBUF message on port SPI -const uint32_t CFG_MSGOUT_UBX_MON_TXBUF_UART1 = 0x2091019c; // Output rate of the UBX-MON-TXBUF message on port UART1 -const uint32_t CFG_MSGOUT_UBX_MON_TXBUF_UART2 = 0x2091019d; // Output rate of the UBX-MON-TXBUF message on port UART2 -const uint32_t CFG_MSGOUT_UBX_MON_TXBUF_USB = 0x2091019e; // Output rate of the UBX-MON-TXBUF message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_CLOCK_I2C = 0x20910065; // Output rate of the UBX-NAV-CLOCK message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_CLOCK_SPI = 0x20910069; // Output rate of the UBX-NAV-CLOCK message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_CLOCK_UART1 = 0x20910066; // Output rate of the UBX-NAV-CLOCK message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_CLOCK_UART2 = 0x20910067; // Output rate of the UBX-NAV-CLOCK message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_CLOCK_USB = 0x20910068; // Output rate of the UBX-NAV-CLOCK message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_DOP_I2C = 0x20910038; // Output rate of the UBX-NAV-DOP message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_DOP_SPI = 0x2091003c; // Output rate of the UBX-NAV-DOP message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_DOP_UART1 = 0x20910039; // Output rate of the UBX-NAV-DOP message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_DOP_UART2 = 0x2091003a; // Output rate of the UBX-NAV-DOP message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_DOP_USB = 0x2091003b; // Output rate of the UBX-NAV-DOP message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_EOE_I2C = 0x2091015f; // Output rate of the UBX-NAV-EOE message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_EOE_SPI = 0x20910163; // Output rate of the UBX-NAV-EOE message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_EOE_UART1 = 0x20910160; // Output rate of the UBX-NAV-EOE message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_EOE_UART2 = 0x20910161; // Output rate of the UBX-NAV-EOE message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_EOE_USB = 0x20910162; // Output rate of the UBX-NAV-EOE message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_GEOFENCE_I2C = 0x209100a1; // Output rate of the UBX-NAV-GEOFENCE message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_GEOFENCE_SPI = 0x209100a5; // Output rate of the UBX-NAV-GEOFENCE message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_GEOFENCE_UART1 = 0x209100a2;// Output rate of the UBX-NAV-GEOFENCE message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_GEOFENCE_UART2 = 0x209100a3;// Output rate of the UBX-NAV-GEOFENCE message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_GEOFENCE_USB = 0x209100a4; // Output rate of the UBX-NAV-GEOFENCE message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_HPPOSECEF_I2C = 0x2091002e;// Output rate of the UBX-NAV-HPPOSECEF message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_HPPOSECEF_SPI = 0x20910032;// Output rate of the UBX-NAV-HPPOSECEF message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_HPPOSECEF_UART1 = 0x2091002f;// Output rate of the UBX-NAV-HPPOSECEF message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_HPPOSECEF_UART2 = 0x20910030;// Output rate of the UBX-NAV-HPPOSECEF message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_HPPOSECEF_USB = 0x20910031;// Output rate of the UBX-NAV-HPPOSECEF message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_HPPOSLLH_I2C = 0x20910033; // Output rate of the UBX-NAV-HPPOSLLH message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_HPPOSLLH_SPI = 0x20910037; // Output rate of the UBX-NAV-HPPOSLLH message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_HPPOSLLH_UART1 = 0x20910034;// Output rate of the UBX-NAV-HPPOSLLH message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_HPPOSLLH_UART2 = 0x20910035;// Output rate of the UBX-NAV-HPPOSLLH message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_HPPOSLLH_USB = 0x20910036; // Output rate of the UBX-NAV-HPPOSLLH message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_ODO_I2C = 0x2091007e; // Output rate of the UBX-NAV-ODO message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_ODO_SPI = 0x20910082; // Output rate of the UBX-NAV-ODO message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_ODO_UART1 = 0x2091007f; // Output rate of the UBX-NAV-ODO message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_ODO_UART2 = 0x20910080; // Output rate of the UBX-NAV-ODO message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_ODO_USB = 0x20910081; // Output rate of the UBX-NAV-ODO message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_ORB_I2C = 0x20910010; // Output rate of the UBX-NAV-ORB message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_ORB_SPI = 0x20910014; // Output rate of the UBX-NAV-ORB message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_ORB_UART1 = 0x20910011; // Output rate of the UBX-NAV-ORB message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_ORB_UART2 = 0x20910012; // Output rate of the UBX-NAV-ORB message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_ORB_USB = 0x20910013; // Output rate of the UBX-NAV-ORB message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_POSECEF_I2C = 0x20910024; // Output rate of the UBX-NAV-POSECEF message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_POSECEF_SPI = 0x20910028; // Output rate of the UBX-NAV-POSECEF message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_POSECEF_UART1 = 0x20910025;// Output rate of the UBX-NAV-POSECEF message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_POSECEF_UART2 = 0x20910026;// Output rate of the UBX-NAV-POSECEF message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_POSECEF_USB = 0x20910027; // Output rate of the UBX-NAV-POSECEF message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_POSLLH_I2C = 0x20910029; // Output rate of the UBX-NAV-POSLLH message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_POSLLH_SPI = 0x2091002d; // Output rate of the UBX-NAV-POSLLH message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_POSLLH_UART1 = 0x2091002a; // Output rate of the UBX-NAV-POSLLH message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_POSLLH_UART2 = 0x2091002b; // Output rate of the UBX-NAV-POSLLH message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_POSLLH_USB = 0x2091002c; // Output rate of the UBX-NAV-POSLLH message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_PVT_I2C = 0x20910006; // Output rate of the UBX-NAV-PVT message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_PVT_SPI = 0x2091000a; // Output rate of the UBX-NAV-PVT message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_PVT_UART1 = 0x20910007; // Output rate of the UBX-NAV-PVT message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_PVT_UART2 = 0x20910008; // Output rate of the UBX-NAV-PVT message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_PVT_USB = 0x20910009; // Output rate of the UBX-NAV-PVT message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_RELPOSNED_I2C = 0x2091008d; // Output rate of the UBX-NAV-RELPOSNED message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_RELPOSNED_SPI = 0x20910091;// Output rate of the UBX-NAV-RELPOSNED message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_RELPOSNED_UART1 = 0x2091008e;// Output rate of the UBX-NAV-RELPOSNED message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_RELPOSNED_UART2 = 0x2091008f;// Output rate of the UBX-NAV-RELPOSNED message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_RELPOSNED_USB = 0x20910090;// Output rate of the UBX-NAV-RELPOSNED message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_SAT_I2C = 0x20910015; // Output rate of the UBX-NAV-SAT message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_SAT_SPI = 0x20910019; // Output rate of the UBX-NAV-SAT message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_SAT_UART1 = 0x20910016; // Output rate of the UBX-NAV-SAT message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_SAT_UART2 = 0x20910017; // Output rate of the UBX-NAV-SAT message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_SAT_USB = 0x20910018; // Output rate of the UBX-NAV-SAT message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_SBAS_I2C = 0x2091006a; // Output rate of the UBX-NAV-SBAS message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_SBAS_SPI = 0x2091006e; // Output rate of the UBX-NAV-SBAS message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_SBAS_UART1 = 0x2091006b; // Output rate of the UBX-NAV-SBAS message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_SBAS_UART2 = 0x2091006c; // Output rate of the UBX-NAV-SBAS message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_SBAS_USB = 0x2091006d; // Output rate of the UBX-NAV-SBAS message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_SIG_I2C = 0x20910345; // Output rate of the UBX-NAV-SIG message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_SIG_SPI = 0x20910349; // Output rate of the UBX-NAV-SIG message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_SIG_UART1 = 0x20910346; // Output rate of the UBX-NAV-SIG message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_SIG_UART2 = 0x20910347; // Output rate of the UBX-NAV-SIG message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_SIG_USB = 0x20910348; // Output rate of the UBX-NAV-SIG message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_SLAS_I2C = 0x20910336; // Output rate of the UBX-NAV-SLAS message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_SLAS_SPI = 0x2091033a; // Output rate of the UBX-NAV-SLAS message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_SLAS_UART1 = 0x20910337; // Output rate of the UBX-NAV-SLAS message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_SLAS_UART2 = 0x20910338; // Output rate of the UBX-NAV-SLAS message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_SLAS_USB = 0x20910339; // Output rate of the UBX-NAV-SLAS message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_STATUS_I2C = 0x2091001a; // Output rate of the UBX-NAV-STATUS message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_STATUS_SPI = 0x2091001e; // Output rate of the UBX-NAV-STATUS message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_STATUS_UART1 = 0x2091001b; // Output rate of the UBX-NAV-STATUS message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_STATUS_UART2 = 0x2091001c; // Output rate of the UBX-NAV-STATUS message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_STATUS_USB = 0x2091001d; // Output rate of the UBX-NAV-STATUS message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_SVIN_I2C = 0x20910088; // Output rate of the UBX-NAV-SVIN message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_SVIN_SPI = 0x2091008c; // Output rate of the UBX-NAV-SVIN message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_SVIN_UART1 = 0x20910089; // Output rate of the UBX-NAV-SVIN message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_SVIN_UART2 = 0x2091008a; // Output rate of the UBX-NAV-SVIN message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_SVIN_USB = 0x2091008b; // Output rate of the UBX-NAV-SVIN message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEBDS_I2C = 0x20910051; // Output rate of the UBX-NAV-TIMEBDS message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEBDS_SPI = 0x20910055; // Output rate of the UBX-NAV-TIMEBDS message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEBDS_UART1 = 0x20910052;// Output rate of the UBX-NAV-TIMEBDS message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEBDS_UART2 = 0x20910053;// Output rate of the UBX-NAV-TIMEBDS message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEBDS_USB = 0x20910054; // Output rate of the UBX-NAV-TIMEBDS message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGAL_I2C = 0x20910056; // Output rate of the UBX-NAV-TIMEGAL message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGAL_SPI = 0x2091005a; // Output rate of the UBX-NAV-TIMEGAL message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGAL_UART1 = 0x20910057;// Output rate of the UBX-NAV-TIMEGAL message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGAL_UART2 = 0x20910058;// Output rate of the UBX-NAV-TIMEGAL message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGAL_USB = 0x20910059; // Output rate of the UBX-NAV-TIMEGAL message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGLO_I2C = 0x2091004c; // Output rate of the UBX-NAV-TIMEGLO message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGLO_SPI = 0x20910050; // Output rate of the UBX-NAV-TIMEGLO message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGLO_UART1 = 0x2091004d;// Output rate of the UBX-NAV-TIMEGLO message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGLO_UART2 = 0x2091004e;// Output rate of the UBX-NAV-TIMEGLO message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGLO_USB = 0x2091004f; // Output rate of the UBX-NAV-TIMEGLO message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGPS_I2C = 0x20910047; // Output rate of the UBX-NAV-TIMEGPS message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGPS_SPI = 0x2091004b; // Output rate of the UBX-NAV-TIMEGPS message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGPS_UART1 = 0x20910048;// Output rate of the UBX-NAV-TIMEGPS message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGPS_UART2 = 0x20910049;// Output rate of the UBX-NAV-TIMEGPS message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEGPS_USB = 0x2091004a; // Output rate of the UBX-NAV-TIMEGPS message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_TIMELS_I2C = 0x20910060; // Output rate of the UBX-NAV-TIMELS message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_TIMELS_SPI = 0x20910064; // Output rate of the UBX-NAV-TIMELS message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_TIMELS_UART1 = 0x20910061; // Output rate of the UBX-NAV-TIMELS message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_TIMELS_UART2 = 0x20910062; // Output rate of the UBX-NAV-TIMELS message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_TIMELS_USB = 0x20910063; // Output rate of the UBX-NAV-TIMELS message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEQZSS_I2C = 0x20910386; // Output rate of the UBX-NAV-TIMEQZSSmessage on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEQZSS_SPI = 0x2091038a; // Output rate of the UBX-NAV-TIMEQZSSmessage on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEQZSS_UART1 = 0x20910387;// Output rate of the UBX-NAV-TIMEQZSSmessage on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEQZSS_UART2 = 0x20910388;// Output rate of the UBX-NAV-TIMEQZSSmessage on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEQZSS_USB = 0x20910389; // Output rate of the UBX-NAV-TIMEQZSSmessage on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEUTC_I2C = 0x2091005b; // Output rate of the UBX-NAV-TIMEUTC message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEUTC_SPI = 0x2091005f; // Output rate of the UBX-NAV-TIMEUTC message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEUTC_UART1 = 0x2091005c;// Output rate of the UBX-NAV-TIMEUTC message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEUTC_UART2 = 0x2091005d;// Output rate of the UBX-NAV-TIMEUTC message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_TIMEUTC_USB = 0x2091005e; // Output rate of the UBX-NAV-TIMEUTC message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_VELECEF_I2C = 0x2091003d; // Output rate of the UBX-NAV-VELECEF message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_VELECEF_SPI = 0x20910041; // Output rate of the UBX-NAV-VELECEF message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_VELECEF_UART1 = 0x2091003e;// Output rate of the UBX-NAV-VELECEF message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_VELECEF_UART2 = 0x2091003f;// Output rate of the UBX-NAV-VELECEF message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_VELECEF_USB = 0x20910040; // Output rate of the UBX-NAV-VELECEF message on port USB -const uint32_t CFG_MSGOUT_UBX_NAV_VELNED_I2C = 0x20910042; // Output rate of the UBX-NAV-VELNED message on port I2C -const uint32_t CFG_MSGOUT_UBX_NAV_VELNED_SPI = 0x20910046; // Output rate of the UBX-NAV-VELNED message on port SPI -const uint32_t CFG_MSGOUT_UBX_NAV_VELNED_UART1 = 0x20910043; // Output rate of the UBX-NAV-VELNED message on port UART1 -const uint32_t CFG_MSGOUT_UBX_NAV_VELNED_UART2 = 0x20910044; // Output rate of the UBX-NAV-VELNED message on port UART2 -const uint32_t CFG_MSGOUT_UBX_NAV_VELNED_USB = 0x20910045; // Output rate of the UBX-NAV-VELNED message on port USB -const uint32_t CFG_MSGOUT_UBX_RXM_MEASX_I2C = 0x20910204; // Output rate of the UBX-RXM-MEASX message on port I2C -const uint32_t CFG_MSGOUT_UBX_RXM_MEASX_SPI = 0x20910208; // Output rate of the UBX-RXM-MEASX message on port SPI -const uint32_t CFG_MSGOUT_UBX_RXM_MEASX_UART1 = 0x20910205; // Output rate of the UBX-RXM-MEASX message on port UART1 -const uint32_t CFG_MSGOUT_UBX_RXM_MEASX_UART2 = 0x20910206; // Output rate of the UBX-RXM-MEASX message on port UART2 -const uint32_t CFG_MSGOUT_UBX_RXM_MEASX_USB = 0x20910207; // Output rate of the UBX-RXM-MEASX message on port USB -const uint32_t CFG_MSGOUT_UBX_RXM_RAWX_I2C = 0x209102a4; // Output rate of the UBX-RXM-RAWX message on port I2C -const uint32_t CFG_MSGOUT_UBX_RXM_RAWX_SPI = 0x209102a8; // Output rate of the UBX-RXM-RAWX message on port SPI -const uint32_t CFG_MSGOUT_UBX_RXM_RAWX_UART1 = 0x209102a5; // Output rate of the UBX-RXM-RAWX message on port UART1 -const uint32_t CFG_MSGOUT_UBX_RXM_RAWX_UART2 = 0x209102a6; // Output rate of the UBX-RXM-RAWX message on port UART2 -const uint32_t CFG_MSGOUT_UBX_RXM_RAWX_USB = 0x209102a7; // Output rate of the UBX-RXM-RAWX message on port USB -const uint32_t CFG_MSGOUT_UBX_RXM_RLM_I2C = 0x2091025e; // Output rate of the UBX-RXM-RLM message on port I2C -const uint32_t CFG_MSGOUT_UBX_RXM_RLM_SPI = 0x20910262; // Output rate of the UBX-RXM-RLM message on port SPI -const uint32_t CFG_MSGOUT_UBX_RXM_RLM_UART1 = 0x2091025f; // Output rate of the UBX-RXM-RLM message on port UART1 -const uint32_t CFG_MSGOUT_UBX_RXM_RLM_UART2 = 0x20910260; // Output rate of the UBX-RXM-RLM message on port UART2 -const uint32_t CFG_MSGOUT_UBX_RXM_RLM_USB = 0x20910261; // Output rate of the UBX-RXM-RLM message on port USB -const uint32_t CFG_MSGOUT_UBX_RXM_RTCM_I2C = 0x20910268; // Output rate of the UBX-RXM-RTCM message on port I2C -const uint32_t CFG_MSGOUT_UBX_RXM_RTCM_SPI = 0x2091026c; // Output rate of the UBX-RXM-RTCM message on port SPI -const uint32_t CFG_MSGOUT_UBX_RXM_RTCM_UART1 = 0x20910269; // Output rate of the UBX-RXM-RTCM message on port UART1 -const uint32_t CFG_MSGOUT_UBX_RXM_RTCM_UART2 = 0x2091026a; // Output rate of the UBX-RXM-RTCM message on port UART2 -const uint32_t CFG_MSGOUT_UBX_RXM_RTCM_USB = 0x2091026b; // Output rate of the UBX-RXM-RTCM message on port USB -const uint32_t CFG_MSGOUT_UBX_RXM_SFRBX_I2C = 0x20910231; // Output rate of the UBX-RXM-SFRBX message on port I2C -const uint32_t CFG_MSGOUT_UBX_RXM_SFRBX_SPI = 0x20910235; // Output rate of the UBX-RXM-SFRBX message on port SPI -const uint32_t CFG_MSGOUT_UBX_RXM_SFRBX_UART1 = 0x20910232; // Output rate of the UBX-RXM-SFRBX message on port UART1 -const uint32_t CFG_MSGOUT_UBX_RXM_SFRBX_UART2 = 0x20910233; // Output rate of the UBX-RXM-SFRBX message on port UART2 -const uint32_t CFG_MSGOUT_UBX_RXM_SFRBX_USB = 0x20910234; // Output rate of the UBX-RXM-SFRBX message on port USB -const uint32_t CFG_MSGOUT_UBX_TIM_TM2_I2C = 0x20910178; // Output rate of the UBX-TIM-TM2 message on port I2C -const uint32_t CFG_MSGOUT_UBX_TIM_TM2_SPI = 0x2091017c; // Output rate of the UBX-TIM-TM2 message on port SPI -const uint32_t CFG_MSGOUT_UBX_TIM_TM2_UART1 = 0x20910179; // Output rate of the UBX-TIM-TM2 message on port UART1 -const uint32_t CFG_MSGOUT_UBX_TIM_TM2_UART2 = 0x2091017a; // Output rate of the UBX-TIM-TM2 message on port UART2 -const uint32_t CFG_MSGOUT_UBX_TIM_TM2_USB = 0x2091017b; // Output rate of the UBX-TIM-TM2 message on port USB -const uint32_t CFG_MSGOUT_UBX_TIM_TP_I2C = 0x2091017d; // Output rate of the UBX-TIM-TP message on port I2C -const uint32_t CFG_MSGOUT_UBX_TIM_TP_SPI = 0x20910181; // Output rate of the UBX-TIM-TP message on port SPI -const uint32_t CFG_MSGOUT_UBX_TIM_TP_UART1 = 0x2091017e; // Output rate of the UBX-TIM-TP message on port UART1 -const uint32_t CFG_MSGOUT_UBX_TIM_TP_UART2 = 0x2091017f; // Output rate of the UBX-TIM-TP message on port UART2 -const uint32_t CFG_MSGOUT_UBX_TIM_TP_USB = 0x20910180; // Output rate of the UBX-TIM-TP message on port USB -const uint32_t CFG_MSGOUT_UBX_TIM_VRFY_I2C = 0x20910092; // Output rate of the UBX-TIM-VRFY message on port I2C -const uint32_t CFG_MSGOUT_UBX_TIM_VRFY_SPI = 0x20910096; // Output rate of the UBX-TIM-VRFY message on port SPI -const uint32_t CFG_MSGOUT_UBX_TIM_VRFY_UART1 = 0x20910093; // Output rate of the UBX-TIM-VRFY message on port UART1 -const uint32_t CFG_MSGOUT_UBX_TIM_VRFY_UART2 = 0x20910094; // Output rate of the UBX-TIM-VRFY message on port UART2 -const uint32_t CFG_MSGOUT_UBX_TIM_VRFY_USB = 0x20910095; // Output rate of the UBX-TIM-VRFY message on port USB +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_DTM_I2C = 0x209100a6; //Output rate of the NMEA-GX-DTM message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_DTM_SPI = 0x209100aa; //Output rate of the NMEA-GX-DTM message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_DTM_UART1 = 0x209100a7; //Output rate of the NMEA-GX-DTM message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_DTM_UART2 = 0x209100a8; //Output rate of the NMEA-GX-DTM message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_DTM_USB = 0x209100a9; //Output rate of the NMEA-GX-DTM message on port USB +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GBS_I2C = 0x209100dd; //Output rate of the NMEA-GX-GBS message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GBS_SPI = 0x209100e1; //Output rate of the NMEA-GX-GBS message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GBS_UART1 = 0x209100de; //Output rate of the NMEA-GX-GBS message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GBS_UART2 = 0x209100df; //Output rate of the NMEA-GX-GBS message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GBS_USB = 0x209100e0; //Output rate of the NMEA-GX-GBS message on port USB +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GGA_I2C = 0x209100ba; //Output rate of the NMEA-GX-GGA message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GGA_SPI = 0x209100be; //Output rate of the NMEA-GX-GGA message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GGA_UART1 = 0x209100bb; //Output rate of the NMEA-GX-GGA message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GGA_UART2 = 0x209100bc; //Output rate of the NMEA-GX-GGA message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GGA_USB = 0x209100bd; //Output rate of the NMEA-GX-GGA message on port USB +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GLL_I2C = 0x209100c9; //Output rate of the NMEA-GX-GLL message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GLL_SPI = 0x209100cd; //Output rate of the NMEA-GX-GLL message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GLL_UART1 = 0x209100ca; //Output rate of the NMEA-GX-GLL message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GLL_UART2 = 0x209100cb; //Output rate of the NMEA-GX-GLL message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GLL_USB = 0x209100cc; //Output rate of the NMEA-GX-GLL message on port USB +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GNS_I2C = 0x209100b5; //Output rate of the NMEA-GX-GNS message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GNS_SPI = 0x209100b9; //Output rate of the NMEA-GX-GNS message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GNS_UART1 = 0x209100b6; //Output rate of the NMEA-GX-GNS message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GNS_UART2 = 0x209100b7; //Output rate of the NMEA-GX-GNS message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GNS_USB = 0x209100b8; //Output rate of the NMEA-GX-GNS message on port USB +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GRS_I2C = 0x209100ce; //Output rate of the NMEA-GX-GRS message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GRS_SPI = 0x209100d2; //Output rate of the NMEA-GX-GRS message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GRS_UART1 = 0x209100cf; //Output rate of the NMEA-GX-GRS message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GRS_UART2 = 0x209100d0; //Output rate of the NMEA-GX-GRS message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GRS_USB = 0x209100d1; //Output rate of the NMEA-GX-GRS message on port USB +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GSA_I2C = 0x209100bf; //Output rate of the NMEA-GX-GSA message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GSA_SPI = 0x209100c3; //Output rate of the NMEA-GX-GSA message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GSA_UART1 = 0x209100c0; //Output rate of the NMEA-GX-GSA message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GSA_UART2 = 0x209100c1; //Output rate of the NMEA-GX-GSA message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GSA_USB = 0x209100c2; //Output rate of the NMEA-GX-GSA message on port USB +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GST_I2C = 0x209100d3; //Output rate of the NMEA-GX-GST message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GST_SPI = 0x209100d7; //Output rate of the NMEA-GX-GST message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GST_UART1 = 0x209100d4; //Output rate of the NMEA-GX-GST message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GST_UART2 = 0x209100d5; //Output rate of the NMEA-GX-GST message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GST_USB = 0x209100d6; //Output rate of the NMEA-GX-GST message on port USB +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GSV_I2C = 0x209100c4; //Output rate of the NMEA-GX-GSV message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GSV_SPI = 0x209100c8; //Output rate of the NMEA-GX-GSV message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GSV_UART1 = 0x209100c5; //Output rate of the NMEA-GX-GSV message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GSV_UART2 = 0x209100c6; //Output rate of the NMEA-GX-GSV message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_GSV_USB = 0x209100c7; //Output rate of the NMEA-GX-GSV message on port USB +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_RLM_I2C = 0x20910400; //Output rate of the NMEA-GX-RLM message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_RLM_SPI = 0x20910404; //Output rate of the NMEA-GX-RLM message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_RLM_UART1 = 0x20910401; //Output rate of the NMEA-GX-RLM message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_RLM_UART2 = 0x20910402; //Output rate of the NMEA-GX-RLM message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_RLM_USB = 0x20910403; //Output rate of the NMEA-GX-RLM message on port USB +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_RMC_I2C = 0x209100ab; //Output rate of the NMEA-GX-RMC message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_RMC_SPI = 0x209100af; //Output rate of the NMEA-GX-RMC message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_RMC_UART1 = 0x209100ac; //Output rate of the NMEA-GX-RMC message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_RMC_UART2 = 0x209100ad; //Output rate of the NMEA-GX-RMC message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_RMC_USB = 0x209100ae; //Output rate of the NMEA-GX-RMC message on port USB +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_VLW_I2C = 0x209100e7; //Output rate of the NMEA-GX-VLW message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_VLW_SPI = 0x209100eb; //Output rate of the NMEA-GX-VLW message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_VLW_UART1 = 0x209100e8; //Output rate of the NMEA-GX-VLW message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_VLW_UART2 = 0x209100e9; //Output rate of the NMEA-GX-VLW message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_VLW_USB = 0x209100ea; //Output rate of the NMEA-GX-VLW message on port USB +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_VTG_I2C = 0x209100b0; //Output rate of the NMEA-GX-VTG message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_VTG_SPI = 0x209100b4; //Output rate of the NMEA-GX-VTG message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_VTG_UART1 = 0x209100b1; //Output rate of the NMEA-GX-VTG message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_VTG_UART2 = 0x209100b2; //Output rate of the NMEA-GX-VTG message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_VTG_USB = 0x209100b3; //Output rate of the NMEA-GX-VTG message on port USB +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_ZDA_I2C = 0x209100d8; //Output rate of the NMEA-GX-ZDA message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_ZDA_SPI = 0x209100dc; //Output rate of the NMEA-GX-ZDA message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_ZDA_UART1 = 0x209100d9; //Output rate of the NMEA-GX-ZDA message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_ZDA_UART2 = 0x209100da; //Output rate of the NMEA-GX-ZDA message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_NMEA_ID_ZDA_USB = 0x209100db; //Output rate of the NMEA-GX-ZDA message on port USB +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYP_I2C = 0x209100ec; //Output rate of the NMEA-GX-PUBX00 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYP_SPI = 0x209100f0; //Output rate of the NMEA-GX-PUBX00 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYP_UART1 = 0x209100ed; //Output rate of the NMEA-GX-PUBX00 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYP_UART2 = 0x209100ee; //Output rate of the NMEA-GX-PUBX00 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYP_USB = 0x209100ef; //Output rate of the NMEA-GX-PUBX00 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYS_I2C = 0x209100f1; //Output rate of the NMEA-GX-PUBX03 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYS_SPI = 0x209100f5; //Output rate of the NMEA-GX-PUBX03 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYS_UART1 = 0x209100f2; //Output rate of the NMEA-GX-PUBX03 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYS_UART2 = 0x209100f3; //Output rate of the NMEA-GX-PUBX03 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYS_USB = 0x209100f4; //Output rate of the NMEA-GX-PUBX03 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYT_I2C = 0x209100f6; //Output rate of the NMEA-GX-PUBX04 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYT_SPI = 0x209100fa; //Output rate of the NMEA-GX-PUBX04 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYT_UART1 = 0x209100f7; //Output rate of the NMEA-GX-PUBX04 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYT_UART2 = 0x209100f8; //Output rate of the NMEA-GX-PUBX04 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_PUBX_ID_POLYT_USB = 0x209100f9; //Output rate of the NMEA-GX-PUBX04 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1005_I2C = 0x209102bd; //Output rate of the RTCM-3X-TYPE1005 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1005_SPI = 0x209102c1; //Output rate of the RTCM-3X-TYPE1005 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1005_UART1 = 0x209102be;//Output rate of the RTCM-3X-TYPE1005 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1005_UART2 = 0x209102bf;//Output rate of the RTCM-3X-TYPE1005 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1005_USB = 0x209102c0; //Output rate of the RTCM-3X-TYPE1005 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1074_I2C = 0x2091035e; //Output rate of the RTCM-3X-TYPE1074 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1074_SPI = 0x20910362; //Output rate of the RTCM-3X-TYPE1074 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1074_UART1 = 0x2091035f;//Output rate of the RTCM-3X-TYPE1074 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1074_UART2 = 0x20910360;//Output rate of the RTCM-3X-TYPE1074 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1074_USB = 0x20910361; //Output rate of the RTCM-3X-TYPE1074 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1077_I2C = 0x209102cc; //Output rate of the RTCM-3X-TYPE1077 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1077_SPI = 0x209102d0; //Output rate of the RTCM-3X-TYPE1077 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1077_UART1 = 0x209102cd;//Output rate of the RTCM-3X-TYPE1077 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1077_UART2 = 0x209102ce;//Output rate of the RTCM-3X-TYPE1077 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1077_USB = 0x209102cf; //Output rate of the RTCM-3X-TYPE1077 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1084_I2C = 0x20910363; //Output rate of the RTCM-3X-TYPE1084 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1084_SPI = 0x20910367; //Output rate of the RTCM-3X-TYPE1084 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1084_UART1 = 0x20910364;//Output rate of the RTCM-3X-TYPE1084 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1084_UART2 = 0x20910365;//Output rate of the RTCM-3X-TYPE1084 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1084_USB = 0x20910366; //Output rate of the RTCM-3X-TYPE1084 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1087_I2C = 0x209102d1; //Output rate of the RTCM-3X-TYPE1087 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1087_SPI = 0x209102d5; //Output rate of the RTCM-3X-TYPE1087 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1087_UART1 = 0x209102d2;//Output rate of the RTCM-3X-TYPE1087 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1087_UART2 = 0x209102d3;//Output rate of the RTCM-3X-TYPE1087 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1087_USB = 0x209102d4; //Output rate of the RTCM-3X-TYPE1087 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1094_I2C = 0x20910368; //Output rate of the RTCM-3X-TYPE1094 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1094_SPI = 0x2091036c; //Output rate of the RTCM-3X-TYPE1094 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1094_UART1 = 0x20910369;//Output rate of the RTCM-3X-TYPE1094 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1094_UART2 = 0x2091036a;//Output rate of the RTCM-3X-TYPE1094 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1094_USB = 0x2091036b; //Output rate of the RTCM-3X-TYPE1094 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1097_I2C = 0x20910318; //Output rate of the RTCM-3X-TYPE1097 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1097_SPI = 0x2091031c; //Output rate of the RTCM-3X-TYPE1097 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1097_UART1 = 0x20910319;//Output rate of the RTCM-3X-TYPE1097 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1097_UART2 = 0x2091031a;//Output rate of the RTCM-3X-TYPE1097 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1097_USB = 0x2091031b; //Output rate of the RTCM-3X-TYPE1097 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1124_I2C = 0x2091036d; //Output rate of the RTCM-3X-TYPE1124 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1124_SPI = 0x20910371; //Output rate of the RTCM-3X-TYPE1124 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1124_UART1 = 0x2091036e;//Output rate of the RTCM-3X-TYPE1124 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1124_UART2 = 0x2091036f;//Output rate of the RTCM-3X-TYPE1124 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1124_USB = 0x20910370; //Output rate of the RTCM-3X-TYPE1124 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1127_I2C = 0x209102d6; //Output rate of the RTCM-3X-TYPE1127 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1127_SPI = 0x209102da; //Output rate of the RTCM-3X-TYPE1127 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1127_UART1 = 0x209102d7;//Output rate of the RTCM-3X-TYPE1127 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1127_UART2 = 0x209102d8;//Output rate of the RTCM-3X-TYPE1127 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1127_USB = 0x209102d9; //Output rate of the RTCM-3X-TYPE1127 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1230_I2C = 0x20910303; //Output rate of the RTCM-3X-TYPE1230 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1230_SPI = 0x20910307; //Output rate of the RTCM-3X-TYPE1230 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1230_UART1 = 0x20910304;//Output rate of the RTCM-3X-TYPE1230 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1230_UART2 = 0x20910305;//Output rate of the RTCM-3X-TYPE1230 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1230_USB = 0x20910306; //Output rate of the RTCM-3X-TYPE1230 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE4072_0_I2C = 0x209102fe;//Output rate of the RTCM-3X-TYPE4072_0 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE4072_0_SPI = 0x20910302;//Output rate of the RTCM-3X-TYPE4072_0 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE4072_0_UART1 = 0x209102ff; //Output rate of the RTCM-3X-TYPE4072_0 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE4072_0_UART2 = 0x20910300; //Output rate of the RTCM-3X-TYPE4072_0 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE4072_0_USB = 0x20910301;//Output rate of the RTCM-3X-TYPE4072_0 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE4072_1_I2C = 0x20910381;//Output rate of the RTCM-3X-TYPE4072_1 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE4072_1_SPI = 0x20910385;//Output rate of the RTCM-3X-TYPE4072_1 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE4072_1_UART1 = 0x20910382; //Output rate of the RTCM-3X-TYPE4072_1 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE4072_1_UART2 = 0x20910383; //Output rate of the RTCM-3X-TYPE4072_1 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_RTCM_3X_TYPE4072_1_USB = 0x20910384;//Output rate of the RTCM-3X-TYPE4072_1 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_LOG_INFO_I2C = 0x20910259; //Output rate of the UBX-LOG-INFO message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_LOG_INFO_SPI = 0x2091025d; //Output rate of the UBX-LOG-INFO message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_LOG_INFO_UART1 = 0x2091025a; //Output rate of the UBX-LOG-INFO message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_LOG_INFO_UART2 = 0x2091025b; //Output rate of the UBX-LOG-INFO message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_LOG_INFO_USB = 0x2091025c; //Output rate of the UBX-LOG-INFO message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_COMMS_I2C = 0x2091034f; //Output rate of the UBX-MON-COMMS message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_COMMS_SPI = 0x20910353; //Output rate of the UBX-MON-COMMS message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_COMMS_UART1 = 0x20910350; //Output rate of the UBX-MON-COMMS message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_COMMS_UART2 = 0x20910351; //Output rate of the UBX-MON-COMMS message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_COMMS_USB = 0x20910352; //Output rate of the UBX-MON-COMMS message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW2_I2C = 0x209101b9; //Output rate of the UBX-MON-HW2 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW2_SPI = 0x209101bd; //Output rate of the UBX-MON-HW2 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW2_UART1 = 0x209101ba; //Output rate of the UBX-MON-HW2 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW2_UART2 = 0x209101bb; //Output rate of the UBX-MON-HW2 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW2_USB = 0x209101bc; //Output rate of the UBX-MON-HW2 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW3_I2C = 0x20910354; //Output rate of the UBX-MON-HW3 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW3_SPI = 0x20910358; //Output rate of the UBX-MON-HW3 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW3_UART1 = 0x20910355; //Output rate of the UBX-MON-HW3 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW3_UART2 = 0x20910356; //Output rate of the UBX-MON-HW3 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW3_USB = 0x20910357; //Output rate of the UBX-MON-HW3 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW_I2C = 0x209101b4; //Output rate of the UBX-MON-HW message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW_SPI = 0x209101b8; //Output rate of the UBX-MON-HW message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW_UART1 = 0x209101b5; //Output rate of the UBX-MON-HW message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW_UART2 = 0x209101b6; //Output rate of the UBX-MON-HW message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_HW_USB = 0x209101b7; //Output rate of the UBX-MON-HW message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_IO_I2C = 0x209101a5; //Output rate of the UBX-MON-IO message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_IO_SPI = 0x209101a9; //Output rate of the UBX-MON-IO message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_IO_UART1 = 0x209101a6; //Output rate of the UBX-MON-IO message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_IO_UART2 = 0x209101a7; //Output rate of the UBX-MON-IO message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_IO_USB = 0x209101a8; //Output rate of the UBX-MON-IO message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_MSGPP_I2C = 0x20910196; //Output rate of the UBX-MON-MSGPP message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_MSGPP_SPI = 0x2091019a; //Output rate of the UBX-MON-MSGPP message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_MSGPP_UART1 = 0x20910197; //Output rate of the UBX-MON-MSGPP message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_MSGPP_UART2 = 0x20910198; //Output rate of the UBX-MON-MSGPP message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_MSGPP_USB = 0x20910199; //Output rate of the UBX-MON-MSGPP message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RF_I2C = 0x20910359; //Output rate of the UBX-MON-RF message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RF_SPI = 0x2091035d; //Output rate of the UBX-MON-RF message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RF_UART1 = 0x2091035a; //Output rate of the UBX-MON-RF message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RF_UART2 = 0x2091035b; //Output rate of the UBX-MON-RF message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RF_USB = 0x2091035c; // Output rate of the UBX-MON-RF message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RXBUF_I2C = 0x209101a0; // Output rate of the UBX-MON-RXBUF message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RXBUF_SPI = 0x209101a4; // Output rate of the UBX-MON-RXBUF message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RXBUF_UART1 = 0x209101a1; // Output rate of the UBX-MON-RXBUF message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RXBUF_UART2 = 0x209101a2; // Output rate of the UBX-MON-RXBUF message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RXBUF_USB = 0x209101a3; // Output rate of the UBX-MON-RXBUF message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RXR_I2C = 0x20910187; // Output rate of the UBX-MON-RXR message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RXR_SPI = 0x2091018b; // Output rate of the UBX-MON-RXR message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RXR_UART1 = 0x20910188; // Output rate of the UBX-MON-RXR message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RXR_UART2 = 0x20910189; // Output rate of the UBX-MON-RXR message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_RXR_USB = 0x2091018a; // Output rate of the UBX-MON-RXR message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SPAN_I2C = 0x2091038b; // Output rate of the UBX-MON-SPAN message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SPAN_SPI = 0x2091038f; // Output rate of the UBX-MON-SPAN message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SPAN_UART1 = 0x2091038c; // Output rate of the UBX-MON-SPAN message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SPAN_UART2 = 0x2091038d; // Output rate of the UBX-MON-SPAN message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SPAN_USB = 0x2091038e; // Output rate of the UBX-MON-SPAN message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_TXBUF_I2C = 0x2091019b; // Output rate of the UBX-MON-TXBUF message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_TXBUF_SPI = 0x2091019f; // Output rate of the UBX-MON-TXBUF message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_TXBUF_UART1 = 0x2091019c; // Output rate of the UBX-MON-TXBUF message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_TXBUF_UART2 = 0x2091019d; // Output rate of the UBX-MON-TXBUF message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_TXBUF_USB = 0x2091019e; // Output rate of the UBX-MON-TXBUF message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_CLOCK_I2C = 0x20910065; // Output rate of the UBX-NAV-CLOCK message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_CLOCK_SPI = 0x20910069; // Output rate of the UBX-NAV-CLOCK message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_CLOCK_UART1 = 0x20910066; // Output rate of the UBX-NAV-CLOCK message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_CLOCK_UART2 = 0x20910067; // Output rate of the UBX-NAV-CLOCK message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_CLOCK_USB = 0x20910068; // Output rate of the UBX-NAV-CLOCK message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_DOP_I2C = 0x20910038; // Output rate of the UBX-NAV-DOP message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_DOP_SPI = 0x2091003c; // Output rate of the UBX-NAV-DOP message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_DOP_UART1 = 0x20910039; // Output rate of the UBX-NAV-DOP message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_DOP_UART2 = 0x2091003a; // Output rate of the UBX-NAV-DOP message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_DOP_USB = 0x2091003b; // Output rate of the UBX-NAV-DOP message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_EOE_I2C = 0x2091015f; // Output rate of the UBX-NAV-EOE message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_EOE_SPI = 0x20910163; // Output rate of the UBX-NAV-EOE message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_EOE_UART1 = 0x20910160; // Output rate of the UBX-NAV-EOE message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_EOE_UART2 = 0x20910161; // Output rate of the UBX-NAV-EOE message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_EOE_USB = 0x20910162; // Output rate of the UBX-NAV-EOE message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_GEOFENCE_I2C = 0x209100a1; // Output rate of the UBX-NAV-GEOFENCE message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_GEOFENCE_SPI = 0x209100a5; // Output rate of the UBX-NAV-GEOFENCE message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_GEOFENCE_UART1 = 0x209100a2;// Output rate of the UBX-NAV-GEOFENCE message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_GEOFENCE_UART2 = 0x209100a3;// Output rate of the UBX-NAV-GEOFENCE message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_GEOFENCE_USB = 0x209100a4; // Output rate of the UBX-NAV-GEOFENCE message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_HPPOSECEF_I2C = 0x2091002e;// Output rate of the UBX-NAV-HPPOSECEF message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_HPPOSECEF_SPI = 0x20910032;// Output rate of the UBX-NAV-HPPOSECEF message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_HPPOSECEF_UART1 = 0x2091002f;// Output rate of the UBX-NAV-HPPOSECEF message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_HPPOSECEF_UART2 = 0x20910030;// Output rate of the UBX-NAV-HPPOSECEF message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_HPPOSECEF_USB = 0x20910031;// Output rate of the UBX-NAV-HPPOSECEF message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_HPPOSLLH_I2C = 0x20910033; // Output rate of the UBX-NAV-HPPOSLLH message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_HPPOSLLH_SPI = 0x20910037; // Output rate of the UBX-NAV-HPPOSLLH message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_HPPOSLLH_UART1 = 0x20910034;// Output rate of the UBX-NAV-HPPOSLLH message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_HPPOSLLH_UART2 = 0x20910035;// Output rate of the UBX-NAV-HPPOSLLH message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_HPPOSLLH_USB = 0x20910036; // Output rate of the UBX-NAV-HPPOSLLH message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ODO_I2C = 0x2091007e; // Output rate of the UBX-NAV-ODO message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ODO_SPI = 0x20910082; // Output rate of the UBX-NAV-ODO message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ODO_UART1 = 0x2091007f; // Output rate of the UBX-NAV-ODO message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ODO_UART2 = 0x20910080; // Output rate of the UBX-NAV-ODO message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ODO_USB = 0x20910081; // Output rate of the UBX-NAV-ODO message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ORB_I2C = 0x20910010; // Output rate of the UBX-NAV-ORB message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ORB_SPI = 0x20910014; // Output rate of the UBX-NAV-ORB message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ORB_UART1 = 0x20910011; // Output rate of the UBX-NAV-ORB message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ORB_UART2 = 0x20910012; // Output rate of the UBX-NAV-ORB message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ORB_USB = 0x20910013; // Output rate of the UBX-NAV-ORB message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSECEF_I2C = 0x20910024; // Output rate of the UBX-NAV-POSECEF message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSECEF_SPI = 0x20910028; // Output rate of the UBX-NAV-POSECEF message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSECEF_UART1 = 0x20910025;// Output rate of the UBX-NAV-POSECEF message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSECEF_UART2 = 0x20910026;// Output rate of the UBX-NAV-POSECEF message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSECEF_USB = 0x20910027; // Output rate of the UBX-NAV-POSECEF message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSLLH_I2C = 0x20910029; // Output rate of the UBX-NAV-POSLLH message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSLLH_SPI = 0x2091002d; // Output rate of the UBX-NAV-POSLLH message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSLLH_UART1 = 0x2091002a; // Output rate of the UBX-NAV-POSLLH message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSLLH_UART2 = 0x2091002b; // Output rate of the UBX-NAV-POSLLH message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSLLH_USB = 0x2091002c; // Output rate of the UBX-NAV-POSLLH message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PVT_I2C = 0x20910006; // Output rate of the UBX-NAV-PVT message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PVT_SPI = 0x2091000a; // Output rate of the UBX-NAV-PVT message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PVT_UART1 = 0x20910007; // Output rate of the UBX-NAV-PVT message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PVT_UART2 = 0x20910008; // Output rate of the UBX-NAV-PVT message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PVT_USB = 0x20910009; // Output rate of the UBX-NAV-PVT message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_RELPOSNED_I2C = 0x2091008d; // Output rate of the UBX-NAV-RELPOSNED message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_RELPOSNED_SPI = 0x20910091;// Output rate of the UBX-NAV-RELPOSNED message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_RELPOSNED_UART1 = 0x2091008e;// Output rate of the UBX-NAV-RELPOSNED message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_RELPOSNED_UART2 = 0x2091008f;// Output rate of the UBX-NAV-RELPOSNED message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_RELPOSNED_USB = 0x20910090;// Output rate of the UBX-NAV-RELPOSNED message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SAT_I2C = 0x20910015; // Output rate of the UBX-NAV-SAT message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SAT_SPI = 0x20910019; // Output rate of the UBX-NAV-SAT message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SAT_UART1 = 0x20910016; // Output rate of the UBX-NAV-SAT message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SAT_UART2 = 0x20910017; // Output rate of the UBX-NAV-SAT message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SAT_USB = 0x20910018; // Output rate of the UBX-NAV-SAT message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SBAS_I2C = 0x2091006a; // Output rate of the UBX-NAV-SBAS message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SBAS_SPI = 0x2091006e; // Output rate of the UBX-NAV-SBAS message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SBAS_UART1 = 0x2091006b; // Output rate of the UBX-NAV-SBAS message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SBAS_UART2 = 0x2091006c; // Output rate of the UBX-NAV-SBAS message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SBAS_USB = 0x2091006d; // Output rate of the UBX-NAV-SBAS message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SIG_I2C = 0x20910345; // Output rate of the UBX-NAV-SIG message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SIG_SPI = 0x20910349; // Output rate of the UBX-NAV-SIG message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SIG_UART1 = 0x20910346; // Output rate of the UBX-NAV-SIG message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SIG_UART2 = 0x20910347; // Output rate of the UBX-NAV-SIG message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SIG_USB = 0x20910348; // Output rate of the UBX-NAV-SIG message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SLAS_I2C = 0x20910336; // Output rate of the UBX-NAV-SLAS message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SLAS_SPI = 0x2091033a; // Output rate of the UBX-NAV-SLAS message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SLAS_UART1 = 0x20910337; // Output rate of the UBX-NAV-SLAS message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SLAS_UART2 = 0x20910338; // Output rate of the UBX-NAV-SLAS message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SLAS_USB = 0x20910339; // Output rate of the UBX-NAV-SLAS message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_STATUS_I2C = 0x2091001a; // Output rate of the UBX-NAV-STATUS message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_STATUS_SPI = 0x2091001e; // Output rate of the UBX-NAV-STATUS message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_STATUS_UART1 = 0x2091001b; // Output rate of the UBX-NAV-STATUS message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_STATUS_UART2 = 0x2091001c; // Output rate of the UBX-NAV-STATUS message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_STATUS_USB = 0x2091001d; // Output rate of the UBX-NAV-STATUS message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SVIN_I2C = 0x20910088; // Output rate of the UBX-NAV-SVIN message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SVIN_SPI = 0x2091008c; // Output rate of the UBX-NAV-SVIN message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SVIN_UART1 = 0x20910089; // Output rate of the UBX-NAV-SVIN message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SVIN_UART2 = 0x2091008a; // Output rate of the UBX-NAV-SVIN message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_SVIN_USB = 0x2091008b; // Output rate of the UBX-NAV-SVIN message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEBDS_I2C = 0x20910051; // Output rate of the UBX-NAV-TIMEBDS message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEBDS_SPI = 0x20910055; // Output rate of the UBX-NAV-TIMEBDS message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEBDS_UART1 = 0x20910052;// Output rate of the UBX-NAV-TIMEBDS message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEBDS_UART2 = 0x20910053;// Output rate of the UBX-NAV-TIMEBDS message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEBDS_USB = 0x20910054; // Output rate of the UBX-NAV-TIMEBDS message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGAL_I2C = 0x20910056; // Output rate of the UBX-NAV-TIMEGAL message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGAL_SPI = 0x2091005a; // Output rate of the UBX-NAV-TIMEGAL message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGAL_UART1 = 0x20910057;// Output rate of the UBX-NAV-TIMEGAL message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGAL_UART2 = 0x20910058;// Output rate of the UBX-NAV-TIMEGAL message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGAL_USB = 0x20910059; // Output rate of the UBX-NAV-TIMEGAL message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGLO_I2C = 0x2091004c; // Output rate of the UBX-NAV-TIMEGLO message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGLO_SPI = 0x20910050; // Output rate of the UBX-NAV-TIMEGLO message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGLO_UART1 = 0x2091004d;// Output rate of the UBX-NAV-TIMEGLO message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGLO_UART2 = 0x2091004e;// Output rate of the UBX-NAV-TIMEGLO message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGLO_USB = 0x2091004f; // Output rate of the UBX-NAV-TIMEGLO message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGPS_I2C = 0x20910047; // Output rate of the UBX-NAV-TIMEGPS message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGPS_SPI = 0x2091004b; // Output rate of the UBX-NAV-TIMEGPS message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGPS_UART1 = 0x20910048;// Output rate of the UBX-NAV-TIMEGPS message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGPS_UART2 = 0x20910049;// Output rate of the UBX-NAV-TIMEGPS message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGPS_USB = 0x2091004a; // Output rate of the UBX-NAV-TIMEGPS message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMELS_I2C = 0x20910060; // Output rate of the UBX-NAV-TIMELS message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMELS_SPI = 0x20910064; // Output rate of the UBX-NAV-TIMELS message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMELS_UART1 = 0x20910061; // Output rate of the UBX-NAV-TIMELS message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMELS_UART2 = 0x20910062; // Output rate of the UBX-NAV-TIMELS message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMELS_USB = 0x20910063; // Output rate of the UBX-NAV-TIMELS message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEQZSS_I2C = 0x20910386; // Output rate of the UBX-NAV-TIMEQZSSmessage on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEQZSS_SPI = 0x2091038a; // Output rate of the UBX-NAV-TIMEQZSSmessage on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEQZSS_UART1 = 0x20910387;// Output rate of the UBX-NAV-TIMEQZSSmessage on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEQZSS_UART2 = 0x20910388;// Output rate of the UBX-NAV-TIMEQZSSmessage on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEQZSS_USB = 0x20910389; // Output rate of the UBX-NAV-TIMEQZSSmessage on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEUTC_I2C = 0x2091005b; // Output rate of the UBX-NAV-TIMEUTC message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEUTC_SPI = 0x2091005f; // Output rate of the UBX-NAV-TIMEUTC message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEUTC_UART1 = 0x2091005c;// Output rate of the UBX-NAV-TIMEUTC message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEUTC_UART2 = 0x2091005d;// Output rate of the UBX-NAV-TIMEUTC message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_TIMEUTC_USB = 0x2091005e; // Output rate of the UBX-NAV-TIMEUTC message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_VELECEF_I2C = 0x2091003d; // Output rate of the UBX-NAV-VELECEF message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_VELECEF_SPI = 0x20910041; // Output rate of the UBX-NAV-VELECEF message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_VELECEF_UART1 = 0x2091003e;// Output rate of the UBX-NAV-VELECEF message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_VELECEF_UART2 = 0x2091003f;// Output rate of the UBX-NAV-VELECEF message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_VELECEF_USB = 0x20910040; // Output rate of the UBX-NAV-VELECEF message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_VELNED_I2C = 0x20910042; // Output rate of the UBX-NAV-VELNED message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_VELNED_SPI = 0x20910046; // Output rate of the UBX-NAV-VELNED message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_VELNED_UART1 = 0x20910043; // Output rate of the UBX-NAV-VELNED message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_VELNED_UART2 = 0x20910044; // Output rate of the UBX-NAV-VELNED message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_VELNED_USB = 0x20910045; // Output rate of the UBX-NAV-VELNED message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_MEASX_I2C = 0x20910204; // Output rate of the UBX-RXM-MEASX message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_MEASX_SPI = 0x20910208; // Output rate of the UBX-RXM-MEASX message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_MEASX_UART1 = 0x20910205; // Output rate of the UBX-RXM-MEASX message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_MEASX_UART2 = 0x20910206; // Output rate of the UBX-RXM-MEASX message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_MEASX_USB = 0x20910207; // Output rate of the UBX-RXM-MEASX message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RAWX_I2C = 0x209102a4; // Output rate of the UBX-RXM-RAWX message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RAWX_SPI = 0x209102a8; // Output rate of the UBX-RXM-RAWX message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RAWX_UART1 = 0x209102a5; // Output rate of the UBX-RXM-RAWX message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RAWX_UART2 = 0x209102a6; // Output rate of the UBX-RXM-RAWX message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RAWX_USB = 0x209102a7; // Output rate of the UBX-RXM-RAWX message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RLM_I2C = 0x2091025e; // Output rate of the UBX-RXM-RLM message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RLM_SPI = 0x20910262; // Output rate of the UBX-RXM-RLM message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RLM_UART1 = 0x2091025f; // Output rate of the UBX-RXM-RLM message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RLM_UART2 = 0x20910260; // Output rate of the UBX-RXM-RLM message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RLM_USB = 0x20910261; // Output rate of the UBX-RXM-RLM message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RTCM_I2C = 0x20910268; // Output rate of the UBX-RXM-RTCM message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RTCM_SPI = 0x2091026c; // Output rate of the UBX-RXM-RTCM message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RTCM_UART1 = 0x20910269; // Output rate of the UBX-RXM-RTCM message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RTCM_UART2 = 0x2091026a; // Output rate of the UBX-RXM-RTCM message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_RTCM_USB = 0x2091026b; // Output rate of the UBX-RXM-RTCM message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_SFRBX_I2C = 0x20910231; // Output rate of the UBX-RXM-SFRBX message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_SFRBX_SPI = 0x20910235; // Output rate of the UBX-RXM-SFRBX message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_SFRBX_UART1 = 0x20910232; // Output rate of the UBX-RXM-SFRBX message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_SFRBX_UART2 = 0x20910233; // Output rate of the UBX-RXM-SFRBX message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_RXM_SFRBX_USB = 0x20910234; // Output rate of the UBX-RXM-SFRBX message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_TM2_I2C = 0x20910178; // Output rate of the UBX-TIM-TM2 message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_TM2_SPI = 0x2091017c; // Output rate of the UBX-TIM-TM2 message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_TM2_UART1 = 0x20910179; // Output rate of the UBX-TIM-TM2 message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_TM2_UART2 = 0x2091017a; // Output rate of the UBX-TIM-TM2 message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_TM2_USB = 0x2091017b; // Output rate of the UBX-TIM-TM2 message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_TP_I2C = 0x2091017d; // Output rate of the UBX-TIM-TP message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_TP_SPI = 0x20910181; // Output rate of the UBX-TIM-TP message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_TP_UART1 = 0x2091017e; // Output rate of the UBX-TIM-TP message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_TP_UART2 = 0x2091017f; // Output rate of the UBX-TIM-TP message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_TP_USB = 0x20910180; // Output rate of the UBX-TIM-TP message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_VRFY_I2C = 0x20910092; // Output rate of the UBX-TIM-VRFY message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_VRFY_SPI = 0x20910096; // Output rate of the UBX-TIM-VRFY message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_VRFY_UART1 = 0x20910093; // Output rate of the UBX-TIM-VRFY message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_VRFY_UART2 = 0x20910094; // Output rate of the UBX-TIM-VRFY message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_TIM_VRFY_USB = 0x20910095; // Output rate of the UBX-TIM-VRFY message on port USB //CFG-NAVHPG: High precision navigation configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_NAVHPG_DGNSSMODE = 0x20140011; // Differential corrections mode +const uint32_t UBLOX_CFG_NAVHPG_DGNSSMODE = 0x20140011; // Differential corrections mode //CFG-NAVSPG: Standard precision navigation configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_NAVSPG_FIXMODE = 0x20110011; // Position fix mode -const uint32_t CFG_NAVSPG_INIFIX3D = 0x10110013; // Initial fix must be a 3D fix -const uint32_t CFG_NAVSPG_WKNROLLOVER = 0x30110017; // GPS week rollover number -const uint32_t CFG_NAVSPG_UTCSTANDARD = 0x2011001c; // UTC standard to be used -const uint32_t CFG_NAVSPG_DYNMODEL = 0x20110021; // Dynamic platform model -const uint32_t CFG_NAVSPG_ACKAIDING = 0x10110025; // Acknowledge assistance input messages -const uint32_t CFG_NAVSPG_USE_USRDAT = 0x10110061; // Use user geodetic datum parameters -const uint32_t CFG_NAVSPG_USRDAT_MAJA = 0x50110062; // Geodetic datum semi-major axis -const uint32_t CFG_NAVSPG_USRDAT_FLAT = 0x50110063; // Geodetic datum 1.0 flattening -const uint32_t CFG_NAVSPG_USRDAT_DX = 0x40110064; // Geodetic datum X axis shift at the origin -const uint32_t CFG_NAVSPG_USRDAT_DY = 0x40110065; // Geodetic datum Y axis shift at the origin -const uint32_t CFG_NAVSPG_USRDAT_DZ = 0x40110066; // Geodetic datum Z axis shift at the origin -const uint32_t CFG_NAVSPG_USRDAT_ROTX = 0x40110067; // arcsec Geodetic datum rotation about the X axis -const uint32_t CFG_NAVSPG_USRDAT_ROTY = 0x40110068; // arcsec Geodetic datum rotation about the Y axis -const uint32_t CFG_NAVSPG_USRDAT_ROTZ = 0x40110069; // arcsec Geodetic datum rotation about the Z axis -const uint32_t CFG_NAVSPG_USRDAT_SCALE = 0x4011006a; // ppm Geodetic datum scale factor -const uint32_t CFG_NAVSPG_INFIL_MINSVS = 0x201100a1; // Minimum number of satellites for navigation -const uint32_t CFG_NAVSPG_INFIL_MAXSVS = 0x201100a2; // Maximum number of satellites for navigation -const uint32_t CFG_NAVSPG_INFIL_MINCNO = 0x201100a3; // Minimum satellite signal level for navigation -const uint32_t CFG_NAVSPG_INFIL_MINELEV = 0x201100a4; // Minimum elevation for a GNSS satellite to be used in navigation -const uint32_t CFG_NAVSPG_INFIL_NCNOTHRS = 0x201100aa; // Number of satellites required to have C/N0 above const uint32_t CFG_NAVSPG-INFIL_CNOTHRS for a fix to be attempted -const uint32_t CFG_NAVSPG_INFIL_CNOTHRS = 0x201100ab; // C/N0 threshold for deciding whether to attempt a fix -const uint32_t CFG_NAVSPG_OUTFIL_PDOP = 0x301100b1; // Output filter position DOP mask (threshold) -const uint32_t CFG_NAVSPG_OUTFIL_TDOP = 0x301100b2; // Output filter time DOP mask (threshold) -const uint32_t CFG_NAVSPG_OUTFIL_PACC = 0x301100b3; // Output filter position accuracy mask (threshold) -const uint32_t CFG_NAVSPG_OUTFIL_TACC = 0x301100b4; // Output filter time accuracy mask (threshold) -const uint32_t CFG_NAVSPG_OUTFIL_FACC = 0x301100b5; // Output filter frequency accuracy mask (threshold) -const uint32_t CFG_NAVSPG_CONSTR_ALT = 0x401100c1; // Fixed altitude (mean sea level) for 2D fix mode -const uint32_t CFG_NAVSPG_CONSTR_ALTVAR = 0x401100c2; // Fixed altitude variance for 2D mode -const uint32_t CFG_NAVSPG_CONSTR_DGNSSTO = 0x201100c4; // DGNSS timeout +const uint32_t UBLOX_CFG_NAVSPG_FIXMODE = 0x20110011; // Position fix mode +const uint32_t UBLOX_CFG_NAVSPG_INIFIX3D = 0x10110013; // Initial fix must be a 3D fix +const uint32_t UBLOX_CFG_NAVSPG_WKNROLLOVER = 0x30110017; // GPS week rollover number +const uint32_t UBLOX_CFG_NAVSPG_UTCSTANDARD = 0x2011001c; // UTC standard to be used +const uint32_t UBLOX_CFG_NAVSPG_DYNMODEL = 0x20110021; // Dynamic platform model +const uint32_t UBLOX_CFG_NAVSPG_ACKAIDING = 0x10110025; // Acknowledge assistance input messages +const uint32_t UBLOX_CFG_NAVSPG_USE_USRDAT = 0x10110061; // Use user geodetic datum parameters +const uint32_t UBLOX_CFG_NAVSPG_USRDAT_MAJA = 0x50110062; // Geodetic datum semi-major axis +const uint32_t UBLOX_CFG_NAVSPG_USRDAT_FLAT = 0x50110063; // Geodetic datum 1.0 flattening +const uint32_t UBLOX_CFG_NAVSPG_USRDAT_DX = 0x40110064; // Geodetic datum X axis shift at the origin +const uint32_t UBLOX_CFG_NAVSPG_USRDAT_DY = 0x40110065; // Geodetic datum Y axis shift at the origin +const uint32_t UBLOX_CFG_NAVSPG_USRDAT_DZ = 0x40110066; // Geodetic datum Z axis shift at the origin +const uint32_t UBLOX_CFG_NAVSPG_USRDAT_ROTX = 0x40110067; // arcsec Geodetic datum rotation about the X axis +const uint32_t UBLOX_CFG_NAVSPG_USRDAT_ROTY = 0x40110068; // arcsec Geodetic datum rotation about the Y axis +const uint32_t UBLOX_CFG_NAVSPG_USRDAT_ROTZ = 0x40110069; // arcsec Geodetic datum rotation about the Z axis +const uint32_t UBLOX_CFG_NAVSPG_USRDAT_SCALE = 0x4011006a; // ppm Geodetic datum scale factor +const uint32_t UBLOX_CFG_NAVSPG_INFIL_MINSVS = 0x201100a1; // Minimum number of satellites for navigation +const uint32_t UBLOX_CFG_NAVSPG_INFIL_MAXSVS = 0x201100a2; // Maximum number of satellites for navigation +const uint32_t UBLOX_CFG_NAVSPG_INFIL_MINCNO = 0x201100a3; // Minimum satellite signal level for navigation +const uint32_t UBLOX_CFG_NAVSPG_INFIL_MINELEV = 0x201100a4; // Minimum elevation for a GNSS satellite to be used in navigation +const uint32_t UBLOX_CFG_NAVSPG_INFIL_NCNOTHRS = 0x201100aa; // Number of satellites required to have C/N0 above const uint32_t UBLOX_CFG_NAVSPG-INFIL_CNOTHRS for a fix to be attempted +const uint32_t UBLOX_CFG_NAVSPG_INFIL_CNOTHRS = 0x201100ab; // C/N0 threshold for deciding whether to attempt a fix +const uint32_t UBLOX_CFG_NAVSPG_OUTFIL_PDOP = 0x301100b1; // Output filter position DOP mask (threshold) +const uint32_t UBLOX_CFG_NAVSPG_OUTFIL_TDOP = 0x301100b2; // Output filter time DOP mask (threshold) +const uint32_t UBLOX_CFG_NAVSPG_OUTFIL_PACC = 0x301100b3; // Output filter position accuracy mask (threshold) +const uint32_t UBLOX_CFG_NAVSPG_OUTFIL_TACC = 0x301100b4; // Output filter time accuracy mask (threshold) +const uint32_t UBLOX_CFG_NAVSPG_OUTFIL_FACC = 0x301100b5; // Output filter frequency accuracy mask (threshold) +const uint32_t UBLOX_CFG_NAVSPG_CONSTR_ALT = 0x401100c1; // Fixed altitude (mean sea level) for 2D fix mode +const uint32_t UBLOX_CFG_NAVSPG_CONSTR_ALTVAR = 0x401100c2; // Fixed altitude variance for 2D mode +const uint32_t UBLOX_CFG_NAVSPG_CONSTR_DGNSSTO = 0x201100c4; // DGNSS timeout //CFG-NMEA: NMEA protocol configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_NMEA_PROTVER = 0x20930001; // NMEA protocol version -const uint32_t CFG_NMEA_MAXSVS = 0x20930002; // Maximum number of SVs to report per Talker ID -const uint32_t CFG_NMEA_COMPAT = 0x10930003; // Enable compatibility mode -const uint32_t CFG_NMEA_CONSIDER = 0x10930004; // Enable considering mode -const uint32_t CFG_NMEA_LIMIT82 = 0x10930005; // Enable strict limit to 82 characters maximum NMEA message length -const uint32_t CFG_NMEA_HIGHPREC = 0x10930006; // Enable high precision mode -const uint32_t CFG_NMEA_SVNUMBERING = 0x20930007; // Display configuration for SVs that do not have value defined in NMEA -const uint32_t CFG_NMEA_FILT_GPS = 0x10930011; // Disable reporting of GPS satellites -const uint32_t CFG_NMEA_FILT_SBAS = 0x10930012; // Disable reporting of SBAS satellites -const uint32_t CFG_NMEA_FILT_GAL = 0x10930013; // Disable reporting of Galileo satellites -const uint32_t CFG_NMEA_FILT_QZSS = 0x10930015; // Disable reporting of QZSS satellites -const uint32_t CFG_NMEA_FILT_GLO = 0x10930016; // Disable reporting of GLONASS satellites -const uint32_t CFG_NMEA_FILT_BDS = 0x10930017; // Disable reporting of BeiDou satellites -const uint32_t CFG_NMEA_OUT_INVFIX = 0x10930021; // Enable position output for failed or invalid fixes -const uint32_t CFG_NMEA_OUT_MSKFIX = 0x10930022; // Enable position output for invalid fixes -const uint32_t CFG_NMEA_OUT_INVTIME = 0x10930023; // Enable time output for invalid times -const uint32_t CFG_NMEA_OUT_INVDATE = 0x10930024; // Enable date output for invalid dates -const uint32_t CFG_NMEA_OUT_ONLYGPS = 0x10930025; // Restrict output to GPS satellites only -const uint32_t CFG_NMEA_OUT_FROZENCOG = 0x10930026; // Enable course over ground output even if it is frozen -const uint32_t CFG_NMEA_MAINTALKERID = 0x20930031; // Main Talker ID -const uint32_t CFG_NMEA_GSVTALKERID = 0x20930032; // Talker ID for GSV NMEA messages -const uint32_t CFG_NMEA_BDSTALKERID = 0x30930033; // BeiDou Talker ID +const uint32_t UBLOX_CFG_NMEA_PROTVER = 0x20930001; // NMEA protocol version +const uint32_t UBLOX_CFG_NMEA_MAXSVS = 0x20930002; // Maximum number of SVs to report per Talker ID +const uint32_t UBLOX_CFG_NMEA_COMPAT = 0x10930003; // Enable compatibility mode +const uint32_t UBLOX_CFG_NMEA_CONSIDER = 0x10930004; // Enable considering mode +const uint32_t UBLOX_CFG_NMEA_LIMIT82 = 0x10930005; // Enable strict limit to 82 characters maximum NMEA message length +const uint32_t UBLOX_CFG_NMEA_HIGHPREC = 0x10930006; // Enable high precision mode +const uint32_t UBLOX_CFG_NMEA_SVNUMBERING = 0x20930007; // Display configuration for SVs that do not have value defined in NMEA +const uint32_t UBLOX_CFG_NMEA_FILT_GPS = 0x10930011; // Disable reporting of GPS satellites +const uint32_t UBLOX_CFG_NMEA_FILT_SBAS = 0x10930012; // Disable reporting of SBAS satellites +const uint32_t UBLOX_CFG_NMEA_FILT_GAL = 0x10930013; // Disable reporting of Galileo satellites +const uint32_t UBLOX_CFG_NMEA_FILT_QZSS = 0x10930015; // Disable reporting of QZSS satellites +const uint32_t UBLOX_CFG_NMEA_FILT_GLO = 0x10930016; // Disable reporting of GLONASS satellites +const uint32_t UBLOX_CFG_NMEA_FILT_BDS = 0x10930017; // Disable reporting of BeiDou satellites +const uint32_t UBLOX_CFG_NMEA_OUT_INVFIX = 0x10930021; // Enable position output for failed or invalid fixes +const uint32_t UBLOX_CFG_NMEA_OUT_MSKFIX = 0x10930022; // Enable position output for invalid fixes +const uint32_t UBLOX_CFG_NMEA_OUT_INVTIME = 0x10930023; // Enable time output for invalid times +const uint32_t UBLOX_CFG_NMEA_OUT_INVDATE = 0x10930024; // Enable date output for invalid dates +const uint32_t UBLOX_CFG_NMEA_OUT_ONLYGPS = 0x10930025; // Restrict output to GPS satellites only +const uint32_t UBLOX_CFG_NMEA_OUT_FROZENCOG = 0x10930026; // Enable course over ground output even if it is frozen +const uint32_t UBLOX_CFG_NMEA_MAINTALKERID = 0x20930031; // Main Talker ID +const uint32_t UBLOX_CFG_NMEA_GSVTALKERID = 0x20930032; // Talker ID for GSV NMEA messages +const uint32_t UBLOX_CFG_NMEA_BDSTALKERID = 0x30930033; // BeiDou Talker ID //CFG-ODO: Odometer and low-speed course over ground filter //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_ODO_USE_ODO = 0x10220001; // Use odometer -const uint32_t CFG_ODO_USE_COG = 0x10220002; // Use low-speed course over ground filter -const uint32_t CFG_ODO_OUTLPVEL = 0x10220003; // Output low-pass filtered velocity -const uint32_t CFG_ODO_OUTLPCOG = 0x10220004; // Output low-pass filtered course over ground (heading) -const uint32_t CFG_ODO_PROFILE = 0x20220005; // Odometer profile configuration -const uint32_t CFG_ODO_COGMAXSPEED = 0x20220021; // Upper speed limit for low-speed course over ground filter -const uint32_t CFG_ODO_COGMAXPOSACC = 0x20220022; // Maximum acceptable position accuracy for computing low-speed filtered course over ground -const uint32_t CFG_ODO_VELLPGAIN = 0x20220031; // Velocity low-pass filter level -const uint32_t CFG_ODO_COGLPGAIN = 0x20220032; // Course over ground low-pass filter level (at speed < 8 m/s) +const uint32_t UBLOX_CFG_ODO_USE_ODO = 0x10220001; // Use odometer +const uint32_t UBLOX_CFG_ODO_USE_COG = 0x10220002; // Use low-speed course over ground filter +const uint32_t UBLOX_CFG_ODO_OUTLPVEL = 0x10220003; // Output low-pass filtered velocity +const uint32_t UBLOX_CFG_ODO_OUTLPCOG = 0x10220004; // Output low-pass filtered course over ground (heading) +const uint32_t UBLOX_CFG_ODO_PROFILE = 0x20220005; // Odometer profile configuration +const uint32_t UBLOX_CFG_ODO_COGMAXSPEED = 0x20220021; // Upper speed limit for low-speed course over ground filter +const uint32_t UBLOX_CFG_ODO_COGMAXPOSACC = 0x20220022; // Maximum acceptable position accuracy for computing low-speed filtered course over ground +const uint32_t UBLOX_CFG_ODO_VELLPGAIN = 0x20220031; // Velocity low-pass filter level +const uint32_t UBLOX_CFG_ODO_COGLPGAIN = 0x20220032; // Course over ground low-pass filter level (at speed < 8 m/s) //CFG-QZSS: QZSS system configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_QZSS_USE_SLAS_DGNSS = 0x10370005; // Apply QZSS SLAS DGNSS corrections -const uint32_t CFG_QZSS_USE_SLAS_TESTMODE = 0x10370006; // Use QZSS SLAS data when it is in test mode (SLAS msg 0) -const uint32_t CFG_QZSS_USE_SLAS_RAIM_UNCORR = 0x10370007; // Raim out measurements that are not corrected by QZSS SLAS, if at least 5 measurements are corrected +const uint32_t UBLOX_CFG_QZSS_USE_SLAS_DGNSS = 0x10370005; // Apply QZSS SLAS DGNSS corrections +const uint32_t UBLOX_CFG_QZSS_USE_SLAS_TESTMODE = 0x10370006; // Use QZSS SLAS data when it is in test mode (SLAS msg 0) +const uint32_t UBLOX_CFG_QZSS_USE_SLAS_RAIM_UNCORR = 0x10370007; // Raim out measurements that are not corrected by QZSS SLAS, if at least 5 measurements are corrected //CFG-RATE: Navigation and measurement rate configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_RATE_MEAS = 0x30210001; // Nominal time between GNSS measurements -const uint32_t CFG_RATE_NAV = 0x30210002; // Ratio of number of measurements to number of navigation solutions -const uint32_t CFG_RATE_TIMEREF = 0x20210003; // Time system to which measurements are aligned +const uint32_t UBLOX_CFG_RATE_MEAS = 0x30210001; // Nominal time between GNSS measurements +const uint32_t UBLOX_CFG_RATE_NAV = 0x30210002; // Ratio of number of measurements to number of navigation solutions +const uint32_t UBLOX_CFG_RATE_TIMEREF = 0x20210003; // Time system to which measurements are aligned //CFG-RINV: Remote inventory //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_RINV_DUMP = 0x10c70001; // Dump data at startup -const uint32_t CFG_RINV_BINARY = 0x10c70002; // Data is binary -const uint32_t CFG_RINV_DATA_SIZE = 0x20c70003; // Size of data -const uint32_t CFG_RINV_CHUNK0 = 0x50c70004; // Data bytes 1-8 (LSB) -const uint32_t CFG_RINV_CHUNK1 = 0x50c70005; // Data bytes 9-16 -const uint32_t CFG_RINV_CHUNK2 = 0x50c70006; // Data bytes 17-240x44434241. -const uint32_t CFG_RINV_CHUNK3 = 0x50c70007; // Data bytes 25-30 (MSB) +const uint32_t UBLOX_CFG_RINV_DUMP = 0x10c70001; // Dump data at startup +const uint32_t UBLOX_CFG_RINV_BINARY = 0x10c70002; // Data is binary +const uint32_t UBLOX_CFG_RINV_DATA_SIZE = 0x20c70003; // Size of data +const uint32_t UBLOX_CFG_RINV_CHUNK0 = 0x50c70004; // Data bytes 1-8 (LSB) +const uint32_t UBLOX_CFG_RINV_CHUNK1 = 0x50c70005; // Data bytes 9-16 +const uint32_t UBLOX_CFG_RINV_CHUNK2 = 0x50c70006; // Data bytes 17-240x44434241. +const uint32_t UBLOX_CFG_RINV_CHUNK3 = 0x50c70007; // Data bytes 25-30 (MSB) //CFG-RTCM: RTCM protocol configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_RTCM_DF003_OUT = 0x30090001; // RTCM DF003 (Reference station ID) output value -const uint32_t CFG_RTCM_DF003_IN = 0x30090008; // RTCM DF003 (Reference station ID) input value -const uint32_t CFG_RTCM_DF003_IN_FILTER = 0x20090009; // RTCM input filter configuration based on RTCM DF003 (Reference station ID) value +const uint32_t UBLOX_CFG_RTCM_DF003_OUT = 0x30090001; // RTCM DF003 (Reference station ID) output value +const uint32_t UBLOX_CFG_RTCM_DF003_IN = 0x30090008; // RTCM DF003 (Reference station ID) input value +const uint32_t UBLOX_CFG_RTCM_DF003_IN_FILTER = 0x20090009; // RTCM input filter configuration based on RTCM DF003 (Reference station ID) value //CFG-SBAS: SBAS configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_SBAS_USE_TESTMODE = 0x10360002; // Use SBAS data when it is in test mode (SBAS msg 0) -const uint32_t CFG_SBAS_USE_RANGING = 0x10360003; // Use SBAS GEOs as a ranging source (for navigation) -const uint32_t CFG_SBAS_USE_DIFFCORR = 0x10360004; // Use SBAS differential corrections -const uint32_t CFG_SBAS_USE_INTEGRITY = 0x10360005; // Use SBAS integrity information -const uint32_t CFG_SBAS_PRNSCANMASK = 0x50360006; // SBAS PRN search configuration +const uint32_t UBLOX_CFG_SBAS_USE_TESTMODE = 0x10360002; // Use SBAS data when it is in test mode (SBAS msg 0) +const uint32_t UBLOX_CFG_SBAS_USE_RANGING = 0x10360003; // Use SBAS GEOs as a ranging source (for navigation) +const uint32_t UBLOX_CFG_SBAS_USE_DIFFCORR = 0x10360004; // Use SBAS differential corrections +const uint32_t UBLOX_CFG_SBAS_USE_INTEGRITY = 0x10360005; // Use SBAS integrity information +const uint32_t UBLOX_CFG_SBAS_PRNSCANMASK = 0x50360006; // SBAS PRN search configuration //CFG-SIGNAL: Satellite systems (GNSS) signal configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_SIGNAL_GPS_ENA = 0x1031001f; // GPS enable -const uint32_t CFG_SIGNAL_GPS_L1CA_ENA = 0x10310001; // GPS L1C/A -const uint32_t CFG_SIGNAL_GPS_L2C_ENA = 0x10310003; // GPS L2C (only on u-blox F9 platform products) -const uint32_t CFG_SIGNAL_SBAS_ENA = 0x10310020; // SBAS enable -const uint32_t CFG_SIGNAL_SBAS_L1CA_ENA = 0x10310005; // SBAS L1C/A -const uint32_t CFG_SIGNAL_GAL_ENA = 0x10310021; // Galileo enable -const uint32_t CFG_SIGNAL_GAL_E1_ENA = 0x10310007; // Galileo E1 -const uint32_t CFG_SIGNAL_GAL_E5B_ENA = 0x1031000a; // Galileo E5b (only on u-blox F9 platform products) -const uint32_t CFG_SIGNAL_BDS_ENA = 0x10310022; // BeiDou Enable -const uint32_t CFG_SIGNAL_BDS_B1_ENA = 0x1031000d; // BeiDou B1I -const uint32_t CFG_SIGNAL_BDS_B2_ENA = 0x1031000e; // BeiDou B2I (only on u-blox F9 platform products) -const uint32_t CFG_SIGNAL_QZSS_ENA = 0x10310024; // QZSS enable -const uint32_t CFG_SIGNAL_QZSS_L1CA_ENA = 0x10310012; // QZSS L1C/A -const uint32_t CFG_SIGNAL_QZSS_L1S_ENA = 0x10310014; // QZSS L1S -const uint32_t CFG_SIGNAL_QZSS_L2C_ENA = 0x10310015; // QZSS L2C (only on u-blox F9 platform products) -const uint32_t CFG_SIGNAL_GLO_ENA = 0x10310025; // GLONASS enable -const uint32_t CFG_SIGNAL_GLO_L1_ENA = 0x10310018; // GLONASS L1 -const uint32_t CFG_SIGNAL_GLO_L2_ENA = 0x1031001a; // GLONASS L2 (only on u-blox F9 platform products) +const uint32_t UBLOX_CFG_SIGNAL_GPS_ENA = 0x1031001f; // GPS enable +const uint32_t UBLOX_CFG_SIGNAL_GPS_L1CA_ENA = 0x10310001; // GPS L1C/A +const uint32_t UBLOX_CFG_SIGNAL_GPS_L2C_ENA = 0x10310003; // GPS L2C (only on u-blox F9 platform products) +const uint32_t UBLOX_CFG_SIGNAL_SBAS_ENA = 0x10310020; // SBAS enable +const uint32_t UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA = 0x10310005; // SBAS L1C/A +const uint32_t UBLOX_CFG_SIGNAL_GAL_ENA = 0x10310021; // Galileo enable +const uint32_t UBLOX_CFG_SIGNAL_GAL_E1_ENA = 0x10310007; // Galileo E1 +const uint32_t UBLOX_CFG_SIGNAL_GAL_E5B_ENA = 0x1031000a; // Galileo E5b (only on u-blox F9 platform products) +const uint32_t UBLOX_CFG_SIGNAL_BDS_ENA = 0x10310022; // BeiDou Enable +const uint32_t UBLOX_CFG_SIGNAL_BDS_B1_ENA = 0x1031000d; // BeiDou B1I +const uint32_t UBLOX_CFG_SIGNAL_BDS_B2_ENA = 0x1031000e; // BeiDou B2I (only on u-blox F9 platform products) +const uint32_t UBLOX_CFG_SIGNAL_QZSS_ENA = 0x10310024; // QZSS enable +const uint32_t UBLOX_CFG_SIGNAL_QZSS_L1CA_ENA = 0x10310012; // QZSS L1C/A +const uint32_t UBLOX_CFG_SIGNAL_QZSS_L1S_ENA = 0x10310014; // QZSS L1S +const uint32_t UBLOX_CFG_SIGNAL_QZSS_L2C_ENA = 0x10310015; // QZSS L2C (only on u-blox F9 platform products) +const uint32_t UBLOX_CFG_SIGNAL_GLO_ENA = 0x10310025; // GLONASS enable +const uint32_t UBLOX_CFG_SIGNAL_GLO_L1_ENA = 0x10310018; // GLONASS L1 +const uint32_t UBLOX_CFG_SIGNAL_GLO_L2_ENA = 0x1031001a; // GLONASS L2 (only on u-blox F9 platform products) //CFG-SPI: Configuration of the SPI interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_SPI_MAXFF = 0x20640001; // Number of bytes containing 0xFF to receive before switching off reception. Range: 0 (mechanism off) - 63 -const uint32_t CFG_SPI_CPOLARITY = 0x10640002; // Clock polarity select: 0: Active Hight Clock, SCLK idles low, 1: Active Low Clock, SCLK idles high -const uint32_t CFG_SPI_CPHASE = 0x10640003; // Clock phase select: 0: Data captured on first edge of SCLK, 1: Data captured on second edge of SCLK -const uint32_t CFG_SPI_EXTENDEDTIMEOUT = 0x10640005; // Flag to disable timeouting the interface after 1.5s -const uint32_t CFG_SPI_ENABLED = 0x10640006; // Flag to indicate if the SPI interface should be enabled +const uint32_t UBLOX_CFG_SPI_MAXFF = 0x20640001; // Number of bytes containing 0xFF to receive before switching off reception. Range: 0 (mechanism off) - 63 +const uint32_t UBLOX_CFG_SPI_CPOLARITY = 0x10640002; // Clock polarity select: 0: Active Hight Clock, SCLK idles low, 1: Active Low Clock, SCLK idles high +const uint32_t UBLOX_CFG_SPI_CPHASE = 0x10640003; // Clock phase select: 0: Data captured on first edge of SCLK, 1: Data captured on second edge of SCLK +const uint32_t UBLOX_CFG_SPI_EXTENDEDTIMEOUT = 0x10640005; // Flag to disable timeouting the interface after 1.5s +const uint32_t UBLOX_CFG_SPI_ENABLED = 0x10640006; // Flag to indicate if the SPI interface should be enabled //CFG-SPIINPROT: Input protocol configuration of the SPI interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_SPIINPROT_UBX = 0x10790001; // Flag to indicate if UBX should be an input protocol on SPI -const uint32_t CFG_SPIINPROT_NMEA = 0x10790002; // Flag to indicate if NMEA should be an input protocol on SPI -const uint32_t CFG_SPIINPROT_RTCM3X = 0x10790004; // Flag to indicate if RTCM3X should be an input protocol on SPI +const uint32_t UBLOX_CFG_SPIINPROT_UBX = 0x10790001; // Flag to indicate if UBX should be an input protocol on SPI +const uint32_t UBLOX_CFG_SPIINPROT_NMEA = 0x10790002; // Flag to indicate if NMEA should be an input protocol on SPI +const uint32_t UBLOX_CFG_SPIINPROT_RTCM3X = 0x10790004; // Flag to indicate if RTCM3X should be an input protocol on SPI //CFG-SPIOUTPROT: Output protocol configuration of the SPI interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_SPIOUTPROT_UBX = 0x107a0001; // Flag to indicate if UBX should be an output protocol on SPI -const uint32_t CFG_SPIOUTPROT_NMEA = 0x107a0002; // Flag to indicate if NMEA should be an output protocol on SPI -const uint32_t CFG_SPIOUTPROT_RTCM3X = 0x107a0004; // Flag to indicate if RTCM3X should be an output protocol on SPI +const uint32_t UBLOX_CFG_SPIOUTPROT_UBX = 0x107a0001; // Flag to indicate if UBX should be an output protocol on SPI +const uint32_t UBLOX_CFG_SPIOUTPROT_NMEA = 0x107a0002; // Flag to indicate if NMEA should be an output protocol on SPI +const uint32_t UBLOX_CFG_SPIOUTPROT_RTCM3X = 0x107a0004; // Flag to indicate if RTCM3X should be an output protocol on SPI //CFG-TMODE: Time mode configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_TMODE_MODE = 0x20030001; // Receiver mode -const uint32_t CFG_TMODE_POS_TYPE = 0x20030002; // Determines whether the ARP position is given in ECEF or LAT/LON/HEIGHT? -const uint32_t CFG_TMODE_ECEF_X = 0x40030003; // ECEF X coordinate of the ARP position. -const uint32_t CFG_TMODE_ECEF_Y = 0x40030004; // ECEF Y coordinate of the ARP position. -const uint32_t CFG_TMODE_ECEF_Z = 0x40030005; // ECEF Z coordinate of the ARP position. -const uint32_t CFG_TMODE_ECEF_X_HP = 0x20030006; // High-precision ECEF X coordinate of the ARP position. -const uint32_t CFG_TMODE_ECEF_Y_HP = 0x20030007; // High-precision ECEF Y coordinate of the ARP position. -const uint32_t CFG_TMODE_ECEF_Z_HP = 0x20030008; // High-precision ECEF Z coordinate of the ARP position. -const uint32_t CFG_TMODE_LAT = 0x40030009; // Latitude of the ARP position. -const uint32_t CFG_TMODE_LON = 0x4003000a; // Longitude of the ARP position. -const uint32_t CFG_TMODE_HEIGHT = 0x4003000b; // Height of the ARP position. -const uint32_t CFG_TMODE_LAT_HP = 0x2003000c; // High-precision latitude of the ARP position -const uint32_t CFG_TMODE_LON_HP = 0x2003000d; // High-precision longitude of the ARP position. -const uint32_t CFG_TMODE_HEIGHT_HP = 0x2003000e; // High-precision height of the ARP position. -const uint32_t CFG_TMODE_FIXED_POS_ACC = 0x4003000f; // Fixed position 3D accuracy -const uint32_t CFG_TMODE_SVIN_MIN_DUR = 0x40030010; // Survey-in minimum duration -const uint32_t CFG_TMODE_SVIN_ACC_LIMIT = 0x40030011; // Survey-in position accuracy limit +const uint32_t UBLOX_CFG_TMODE_MODE = 0x20030001; // Receiver mode +const uint32_t UBLOX_CFG_TMODE_POS_TYPE = 0x20030002; // Determines whether the ARP position is given in ECEF or LAT/LON/HEIGHT? +const uint32_t UBLOX_CFG_TMODE_ECEF_X = 0x40030003; // ECEF X coordinate of the ARP position. +const uint32_t UBLOX_CFG_TMODE_ECEF_Y = 0x40030004; // ECEF Y coordinate of the ARP position. +const uint32_t UBLOX_CFG_TMODE_ECEF_Z = 0x40030005; // ECEF Z coordinate of the ARP position. +const uint32_t UBLOX_CFG_TMODE_ECEF_X_HP = 0x20030006; // High-precision ECEF X coordinate of the ARP position. +const uint32_t UBLOX_CFG_TMODE_ECEF_Y_HP = 0x20030007; // High-precision ECEF Y coordinate of the ARP position. +const uint32_t UBLOX_CFG_TMODE_ECEF_Z_HP = 0x20030008; // High-precision ECEF Z coordinate of the ARP position. +const uint32_t UBLOX_CFG_TMODE_LAT = 0x40030009; // Latitude of the ARP position. +const uint32_t UBLOX_CFG_TMODE_LON = 0x4003000a; // Longitude of the ARP position. +const uint32_t UBLOX_CFG_TMODE_HEIGHT = 0x4003000b; // Height of the ARP position. +const uint32_t UBLOX_CFG_TMODE_LAT_HP = 0x2003000c; // High-precision latitude of the ARP position +const uint32_t UBLOX_CFG_TMODE_LON_HP = 0x2003000d; // High-precision longitude of the ARP position. +const uint32_t UBLOX_CFG_TMODE_HEIGHT_HP = 0x2003000e; // High-precision height of the ARP position. +const uint32_t UBLOX_CFG_TMODE_FIXED_POS_ACC = 0x4003000f; // Fixed position 3D accuracy +const uint32_t UBLOX_CFG_TMODE_SVIN_MIN_DUR = 0x40030010; // Survey-in minimum duration +const uint32_t UBLOX_CFG_TMODE_SVIN_ACC_LIMIT = 0x40030011; // Survey-in position accuracy limit //CFG-TP: Timepulse configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_TP_PULSE_DEF = 0x20050023; // Determines whether the time pulse is interpreted as frequency or period -const uint32_t CFG_TP_PULSE_LENGTH_DEF = 0x20050030; // Determines whether the time pulse length is interpreted as length[us] or pulse ratio[%] -const uint32_t CFG_TP_FREQ_TP1 = 0x40050024; // Time pulse frequency (TP1) -const uint32_t CFG_TP_FREQ_LOCK_TP1 = 0x40050025; // Time pulse frequency when locked to GNSS time (TP1) -const uint32_t CFG_TP_LEN_TP1 = 0x40050004; // Time pulse length (TP1) -const uint32_t CFG_TP_LEN_LOCK_TP1 = 0x40050005; // Time pulse length when locked to GNSS time (TP1) -const uint32_t CFG_TP_DUTY_TP1 = 0x5005002a; // Time pulse duty cycle (TP1) -const uint32_t CFG_TP_DUTY_LOCK_TP1 = 0x5005002b; // Time pulse duty cycle when locked to GNSS time (TP1) -const uint32_t CFG_TP_USER_DELAY_TP1 = 0x40050006; // User-configurable time pulse delay (TP1) -const uint32_t CFG_TP_TP1_ENA = 0x10050007; // Enable the first timepulse -const uint32_t CFG_TP_SYNC_GNSS_TP1 = 0x10050008; // Sync time pulse to GNSS time or local clock (TP1) -const uint32_t CFG_TP_USE_LOCKED_TP1 = 0x10050009; // Use locked parameters when possible (TP1) -const uint32_t CFG_TP_ALIGN_TO_TOW_TP1 = 0x1005000a; // Align time pulse to top of second (TP1) -const uint32_t CFG_TP_POL_TP1 = 0x1005000b; // Set time pulse polarity (TP1) -const uint32_t CFG_TP_TIMEGRID_TP1 = 0x2005000c; // Time grid to use (TP1) +const uint32_t UBLOX_CFG_TP_PULSE_DEF = 0x20050023; // Determines whether the time pulse is interpreted as frequency or period +const uint32_t UBLOX_CFG_TP_PULSE_LENGTH_DEF = 0x20050030; // Determines whether the time pulse length is interpreted as length[us] or pulse ratio[%] +const uint32_t UBLOX_CFG_TP_FREQ_TP1 = 0x40050024; // Time pulse frequency (TP1) +const uint32_t UBLOX_CFG_TP_FREQ_LOCK_TP1 = 0x40050025; // Time pulse frequency when locked to GNSS time (TP1) +const uint32_t UBLOX_CFG_TP_LEN_TP1 = 0x40050004; // Time pulse length (TP1) +const uint32_t UBLOX_CFG_TP_LEN_LOCK_TP1 = 0x40050005; // Time pulse length when locked to GNSS time (TP1) +const uint32_t UBLOX_CFG_TP_DUTY_TP1 = 0x5005002a; // Time pulse duty cycle (TP1) +const uint32_t UBLOX_CFG_TP_DUTY_LOCK_TP1 = 0x5005002b; // Time pulse duty cycle when locked to GNSS time (TP1) +const uint32_t UBLOX_CFG_TP_USER_DELAY_TP1 = 0x40050006; // User-configurable time pulse delay (TP1) +const uint32_t UBLOX_CFG_TP_TP1_ENA = 0x10050007; // Enable the first timepulse +const uint32_t UBLOX_CFG_TP_SYNC_GNSS_TP1 = 0x10050008; // Sync time pulse to GNSS time or local clock (TP1) +const uint32_t UBLOX_CFG_TP_USE_LOCKED_TP1 = 0x10050009; // Use locked parameters when possible (TP1) +const uint32_t UBLOX_CFG_TP_ALIGN_TO_TOW_TP1 = 0x1005000a; // Align time pulse to top of second (TP1) +const uint32_t UBLOX_CFG_TP_POL_TP1 = 0x1005000b; // Set time pulse polarity (TP1) +const uint32_t UBLOX_CFG_TP_TIMEGRID_TP1 = 0x2005000c; // Time grid to use (TP1) //CFG-TXREADY: TX ready configuration //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_TXREADY_ENABLED = 0x10a20001; // Flag to indicate if TX ready pin mechanism should be enabled -const uint32_t CFG_TXREADY_POLARITY = 0x10a20002; // The polarity of the TX ready pin: false:high- active, true:low-active -const uint32_t CFG_TXREADY_PIN = 0x20a20003; // Pin number to use for the TX ready functionality -const uint32_t CFG_TXREADY_THRESHOLD = 0x30a20004; // Amount of data that should be ready on the interface before triggering the TX ready pin -const uint32_t CFG_TXREADY_INTERFACE = 0x20a20005; // Interface where the TX ready feature should be linked to +const uint32_t UBLOX_CFG_TXREADY_ENABLED = 0x10a20001; // Flag to indicate if TX ready pin mechanism should be enabled +const uint32_t UBLOX_CFG_TXREADY_POLARITY = 0x10a20002; // The polarity of the TX ready pin: false:high- active, true:low-active +const uint32_t UBLOX_CFG_TXREADY_PIN = 0x20a20003; // Pin number to use for the TX ready functionality +const uint32_t UBLOX_CFG_TXREADY_THRESHOLD = 0x30a20004; // Amount of data that should be ready on the interface before triggering the TX ready pin +const uint32_t UBLOX_CFG_TXREADY_INTERFACE = 0x20a20005; // Interface where the TX ready feature should be linked to //CFG-UART1: Configuration of the UART1 interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_UART1_BAUDRATE = 0x40520001; // The baud rate that should be configured on the UART1 -const uint32_t CFG_UART1_STOPBITS = 0x20520002; // Number of stopbits that should be used on UART1 -const uint32_t CFG_UART1_DATABITS = 0x20520003; // Number of databits that should be used on UART1 -const uint32_t CFG_UART1_PARITY = 0x20520004; // Parity mode that should be used on UART1 -const uint32_t CFG_UART1_ENABLED = 0x10520005; // Flag to indicate if the UART1 should be enabled +const uint32_t UBLOX_CFG_UART1_BAUDRATE = 0x40520001; // The baud rate that should be configured on the UART1 +const uint32_t UBLOX_CFG_UART1_STOPBITS = 0x20520002; // Number of stopbits that should be used on UART1 +const uint32_t UBLOX_CFG_UART1_DATABITS = 0x20520003; // Number of databits that should be used on UART1 +const uint32_t UBLOX_CFG_UART1_PARITY = 0x20520004; // Parity mode that should be used on UART1 +const uint32_t UBLOX_CFG_UART1_ENABLED = 0x10520005; // Flag to indicate if the UART1 should be enabled //CFG-UART1INPROT: Input protocol configuration of the UART1 interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_UART1INPROT_UBX = 0x10730001; // Flag to indicate if UBX should be an input protocol on UART1 -const uint32_t CFG_UART1INPROT_NMEA = 0x10730002; // Flag to indicate if NMEA should be an input protocol on UART1 -const uint32_t CFG_UART1INPROT_RTCM3X = 0x10730004; // Flag to indicate if RTCM3X should be an input protocol on UART1 +const uint32_t UBLOX_CFG_UART1INPROT_UBX = 0x10730001; // Flag to indicate if UBX should be an input protocol on UART1 +const uint32_t UBLOX_CFG_UART1INPROT_NMEA = 0x10730002; // Flag to indicate if NMEA should be an input protocol on UART1 +const uint32_t UBLOX_CFG_UART1INPROT_RTCM3X = 0x10730004; // Flag to indicate if RTCM3X should be an input protocol on UART1 //CFG-UART1OUTPROT: Output protocol configuration of the UART1 interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_UART1OUTPROT_UBX = 0x10740001; // Flag to indicate if UBX should be an output protocol on UART1 -const uint32_t CFG_UART1OUTPROT_NMEA = 0x10740002; // Flag to indicate if NMEA should be an output protocol on UART1 -const uint32_t CFG_UART1OUTPROT_RTCM3X = 0x10740004; // Flag to indicate if RTCM3X should be an output protocol on UART1 +const uint32_t UBLOX_CFG_UART1OUTPROT_UBX = 0x10740001; // Flag to indicate if UBX should be an output protocol on UART1 +const uint32_t UBLOX_CFG_UART1OUTPROT_NMEA = 0x10740002; // Flag to indicate if NMEA should be an output protocol on UART1 +const uint32_t UBLOX_CFG_UART1OUTPROT_RTCM3X = 0x10740004; // Flag to indicate if RTCM3X should be an output protocol on UART1 //CFG-UART2: Configuration of the UART2 interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_UART2_BAUDRATE = 0x40530001; // The baud rate that should be configured on the UART2 -const uint32_t CFG_UART2_STOPBITS = 0x20530002; // Number of stopbits that should be used on UART2 -const uint32_t CFG_UART2_DATABITS = 0x20530003; // Number of databits that should be used on UART2 -const uint32_t CFG_UART2_PARITY = 0x20530004; // Parity mode that should be used on UART2 -const uint32_t CFG_UART2_ENABLED = 0x10530005; // Flag to indicate if the UART2 should be enabled -const uint32_t CFG_UART2_REMAP = 0x10530006; // UART2 Remapping +const uint32_t UBLOX_CFG_UART2_BAUDRATE = 0x40530001; // The baud rate that should be configured on the UART2 +const uint32_t UBLOX_CFG_UART2_STOPBITS = 0x20530002; // Number of stopbits that should be used on UART2 +const uint32_t UBLOX_CFG_UART2_DATABITS = 0x20530003; // Number of databits that should be used on UART2 +const uint32_t UBLOX_CFG_UART2_PARITY = 0x20530004; // Parity mode that should be used on UART2 +const uint32_t UBLOX_CFG_UART2_ENABLED = 0x10530005; // Flag to indicate if the UART2 should be enabled +const uint32_t UBLOX_CFG_UART2_REMAP = 0x10530006; // UART2 Remapping //CFG-UART2INPROT: Input protocol configuration of the UART2 interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_UART2INPROT_UBX = 0x10750001; // Flag to indicate if UBX should be an input protocol on UART2 -const uint32_t CFG_UART2INPROT_NMEA = 0x10750002; // Flag to indicate if NMEA should be an input protocol on UART2 -const uint32_t CFG_UART2INPROT_RTCM3X = 0x10750004; // Flag to indicate if RTCM3X should be an input protocol on UART2 +const uint32_t UBLOX_CFG_UART2INPROT_UBX = 0x10750001; // Flag to indicate if UBX should be an input protocol on UART2 +const uint32_t UBLOX_CFG_UART2INPROT_NMEA = 0x10750002; // Flag to indicate if NMEA should be an input protocol on UART2 +const uint32_t UBLOX_CFG_UART2INPROT_RTCM3X = 0x10750004; // Flag to indicate if RTCM3X should be an input protocol on UART2 //CFG-UART2OUTPROT: Output protocol configuration of the UART2 interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_UART2OUTPROT_UBX = 0x10760001; // Flag to indicate if UBX should be an output protocol on UART2 -const uint32_t CFG_UART2OUTPROT_NMEA = 0x10760002; // Flag to indicate if NMEA should be an output protocol on UART2 -const uint32_t CFG_UART2OUTPROT_RTCM3X = 0x10760004; // Flag to indicate if RTCM3X should be an output protocol on UART2 +const uint32_t UBLOX_CFG_UART2OUTPROT_UBX = 0x10760001; // Flag to indicate if UBX should be an output protocol on UART2 +const uint32_t UBLOX_CFG_UART2OUTPROT_NMEA = 0x10760002; // Flag to indicate if NMEA should be an output protocol on UART2 +const uint32_t UBLOX_CFG_UART2OUTPROT_RTCM3X = 0x10760004; // Flag to indicate if RTCM3X should be an output protocol on UART2 //CFG-USB: Configuration of the USB interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_USB_ENABLED = 0x10650001; // Flag to indicate if the USB interface should be enabled -const uint32_t CFG_USB_SELFPOW = 0x10650002; // Self-powered device -const uint32_t CFG_USB_VENDOR_ID = 0x3065000a; // Vendor ID -const uint32_t CFG_USB_PRODUCT_ID = 0x3065000b; // Vendor ID -const uint32_t CFG_USB_POWER = 0x3065000c; // Power consumption -const uint32_t CFG_USB_VENDOR_STR0 = 0x5065000d; // Vendor string characters 0-7 -const uint32_t CFG_USB_VENDOR_STR1 = 0x5065000e; // Vendor string characters 8-15 -const uint32_t CFG_USB_VENDOR_STR2 = 0x5065000f; // Vendor string characters 16-23 -const uint32_t CFG_USB_VENDOR_STR3 = 0x50650010; // Vendor string characters 24-31 -const uint32_t CFG_USB_PRODUCT_STR0 = 0x50650011; // Product string characters 0-7 -const uint32_t CFG_USB_PRODUCT_STR1 = 0x50650012; // Product string characters 8-15 -const uint32_t CFG_USB_PRODUCT_STR2 = 0x50650013; // Product string characters 16-23 -const uint32_t CFG_USB_PRODUCT_STR3 = 0x50650014; // Product string characters 24-31 -const uint32_t CFG_USB_SERIAL_NO_STR0 = 0x50650015; // Serial number string characters 0-7 -const uint32_t CFG_USB_SERIAL_NO_STR1 = 0x50650016; // Serial number string characters 8-15 -const uint32_t CFG_USB_SERIAL_NO_STR2 = 0x50650017; // Serial number string characters 16-23 -const uint32_t CFG_USB_SERIAL_NO_STR3 = 0x50650018; // Serial number string characters 24-31 +const uint32_t UBLOX_CFG_USB_ENABLED = 0x10650001; // Flag to indicate if the USB interface should be enabled +const uint32_t UBLOX_CFG_USB_SELFPOW = 0x10650002; // Self-powered device +const uint32_t UBLOX_CFG_USB_VENDOR_ID = 0x3065000a; // Vendor ID +const uint32_t UBLOX_CFG_USB_PRODUCT_ID = 0x3065000b; // Vendor ID +const uint32_t UBLOX_CFG_USB_POWER = 0x3065000c; // Power consumption +const uint32_t UBLOX_CFG_USB_VENDOR_STR0 = 0x5065000d; // Vendor string characters 0-7 +const uint32_t UBLOX_CFG_USB_VENDOR_STR1 = 0x5065000e; // Vendor string characters 8-15 +const uint32_t UBLOX_CFG_USB_VENDOR_STR2 = 0x5065000f; // Vendor string characters 16-23 +const uint32_t UBLOX_CFG_USB_VENDOR_STR3 = 0x50650010; // Vendor string characters 24-31 +const uint32_t UBLOX_CFG_USB_PRODUCT_STR0 = 0x50650011; // Product string characters 0-7 +const uint32_t UBLOX_CFG_USB_PRODUCT_STR1 = 0x50650012; // Product string characters 8-15 +const uint32_t UBLOX_CFG_USB_PRODUCT_STR2 = 0x50650013; // Product string characters 16-23 +const uint32_t UBLOX_CFG_USB_PRODUCT_STR3 = 0x50650014; // Product string characters 24-31 +const uint32_t UBLOX_CFG_USB_SERIAL_NO_STR0 = 0x50650015; // Serial number string characters 0-7 +const uint32_t UBLOX_CFG_USB_SERIAL_NO_STR1 = 0x50650016; // Serial number string characters 8-15 +const uint32_t UBLOX_CFG_USB_SERIAL_NO_STR2 = 0x50650017; // Serial number string characters 16-23 +const uint32_t UBLOX_CFG_USB_SERIAL_NO_STR3 = 0x50650018; // Serial number string characters 24-31 //CFG-USBINPROT: Input protocol configuration of the USB interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_USBINPROT_UBX = 0x10770001; // Flag to indicate if UBX should be an input protocol on USB -const uint32_t CFG_USBINPROT_NMEA = 0x10770002; // Flag to indicate if NMEA should be an input protocol on USB -const uint32_t CFG_USBINPROT_RTCM3X = 0x10770004; // Flag to indicate if RTCM3X should be an input protocol on USB +const uint32_t UBLOX_CFG_USBINPROT_UBX = 0x10770001; // Flag to indicate if UBX should be an input protocol on USB +const uint32_t UBLOX_CFG_USBINPROT_NMEA = 0x10770002; // Flag to indicate if NMEA should be an input protocol on USB +const uint32_t UBLOX_CFG_USBINPROT_RTCM3X = 0x10770004; // Flag to indicate if RTCM3X should be an input protocol on USB //CFG-USBOUTPROT: Output protocol configuration of the USB interface //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -const uint32_t CFG_USBOUTPROT_UBX = 0x10780001; // Flag to indicate if UBX should be an output protocol on USB -const uint32_t CFG_USBOUTPROT_NMEA = 0x10780002; // Flag to indicate if NMEA should be an output protocol on USB -const uint32_t CFG_USBOUTPROT_RTCM3X = 0x10780004; // Flag to indicate if RTCM3X should be an output protocol on USB +const uint32_t UBLOX_CFG_USBOUTPROT_UBX = 0x10780001; // Flag to indicate if UBX should be an output protocol on USB +const uint32_t UBLOX_CFG_USBOUTPROT_NMEA = 0x10780002; // Flag to indicate if NMEA should be an output protocol on USB +const uint32_t UBLOX_CFG_USBOUTPROT_RTCM3X = 0x10780004; // Flag to indicate if RTCM3X should be an output protocol on USB #endif From 0ef7394e57b7f7d5bd3938f228573b0c5c77ffd6 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Fri, 30 Oct 2020 07:42:55 +0000 Subject: [PATCH 07/36] Fixing issue #143. Thank you @markuckermann --- src/SparkFun_Ublox_Arduino_Library.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/SparkFun_Ublox_Arduino_Library.h b/src/SparkFun_Ublox_Arduino_Library.h index 8feadfc..7405335 100644 --- a/src/SparkFun_Ublox_Arduino_Library.h +++ b/src/SparkFun_Ublox_Arduino_Library.h @@ -574,9 +574,9 @@ class SFE_UBLOX_GPS uint8_t setVal8(uint32_t keyID, uint8_t value, uint8_t layer = VAL_LAYER_ALL, uint16_t maxWait = 250); //Sets the 8-bit value at a given group/id/size location uint8_t setVal16(uint32_t keyID, uint16_t value, uint8_t layer = VAL_LAYER_ALL, uint16_t maxWait = 250); //Sets the 16-bit value at a given group/id/size location uint8_t setVal32(uint32_t keyID, uint32_t value, uint8_t layer = VAL_LAYER_ALL, uint16_t maxWait = 250); //Sets the 32-bit value at a given group/id/size location - uint8_t newCfgValset8(uint32_t keyID, uint8_t value, uint8_t layer = VAL_LAYER_BBR); //Define a new UBX-CFG-VALSET with the given KeyID and 8-bit value - uint8_t newCfgValset16(uint32_t keyID, uint16_t value, uint8_t layer = VAL_LAYER_BBR); //Define a new UBX-CFG-VALSET with the given KeyID and 16-bit value - uint8_t newCfgValset32(uint32_t keyID, uint32_t value, uint8_t layer = VAL_LAYER_BBR); //Define a new UBX-CFG-VALSET with the given KeyID and 32-bit value + uint8_t newCfgValset8(uint32_t keyID, uint8_t value, uint8_t layer = VAL_LAYER_ALL); //Define a new UBX-CFG-VALSET with the given KeyID and 8-bit value + uint8_t newCfgValset16(uint32_t keyID, uint16_t value, uint8_t layer = VAL_LAYER_ALL); //Define a new UBX-CFG-VALSET with the given KeyID and 16-bit value + uint8_t newCfgValset32(uint32_t keyID, uint32_t value, uint8_t layer = VAL_LAYER_ALL); //Define a new UBX-CFG-VALSET with the given KeyID and 32-bit value uint8_t addCfgValset8(uint32_t keyID, uint8_t value); //Add a new KeyID and 8-bit value to an existing UBX-CFG-VALSET ubxPacket uint8_t addCfgValset16(uint32_t keyID, uint16_t value); //Add a new KeyID and 16-bit value to an existing UBX-CFG-VALSET ubxPacket uint8_t addCfgValset32(uint32_t keyID, uint32_t value); //Add a new KeyID and 32-bit value to an existing UBX-CFG-VALSET ubxPacket From 588ba0f316ca1fb1ed8d7e28adec4032d9fbf174 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Fri, 30 Oct 2020 07:45:25 +0000 Subject: [PATCH 08/36] Renaming Example11_setStaticPosition to Example12_setStaticPosition --- .../Example12_setStaticPosition.ino} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename examples/ZED-F9P/{Example11_setStaticPosition/Example11_setStaticPosition.ino => Example12_setStaticPosition/Example12_setStaticPosition.ino} (100%) diff --git a/examples/ZED-F9P/Example11_setStaticPosition/Example11_setStaticPosition.ino b/examples/ZED-F9P/Example12_setStaticPosition/Example12_setStaticPosition.ino similarity index 100% rename from examples/ZED-F9P/Example11_setStaticPosition/Example11_setStaticPosition.ino rename to examples/ZED-F9P/Example12_setStaticPosition/Example12_setStaticPosition.ino From da22ac8b725eb85e7b35d5d49b9028c361625310 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Fri, 30 Oct 2020 07:46:35 +0000 Subject: [PATCH 09/36] Renaming ZED-F9P Example12_autoHPPOSLLH to Example13_autoHPPOSLLH --- .../Example13_autoHPPOSLLH.ino} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename examples/ZED-F9P/{Example11_autoHPPOSLLH/Example11_autoHPPOSLLH.ino => Example13_autoHPPOSLLH/Example13_autoHPPOSLLH.ino} (100%) diff --git a/examples/ZED-F9P/Example11_autoHPPOSLLH/Example11_autoHPPOSLLH.ino b/examples/ZED-F9P/Example13_autoHPPOSLLH/Example13_autoHPPOSLLH.ino similarity index 100% rename from examples/ZED-F9P/Example11_autoHPPOSLLH/Example11_autoHPPOSLLH.ino rename to examples/ZED-F9P/Example13_autoHPPOSLLH/Example13_autoHPPOSLLH.ino From 5cc47d9bef7e7ddd8a4de3f5bc0a498cecbe3d53 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Fri, 30 Oct 2020 07:53:38 +0000 Subject: [PATCH 10/36] Updating CONTRIBUTING.md --- CONTRIBUTING.md | 7 ++++++- img/Contributing.JPG | Bin 0 -> 62131 bytes 2 files changed, 6 insertions(+), 1 deletion(-) create mode 100644 img/Contributing.JPG diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 18414f0..aa1d4c0 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -3,12 +3,17 @@ Thank you so *much* for offering to help out. We truly appreciate it. If you'd like to contribute, start by searching through the [issues](https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library/issues) and [pull requests](https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library/pulls) to see whether someone else has raised a similar idea or question. +Please check the [closed issues](https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library/issues?q=is%3Aissue+is%3Aclosed) +and [closed pull requests](https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library/pulls?q=is%3Apr+is%3Aclosed) too - you may find that your issue or feature has already been discussed. If you decide to add a feature to this library, please create a PR and follow these best practices: -* Change as little as possible. Do not sumbit a PR that changes 100 lines of whitespace. Break up into multiple PRs if necessary. +* Change as little as possible. Do not submit a PR that changes 100 lines of whitespace. Break up into multiple PRs if necessary. * If you've added a new feature document it with a simple example sketch. This serves both as a test of your PR and as a quick way for users to quickly learn how to use your new feature. * If you add new functions also add them to keywords.txt so that they are properly highlighted in Arduino. [Read more](https://www.arduino.cc/en/Hacking/libraryTutorial). +* **Important:** Please submit your PR using the [release-candidate branch](https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library/tree/release_candidate). That way, we can merge and test your PR quickly without changing the _master_ branch + +![Contributing.JPG](./img/Contributing.JPG) ## Style guide diff --git a/img/Contributing.JPG b/img/Contributing.JPG new file mode 100644 index 0000000000000000000000000000000000000000..39ebf68b048682d490c560ab5cd4fc67656ae778 GIT binary patch literal 62131 zcmeFZ2{hFI|1UnG5R$TmsO*HuQr1*L6QZmWlVlxZl6{*gLPSCcA%wC`*0BteJ$v@d zU@Y0U8Os=Eap!Y?_x$et-gEx<|G)op{^$JfIp3Fgoim5m`}KZ3U$5u#cs&=#e~xDX zXYb!Lz6W4pVgfv6`~Z$|fIEN_OicfL{##={$^1_{#lpgTlJyiT>pz{1or9f??KB%J z>uIjjr#Uzojg|cjHy7ubf4=`S$v=<(^AzL5$;Qg|&xrrMcKj2-!^ZTOxr~|V3g83} z6EhFfaXUZ^0AM=FSnWRx{%?)x1T)LYQ;a2@=3oq{Im=i+GxG_?Vo$QLFh++l{tjT_ zIeG4~vi_;_=8mjaym?h#C+4z=->zukvlt>sTz~d5oc%PvfZzooNhxU=SvgfTb&VT0 zHSgRtxMyf&eE-p7ODk&|TRW%c&MvNQ?jAnAe*OW0LBSDkBBP?;#>6Hie@sbD`;?xM zm;be(u;|xP(np#v{eM4(odq-#2ukN1VkO~we^T~8(Z$1{>jdKrSXlp|i|IrFqciicoV=`j>YTngtE2b%D=M$q zcyA}>R~m)+OlZfzDEv9;I+k4mtV>f_{jJ(%1j zpIY)6x7=Xu@(K;0_^tTw1iptc!~2gq(}aYR=Qy|^meEh1cN#Q+f8{*CPU?<#lFoPf zmJtxfAAg8F1|*xAlp`BXWmK*m!NqdIHa4qtJMz$jBtzPFUv8YhEIexgdIJA&Bq_+H za#Xedwvrs>Ui0g|0RH46LbvdD*3qZ$xq_V^=gn_)T#%Ljk{2s?kJsD)@GsqZ;22Qt z+GWN|<|a{SS=xLB9y!j0!T0k-c}OkTC6Yb`y_%bfKZuLE`)8}`z_YO`>eqZ0MIqFO zXx0wHX^|2M*;U11Rc&;pF;y+-sfmr>T_mR+C`~t`x-*^!}=e! zDbJD?3+rN8g#Vn)_boPV`N;~%mi>w|2Q}%^cpknfk`&w;w8_h_$=Ot|=#C4D6&6ju z@Ld*~%afF30XoTReycTB=F5rygF1Ht2C_OkbPSj+!Bj@ltJPPAv4KTqBa6wj2X>{k zsY{nXKQ$thuA#$&n1`ENkk@IYde67n=#vO`NkYQAxh5n2d3lX7| z`!$vh718+cYetvC^$9VUr(#&{_G`Ug)-kn8apj>rnwdcgd-#*?cK#U2X5}i1d4Njy z`^v9!e|JAGOMploIhG;*RbF;m+y3J?&^Kd6IGoh5R#up$0MVh#Hx~_L%kTIp{Sly_ zRxVk+jF=M~)3!$o7 zZwtxwu5;eSIO=Vams=ODA2b2YlqG8A<-YQ-v~iq}>pbgpq1W2-7;y3=`TQJgGKbFA zO?3T9=NO(JqH4??16aWN?w)Z^b|SN;2W#ZU-)BOFib!~8fl#D96-233L-BP zTw5|eqk;nxHnsJ6l~*)KWuE;|Unk)DXM01mzof9LJ`@?)Fcjzr}VRAp=T?_mo zfnM-E^yU~`p#S7VxL*_3!>PcRHjX>ymJ*yvtwZNU5^u<@y?F8BE|>}7suZWoL!`a0 zAxbZ7F1K^&C<*|>#2*k+a-vJipFfvfn)VF(n*K_nxTN#-4}X!<>!d`|$oGp%vU-n6 zc0nPvlcC1bjXrh2KaUFcJ!JKy1A-ts0n^w)XGHojz!~``vn8hnwRxUmg=p;#Kj?4} zs3GTS^JfcZkZe?cEo+9QC#@OHKi@5obFx(7=XoP}BJEk22A%!f0h}2{UL^eR$SC|i zH~0p_5Y3^UcMlPSO6qiWti0r`+HEv zpA3I&pct#s1vd3q16T1NH%CN^VJKoY=QNe2c1RlnTZn%qCJa%};LrRLczXwA0&Y+9 zqCcB7y!x`;Lc?aN=RpNBwVcBzf>`u(n`sSEnMUQEm~}3Sq_a-L2jPO`RJDG2n-T8# zD`QKMjDEHX!+`BJFMXsF*jK5E1R zU~mjzquP08s@05rPMtAnLOLcm z?F)bMRF#QB`U;#owE~t42T8quNNRz2L_siTaN(*6D5F~UWH*yGn@SrNJewQ z!t=(QcFnQrp60R={1hr(fts|5TRwbk3xC3`aEeWYJkdEwmWXDs8rGHMGz#aZ-li4$ z?Qxv6sk^OgKd$m)-h892q$>oAmGT#>(V%|aCTSl7kY)nMfYnNDDZM>s zCs7Z_N^gS;9Ro&Gtc$6SXeo#}XZi?+wORZa01JCZ=hN0~2T$Uk?dsLQ@lf6iyP++M zL2!rdI!aYS+J*1BY>X2j6zRDk<8*cDQ*?$+imV7JbO?*sa4;iT|Jmp$$6sd%8(hQH z1G*Y9UU=OYLahXASQ^7mGj_{VO`zMvzlMGvxNrF37|`N(vW0QVc2pomHqJgM!!YSN z>r=#Lh(cqbhdZI5dC)!KwE=iM&%*0wlP$QU3U<}l$^5@1{TCn>z!ISlREKj zS2RmX{GC}d+B{5gyWny4_^~4&+oGN3U#m@r){w)=RaY-`nSCfFCk=mAm#3&Lht(!9 zVHUR0O|c|H77Am6+(WqI)Cyn!C7!=JR5Kb}8u94kl!3R(sq@tk|hlQt>%wMEM@}Sz;BET((0lgEstTqXZiIC^m!_vl7cAsUZrZD?bkyEY5|?>) zQ?Ro#!%Jh#!EBgVIRcR$^4yjzwqAZ?7neHF9gH<@3JA8h-wx4HC+NM=s}hJtIK#*a z$AD;GCDz_zEZJ^(?o6(aQ{EIF7y!)^+XVUrZayd-6uEi}A@4u-jB<teBS{}| z&D`7D(eFBUWp$?h<@xkG=Wg#Y4JSYTUb`LmIG##tgx4O};c8zyWTY56d)eN$+99dK zt!$8dFZ z5AwAI?#{M52$JtLEVt%NVo0^a7&m+6OU*yXF(qo~w2Y=571ij6A3TzRYIh@@1JSj& zkUL*G!n!G{43W7;RU+t-jLEMF5b&5+C~QtoY(2QP;HB5IK(8yP=BIZRE~gqM*|a9$ zQ#t!Io(lC6G&Yopy{+n2y26>HVxrpitGy*7$5(-RW(n<3w^t`oVGpZLY27+y?j@<6 z^>g%_yZ-}&1#GGyzd*t3pr&)WgS)fB}n~zEk8-rj}UXD5SvITCe zQk&>?auosDi5JvmE_(9}mD8pxoB=E(4AhSBr;-*P7(0b@?AXZoofAaYYB61!VBmum z(`F9(AcNTuvys*g@?@~1BQg-w;w{n4fk|Cjn}-T)+BH?$w-?UE{d#G4UF7qZhjjoZ zFf=rbe3F>cC3emfN!VQ&q{?reo9|aBR|O58#6l7!icP&PrTmRq_z-?duv^qx>*md6 z9GyL8mSNMMBF=}h)Q{~qPHS}FA&p~d-R)qnx=}Ah`1nDiCBLMjcTEc4?N_;BtGDyw zU{K!rq&3>o#*vP&aczc`>Cw*KZx6oJ0aJ?oUS+J=z;HgaNA&jm3V{Zp(mt*!I7 z?!`dR143#-m^Uro+x=OocAhe-`{fY)5l#K(ia}?_J1a*6UMrVe z1Kx*B|3~#8a65*VoY0vg1ci%{1MFa@+xQb`^(7E~((fs-1oURCphBu|pk-E)#g4Ja zE4h>G^=)|&{CT9FtBtnS7mv!ou6h6Z`FHD|REtoRBXdn34cW7jT)3c~>c!1so=!~q z%S+ERyE2<+{Qarh2fvA_M_NjN239+)_Z-5o+3`#X#j*H`dZDoy_-1vY*IPVnbnat^YX^B~;arB4@t)#li ztYe&HJh#oO80}M1`zl)^a5E~6_N5>ng4jO>xD9@e=l*ngCobR3=K~N&%D6^eMeBLb znu*vt*yF%Y+=izL!VTt`-K;}jAPT(V|J-;kAkXI9{j23G?N7b$F+dX0qE#?C?bbTs zAA+1fbl4u2=$NnZT1SM6(kfw8FyZ=7>`cXM&e`tyZ6VNMYOvI&tEe@(52aO4S{=4~ zVyEwLlgSwx-5e+%8JG>(oha8kpY|GBvo51l_UvOX?|7Q|$%e}83qrDA5+DEA6;HW% zX1M2mR>5nQsrfm$*DGID>+DcvT5di$a-lJS3m<6Kk$rY-#Kgn_dl9O@irciib0JBu zYEXSRzFGxXG0|t1mK+C*6{|g!N$Bd(l^W5TJ4dW-V&F&TIH5vRKmit~(-c`+BF^q3 zL9EWaYicWQmCSru(82?YS;tJf!p8&ts6jhmG|+YfDx^fT-XO^OJpvLY(1Oi60x_fF7HgEN6k7jHHoeq#QX2WP!R zoP@Imf=B@bIWnXL3nuBtqSCDAahunlJcFdq1m*aa_wNG5Yp=il3;>)cARszG+`@rc z78B4*O+YU7t)mg&*z|h!WMj?*QNi%Y`LM4ai~zGPb+9OWt!X0h0{Z&(Ku~AKsgwnv zdgE_D1-tPX88vkfqFVKIj8?x$w%xC`{&=4D2j-vo6m&!y6~aU@@wGdrYTs?q_wy-8 zO!@||bv8MGlsKgQs326cp|)nsTvr;aw)9aV_8N55P8e+#t!0*~yt94SRFBl5eW2eU z=g&t~&3&qwezZBXCU$18-+wkNLlE6NSuSlXdC2P|7ptD|(dm_3Jy@&o0K6!D45-EY z)NT7#YelsKf8UejTNWDJR48=A@cIpMHou!^7bn!j*`=aN-)RQ!`W|MKd|5ap*u<7QP?_Q0a_-eywT@mv8r8c{ywH_^{GgE-RJ8NljSv zUdh1(`tpL_84v%VNuXzboJEGUFU>9+7HR<&6aHQGFOZD-?ox=Uc_7rJ?0Y?1fvHbJiU z@)bx4T7jJp!! zqGklQL?5SGC21SJ{O|*x#!*8mD5=Dob-^-MQ2t?2W2jxK2Bo1Xas9FN2MB5hmS$2;V_%f$E4r$OCh}{7T(I@N>Z?rAvvsA>`$oMB_EN=@Ylzi8LJ`hKi4_D`(4x7ebV>4E6 zMap0i0a3Rnn#JKGsfaTU16yXCfhCU#9j%k-8S#quqpP(*Ke;Ypy>v~}H4Sm)DA^Mw zC(Kw_lKoNO2^i*!js_i5f@})VQHdp*y~Wf-MwX`&PY6~cMcqjbCq81=GUz$5qL1O; z7tyUZ_~-BKUP$u$+xpnC^6{qBV8SuLb5m??J4V~wRU;A&2`eIBS;OMng*plf8f{jM zwIB9XDF=<)np-@5a!2I-Yr)EsGEdCC#@EW5R40hw;4@|ZQzSY0F1R>+606r%P2^jq zUYWT*PSu;cN$)6L4|cApPW4CG+=<&mDqvf*LNmp|P@Q82Y6$_P{uG{Eo=#&FrNcDk6?|U)F~%GRsJu zJFvu?ty7WMHA;Bn;V}Tb>_zhbv#%-NRj%jRbcGhm5I+Jcj%x2Sk2%8t4(3j<0vt&G zozBqrU7`8FM2SUg7VP8I8yg8`pz8e z%$leOAtzqjGGJbTEA(E2L)OVW2b(`A4vCqtcJosTDLh}}#rdGcw=0A+T{kVilB+uH zyprd#INIUzu?MCj`yugnOhJ!pM--IOJE}*n?si;1#q+D(KqT_E$g9AjZh71QKIc$v zEo1sxFpLJJL$)96pO()eCoVK%m{$EqY{=kt2Ze@)oNkbg2#Eb5)I~*Ws-M#+!b{hk zN%AAWxLnMPhV(_AC;POe=?W7j-f5c`li&B4==*{MH?K60%*C3?xzs`4z5NpRb@Cl7 z19$`ay0wa(j^UsiW0IN2zaeGJunR_JXUXB?>uodgBOBV6okVlzxvaKk*h=@@X8P>E zeYk+02z&G0$SE7=X~7tHf1OTRKL#*UCkn;}9rMs74{6^9;^&$amnc7V z)hoyLHy^s&nZ`(D*rG?La5aPJA|=7l+d=M^50|a_o}ZNX20aGEi1GR9jpm<3p4?_<`C$(Q zl3ED`QPGOIBAWf1bwkxn4oncwO5SQ$s zKz2=nOS~R{RUc!@6Lyl|@0W{*o*1^`yc5y5z8d_i_PP!l z*KW;58ZqJM>=*goo|%&Wv`6LZA;URU1J9l1j@9EU+lR7tWM3v6MED6?Cv88dsHNz> zeujmX?r2B;eQ!+n&%XU3X3{b zf`Q zo7Y`bmyFjl z=QzP`#dVAdZZI^p!gYtnq0&9G(}8UcV>R+Ow~?yrHXHK|L*n7f8&&TjvzM;*i_9&^ zGksiAxpgVm(V#YIoo-DjD2wP3CY0 zDs|~9G&KGQvn})ertd)U8-t6!yVjX#?*RW7UM;`-9 zf>-M|My)w!8PS(E%z3{}l`caKlza&}SnnL}Nww}16VII>M;qlcl~HR2_pa&R6f9c7 zi|&Q_kV3#Q<{%GXuI^hb;q`)kz~8I)EE?~8#8>;`;|{j(HjOSSq#f2AcFw&3S ztK_c}vqQKc2ZOn7oP68Cga85;EFFs$cIA%#OEjrjLb;za_~u>#-F1m~Of}~DV9euI zXtXHmK~4X~jiMp<;0hRQuUU)lGFBI}WJT%#86qFyO=O!fA!VPieb$63V*W z%qD_z8kRSVwLoApqRX)GNVgAYZ@rH?Eb|`=Ff227u)x%0rx&_q{LMIsLR-&ex!ws; zrMF>X$3e7PGdE)gH$DoyqS9SRaEb1eMC33j_r5JHe-mZ)7$woUBq848-EktA6nJ1{KWO}w1 z@niPIQL6lW&dGJmQ=x*I?7FMHU=WN4omf-WzPh7z$=AgitgQP)AA!Xg)sB0qHYv5yWr_Sh8u~k9T%C&Uc#1tr#Y&{E*<`!t$2dj@-V4gsn>q2W zJ~lkH5dlK%0JraW;En;HW5ABmmTQT*gZmseHu(+_UN%Qlb;mB{6t6dAk>Vxer_9-_ zOB!$3x#!i%iYrY$w9Iqoi^yel@AuO?1DCQ1HLVzP{SmvxJFEt25_No0Tu@m1v2&x& zY3JM-_Li=PvN%u>c(JfK1Btd*T83pe1h3E+^77CltvS`z4jLKr%RoXvJWN|X@kzHk zvIFW#vdfL|Ukq2i*`PIP>*Vs<;=zeh%fw6}3hDPSS|cwoX`x+|hlq*xdOOK{-MA5UpF$(%*<>cOEG|2STJQl?yZPP*%cuIxIN zo}d>^y)$PY(gtNhkNAB=?V`W`uD+g+>sK`s-bf5ge&P1{n&5?^c_da!>13$U$es?c z6|R69QPVNKD9X*FYK57E23!qUNDxI}U-Z9id#et+Um33z69Bw_b*|D341bT}+lU(eJ z|2+4j9TwO|Y92vLhm;;V@DOXOipW-MMMTkjCkT{;;NE^~+AY_u3A~!v+ACgA?W0nn zB>dJQwU};9I|hs)YxacSS$MByf_+f?qx(Jbtep%`;T|j+!(C1_|8CEdZ;N}En#zXt zPXP(DxV@@=|M#2aLm)rkgi-lv3aKpfUS;(=bB6zezpY)fX102`nqA;Q>VYK;242J$1>`mA zc*X9pGjw*A5Uw@VI$;hei14%zj3b+ZxeIF08Cj_Xf6?o_jjq>Dx{Zn4udH@-F?4-A z(`&{E5TVYzn}=FIc#Rfc$WkBQw$~MC#x&Q|yvN@itF-i7?q~_PisJ=(Ro$|CEb0|- za>Z6%tw@qfAcLwlPE%+aL;8gle43j_$_8!h%^Zx}E8WVXqRaIsb(A(d(4AcA{zlYZkA?z06}k;4h0?nQ8dyXKr|Y z-$70fU2fw0OxLotbMAIt7;nBJlJjPdzQmFm#|y8Ad{GaP5^Qmg7N1?xfn9&iQ z_8r@w>DRU2*oJt3I)_XON&b`D$kYiFjY{{ztpvk7w5tKFMs<)U5JoW_Y?;x>5bn3K z=f0QFU1=M7Cxml?2gp^KnOH47>Fgsxe+)Vm@cFD3{!I>FS+fE;cx_8Z>(LOb8aL^? zX$TyBb9>jME`g;>I$0v|bH3q~kBKQscQ5Te0xPr+y-Tsd4h&izI#5<-x>xppzvwj2 zG=GPOiY=oIog0m7sx&j?`jj-3YJ;pac05>TE;kdLP0qRXIYLd z^E0d%asZaW8BX-HA@@`aSM#Mcx@3Z>PcM?v>g!Ha5O?^k?5rez5sG{r5wv z6HzqxRN7q5xvt#FCk}Q7^TG)g@tUL%U^0|}?5EUvexy8S?tjh9_9^_{6v07<_QkJ^ zl~j!$+Gj(XGlFAGf%0v4+BFOFy&6D&9K>lwDo+mIA3Wow+FlFOtp0I zb2gZ~ulzCqQe>jEtx=?SFs)#C6{=>2y|-y^c=rIR)M!saJz^k6Swg}l2vWZG&b@@J z(Mdm(x@(+cH=;necmzeATQXpO5)-c>uGjHd4 zZnnVl?LwCH!u1jy=)%4Y`kk?YgUno#`yKwzd0jh3TesLIZ~GP{IY~?Ni6#^W44d{$ zG=^4MS&yl1y1LuGzqo@1ZSE{K?j7W9*JRYL94W@`{6e_z2F=st^-%(D ziyCHAV{IL%_w^3mH2tm7+6NXDAjHj&m#4m@JX-T>d)qpe3-c&Ls2&5P4+8AUxK^zr z?~Q7L^4E}uVbix`Wy*b?{?{zBe=^Db?>qBgvn*A3OdGfmFW=G3N91+5K)ja*QOq8; zU&Uk9h2p=)l{?nGGUknjeDif%icdkgzO0f(vbI1&Kt$JYHHQ8qXLmgsLraE)1xWw# z^>K#0MDY)LRdflPiMOA<_RPT;sJ`|1tiZ5aVJ0vIN)M(ZkS~H8G3!Ui0L5R7t1=@{ z`y(oG78ds?FnrL2Y#&erEbT87UlV1CMVtiz<O6T!5VomRNs ztPS<&fJF&aqsINMu;Jv!pWf@k*KxOhuq!N5?kceb4l&(Rk&$6*QD_Y5z*j)i-lFXn zrtNTcVK>MqQ=@9~xjw-?;n^Rjnr?}}o_UrEO7}*1!ky3yxNk+WJ$4wqF2s6~zJbi< zA{k2aTl3BqBEP_J(og8mg&0d>G)?0?Q|&(`ZRzuHQtrWBJ!?+|dF&)iGH^K2exi;0 z&2AtL{N7%#<%kQNDs;{bR^))4Zi=2vz)8B}`rPtTeNX>r>s@>;C%;lG>p(rASWy`c zcN|_Z0^#hBS%B>v?rTfWCczeVZ>4y$?UI`y8&!~-xc++_2H_wjm+Y@Z^M;Tuo10&2s-Fq0JIoIE)A>Cg0% zlj{xK!F$NKOwpjUA|u660wb%3Va%Cec*GD$)dcNkTJ}Z_o-gzsdK4+NZc2_Xuv-N~L0Zn2gC zsMTBMLGi^hGfw)iqv-HG@Cin?6gkr@;xWOYBIx@i=!JpQOO3`(sRP*2C-R`XF;bVo zH`_yASavbvR*FeUjQ9_d3{yaDbuU^~Ea!;Oxz>&W=lup*J&pw4LbY{@$r=oMjS6YH zx_0(Sx`I@#a<^cH6fP!4>xt+!wgE^F{bs1pI9iXuNMpEP(=uYl^PV`1_k%W-4tFRQ+Ni+gV|8pL;CTQJ3>-42!DXp6MhvyJj0q#T%8Ip=@#vlw z0~}f)T6M8Ee@`;giDlUH9HCu$$`(O71Ca1WT4t!zd`@)!Eh2Y2hwwtauzbRqHGcR- zio$ft^w$yz4GmDa1Ta-f^-RUr(=A_r+V?!d$`th-)*r3VA&=?SBR`sZ5)6*8$AGY! z?NOpm2*@JMe;0pOeO40}ssx_9S=J4VYc``^Eql1;fcS$# zxVp@YU(5_`SweKx`lL|ET^CdW*GcxrfHxzp7y;G6&A}=}>xeV#`Ud0nR!REP;y%(R z3OkCYJ=%7vH7gx5Rxk64G5lKw>keyc$_&FH)VB|FgsE_{&-{h7PRto?;bL;eR&VDK zcBa3mdsgR`oy@aNZN+Wv0&AdvTO3Vlssa(Z`Z&(M= zTChv52vE5KjY@Hex+DD5D0B4GgZN)Ir!4NAX^vIe>eTCHm>)FCYlT&P})kxw!jh(?B@n>3|gL6Q@GV!XPkN$rJ?+-{*C;V2e@Y2=fbBJa`N21 z?uX7;9lFzRmFM@4Ao}5@J4&xHl|RS|A+wYB8fkgj;6;u%kwlV zbpZTeW`S=}bFh`NJ$Ye6oG|g00fmaE0-te-S;Y3vjTe-66-%P4s<~kbjc>1(BM>Ss zL85vY>d#yM(p(L0qI4u{F$veyEi>x>2Mk| zmfRb*Ka5w<>lU@A0O5nydSaJGJq#nK&mm*AD8q7wHc za_Sf$E5Y+CRGk)AgL&O7M)*wt$5m(P*$t^uvaYN#7>$dTZ#t?ViB8>8qg=JVR&92^ zr{%N>3UWi{Wuxw^yed0+C2$L}yIP~1Rz}9}T5DI+`>e1_$wbjE@YwR00$mcM4-Nf| zE35wDSphWkyXidD!5Oj0WX#MevTO?Z2|NZEj;WQg0&~oMD?&pR6`s{* zI2$CbZK>BMZ=U0exAugda9lim`OArx;Deiae>Fh_56onuV?kz!O_vWXAJX0Prmz^W>;JA zy%1%3us z8qAc!FASCkCJn4n=5}bQb9*$&vMb}IL%FGf7e#~a^Jb3^Ti`w^$NaS9-WmUmc_xqG zrm8cX$gp|f$5AEp9f7y1_6D94bO{qrVoiKqT*bK#ws`Z`@=dlsCqEcA3_V_!I?O)6 z46Ne2ryb}`)5AkA%W4lHMHy8!jkdmqOoYZc#j3e&;k6k#qC-k19-OR7G z>8|eX97by~=liuIvBvi485Z2XPLz){CKA&&n&z`$9dO;``^3f6WX4q_=6b7Nhz5a? zSE;0s3%a2o0RrVz%XLX4w&S%E@0ZkHdsQMp!pa5UYtW_BGnue&Bkt2Wh>PkIJzXEc z^u7lT-0hhPO6}Ic%EZN{rlzl?p=)XPc0BWIZVUHuNgw^_I(dej>GfgBYeFmk{m0JEHQ*_cYB1!SN;!ZT1ZH~B`cAU(SRcv_>II}z_s+A(A zBS7WeF--a=?{Z`w_Ab<{6ah0UoPk)`X$OaXz{=OA1Qgm&*|+%Yo~w+I=xtERmy6Q~ zJp|byTFiJMa1BD*+fdv2elbf7p)fZnfH&A7{3r~aXyyWaqwla5aMCv8_9E^kmhV=7 za3pWCyDdx8a^Jx*K!H1(8b)EU2^=54P*6`O|NecF9BnziInkcYpI|$pU3S}KM_;n) z!JEpIxYF-(>Q1XQ>G#Q1B&kNCVMM6jmpcf;@~z>$R}#29GOsPLU@9GJ*f{(ggkC-1 zEV#G3O#LM+QknB*rHK7Tf#60NEUA*-xLyaG@v@l469W+MXzI$^e7O)Io-s_#{zy4T z(>-7&z;hT!(HskZQ`?g>1HGK^39pPFyuW)4I7ROT6Jj8`JdN~eiXrt%1HG}_lfpk( zgK%#eIkeq9gO}gDF+D)b63`Bw<}II?hOJW%9EWED4%g{0lZ3Oh8me*-QCgpXkMb4t zBZq>5O4@WYi~sz&PdZZ7ZVS<#&9JKSS1VIzG)h-~hA*gsJ1r zYjG2Ks0GB}j$tg8-hGttYF~_NiY(Sqv68fM#6k8anV1>b8*IiG(<;iK^IU=TTYGY=~*^O zXOWYE7NzWPv)+hyHwFSQk)hrsibdBz#NH4PP#Xhx(cC#?^ar;I{e=`M>-uSL)c`i< zSSh#KE6NsZEr2j_gGZk;q5CutA`>xuw`b zEYzgP#(nVQ`wv*Zf+VcYh|g4*W0b_ zvc{uaMvyM`)#^zdB-Oh3VaYf5D&}V!4)5Fkl|=dH0Ghd{Lxl)Y1x5IE*LIDJ@`{jB zvp~=B5hWklln+;`0KB!VI)L)E*nrhVSZYSFTk{<30KZ(`+)&lFhoAK6z>2lffIou~ z?KY*MVzewD$Naad>Iv2(Ru2y4C9zX)Qg1}?FZ~ljB zyxPE72AxB&mcMiK@@*tF4l-2Z)txAYYLu@Qy~i6SYoI2Tb+k&>Lbi)G9$*&fMt`-n z2uKR^f?2G#Bk35RK|g1rW%h$2orv1NKZhh^;0uiitR_@&a&$RCoT#)RrV$Ijfg?9> zKOxk#R1&pC#ZirQgfE%xY!yc3R}s~A%kQ2tS>BEGkFQsV!tdGh=1X5Fe&>gp$yxdU zoOu^9xqSfIaSbQqNgAV}KmvWc?FFRyeEo>uMTqsdm*WIgnpPrY9OAp%Apx+z`+E?! zWb|FeDeLbB3#VBCMKUnjsCHrxd4_r`M2S1FXNIk2f6Vvw_qAlB+KS13ZcnR&NYdXtvin?S-h!s*Ks;z49%MA^w#li7SDz1sX8&6IlfXv8|1TVxX$&P^p zcT(V$%KDnq!>c=>N*(k!5tU@&E@8mW2o#I;g;MG%a^$?bZ@kwrV4gK0H?AC9B z%5$0rl{&^hQ+D#{!8hK#3c51TL{T{gKv%*CDKJJTDs~Cq8A9*M*|+wd3dgoaAww+Ff@ZoOV zWys2!disKp>E48f%iv>+L(-a4piDnzJMS&%= z>M?U1y-lpvXu=F8Xr6<8AH!|n+=41(A1XIdCq0 zg4R#5KZI2}>&VyC#VuFS^MpSH+MDt}O!Pjf%w=pb$AvTg?_WJ@%#crNhQ*Z`%&iR& zU%rHx%pi-xps2(G?M5!suO6+sd)9Jr@PzgEUG6uC+0WN(14%ky@EBUQSZ;F7&To1; z;Z4W;`tr6A=GM(`9$Y^1Pww-*=HwC3YBsfY^}tsp#A`E(BIrEebJSbI`(Y|Q_R7_( zUkH;mjqx{;$Da> zr0B3m*+xYkp0|cJIhUj$!~B|3ms81%B>jMxocXY0z`Ib6`F{B}q3p)OmVz~+N5gp! z(gBCpa1DA$?g_5iM>m8E&>dcku zBw6Nug~EK1Ar6;1Y>CX-8{&Zgd8p`tfs^6Qj$!k^NeBL+70odP0gy(i;iiphD}&MA z6;o>WdUKh=$fTCX7#?2qfqI>mQKQ604H%{R6hj;?)-^VTLtN+`gk_RJ=eM}-MfDr%Ihc*tCqBiwfvZ* z!Q=Bv{l!C(&$GcMe4lq8JT5H0>5&v$dB58usdBjUL#xG`C+CJux67utC;Euf=ShA< zhniyCGEtnS5S2*dqK z&z#4fqFx(TyXf&ZC(2HC#B&>ZiK6qTo}XaAw{ebDie3Dg7xOa9O{<<%t}S0H49=!Q z@&+)atTbQPT{6#D@@oMqWb1k5hGwOA6S5Ho2W{bZ47YXz2uMMAudWg~>wBOb;yih< zRWpt*>gKGOf{8FGuVAU@Z~9x&C=k8j;x?}O(KFG8^`~XnyLI@mBqq=C&gdS?^ zYg2u;d_qM5GMTw&kW={KhtO|5UFaQ(hNubD{K=osVV-xLgyQWqDvc$|xAu$``rYQf z4?|lI{F`tv1LqQ05x#MhQT@B1`M+0~TNY5z6>`ZH^6FOC)|NyZ{Qj+Pm^rZs$^nLa zLE{*}cT3<~u^u6zg+{?sIT=B)*nBz!P9?DHA^%Wn{^daMWA2!aZX zE*B4IGB9VjE#TbSGYpP?j;C6R{mWdpl*}I^{>RrDQpE`fI0JGsOEEO_x8EG2mSSYX zSvqZ-e#+z+(7~9EjVgv`_{e^CW5~s-|9<_yH~&AG@!zcXZ^8Sw+WEKA{r}qJ@R}Q) zh#eSHTGR+1KgCl!sA7Ej?-z;JuScX=kp=ps5abct^f3U>gCO#5$Y=6e=}m57R>T92 z0ilfA@OJZm8E|;#6O~cN7x59Z^zIli<#P=19y^4Ai*g9w8!x+#0jFVo^%)EFJApH2 z8M(9{5GKVkjL|RhQ_aXvvhDlE8R=Ur13#k!sBbj49)UkCec~>6t!(poxeEe;-J}=) z90T;Vbr!hy!7UrQVH;t{%H@Nb^|1cfkGk^r$CsK1NT~F~Xaj_qm}|o^;7dH7Mie*( z@EHEfimh|$d5nVRV*oXcQRt#8PnV+53-k`b$AG$=J{>-qc3Ad0JzN8E&<>^={7(}d zF{m(H&Fx=^G6ECvV&H@f{aELPJCf!uTH@ z5-fB^K`Gmh4sNPcgSw!TJUxqBo9H0wn2d0J_We#yW_nRg!%@}#02|Sa7BDx}%eN19 z+{l-~&6m;tp5Y%I_V0c8 z4_*H6efal2{ExQ%-~8~u!4KvJMt$kWfWJ<4j7WqVVo%n?BMNaSxTUv_6{DTD%c#C2 zgUn+12)=-85OD}+M$kK%{HU!~vu?u6OQA4(XLxIst2HkZA~^iV>-@A)hq(VCZ|<*~ zrI`1KT2um^mFmj#geo(x#trxrs0lmg5kN|NDEyZ5r9LT~qal}nWkcw4x=6#1R#~Z! z!qVwt3XFujUpc$?5L+4y6mz6Ei^Ck2;tyCD5G?XB17OAMmmto$-cyv|yE<3=>{}N{stMqriBPOby;lEDwU)X!^u%^0hUlbKZ zML?t}D3LDG6e$WwM5T#z=|YquB?3xE0uiKF>C&ZlB3){vD~L#!Py(TsKtc(D6wiA1 z{r27Ze*3uhbMHC#InRB*e<5qlHOKsoF-IR)s1pr<`lglrlLmhIC(V2!>|(en&A{LO zngD+eg!#V<>HMTYzJ=p^zvsNZ0Vs4%@D-LKIQlb~xCok*1@E330dFmU$SyTEOlDZ{ z!SVBGrnXYMR|OemtDa`f5qOIuly_`y+oRM?xkD?-!5hGxOdp*Vh_(-&7lZ zcuGuM&n9(7=b^r^o>J5&0pk}xZVDzpz&}f_SVLt5CFMW$_RZ;i>mc*}F@M|-HGEl7 z+6ND5@%S& zAFkX{@LZ_dH^%ZNqCMtE2vu8kj)@Z8C`(Pg2LX<`{^|xLiF%b8h)XMipmQw*)I~qD zv>ikT)8BrZg|{UUK!{hn+v*v$O0Scohjw^uWs zlnT>rt~$wk!P^>jGVCVmLT|8hqKj8W0rCzw;%;?vC)j>z?AzwH=&S}PsR(^{qk(%q z+%w$z8qrGE=B7<;LV*0G;EImZlkJu7QK3hRF1>z#8vHAqVt%ERDXY#2_-{DK{S5>%3@k#5O{4z%GWBiugn?E-6 zkpWFy=WCj!96ViQP0Xp4dyht?&1E%*P1P(pJUxZa9AApH3iGU_PmUdAwOz8+kyLwc zh5Yqvf8)Kfi8aH+uye zq*-$g`%3jg{iuNyR)s5P%#({8@s>Ge(`I{NLTK7ID&Pe)k(f-fC36$6W4^e7&clp| z(7D?#wCV!GwOK3^#^&7>vLaG?y%k3;2B`9;4Nhzdlr+jCe+9~&7Vs#M>$nnozsN1&ZRo55Etos0^$?^uPog!x$08eE#X@;zTVgJJg;!7a~N zpTep9{^m*JT*uf`h8-fu-n7wryr=t{Nc|mux_2)8r1={U_>Zdt^7SJiZ0&UKoCS(0 zOw+kALW`DVWlbjM-D$al3(MA}CopL^Iq7Cb)l#Pf+il$g!>E5W8+{z}+E1RSjR}cT z01kuRNP}Y4wv%6naJrlXu4>|-IblP8`K!~ZyPw!Dzf}=7ha^`3Y1uQa+cr15r@oDQ zSUB}hcu&Z#q>SqtgU(Q|*ul@k_}H7~`nIr_uG$yYtCLfBrf|Ku&oZA&rqm+kp3L*{ zornoWvr2&@S}*#Vb&R7?oG?$2UBiur38LmhY1mPGdCs4XF5o2gk@+Eq z-LKtC-=`a;te|U=Z)&%=C4wC>_zz<-Nv1Q-wCg%2^;GSKOfF%Msxn)@;D2^%TeiO~ z*VCFyqy@+cuYxhn``*B)29`-rvoB*)4GYhvqTpilJ1hkrAJ52n9<43PMIfPd?e`Wf#T?S?$6 z^i;6ao$zR6Yj#*HCShx7>LH0&JK>^}Rb={+sqpdK{J-iC4faFK;E=I@^l~k+dqs%} z+x>{BnVi(T+Y3AJWFHo-R1bJGm+L; zgO8a{lp;s-o3EOr{V!*&qm-gXsuOiSiF=+sd7_KeP&-QDWjH zb$dp8W>AU-RUbi{(tTJIIE#1I$Ej;#Px&dVpiVxE!S2t+tGQkVdt_x zMjhfI6IHHok2b*b1tTM4-7(X@b&_QOUQ;b43CQ-?=y_@jpwoQMFD?N%=~w0P`Upw}Q5x3q*^c1$*v%hm(HOn*v!9=4G_|&uX&{4J zFO4W%_sw~1CwIF$szg()3mZ80@{6MRE7fVQdu z#AGYHE5VYI;@+<)66&}(>>hFN;@rKwx~29R-9!Pk9bVWq{%dU)Id>66nJp?*meK~^ z=P`iKV#?1Abi7h+<6@8-LbbPlyY0Z`1GP)K^5l-xEF1Hk(=;nIT|oZw8z6CEsTw}T z@{`7e?O^98O+klQtKj2>`WVzB96R-r!R(4eIAb1b;h~O_a(n;@YeK3nuPMk zg#4s&K(ix*sGJbun{F9A7pbX~iz2K1ak#swte-t^?DMLl>vM*#)=ov+iQE9)zN;}G zk=BD+v#>B6i4aA6gF%PS@M}W46`67Und$Cc&Qh{X9Ipx@XpcHqPgi7$o_pHPAGFYv zB>adRPER8b2GTPz_2Fb5+`T#2%vrqa>S0JkGh_~XZrMyz@)Y;d(56njT$`jMpImxO z@*@AMClqF74WpO7>1I3X1>^l9V2f;oXxwx`3zw$Q_7qWTpO8Xq17RnGUWY_0u;tiO zFOJttUq!3n1uGc}O6PBjtBIlJb;7%E^W-(8q-@IcLA{-6wb>_lPDh$``dy-qfLL1D zso&72U?OwcY+fRBby41P6zxaV+{R>#GKkUDeb_d%V_HK6+OPAFueWZPH1N5(>M zY!+{rmFpGX1D@o(N0e8p|5%r*`f<2cU{A-PiFDqMXGq7J*1bSly{NC!`B|hID z#@QoV2Ntm9A){bH|~KuLFt((jt##T7>E*%ve#@5L}$Q0jVg z>P;4mA8Uz~=O&UrWG{rD#CRN!z57;VY2c*a#TTKJ#&C>6w%~-ZeF;K1d-_m6$QKkE@|tw;xlUdo1y0l z5-}7r^?DxT4`K}EUv+O-r&kN}KRI{jXw&6u!hJLGNz-B!cv1%E$<^F(z|Nb6L!|s( z!5An-R0-HqysAH$L$-6Vo;rdD`X|V{V#IjSTBCv^&&77_5al-Ep2M?sv2S`$_a@nM z6+zE8`o{IKt@{53iQZi3b za$&kS_$RzflxKlmU8-lhbMVV)DEi0P@MABJUc%ZY{p9}JZi`0&Y?4>MTAUBH6|VWx zqzY8d*1RA-|4GB;ToV&+gnEZp?G$5Dtt7tQS(S6lW|*5)KZjY{)VQ!=6+}jeYOJu6_7wju?LwsnBvnX8G z^XQjyUje}c7CJ%#F@>Z=Og-G`R=kO00&?qX)V3ttF5NIP>o;ph#l{{p({xHTPc!a5 zrdB{->}1PuzO(Gub&^bY=(ovo>H5Se;L@!3J3Z$4{Dq@)Mevn;tQ3->Ln z4n&rPkkdKH#!Lr5_3`J19Qbn}q2}NOyeFCZ5bj!cNYV;F^{Gfo!H2L6G>JDM=KiFy z1|l@zLv=_nsFI1C8Pg6TU2=1#hzBEGkd8i`qhK3&MAWfUeAPJiri;;qA}t$J*ny&x zu6t}mDCsQFGi8Mg$p6Z5m7g?2E6^RP=uaBIj-NE<+E|HPTXNj30U*|+c>@S+%x!J# z;lXp-*z9=*2_0e<#+-d0bAQ|6l|&d8cB^nr*Wr6-epKX;HhoLY+snP(C&LoXWvjK3 zKy!3&6z7hQ>! zA{S=Ffy-21AJ8z=;k(C7rL{ZRV~Xr#KM0{SXVk{m9{FqJLC>zsMDS~!>iL#(kbArB zQk2?&1f@(-2m-q~Oukl%0TJ)+-iGMQH1>pPHtR)kGM|YKar=x@UQ#Q{wC?>ZYjuIQ zxe$|uWkGoP=pg<51Pc>1ELTK}G7Yk$x+xQ=zL+w* z_e}9l)a&GjV+y(zkCidqgTvbR&9u4vq3C3&=(Wk|2~(+(Z~9*)Rr$ZIOnw?*y!3)r zE(UuX13$BN$QXnHg@q^$TjEuaISv+lA3uBuw=c&!j;V30*!c~xhCj9;e+apVxQVC` zNyBTmuQPFY?RRkVHl*SuV1oM+cCm7yc{jQi!S7DTy=b|ndfRpA9f${}MyYImnF|vg zu5g5D5ljhLh0_k-HNEOQWT>K5(FEh1#LGUn@-kHdq!Wiu)EStt)t7weamNfqLztza z_~uoRg$`SO3Op5Nt8&lYjeStHFP!Xiw&EYx`}|g2-ute7I_TY%DHDF0BYU@5kL(~e zI`CFN1ELW(<60mM-+}{A>zO=WTv!k$`l@~XT7I**q1j3qmdAw0!-Fz)^`wn{lD>rz z$;V5-2V5J{Q*PDhBAoq?9)Iy!harq(&hjmCp}q-7T4>AQT@KyGYZ^njPjb%y#k z%LgAxufoFwr^nXGsbmTFiM&HL4yO#{iL6y;m4UBF6p1j`KzR?h-3krZ>gmVrtBv}d z6Re!uVu+wO5x^(*_vil@;eD1WltS&rho*>`%7zBV-PQ3of;;@AiAgPte2Rwz?QMn9 z-D$)vM=D&h%JDibDyB^sdpCY4g+`TOnHyi&dh4jOmLBz%zod~qzIvOjDph}}F1ZDX zT}L$7m+!x_7>n|@;v(OE)l^iGL^WG|cG$9oQw>zSM%?er;-15K1Yv4$=7?~rxL1$r zh_h4%-9yr&BtojC=CNY}O?+AvM|Sd6pM7F#ra8d`C?L*E7l~b_G(F3!(I)M%*g)<<-GdTX*a39Q z-q*7wqR{4+mbn>RNby;l0u^QvclL#Ngv_6H|qmA2lXtFiuwA~ zGJZgCL1`q2Xw{*}Sx?-0?_juJZwe1B9I24LWFIwF z=}ZW<_(ofA696UNF%iVvjQ3!6B`AiG#E9A*rmcydh3Xi}Z7H?#_f;Qi@xchj6j~2_ z-b+=2Km&W%(VTz+Nv^n0ajq+gtzvh})*f&Qn+r_H7p~`;K2O&-#b$D4X2`wBtdMaw zM^!^ywfhUkGT@5f*(mU~oIRYN1UvGRCfi|jmjml-%ZjYo+W2gGI*;O%gw3fW59qKk%BrQltH3lxd(b?lJanM zXyHwOPdEC2H)a*y@s$YvM3uwGv>_u}Z{XP@Fr}eVnnr{$?(-uG@{}wGLTVsK9pR+{ zLWJj|iEen+`Gx!KZ$BS=UIkQwEOafnD=Q`oRLCukhrE7T4+WClu!y6o>o?R$iR{gp}x8K-5jGJxiy$MFWXG4}b#GIX=BBom=752RvTDI_3*Lc!M+|DA> zTu}%}2e0)4cXP{AvK5(DTTj_rj|$?@5u9tiDE;G02+LdzXp@Nj4mIYb!cny$J;t$@ z`|6C%$z7ny+|I+oNSIJAPmA=FfN|9;wwiT#0&tYARR=w~-$kX-KM(9yh}k!CcGs118_mgA^dLw>B$X+@nj zr7zv;LhsA%YZ(n1wWvsz`ofmNT)2Ir%rc`z9e{mm>f|n9H!fr8@p?VLXaU9ZF|4a{ zD%Eor_MFd8c|{mi`b)LVja7>k^t@;@(35US$?-h@wk!W@m%rsGV6wEyQ)SjbH4>k; z&T?&WthWk`NlmNouE(4$?~Z{Ns$Z`Q>RUDPV6QZLJjoNkDA{Rk#29z1o((9KWoSKa z4|HH@_Q7Pa$`qDCXL#_>D?#udVTs{rGvM<1`FL?+98K~_hxQGw!`zG=#QAEVSQX)eZTKe%H%3I99}Aju-> zC(Yr-y<8Jh>7g~Cr}zl}ZDcZAqnhG+g1GUUDcG#-kFjzZ@4qHL+(&FoGlrC|p)O&^ z-&9njPMVFCXT89!IObef3>1DA^e)kp)*E(?5&9)3Sr5PpkOFV6xfzdRQfoMq=M*rH zIfJjC{Zd#eQJD~<-L$(%3FIt~9IMyqa&>g&QkIf@@0NJ}X<`CSi8g@g@!->^Claj~ z%FC{snVG3n-aDhl%&UCfz~CY0rrLWo#rDi%YHy%uc)e(!smh)?Wz-vXqQBHun;3=BK1M!`Q*No< zBB%!JNnG^Ga9>}-v-N~LJtqbb1$*8O?!`)Q7k>XHWG5k?i|GXu?frcPcx8xfA>#7TlteH|Vgiovs3r1kkY z8mW(N3dTXfa>^kbO_!6@4HO}_4Ky6joDvdRaGpSS8!8^Bq*B@KhKR^6ZZ2Hdc&&(b zsOK1}AEWvHThy)UT5k=O!Y&a8+n1%a})YK48dpqO}; zTP!!36f~|U>KTd}3m-&_EH_Q-ca2-HJG1oQIeNw#BaxgoxIA_4Nh?xV0%cv}odH1#Zq)5}T=FA*xyd?^>ER8@-{snupSS8~r^9bWx%D`=1B;!fL< zk5>!K^WHyqGVcl+podZ=@RqPEgPV%H7Up)`mx*tRQ1@Kt2H4Hlm}K5H`t*t}j+oxG zZkW9MLZHot%cFIV))%d;d{HD~fAMPznw%W3Ep-6iw)gbxE-(<*?3Ly;T}5qbtNKj1 znojx@?;4WVD0!_{r@i#skX*^S#3(#V2l6agtSCZ)-mdP|=!Qe1oG3lRr%0CNFt$8< z@$~9okgxU(it^r140bo35P}a*nh~qQ`4vAE2SS`DpNznsdgo0JoQjr_6bLko>nTnV zkmUEDO6>k#9M!WX5r(e19nuXtt1T1?dpchVNVMr0WuKRGuD(ZCZ`Ala$USm0y{myO z($Aya#{*$}>}XCBr)OlLSUFtYS-dXZ2WezI^X^ga+DL|4u}fFocxKhmcx@as`B{V2 z&{ZP$abiX;?0&--wx)EnxdAu!9;H=OdL}jknt$ITl0&!Y^qNb<#l6qc9d__-A3wl= zspL}wteI)8mS^D`wOC&)v1XXO<(zn!x$BgDMG+Yn2VJ%Sw6y1SigJ{UU3s8+qv9zd zUD>MKR#;&Z{_*%L8%op|LMqj<6FjkB&&-R8U$}1-hq_#)629Ox^UWlz9q369?rdDh zavo9SB|`4uX-ydmB_igMd+Hm%6+gWe6!B2%RJm1jKemF+Pc6LYPG5?wh|$y|gL*&c zp5ZRq8EJ9I`M@(SZriUBjrbytPVQL8d+JsanSGc-YP0Ho{P2o+ScMk~<0Zxxt)&Wd z<;C8Log3r4RahygbXtq=&Se%J#-_+OXt-s$;l2^kz4VJvC|MR`E=01>-bitaIj@PS zv^f&ZJGPDPXLWh7E*$&X_iHh)IEK}za?of1FFQ`9JFrJ>PBtx##cC9iK|N-&W4+6r zpyM$8+|^XS2#yf4Yxw|PjLKv2N#O=}y>O&`fB6((UZtMD|Af5&1F31Y>F^Kpx_?O~ z1yiKJLy}mt5W+Ks!VFx!F%UNN%9ba5-mG7+z5V8%PkxZa$_@IHNAL65TWU)M-=|%? zbGQVw!OLH!8bRb059_CA5T)>OGv9tHeJ}bP`2yaf5^piw_KJ2!beuyu4I@*TpHkz@ zF#}2KJ3jQRQSC%QyL?2bXZ;u9j-Zpetp*M=F+D6^Q1x0s5ep|j#fVAFeM^~lv{fB( zUypLa%&eWyIjX@0&3Gu+&#F9~+;4C2uIG%+w{5QV-mB@g$T)~}AEF}WC(Tixb$4{j zBKv}`ZR7Gb<+o&_W{hK{gR0W{K0-L5T)s-WA8;W2X=?~VwEDfv*6ZNy{de$uc{qqOrLO9QRy#N(QtAV0Wt8#cT>5q0r3 zQ-aJsvQ{W!!wnMV$2bg7NMnl*Cf8cq8n>fgKPEV6m8PpLv@x~=xsPwYVg*e$I4AUO zkVeA_>J!8|6W~tOFyYFk^1Ug5%CGBTwm;RP`kWm79B!kEc{=j(%16Y__rF9o}qX z+(YnXW8Sx%pD_Ytikt6v6Opnzje<|t&kMOvkkJ0~ph`uAcNvk-Jb@VBVZlELuT>kG zRK=%L#o13SbXwQPQN3~#f2f&!^8H$Rg{v;;SeKi2Gy>y{+UkfU`j}-fRw95@B(&pxcU6Pu(tSdF-1?1DU5 z3jxNX%sO{acuC!_X8dOt$M!@ZoHIh}Dc@mIWWO=Xgo-;iL`vl-(uyDx%^>PPKRhSjt1jVN0x3z&1w$keEdc`ib2OS&lq zNE)unTzyXx9pspBX|sOD@{QZK4eTALqgbDlAkZf*hIP(pklk^R)F!*^G-b8Wu@ywU zf@E#GXrZSp42d0p^NKSUkHEs`N;UZ&H-)1i_!E`6>zr;`XC<86Z(^dy_7~IC9-Xz% zwyD=Y6Z=^J#hR6ZW};*{KgBV1NQ7`21=mh%X+DCfjMPwEwbD8{WS!4MFjlNW6vT z%`jOe(_A5F@(VIED=TH&JfxEz;jai9q4ID=DL3EF_9Au4C|<4#%9xKETUt%J6?kt; z<5I6#N&2~S_NuS;7&J8TLLD(bVmjj;4(sx$Vh~0L)BNd)CXb|{ZR7m(JnfGOYUXvS zHLpuK=h-I78boC%-jt+59Uj@U8FDMlYe`e{&(XMTXgpy7s$sd1m~@NnF|S&9C&OG1 z+t`@6Z1x7CJi+%|I3%KpR4Z+uS5Y#l(O}lHfS%1j5JAFbFrn5KyrsP>aUb(A^;sc~ z46WCVVVO~tN=?0~>k-c%j*C>A2soFt5G2S~V-<)W?{;P(rfJ~rXA0mABj%>TrbM3t z2UDl)64>2l+#@5-^v~U!?r}4y`%SSooM2&QmLUU4ZC^lcx@HeN9rr!Sw)MTbM%d#{ zNo^tmtIqA9v;}m zoz2mzQs;CuVJ$wQ{`j>j&zy7kE7*(zPJ%2V5vs_Kud6L4x*9c)iaNoRUlvdH73GIF zA6??`G*fRL`cbdTiRcgQZi}hfoP(yRTd7;ud)_S-eX7x|M(TJW51-U2Xf~QRw;USWcrGyJ*TrdHsn$E}t!rvF*=>3^Hej>t)8*5T*cl}E)5m(v z8)0TJ6t-$3_9d(t;o}Ep{7K_VLIdTJxM~a{ssJYu>JC3mM0OmGhh4^*kABO~pY%H1 z@8TSGxO`u4!Csaous?sJ>gog_s2$Ip?FmaYJX3DA$L0KsE3!B^Gwb$#zhl z{iHEOejKl8zQ+)&ks+NPb}8R2;r;R1d)u!ARr0sdPjwBKw|4RYo@Nu+7kA`H&d<3z zxCr>tb;)k%{(~72`Hy?_YK39-U6QAmKd3D7UEa|fRDMPdMmXO6CCK{k0oM6WSJI?% z3~9|YHGiZASudv+ciuMgY{dyBy2?vkhSXkQE?`!ZHu<2Sq7}C|BG*mw< z7{`VLvc)CGwi{pS*+y*%5p2vj1tBlB^NTPPL0Y@-=?lQHH#7Gg`Bw-b` zCl$W>W^g@2%Ya)VH|Mhv16RiiHNt%J)dko_SCGMQXPX138=zUK7w=B%l^Pn=JmnCU zw-U#)rg^B>JB3vfM05wsrQMIN%?P*e&NOrZszM-06-b+N%%U8!?bwnP?-4wI(&Ur^ z8wxBy{G{nF0(9BWe&2r5>>4mxa3ncwH;a7v5KK7+#C5j5 z=l;h^M*Ug11EvWyHN3o#dM*_>HqKlsh=&B|;)4b?faR+X9Mt?HIP?j4c^8Zmpg#Rc zb9OQUxnd9okev;Fz_JP^D(%7#PfP%HE7_{Qtb#}b&uxRMkrXyFZL)wgoGPvQpU{aq z_6a_)0-x0&cTjoiIpE(*Spa_oK%8x%Avo?00E1nI>Yp^oyD`5k1CfDuEP!p{hga*e zhz}fqh>%0fe?lWl`D*}%-+_wHh{9!zjHP|+%}!$b_8GXF_)w@jOQ5;%^}S?U1wn1Hy@cbNey z%hBUUl~R{*lYe_7|C`ae6qf1(!l zIUSiALZwB11!K5gdQwhTf{DBUaCvuKc>re;eo^)DNZu3KRJh{2lzig>l0F z5{$29)#c|If`N&JpXZ|ZqF^j#bB~@d?OrIw#SnOaDBU0tU_GmfsGl_X-^bUSvA_Wx zw;(d(IS4kyh&c)$#-N;9UO}|dA1*5o33gYD>fABaxM<&kIoXu*daCnCIxtzckHBjf zPxwx}&l+SkZ+`x$${ny%NImFVD__`4Dd@bN+zmaAY9xg3aQKrVs9&;z^_%e0JBI`T zk(!dbI*vZFC3+8*cb_V&0@EJ>PU#Hgp>8S+s<0*gn2>x%m@%!23{_$Bm_Gg~Iukv%=TB{{>C77K;uI| z%neKzQjgXHslK_t-5#9(+UdVtJpad^DP4e6&@JjW0N6;&8u;I!^BW+ej(&C16=e=o zK3#H+=KRqN;C}l9063JMssEXW{^AA!Hqalj>&9-K@Cg1Vj4FROiPQbpr}SL;jdTA+ zm49fh|DwwOcN|o#%=(C(Ro4QJ=OZNG#ygx@#e)=*JsaomI4Sl)oi^VVbt_C%E_Q0o z-mRv4;!<))@b&GZ;Y7MwG?`t$E9(MPglq;lCCOKT#CTKK?N*ZD8&9}YT+BsrCr@>2 zlcy>)cLK4t#|9o}rVv2%fNpyih3;z9ipBWyJs1|RjX2_dym9S8a@@mf*16OsWN1^SaFnK2Q_HSH-TU*#X)+>b2+uJcjAmn-|%b^dt-zP#24C=pzM5~2T-Mh^!b zkZ6H#15U|*d-;ife>p(L|93Cf1wI{~3Hj^O?@pYqBPB@ z07^XA5JEipm#W4GTnAaD5@cGyvQ<4y_NRLLt^*o56VMh{3@`|v%TkX3J5B}#g8xPX zz|#2pPW-mKIb+Ba6ZkA2pjs(JsZ#yQkT~YQ?Bg{Hx`V}i1ob``1N~S}_5Xk-9*0mL z`?}|G2wXQSMMC&!N8&z4EA(#wV?-Pb8+oz z%x};A&Y(u4S^fEc{)x`WeB{)LZ!;p2a<1N{GiKf*%vEc^zo0g+9~NCrpm z#Pk*nBU-1A2*4Vc!_`v0b#nuX!XdL7J*%$rzZK=duz&|jfzl+3oXyIrW*CSRs3%#J z7tN^VtzP41ol>hW~JX#q^qs z*0`qIBQ;}k(QKTTNcTw^qiG1vHQ`GcnH?t+`(iU`pyJ-n6`?-+r;VCFF5*9M-})r| z*VUqg8=ed4gdFM1@ru`|Z^hqR<2DbA(M>RacW$iDrqNWaT|PN>Q&yuUC*f^euARk$ z3avunop)tgJoA*O3lsb`&5qN%PK4pgX}=CQ&d8C4&_F4NiQ$GMg2!H&_qXKtQwj=Y z1md-s?3_S-u1Smsr-^*Gp2r(gfl~8;G<&(EG>X43kbHbtn^8_|2p3ro+dLzCy>;=$ zw-81)-KxT%s~ljDW@o}pd`K|dT6@w;om&WR5$hvRQ0eaMCyLz^a8xvL%zx7--g}Jq z^SrCzJ$G7s)4)#}OKrw=_<7`Pj*Q|td_i0s89cfvAyjQ0-$!~jLZem@)s*r2oZNxe zhO+D})cCdH@SZDouf^&4+StN_+;$rFmIL6k$OzmpCO88V0X||Uo7#6@)TSYPF3!a9 zLgLJHf!le#PfuO$xY5x*$(lmGL9^B%qd}+0((_^r3%;N(Q}eoVYio1F!T{%I|Bz=# zUh`F+Kym`-ZOIh)>O@E}bcT;8OV@!FQ6G69z?ESOiL-a+wY_4aTpM>c5S$#<~!~rOYZkIT8LzL^|?}!z4Fvf~j{pdOxbX`twrL*IiAcWJ$`a-SxErml7FZQ8uw3TLe3xpN_wV)~LH+^>tumNn?Y6rQw{{LYyz% zlKr7ms?@Hfai3J7TUEUY)9$ekShGuW#cR-l&b(5!?Mua+d#+UOJq?m2`6o?Z6ms4T zP7L=b|JDMy&L0bciMs{@Qi);DLr7LSfO^d3Or{3|{hTH_L&(HQ_^b&~Ty-@jwAnG& z>+br)H-@7%{o;;J8^-p7)6@K7a2JF0dnYZ=#IdP5M#KkNMIJLv*r;clU?Ow_o^?eT zG?5)x3|uG8bbzk|lvp4d&}k?U@Yn2%vwMhhtEk)ilM)J`;wX?&%$GQDl{uG}6Rf!k zaQ!!Y465g`Il!j}9A{!Dd*KTGYbjP5qtcKnv>hisF_S7${gA4X9=SHWs1w{4)!ijP zBsGWYy$shfHHR!ZK@VNl{ZJdyU#F9OU|Y~vXl--G!awY=Fi^qd>HNT{)mKaLU{p=Z zvmp`EErO)?_g?>b$FgH*N{W>lda`z+<6#{U} ztoqxoQ%e{;zSMRPA73mUi96ypmg#V?e#$cEZp!qB4}2ci->N!fNLW_v3;gLF5t`#= z?h$D}93{k@3WjY-(o-+KR*Y3`s4Fzk+)|m~l^K3hJF?50hS6p=A>Qk#Kdj4@o!F&_ zqcoa|J#HN6s^m|9cn|2Bvqyc83*YQk7#eim-n?0?zTM*j$41?w!W@w}AlD1e3V-eZ z+L?C*PzInPv;xwGyhIkk({-|QTEv!6DoBsMJCe1b91{gG{lkTpmD)*nPGC;{PdGv{)QDR2RO^_Z4gE)evUq3OQO>h8#R`1WxwzlM)I)@ z67rq%5Fh&(`1BnM^7V_CNMJS^waM?*i9DdX?mw95pH_YFS^P8nH_p)gl@0|Z$((yp z1(lA4ecn*^Y@ds9lamU)f*;igVHa3#J!u+#c`Rx@GbiG5(;pEO&hV;x-Sp$IRda2; zZgu#rO)*288%h_0I-R~`^p;t@73QO}vdS*8I7tk74}181QXjX}kV|}{X|h-gZIL0! zf26IvrEVp0L8Rr2`{d^!HxlZ;J z(AfLJ3jT9osqlspZ;kN8@jr@*MJCBjTMu||F@z0$Z z3ke^o-ItHOOz!{I|8CjiW%OPVqq6+vCf?sq{C7U>CdT5y_lU}KKWd%NqT#lyI}lU* zVu^VEhL4ROqfxZ&BA`?gtH<-lJk)8<$*xq@QXxQXJ#OSr0@Fd*_`Zw{vK1CPkC$!=x zkLL(;%eYFynuY{Q#A}XXwym|R9^Y;@x|(8hWQP?mqx0dQGCknfgTs7^JuvCq{QKV2 z#Gj~_8g6*E>CbYo8HoX7<8fPaiuD)~24@pw5Shn188zM28V2{a&CA`GxVV3<9oHi- z8vD^d{ekXiyCYB0cj>8J#BZYDcM(-=P+!ECy`VQ}&GE<8-*jg3R^viKocA16oU2z4 z&1;f9^)_s`bD}hwo+=5QKJ|(A@N`If=C$Ew=UIv@WLR9eJGOM9tH|^*t6k>@wSfDu zYiAk^>CPOv^eI;pRt^`LLHlO3fSxopngi*uh<$U&s3#Dd{)6Nx1MAKki4qi2+#5bQQCI2-=F zK548G9lEi5P;uGPz~+9E^0T`Jub*6dkm*&}hY+jfu0!ropu~)=-|nB^Z(W01%{F;v z>x~}Cf~5ro`K4`Q{Xw~p-)9o8)OlCkPnlZFwDw6+qF)kx`O(m`3lyn%j(BM<28-kb zl2kI!4mK~=ll*b%r!@I_NE zbUTN|>68Ff%t`kM&!OqanL9@pzzNzJBxK7z)uP!D9jf4*L)6wCDwo!TXbd~;ts^S! z-)0jkDxRmal!+Vk@?!!N6=Eg*{ROGArWe?m%8zO{T6=#Rh1`7dPg zUt;!;FiQ;=4cPdjM7UB#|F4huOPO@|7bX6oR{!s!#Nq-(D(Tb*_w5SV`q|TwRCj4s z;5u>{Ta*wRGXA#k)=3|0;D#VlN093a@RfJPtrzAW9BN+!?eDFkjL7gc$lf-1_o2b< zUI|@g*5JP^UOzi{TW>2}vMG@Tcen|bHr(pO+i&1IhpH-To39={O3!{~mPYC3?3*`` z1VFNNVWJ>&QV?dhBCFI1ME4t41zGT_Kq$jbu>n1cRFSNApv@h=Gc?p#xP$gYatEk( zxy{_{Q>$2jx>Spfi0Fs6Le(jZdUihojtLXmj{AS+H(IW%1ag0fb->&~<+Zjk zo5ji?4+kA$^S^Rmt#7IEvJSk4%06P|P0z}skaIdd-Ni5`Z>nIo5@XbR)$(QYw%@#_ z>0x1xp&{0_U&YB<$iTDI95ih9Qq_ zmYw7}tKBYUc85|duE;YpP^d90OxEzuw)fD?i-i_=v-oCSz&3ZvpYOj|oOtzarDdnE z={6W;I|twBAjy#3akg(@zH^|^?_Px%_o0F;_Dq88wT|V-VvPsiHp-AUUR|FYPY-{0 zm&K^?d$X4E#w{y#Qxth@`oiYS2}%R`2nG=X=*4(QNNryO!tru_YlwdG02D1`ZzA}R z_RNFePoIC%w5EYe&>uF-fwVNEjmc~vwq%vsrf?sm8-B`<7#95_ZDla$CgCZBnJf($ z{13dG2}5a0ql!GMMs+wZo2a0TI7R1L<3}p@^`?#*>P#}nBo$R|B{02PLOyhBkwo7g z7TYMB4MzbIr$z4;trKOESZ*9@tTQ`Q?yv@V=?d*Z{av+5aFvyGa1GT>A)V;L!0G8s@O4@y|@ zdBxfE=n3GRiC+Il*BHc|?NE2f0h1|wycvDSY9OFmf=2*$?D(k=>TNdlM|C`#FgDT) zvdWO36!kl|)F8EQKB5#Im`eMCXt||?(`u?{v&S@KGAQ%MLFr8_RRZLFZ#vpCRBhYv zK{@y2nNUzeN;#tStUWh9-Ui)40*6|#w&4A8izZA_eWe`)qZ3YZ3V?K_o2sZ=n;`gwO#$<(^h#_*RLQ`1w52i9VI*B54w(>Yu%0j9Fk zF_cp~Aq?(lrpeWw`y+0!|27c%cPEoj=*p(bHMM^;5h;6-T%*yUdi| zL;awHM;4`?!)EdaWX;>2I+1Rg6Iz>E8d@2VS=fY)9n$9IQCq^FdId?G=t7nC< z()edfkWjhgNa-l1*}T+(MH`Hbx)*IdUa%2AMi!z9d@aeoOG`8js zbRRIy=vMi_`(mDzJC@7$uA6~HI4+2LEVPXC1J0C}-stjq8^?;~fy=o12*oSZ?{H97 zJ0XkJ-I;d9FzXCi9;0qExa%khJ#d%*;v745d%9-uq{&mR`R%F)s+E|dN&?Prz}&5u z{l-$A+XonhqWN)*xy4?R&a7S`hTqGcLv%+4!&jfjip{qKFP(fMv7{kx=|S5bz9!3l zXw(j(g|%#d7a^>lrVA28Y_gPn*qUfC#tZbxnE`9Nr_w4eXGtSNYLSh;cE|=_0$otjdeX z7Pf*6Xq!*@7r-%btgc1OyWQGr+Y1 zViqMsFluBY-#PHbHM!#z?~~3>IMoH*RH(JX9C!v`8=5MgA3v`g1%C==%tAS6hv`WN zBEnz$Ub6KT23wzyeX>r`9GL35Xpfb~Xq;Is0j^t;{M=Z)I}Ap8fmAyjGdJ@`4;J)BV(8WV&LanA}mM1P zuR%!{Co5t<%LO39g}^)Z{&!G@@j>X&AjR_+Um7mVG$uJ+^TtNntjD-|5(eOHkwxcs7;$ z{1EkwLo0onZoHwUk#2pnFI?ZhqWOMoNn$#~oxbH$?=)XkKFrr_oRQ0Udy4=+K|X~; zbZnh9c1Tr8BxqnIfYI$;Wk~x}Z4oX12;~zOKFg~Mc^@C}kRCU(@$PnUba#dOs`tZJ zA5dR8L@P+&8ZLN0(<`F2mJZ0{-DGF+uNc3hWR5q$RVc!A8a1sdKOvgwl*^W5Hsx@x zsZ!Pm95qm4eD@gAKu&q z181k+_Sz@hz-Pr+jVKru!*r_ai1x)^^V)S|qR&OMv|B1OnBPZXd0umw9Pfl_wyKc> zU}G~ci%DW$S@U`53%fOV#dy5rR@^4wC#+%5*}BC0 zN6xo?{}jvQ}?CO%zE#pgW%*MYJEF|I+cf=I9T1OC$pLnLK-M3U}#KGCSTQu zfVEBdDdL-l!D|PZw?$N-5tMU0zC{n-zk8l7%BgznHBE1TiMyY)7YT#(SAfn!f?g>$ z;8nX*Q*hYvQ1<~AFVntu57bP~`pb*LoXREVpIg@Ii)=D?h<;@M^zmRuJHJ^2=h>-nr>MB5IW zy&uo;2KmBLL6B=95n)s{QWI*!Wr3n!aWY(!x6J0)SeJ;Sefj#eivJ_0?30Sg$c!nJ zg`tZc{Oj&3{{BM#y=Vd?AKfq5+$vDq4!cV1!CQSg4C&z7ieT=~wx?c>F~$+fZQPHC zC5e1A-*lT+!I-HF1Pi71qHL0GF4_eSy}#2e=He$w%*RJ-2es1UJ<7|8`*(5RP{u+8 z^i^R%4TG%SxPmNH3SSrhoZ!hmE+$rDs4C)A_3{sFvx4 zjoQb>CW>Z!0@m1tv^~_9$@{qrTVgg9r8z|1a-lFqcA~RRgc*cI)i>XCO8mnI(PIWo z=ju`S9gY^JWRUKBn)V-RuExMOq_fGIlpHC*U93M~Lrb_}+9#7tN#*V_xsK;U_cV)* zWLwq9Y&UM!#r3kAe_~Y(n}xrgVJ6zoYie|Io(O^Ft-Y#Cd7ArJx?1iYL7w?aO%TsO zB4@Wy0z-JinIqpudmu)7x`fuDOPh5RDYDqYL)Jt^b$3I!N2?FE{BYCDwqxsul@igH zq-H@@34jVaNr;OVMm(T?Gx#u+suY;??e?N$g{#=QrX0gQL(${SPghHKAR8)3TzA&# zW(oYcDQ_@CEB{(D3LcfC8(L#-P%9rQx|gLKeTT2M;eWLE-BC^b*}kYKDjag-jN<45T*B~fPjGX-lW&iJ0iUkihv-21Oy@p@txoN?%X@hT{ExE zT5ru;{&7~|99B-)+2>pKr)-%E0y*sbF4s<|jQm0{1C%iO3nXV7HW>!qdh@~4xAu^}_Gp2zI|JH|#-4{`!`>ca zC#9*fYNz2fG1qa^%Uo@8!IBIS<$f%N`JI`oEP7fMmDTlAcOqSSomgt>n}h9OqXx5i zmj22c9LA{;x*BN3LfFV^Mq6^TXbG?yFjNPxAba*GVAghx#=&W{jbF5EnI2+{jMx*$ z6z)978*cWiuG)Oy$lC7@Zp~;p4f4Cb2p7ZX1>O35D8Tm^=X$0FGjXC3D#~(CdvxXQ z4-Hzqql4U~-p6FxJXAH}7kG2*7l;EUXBu%lT%F^Y!Y~t?#BJ3zWJB~mI5*kx{JaUj z|Chxd+Z^JYP>DYAB!OW!Wt(R@thGtvc5_83->daFlXmZmv+w0jwg>_?0D@izc8-fw z8GfP!ZCpzM+<6LqW_i|6a9ap>?!kESp%eK$?ta-}>bJKE&8JH4Hn*UBSIcMyF3OuZ z z#KAtgOpUV+ZiT1Ek@tEBzwz~Ah$wY;V1BI5DgXgv)2ni~lRQG@Rm=7S%Cgfjd=?>d ztO|^aRSh%g#hr5Jn)#MTK*Ot1f~wJjkPPK&%z?&@F?)LYa=vzw7|+B^CL1AzV5tf< zk~8!X5s@`b2+tOx-IElNJvx`&W8&o$zu%oR}_1J^6269an)gz42Z zdr)eZ|F~76!>8O0uMMZFq$&mj8gj9H=eP~f3!DljTrM!JB#>6z&6$}$ zqhvk4plXoQeD^B$$G++?U)2RFwkjd1(6WJWW(ScIpv4MO@L(a8eH@MGMxCHPekP1! zre8^`t(4<9JxbYGC8yly@q3YBPWhZIW~N7m4qYa)ZufSo{pSp%x9+kRjIG_L_Tt-* zaljSgAHd~LqiZt48|{g_6RkW5GZ+o3JB2;vG&;@B*#dU#YAAAN%GsckqxPW|9M7nr zmR!-6U*C-fES!Q_Jg9SQl?S`L7wbaz_#wsav?{sf#oNvhJuQhX#p1yjUzm!-Z z(6OT5^iPU4!R|S zT0(1U#_zzPUL_0f-j<)pWE|CC{byJTV?RDDUFTf&;JkSnE_-&7a1JNVFWKFaRu@Ll zEH=TtfrL*%CHd@98zTK@*>5_5Ven#i36c`>R~JZHtq+Xdj2l}m4n~1ydKNvBcCLI< zUUh&->2w}4l6*8Ca9T|igoWaIoGue&uw>mjSjT|OG)w$rjBMNo^&@Lb8?B{{lQoE@ z!+KI(Ec;c}B+4*miVwoBa_5OM1m)8rSvCTQ8rKRX{n{#k16!=Mn2sxtm#im3w(bZZ zJZY1j4u7Qis2V8*v0*k~-YoaP)!^A+RBMQcBnXy1xp2^LN>RNDPv3;dmoGNFU`bzB z0a3r9S8PokOmVX=UswTkfx4nugnWa<+Zk z(IgGcA3m`$8jsX_drD;mNydlk~*+Tq$=1Z4l1+i*rT_E2}Bwsh-g=bKM)#j-~#} zFIDfBS4B!s;^)ENzNVFx`W{;#2KzOp78`5w9dr(xj5V=j5q^TO%^u_hY4;VU8~G8f zr#_ z%J-rz5zq``B`L(s*szC+Y7xxZQn)oYl`pzfk25C3G`Z8Y1J6mwBhe$uHwH;)Of+SKxofPPIMnz8ABf{k%9v+%Nnh*ziGK& z(4%&!fjyEVl%gg*Jp^}Mklzdt$;*0_SgExTG#fUxGsUwzN~4XH1@mGoIMOYO;cPfHl&!jwyc#lw-nmm< zPP1eqP&}vfyi)yqTV;LN#w)dEFFlgXg|0i--|RAh+BL{wRy`g}#MG?0t&_y0EUGI8 zsivcfcKKoSN_DEwLK75kuaU>nk~nv}jyArvkV#lXqWb|h^&5#MhkVFw+oh*(&6;YP zd<*L8j-MqQPY(ml?UB`jy?YU@LKD&&%X^e?)rQJ(f`xk)WD6iCcd$#d$!1s;%DhE8 zvrmId$OCA#!3|>PlT1U|m1G?$7g3Q&t4{n2*t9Y(X0^CfQ5CE7)?;eARuVjy12k2= zcc~A|;ezejIDh`g>p8tjl5j=|)sFKrDK5i#^((*VAj^}3r^bf6Q@ve{X?bzJuqJ0$ z*;KlYS#=)nNE20qyh7F5C^lGz0yb?wj8Q?kguKg68iq;1#U1{AuAK z7o=4jE_PYS&&FS3PvenQoiFDzK6Iq=D_-=Afhy##k2scV2u1*WlVVnC?B*I^q(=l1 zr=Akn#tDy2BUaxhCZ!05ri_h_yuNi-)7^gCbY{$m+!BAO-avf7QPBzVGO8a~;Q+Q$ zr&#h{xvQd7!9*5ZLIKj>{l|9!hDYPQf!dvt#fG#?qCA3*Z=UHUN353;Ud1(YXep3` zzazsC=7J%u0Hgo2#vSd6F>Gl(;=Vh!=@7x&)thy4e+V_4OLH5dUlN zUD0M0{B?pX;7C(kP7i)jZeTkXS)oy=k_StDHf}GPeCv$no_*;qTWzyLctOu94|z{` zR0AzHcdLQY2*iX~7u90_HVKSM25h^G9kZ+IzU-BKO9rH(9t*{a-F|%AM9I?+(&w-! z<0@Q|>+&LV8*c{XK#MJo>elk(u?2r5xnD3_WrwpEaeaJn2}eE@KfKT+}th-g;%v@pxw8xwqyL8Lqy z05b>Uq&Vt%+cq?IHe#&s^F$-}#a_dfH9cIM6W#?E-n+6F45uo$)kBbpK1^vE!j7*aUI+$Xg*9()W!XkB+se)LDzV<4K(4TerxH+d zcs4X>mS~lRN29Dp((tIm3>jlwIsOKYc}=`a0|yp$$NFZ3UP)z*;n)aT6R?s86!`u8 zg|x+H?w9-MHr_@pXS_cMUuvGTjm}Bx`Z9MInegT?DKVnqW1+2M%2pkb?1>?v@W(bD zBuN|=mt@ZX<{EQ5#1*l!IavU_?mBr4nv3gZS!GrpMW>nTCudWXU!#gkmiVC`$#jT3 zV}0}&o%Q=@O7juxJ3Jt|dXLtx$K(t) zDWqA(tN?<`f@E9{(*{qy>7Hwry;-B?ViAGYxub+vCOAgrbI-H&$r?IviSxJ)`t$=j z@+VNwZp$WPqaIY#Q{SYe6ogFqmz@(D6> zYN#%$zwArN=JxnW^5nge151yBgep4h;l0M`F%3K&Tr_D#tb92cE+^CTdQ~d0X^L=a zcm!GWduVYJWg^nEv3HC5x&~ix)I)m;1cw`l){ooKf25`-|wZZuanpI{`x8H0k3!t*F?vT&@&s*S?VHG%uv)8J~? zg#C|*X1+0=SARvq{>uOSSH7|`0X-f8fTBDMD4W@hSC`{{lKdAo2VB!UPImOT-gl0?sr=_Brq7v_NA=a;j;s-|r+(A|05snF`?2Ev$KF4`gY*9;IF)-GD|@Dwy?^Mg zLoghnpafe@r5OY2xp|JXMy2a>aZyFQWeGn?KqpD3@R-wUbS6cT|HbQhM$w`~tMzAp zqj2v3d@T|}^z{OaYLm}^8dz&`JDZFU=?OhVQxSs%L`bKPSNorm+L$NQW(n%bAkJ*t zf~sw~h{gbUoH-9rrn(uqu1aZ~9KQp89@W_<$==+SJNO1nYOeuKdC%8_L(q@IkAMIG z5CN>Ydx!sDU(cn_=hwU2?M@On@G?54<BvC3#F!{*ZG3&LEKj1^s;dn)^oJ+94FWa&l{a}KJtE)pNGy+*;DS3lV>>xB zeXU-w40q|q)^rVCO|-er?#yE z^=_=>?4C|*_@{GhXBC{{5^KnwfKd zI2+!F1$9x9wI5S=7u$I&C-E1z(CqI^Fiz(^Fcs#_OT+qfc(;7ltxe~`98M1H(r~|+ z$n=bhc~Hd6j=wB=F6+;`{WXjBf09H(Ydt$PUik^teunF|h zK;k+@Ep;DbwwLz(D1EjBev(@d7D_-B*0G0yo>CGS0475Th@ivtdO}kZATayd-N2~6 zQ4|h|e)xP~TUnm;r98{_BNFa%QQQdL1TJ@)K~S^H#$_8c)l51Gw9c~RrpNgds(;z# zX1AyfR|q%IKReT}9i7k%2E=5-xcg4mq*FG-*rLu@dSWbVi$1usr#?Qjr!&a)*{JWh zkvA!rpaL|^xiVurXEEfN;#lawvYy*z^(CE=R#M`KR~}^8q$Tv1Z=^mWDx(?{gtS86 z1`KIEt8z{N9~bj+5xkUvpx~hzHiMTOmXH(I8gPz{-Ljq%baaCG&a{+av${t{P{G+b z6%EuLdB=pi$=l@*@?E77s}EN*3rdx~6z;}WCj0|3p{LR~`Uf>oj|a=V*87@BWdu=L z-D8AlcZ@9+`nXLV`uNbNsgQn1n10EEH30nJ4KRBTacYGyw6tEH9qZN6FZ<;+>J+tA zl*N=y0jIH#ls|7nV%`dIolEgBZIrnT5TPmHR+wot8D?4?iwG1IL?f+)FRjlm{?W=za|{Vo zt+arxNgUw2U|Kw>0uZNSPQk=h6=GO;%GtyJe1QIVi2ma!X#YvV#xi~CqX;A-Sg$A& zw10OIFZ1<)t&(2hnNGL^ZU}Y3QToRI zR)z!Mj=l{5XGPa^a$XAu^vBZ z7%P=!*m`N}YNtoDiqgCQl0=(neOR3}uU%@Z7;d{^mY#Z4g){cWzBzKiI=;@aCoZM; zEm{xZomsOT2c0c{n3Bvp)2iIR-s2)UJuEA68tcc7LoGu)x0E9&lYo9&o`nVk<#C9v zfwua%J3gpkV$DD?D0OGa$oXardte7VXDr+ii$uJMf5iNCEdI@#*05s0nbN@E1Uf4A z<%HI%YUT5sjdT8nEZ#_sdXjNQ?6tWlUAEPQs5y(<3I zK#oGwO4mD*W%*qAwF$SMBy)DF8DUAvwb7kjtXD>>)p@ao0tYfp+YbCBR^Gg)S?3N& z@;w>bd*8t&PScxxmrvPNh;esFd+Rg5IaxH}6(r>6T56^NaK z)t@9JxcN>kSkK@ZYlhsbW?tfr3S(O*GP&R>PO$-uf!G@gy**ZkZ0o2YO?X(Ml51^i zb184fU5^MPK?+}1QwN^gbJl*F>u2GFvpD{iJ0J@b0Ahe#EC*&t7oF|wRVo}tGJuZ4IV zYte=BUhK$tbbz8J4vVF;VmCX=amv-udn%aCFhTb;!`1DbMI*2Ht!Yhxp^pnv>e6|7 zVV;Se=bUco#Yi=trV?~OJ%ZE*wrs2Js1RxSHEwAQx}^~DVC2Wo#q<5w7JQ3DCp1@t zc&Vx1-0e!G8q##+6;6pBm_O}5J$CShlukU_z5+(z5Ci>iLd}W7w^i9tX>psv`EQ1_ zwR3se>S>1;$L5H}B?DpHefEK7`@s^r5)vgodoe^BOh{m}Od()@oqie6`V!b*s&J$Z zmMb2Cr_FV5PuWN#Wn`U$FN&Y% zQKAy;Z{j)fuF$yC`vxdzvimvm$e^V}v6krc7W@_QsaqwaF4`0rBAVlmUpj zv-CAeoyQKe>7Gk|qEh*ru;La^ljx0j4->?Dd*a4owYTWjVZwBm z2GvZvC;Qw+421_n*=8u?n??6Js-zfchb&xGJH7(T* z4dd+kPPf7>yaZU{%%8={58PEzCRt8+?oAC9f^8n%nS$$;FTgDCx^um*Q9nC&1o^hL z?1QT*=HuZn9m;fWRVcNSdzWqP!-^E5+W};ufdqM=)EvLfDp(l-%o$;a%xPwNwb2&u zAZl#raDlIn>XIB|RT9|Y?{!>yjXen7e(`?bBtKJcu1TO(;UReVz`Dd+iB|LJnLo;D z|AzMOTV>{qM4{GEJ{BDL7?0h^IR36Eo9cN^F*8$3I*cHO7hn<@-NR$U$;84|eb zeqWH9z*45PN>1%`yTB)OC7q7O;)%BX?n@yThV%~^^sQ7jP{!(0nt9#$eUZV0$+;;c*su9ZPKB%KhcFaQjw;b5TlL9d^k*y@aAp_}327O-)vYhM z@~jeXx}mu4E7d)G6#^_d7eoy-cKcsGBfC#(*__>elyck=GyiS*IIL#xRinnv=?YqX z4=(tVgqE0@k5Vc(F;?igSf-ScH%amO?OQIFFSTTYM{;OwJfC$&4bopEiotM_0RHfk zg5j{9Ixe6Mbr-&4EC7>0;@0&`SE8&aH&esRsjXujI*Yh99G?mMLqCu@Oet62niW zdE1TCL1vf&I}Lu(w>C0egU-}$FE%gKODr(@DGj9C0Z@G~+a%&^3mkyp+G_z{%0AI7 zyORb=*6EdDg-u<|VbCNxuu*4yxY=dF0L=U7$VJ$H^j;=+w;J!G`1~6*yl5@VM*Im` zNC`iCOY=nfz><_lbF+nAL{>O$|1BMzJ2QiHPlcb5Z0Xqvk0c;$(7Kze(`cU+ryxJ^ z=8O&`g($Q6ggv2-yH#d_?Imhe^gddV}4qpAGFV44h3pM*>W7fekYSqp#D?xoMl!VC|Oof&mC#O#NK9_cO`X1op~L zY@VB(ckUsN=9 z@t`u}vRD<5`KFE0dzA3!XuU#)HWWQ9pchI;xL*P*R{-b)R$bmvf!fBSuR)7FSxhx25Un~&MQ48uhdBrvb`eBa? zn8@77`CaY_t5PXj0APV9#%$%8+yy_@*{_TUgm)LNhouf+GV z;BY?S4q%x~C4-$x7TD~fG(&W%i+AY?$LO1uw!f{R&k{YJcloMOYug@5lCbh|I|u%@b6-p1NsFk)uK z%>v-Pk+WT3esEs394O%bw@l74gpS|Jo@u`1SnEeNnq|x=Em5i87NDlV=a@)pEBi@0 z%d89G`bEmmM55bHNAQY`;0Ks@M+bO*SZ$|ttztyeF{6=mbWIRx18Q@${Q38*ITJh>I8PU-;q??nudeEX0`YpPfCZ|KMKmk z+wHFTPOap)Z?5;TZ1#X=GrE^JaTw%PX-e#c>GqWB6H*h_mhj`w;^IzcQoUDmV-@9uYFsdmuV>26t2>$sq-B+jN(l-Wzb>JkP`7cQA#52}0lQbV-J`Q8Lt z&J^@RcrQ@yh{GCnqDbi-n}z;to2jslgIX0>mZWfL@Rdp@8ges!2F>AE z0Yb0#Ri)cvsQsJfeJAJEP&=qX*nCf-i3CO&$q)qh$>9oz%LL3mLao!3F%bl6{Qa-3mz$H&l2b_^V1D3PX?fn_(4k=1YP3uV zeqT#>jfBuOHSnlM*HaRO<8lfwmwtg~f^lw~?5;gcTw+d>mFk~qPO5+y4?9q-$3TU- z;|@?B0Hu>s>Pg+`cbtR3&ETo2S< zz#yIcP1E+oB(12Mtdm7_4LoR`W@c7|@_+FBr7a2#!i$T?Y}SqMZH|k8{3Ti5r+jM3 z{EEFUD6QtsDELLaQb(!aq4F(=ThxB+*{v&1jL=By%4M*{^snTc<32nMIq8%UQ@!br^E(?5(I^SW|(9FqEc!b}7b@ z*_3v42co+jP4Dh7x$gTbd?zE(%hVC1gt_##6Im+8+yz9|B`@_bpEw$p6w}DbuJ1Of$wi+v2KZ@}~5a|NU>p574*N1*QkiO`) z{>{7Zj(SKsUI865FObpY%Z6PWEFG#R-Wpk>3P>dKVk{EsLfI#)>uc0+Pi0#*ve{6ypaxOZbB;>+vNDvEVt9j zS*YW-N1C}wpT|3!tYDBUhe;jK;B*#D*}G$5n^wkgf$CJ1`sd@)Pajs^av?ci`=OtN zV)HemMjWE=9rVx3F!7Au;oLd@ZK?jN8nAdWiPs1jx+^=ve;1Lh*fbQ{>SyB|DZH9; z7Qg|&nAHG#VH@Ok?xI7@+VNdPb^Pio%zS?hHfa`^zr?G?UP;{hrdLP;;QxO;&Zu;O zu*(^T2V}(PM=d%v5Y7pHLbWqE7p`)Pb&C@7NshhCHqeqRO1 zZ+~^>{2h(IO`YG-_#KVxf8b&JJ2(D!a-%5vZy{0Eza5Q}Ka@l`7lawGYE(yly!8~} z4l>m^_@EpG6!~BLeIe-YXBTCvAJ=!0-{wH4mkq5iiK=V5^}S87VO5^LVKX^`^G?Y> zt@J~z_n@$ifUTO8?xqPKhb_AXB6#!0r#$Ma{U-)TqO?0f|I7gCFRl*%%xUe}#J|8~ zNL`6u{`aT&(-rS`EdI+O@OLbJ$Kp@7z+b)ce&@yi2YGR&$?M{xfWQ5X3|?P!%=2ef z2ZaTIN7i6s9gmk|e2*u4fJ=sL@h@y-b<7Bp-k8@bpkGY+lV8#E!d=ZjHfn!T6aJSe zN0Q})kW#sCOHQYkdB6DGhby+=td{XC3LSg~mJL(iTSOCA4Bo1^`cgIrJ`bp8kJNi_ zPT?<6)}#XvrVsN^eTM%4){QjK)XOZ;Rq@XHlImM|t@&2PHjO!&s0EqkYrcv5%)!cW zY;j31BVQ?qUAm#uyBc-Wh|HY?Y%}iLqgygi4wO!zPhm}Nt06Zlc?qszZjnahuR0Z z>v7M+=GcupIUm&DCyRZs;>QiA$JTW_ARb^vHxe>u?Idp-Z=v(HmZT=DagG93`_N&A zsoK#;ANgZnnF=)uzu%>}aIe`JF9K`Dx6fGyWhz;iWkvCBT&iwgD>a?bhmP>|9dmAT zH=oLDyhC{2kAlf94YXwYAprz2JzdYkV}b5{Gjh3n5Guu2)>}G^9q)*>mORRH4~>cm z4zzljU>et4$*wuf_<%upPfgn?k=fZHoBv|b0r7JyO&xXs=3Ybora*LThLPb(qaAly zvZp|ElBrx(?_E+;&RAIk<%J5eR~4s;q7YwbTlloM>X^tW-{Tsrdi_$bW`;WR&JdLQ z{KKK{h~zhULwkub50sFN#J48JyzY@~QAN zXLZ-9s+}%4*|lKd5#FFr-(f9Y;cPLD$MTteI=KLXXATbEUTbo5%*m_~0GiJaF|dm~ z@7->HWzI%=wT%(14xs&hvZ~wql-#ty|qFyel z?xsjS-(1039!WfdL#{Ua-JWz0#C(b=-LBX&^es(GARK#)LS|9ErAuma7n5^LKhZtB zd1v&1rK_yDV(t`iC1!Kpy=li{n9sP>4S!Oecsz^VaErp55oBUM(b%3u(ilETJg(Hp zgm&L~2enE(R{eaTHjL0c&)?4G=utW6?AZgVk$Xirw;F+F)0kVgQ<21b* z+u$7OC^13@iUj5ZxC^=fHwd>lHp62@55r7jH#1qhi-KBID{juWxDZ0^$lm z9EYuY3Kja=GSDyatM^e>N=c$_E92#p%!v|5ku3r0B=7s+(9r?O{gN}f?Q`nday4?V zmobw^rS63&o5V(k#g>FITS+B!)DVr4kiNLQh}xZ5@)uCc4n^1f_pS&4hOvxNS&h!e zUg|!~Es^CJ`ySzQmNS`EDuOv|^K>i5ti*og`XcjQ&(8DL?~TQeo=-XP!+(5lqGU55 z&W<{+*QfR})rC-5odqRR-oqHx>PhBfv^=Cs>$RgbLr zPc_?(y1A@Xx$}!1b(iQSz`XnzX_}&nK1${wH%BjBl7M!S=~y`UG`$8zw*(_@_pGTM zz!iNmx0&2rhi~&Z$g1XAZ@ggkdNJgq$PK^nX*?1H9m{P@1=rbqbwKYKa@^}C?Xk{& z0}?JyP#C!Kvin)wC$dL|F0IEvA5h4N`&T9u^c!@aUsH*WF>I^pDX`UmIJOKLs>K6`X*m$|cE{c`**1J~qNdhYlGMF0ow^ByO* zYye#3E*H4lY(XGF6A$_L#pE5_QpR@l1UXJ+# literal 0 HcmV?d00001 From 9d70657bd361f5589240e44e808611b643bb8e8f Mon Sep 17 00:00:00 2001 From: PaulZC Date: Fri, 30 Oct 2020 08:04:21 +0000 Subject: [PATCH 11/36] Adding a contributing link to README --- CONTRIBUTING.md | 6 +++--- README.md | 6 ++++++ 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index aa1d4c0..ff79aea 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,4 +1,4 @@ -### How to Contribute +# How to Contribute Thank you so *much* for offering to help out. We truly appreciate it. @@ -10,8 +10,8 @@ If you decide to add a feature to this library, please create a PR and follow th * Change as little as possible. Do not submit a PR that changes 100 lines of whitespace. Break up into multiple PRs if necessary. * If you've added a new feature document it with a simple example sketch. This serves both as a test of your PR and as a quick way for users to quickly learn how to use your new feature. -* If you add new functions also add them to keywords.txt so that they are properly highlighted in Arduino. [Read more](https://www.arduino.cc/en/Hacking/libraryTutorial). -* **Important:** Please submit your PR using the [release-candidate branch](https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library/tree/release_candidate). That way, we can merge and test your PR quickly without changing the _master_ branch +* If you add new functions also add them to _keywords.txt_ so that they are properly highlighted in Arduino. [Read more](https://www.arduino.cc/en/Hacking/libraryTutorial). +* **Important:** Please submit your PR using the [release_candidate branch](https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library/tree/release_candidate). That way, we can merge and test your PR quickly without changing the _master_ branch ![Contributing.JPG](./img/Contributing.JPG) diff --git a/README.md b/README.md index 55fc8cb..0478877 100644 --- a/README.md +++ b/README.md @@ -53,6 +53,11 @@ Need a Python version for Raspberry Pi? Checkout the [Qwiic Ublox GPS Py module] Need a library for the Ublox and Particle? Checkout the [Particle library](https://github.com/aseelye/SparkFun_Ublox_Particle_Library) fork. +Contributing +-------------- + +If you would like to contribute to this library: please do, we truly appreciate it, but please follow [these guidelines](./CONTRIBUTING.md). Thanks! + Repository Contents ------------------- @@ -60,6 +65,7 @@ Repository Contents * **/src** - Source files for the library (.cpp, .h). * **keywords.txt** - Keywords from this library that will be highlighted in the Arduino IDE. * **library.properties** - General library properties for the Arduino package manager. +* **CONTRIBUTING.md** - Guidelines on how to contribute to this library. Documentation -------------- From 57c774df0e8891c64d91f2c4b0068af88646586b Mon Sep 17 00:00:00 2001 From: PaulZC Date: Fri, 30 Oct 2020 08:10:40 +0000 Subject: [PATCH 12/36] Adding Morten to the Thanks --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 0478877..7607ef4 100644 --- a/README.md +++ b/README.md @@ -48,6 +48,7 @@ Thanks to: * [averywallis](https://github.com/averywallis) for adding good comments to the various constants. * [blazczak](https://github.com/blazczak) and [geeksville](https://github.com/geeksville) for adding support for the series 6 and 7 modules. * [bjorn@unsurv](https://github.com/unsurv) for adding powerOff and powerOffWithInterrupt. +* [dotMorten](https://github.com/dotMorten) for the MSGOUT keys, autoHPPOSLLH, autoDOP and upgrades to autoPVT. Need a Python version for Raspberry Pi? Checkout the [Qwiic Ublox GPS Py module](https://github.com/sparkfun/Qwiic_Ublox_Gps_Py). From 27400368866ee6f2d04bf03336dca4751a6a10d0 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Fri, 30 Oct 2020 08:59:53 +0000 Subject: [PATCH 13/36] Updating Example9_multiSetVal to use the new key names --- .../Example9_multiSetVal/Example9_multiSetVal.ino | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/examples/ZED-F9P/Example9_multiSetVal/Example9_multiSetVal.ino b/examples/ZED-F9P/Example9_multiSetVal/Example9_multiSetVal.ino index ed1b471..250f8a9 100644 --- a/examples/ZED-F9P/Example9_multiSetVal/Example9_multiSetVal.ino +++ b/examples/ZED-F9P/Example9_multiSetVal/Example9_multiSetVal.ino @@ -67,15 +67,15 @@ void setup() //Original: myGPS.enableRTCMmessage(UBX_RTCM_1005, COM_PORT_I2C, 1); //Enable message 1005 to output through I2C port, message every second //Begin with newCfgValset8/16/32 - setValueSuccess &= myGPS.newCfgValset8(CFG_MSGOUT_RTCM_3X_TYPE1005_I2C, 1); //Set output rate of msg 1005 over the I2C port to once per measurement (value is 8-bit (U1)) - //setValueSuccess &= myGPS.newCfgValset8(CFG_MSGOUT_RTCM_3X_TYPE1005_I2C, 1, 7); //Set this and the following settings into Flash/RAM/BBR instead of BBR + setValueSuccess &= myGPS.newCfgValset8(UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1005_I2C, 1); //Set output rate of msg 1005 over the I2C port to once per measurement (value is 8-bit (U1)) + //setValueSuccess &= myGPS.newCfgValset8(UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1005_I2C, 1, 7); //Set this and the following settings into Flash/RAM/BBR instead of BBR //Add extra keyIDs and values using addCfgValset8/16/32 - setValueSuccess &= myGPS.addCfgValset8(CFG_MSGOUT_RTCM_3X_TYPE1077_I2C, 1); //Set output rate of msg 1077 over the I2C port to once per measurement (value is 8-bit (U1)) - setValueSuccess &= myGPS.addCfgValset8(CFG_MSGOUT_RTCM_3X_TYPE1087_I2C, 1); //Set output rate of msg 1087 over the I2C port to once per measurement (value is 8-bit (U1)) - setValueSuccess &= myGPS.addCfgValset8(CFG_MSGOUT_RTCM_3X_TYPE1127_I2C, 1); //Set output rate of msg 1127 over the I2C port to once per measurement (value is 8-bit (U1)) - setValueSuccess &= myGPS.addCfgValset8(CFG_MSGOUT_RTCM_3X_TYPE1097_I2C, 1); //Set output rate of msg 1097 over the I2C port to once per measurement (value is 8-bit (U1)) + setValueSuccess &= myGPS.addCfgValset8(UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1077_I2C, 1); //Set output rate of msg 1077 over the I2C port to once per measurement (value is 8-bit (U1)) + setValueSuccess &= myGPS.addCfgValset8(UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1087_I2C, 1); //Set output rate of msg 1087 over the I2C port to once per measurement (value is 8-bit (U1)) + setValueSuccess &= myGPS.addCfgValset8(UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1127_I2C, 1); //Set output rate of msg 1127 over the I2C port to once per measurement (value is 8-bit (U1)) + setValueSuccess &= myGPS.addCfgValset8(UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1097_I2C, 1); //Set output rate of msg 1097 over the I2C port to once per measurement (value is 8-bit (U1)) // Add the final value and send the packet using sendCfgValset8/16/32 - setValueSuccess &= myGPS.sendCfgValset8(CFG_MSGOUT_RTCM_3X_TYPE1230_I2C, 10); //Set output rate of msg 1230 over the I2C port to once every 10 measurements (value is 8-bit (U1)) + setValueSuccess &= myGPS.sendCfgValset8(UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1230_I2C, 10); //Set output rate of msg 1230 over the I2C port to once every 10 measurements (value is 8-bit (U1)) if (setValueSuccess == true) { From 396650d88ab0866fddf0b361ae72d46cd1544123 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Fri, 30 Oct 2020 09:43:08 +0000 Subject: [PATCH 14/36] Updating Theory.md - as an aide-memoire! --- README.md | 7 ++++--- Theory.md | 13 ++++++++++--- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 7607ef4..7e4504f 100644 --- a/README.md +++ b/README.md @@ -64,9 +64,10 @@ Repository Contents * **/examples** - Example sketches for the library (.ino). Run these from the Arduino IDE. * **/src** - Source files for the library (.cpp, .h). -* **keywords.txt** - Keywords from this library that will be highlighted in the Arduino IDE. -* **library.properties** - General library properties for the Arduino package manager. -* **CONTRIBUTING.md** - Guidelines on how to contribute to this library. +* **[keywords.txt](./keywords.txt)** - Keywords from this library that will be highlighted in the Arduino IDE. +* **[library.properties](./library.properties)** - General library properties for the Arduino package manager. +* **[CONTRIBUTING.md](./CONTRIBUTING.md)** - Guidelines on how to contribute to this library. +* **[Theory.md](./Theory.md)** - provides detail on how data is processed by the library. Documentation -------------- diff --git a/Theory.md b/Theory.md index 2153f64..227a081 100644 --- a/Theory.md +++ b/Theory.md @@ -19,14 +19,21 @@ A method will call **sendCommand()**. This will begin waiting for a response wit Once **waitForACKResponse()** or **waitForNoACKResponse()** is called the library will start checking the ublox module for new bytes. These bytes may be part of a NMEA sentence, an RTCM sentence, or a UBX packet. The library will file each byte into the appropriate container. Once a given sentence or packet is complete, the appropriate processUBX(), processNMEA() will be called. These functions deal with specific processing for each type. -Note: When interfacing to a ublox module over I2C **checkUbloxI2C()** will read all bytes currently sitting in the I2C buffer. This may pick up multiple UBX packets. For example, an ACK for a VALSET may be mixed in with an auto-PVT response. We cannot tell **checkUbloxI2C()** to stop once a given ACK is found because we run the risk of leaving bytes in the I2C buffer and losing them. We don't have this issue with **checkUbloxSerial()**. +Note: When interfacing to a ublox module over I2C **checkUbloxI2C()** will read all bytes currently sitting in the I2C buffer. This may pick up multiple UBX packets. For example, an ACK for a VALSET may be mixed in with an **AutoPVT** response. We cannot tell **checkUbloxI2C()** to stop once a given ACK is found because we run the risk of leaving unprocessed bytes in the I2C buffer and losing them. We don't have this issue with **checkUbloxSerial()**. **processUBX()** will check the CRC of the UBX packet. If validated, the packet will be marked as valid. Once a packet is marked as valid then **processUBXpacket()** is called to extract the contents. This is most commonly used to get the position, velocity, and time (PVT) out of the packet but is also used to check the nature of an ACK packet. -Once a packet has been processed, **waitForACKResponse()/waitForNoACKResponse()** makes the appropriate decision what to do with it. If a packet satisfies the CLS/ID and characteristics of what **waitForACKResponse()/waitForNoACKResponse()** is waiting for, then it returns back to sendCommand. If the packet didn't match or was invalid then **waitForACKResponse()/waitForNoACKResponse()** will continue to wait until the correct packet is received or we time out. **sendCommand()** then returns with a value from the **sfe_ublox_status_e** enum depending on the success of **waitForACKResponse()/waitForNoACKResponse()**. +Once a packet has been processed, **waitForACKResponse()/waitForNoACKResponse()** makes the appropriate decision what to do with it. If a packet satisfies the CLS/ID and characteristics of what **waitForACKResponse()/waitForNoACKResponse()** is waiting for, then it returns back to **sendCommand()**. If the packet didn't match or was invalid then **waitForACKResponse()/waitForNoACKResponse()** will continue to wait until the correct packet is received or we time out. **sendCommand()** then returns with a value from the **sfe_ublox_status_e** enum depending on the success of **waitForACKResponse()/waitForNoACKResponse()**. If we are getting / polling data from the module, **sendCommand()** will return **SFE_UBLOX_STATUS_DATA_RECEIVED** if the get was successful. If we are setting / writing data to the module, **sendCommand()** will return **SFE_UBLOX_STATUS_DATA_SENT** if the set was successful. -There are circumstances where the library can get the data it is expecting from the module, but it is overwritten (e.g. by an auto-PVT packet) before **sendCommand()** is able to return. In this case, **sendCommand()** will return the error **SFE_UBLOX_STATUS_DATA_OVERWRITTEN**. We should simply call the library function again, but we will need to reset the packet contents first as they will indeed have been overwritten as the error implies. +We are proud that this library still compiles and runs on the original RedBoard (ATmega328P). We achieve that by being very careful about how much RAM we allocate to packet storage. We use only three buffers or containers to store the incoming data: +- **packetBuf** (packetBuffer) - is small and is used to store only the head (and tail) of incoming UBX packets until we know they are. If the packet is _expected_ (i.e. it matches the Class and ID in the packet passed in **sendCommand()**) then the incoming bytes are diverted into **packetCfg** or **packetAck**. Unexpected packets are ignored. +- **packetCfg** (packetConfiguration) - is used to store an _expected_ incoming UBX packet of up to 256 bytes. E.g. **getProtocolVersion()** returns about 220 bytes. Message data requested by a higher function is returned in packetCfg. +- **packetAck** (packetAcknowledge) - is small and is used to store the ACK or NACK accompanying any _expected_ packetCfg. + +**AutoPVT**, **AutoHPPOSLLH** and **AutoDOP** packets can arrive at any time. They too _have_ to be stored and processed in **packetCfg**. This means there are circumstances where the library can get the data it is expecting from the module, but it is overwritten (e.g. by an **autoPVT** packet) before **sendCommand()** is able to return. In this case, **sendCommand()** will return the error **SFE_UBLOX_STATUS_DATA_OVERWRITTEN**. We should simply call the library function again, but we will need to reset the packet contents first as they will indeed have been overwritten as the error implies. + +Need a command that is not currently "built-in" to the library? You can do that using a Custom Command. Check out [Example20_SendCustomCommand](https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library/blob/master/examples/Example20_SendCustomCommand/Example20_SendCustomCommand.ino) for further details. Note: this will of course increase your RAM use. From 5edcbf6e9caac52406a865d44636d2ca63b64b71 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Fri, 30 Oct 2020 09:56:44 +0000 Subject: [PATCH 15/36] Correcting the multiSetVal example --- Theory.md | 2 +- examples/ZED-F9P/Example9_multiSetVal/Example9_multiSetVal.ino | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Theory.md b/Theory.md index 227a081..53511e3 100644 --- a/Theory.md +++ b/Theory.md @@ -34,6 +34,6 @@ We are proud that this library still compiles and runs on the original RedBoard - **packetCfg** (packetConfiguration) - is used to store an _expected_ incoming UBX packet of up to 256 bytes. E.g. **getProtocolVersion()** returns about 220 bytes. Message data requested by a higher function is returned in packetCfg. - **packetAck** (packetAcknowledge) - is small and is used to store the ACK or NACK accompanying any _expected_ packetCfg. -**AutoPVT**, **AutoHPPOSLLH** and **AutoDOP** packets can arrive at any time. They too _have_ to be stored and processed in **packetCfg**. This means there are circumstances where the library can get the data it is expecting from the module, but it is overwritten (e.g. by an **autoPVT** packet) before **sendCommand()** is able to return. In this case, **sendCommand()** will return the error **SFE_UBLOX_STATUS_DATA_OVERWRITTEN**. We should simply call the library function again, but we will need to reset the packet contents first as they will indeed have been overwritten as the error implies. +**AutoPVT**, **AutoHPPOSLLH** and **AutoDOP** packets can arrive at any time. They too _have_ to be stored and processed in **packetCfg**. This means there are circumstances where the library can get the data it is expecting from the module, but it is overwritten (e.g. by an **AutoPVT** packet) before **sendCommand()** is able to return. In this case, **sendCommand()** will return the error **SFE_UBLOX_STATUS_DATA_OVERWRITTEN**. We should simply call the library function again, but we will need to reset the packet contents first as they will indeed have been overwritten as the error implies. Need a command that is not currently "built-in" to the library? You can do that using a Custom Command. Check out [Example20_SendCustomCommand](https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library/blob/master/examples/Example20_SendCustomCommand/Example20_SendCustomCommand.ino) for further details. Note: this will of course increase your RAM use. diff --git a/examples/ZED-F9P/Example9_multiSetVal/Example9_multiSetVal.ino b/examples/ZED-F9P/Example9_multiSetVal/Example9_multiSetVal.ino index 250f8a9..b701efc 100644 --- a/examples/ZED-F9P/Example9_multiSetVal/Example9_multiSetVal.ino +++ b/examples/ZED-F9P/Example9_multiSetVal/Example9_multiSetVal.ino @@ -68,7 +68,7 @@ void setup() //Begin with newCfgValset8/16/32 setValueSuccess &= myGPS.newCfgValset8(UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1005_I2C, 1); //Set output rate of msg 1005 over the I2C port to once per measurement (value is 8-bit (U1)) - //setValueSuccess &= myGPS.newCfgValset8(UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1005_I2C, 1, 7); //Set this and the following settings into Flash/RAM/BBR instead of BBR + //setValueSuccess &= myGPS.newCfgValset8(UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1005_I2C, 1, VAL_LAYER_RAM); //Set this and the following settings in RAM only instead of Flash/RAM/BBR //Add extra keyIDs and values using addCfgValset8/16/32 setValueSuccess &= myGPS.addCfgValset8(UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1077_I2C, 1); //Set output rate of msg 1077 over the I2C port to once per measurement (value is 8-bit (U1)) setValueSuccess &= myGPS.addCfgValset8(UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1087_I2C, 1); //Set output rate of msg 1087 over the I2C port to once per measurement (value is 8-bit (U1)) From ccb20f28151b992056816fb1ed2fd8ac8e623369 Mon Sep 17 00:00:00 2001 From: vid Date: Mon, 2 Nov 2020 15:49:34 +0100 Subject: [PATCH 16/36] code cleanup --- examples/Example_Zephyr/src/main.c | 27 ++--- src/SparkFun_Ublox_Zephyr_Library.cpp | 140 +++++++------------------- src/SparkFun_Ublox_Zephyr_Library.h | 12 ++- src/ublox_lib_interface.cpp | 91 +++++++++++------ src/ublox_lib_interface.h | 20 ++-- 5 files changed, 129 insertions(+), 161 deletions(-) diff --git a/examples/Example_Zephyr/src/main.c b/examples/Example_Zephyr/src/main.c index 74ff49f..e62adac 100644 --- a/examples/Example_Zephyr/src/main.c +++ b/examples/Example_Zephyr/src/main.c @@ -12,19 +12,22 @@ Open the serial monitor at 115200 baud to see the output I2C clock speed: 100 kHz - Ported to Zephyr by Vid Rajtmajer , IRNAS d.o.o. + Ported to Zephyr by Vid Rajtmajer , www.irnas.eu */ - -#include -#include #include #include +#include +#include +#include +#include #include "ublox_lib_interface.h" #define I2C_DEV "I2C_0" +LOG_MODULE_REGISTER(zephyr_main); // init logging + struct device *gpio_dev; struct device *i2c_dev; /* I2C pins used are defaults for I2C_0 on nrf52840 @@ -33,15 +36,15 @@ struct device *i2c_dev; */ uint8_t init_gpio(void) { - const char* const gpioName = "GPIO_0"; + const char* gpioName = "GPIO_0"; gpio_dev = device_get_binding(gpioName); if (gpio_dev == NULL) { - printk("Could not get %s device\n", gpioName); - return -1; + LOG_ERR("Could not get %s device", gpioName); + return -EIO; } int err = set_gpio_dev(gpio_dev); if (err) { - return -1; + return -EIO; } return 0; } @@ -50,12 +53,12 @@ uint8_t init_i2c(void) { i2c_dev = device_get_binding(I2C_DEV); if (!i2c_dev) { - printk("I2C_0 error\n"); + LOG_ERR("I2C_0 error"); return -1; } else { - printk("I2C_0 Init OK\n"); + LOG_INF("I2C_0 Init OK"); return 0; } } @@ -63,7 +66,7 @@ uint8_t init_i2c(void) { uint8_t init_gps(void) { if (gps_begin(i2c_dev) != 0) { - printk("Ublox GPS init error!\n"); + LOG_ERR("Ublox GPS init error!"); return -1; } return 0; @@ -71,7 +74,7 @@ uint8_t init_gps(void) { void main(void) { - printk("UBlox i2c test\n"); + LOG_INF("Ublox Zephyr example"); int err; err = init_gpio(); diff --git a/src/SparkFun_Ublox_Zephyr_Library.cpp b/src/SparkFun_Ublox_Zephyr_Library.cpp index 5b21fa5..3179e3b 100644 --- a/src/SparkFun_Ublox_Zephyr_Library.cpp +++ b/src/SparkFun_Ublox_Zephyr_Library.cpp @@ -17,7 +17,9 @@ https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library Development environment specifics: - Arduino IDE 1.8.5 + NCS v1.0.3 release - this port + + This port was made by Vid Rajtmajer , www.irnas.eu SparkFun code, firmware, and software is released under the MIT License(http://opensource.org/licenses/MIT). The MIT License (MIT) @@ -39,7 +41,6 @@ */ #include "SparkFun_Ublox_Zephyr_Library.h" - #include SFE_UBLOX_GPS::SFE_UBLOX_GPS(void) @@ -49,18 +50,24 @@ SFE_UBLOX_GPS::SFE_UBLOX_GPS(void) moduleQueried.versionNumber = false; } -bool SFE_UBLOX_GPS::init_gpio_pins(struct device &gpio_dev) { +// Get i2c device to the class and configure checksumFailurePin +bool SFE_UBLOX_GPS::init_gpio_pins(struct device &gpio_dev) +{ _gpio_dev = &gpio_dev; - if (checksumFailurePin >= 0) { - gpio_pin_configure(_gpio_dev, (uint8_t)checksumFailurePin, GPIO_OUTPUT); - gpio_pin_set(_gpio_dev, (uint8_t)checksumFailurePin, HIGH); + int err; + err = gpio_pin_configure(_gpio_dev, (uint8_t)checksumFailurePin, GPIO_OUTPUT); + err = gpio_pin_set(_gpio_dev, (uint8_t)checksumFailurePin, HIGH); + if (err) + { + return false; + } } return true; } -//Initialize the Serial port +//Initialize the I2C port bool SFE_UBLOX_GPS::begin(struct device &i2c_dev, uint8_t deviceAddress) { commType = COMM_TYPE_I2C; @@ -79,8 +86,8 @@ bool SFE_UBLOX_GPS::begin(struct device &i2c_dev, uint8_t deviceAddress) return (isConnected()); } -//Initialize the Serial port -/* // TODO +//Initialize the Serial port - function not ported +/* bool SFE_UBLOX_GPS::begin(Stream &serialPort) { commType = COMM_TYPE_SERIAL; @@ -213,37 +220,28 @@ void SFE_UBLOX_GPS::hardReset() sendCommand(&packetCfg, 0); // don't expect ACK } -int SFE_UBLOX_GPS::transferWriteI2C(u8_t *buffer, u32_t num_bytes, bool stop) { +// Write data to I2C, Arguments: buffer - data to write, num_bytes - buffer length, +// stop - if true send STOP, if false send RESTART after message +int SFE_UBLOX_GPS::transferWriteI2C(u8_t *buffer, u32_t num_bytes, bool stop) +{ struct i2c_msg msgs[1]; - /* Send the address to write to */ - //msgs[0].buf = 0x42; - //msgs[0].len = 1; - //msgs[0].flags = I2C_MSG_WRITE; - - //printk("transferWriteI2C: %x, num_bytes: %d\n", buffer[0], num_bytes); - - /* Data to be written, and STOP or RESTART after this. */ + // Data to be written and STOP or RESTART after this. msgs[0].buf = buffer; msgs[0].len = num_bytes; if (stop) { msgs[0].flags = I2C_MSG_WRITE | I2C_MSG_STOP; - //printk("transferWriteI2C - I2C_MSG_STOP\n"); } else { msgs[0].flags = I2C_MSG_WRITE | I2C_MSG_RESTART; - //printk("transferWriteI2C - I2C_MSG_RESTART\n"); } return i2c_transfer(_i2cPort, &msgs[0], 1, _gpsI2Caddress); } -int SFE_UBLOX_GPS::transferReadI2C(u8_t *buffer, u32_t num_bytes) { +// Read data from I2C, Arguments: buffer - store read data, num_bytes - buffer length +int SFE_UBLOX_GPS::transferReadI2C(u8_t *buffer, u32_t num_bytes) +{ struct i2c_msg msgs[1]; - /* Send the address to write to */ - //msgs[0].buf = (u8_t)_gpsI2Caddress; - //msgs[0].len = 2U; - //msgs[0].flags = I2C_MSG_READ; - - /* Data to be read, and STOP or RESTART after this. */ + // Data to be read and STOP after this msgs[0].buf = buffer; msgs[0].len = num_bytes; msgs[0].flags = I2C_MSG_READ | I2C_MSG_STOP; @@ -341,29 +339,20 @@ bool SFE_UBLOX_GPS::checkUbloxI2C(ubxPacket *incomingUBX, uint8_t requestedClass { if (k_uptime_get_32() - lastCheck >= i2cPollingWait) { - int err; - //Get the number of bytes available from the module uint16_t bytesAvailable = 0; - /* - _i2cPort->beginTransmission(_gpsI2Caddress); // TODO - _i2cPort->write(0xFD); //0xFD (MSB) and 0xFE (LSB) are the registers that contain number of bytes available - if (_i2cPort->endTransmission(false) != 0) //Send a restart command. Do not release bus. - return (false); //Sensor did not ACK - */ + int err; + //Get the number of bytes available from the module + //0xFD (MSB) and 0xFE (LSB) are the registers that contain number of bytes available u8_t data_buffer[1]; data_buffer[0] = 0xFD; - //err = i2c_write(_i2cPort, data_buffer, 1, _gpsI2Caddress); err = transferWriteI2C(data_buffer, 1, true); if (err) { return (false); } - //_i2cPort->requestFrom((uint8_t)_gpsI2Caddress, (uint8_t)2); // TODO - //if (_i2cPort->available()) bool data_read_ok = true; u8_t read_buffer[2]; - //err = i2c_read(_i2cPort, read_buffer, 2, _gpsI2Caddress); err = transferReadI2C(data_buffer, 2); if (err) { data_read_ok = false; @@ -371,12 +360,8 @@ bool SFE_UBLOX_GPS::checkUbloxI2C(ubxPacket *incomingUBX, uint8_t requestedClass if (data_read_ok) { - //uint8_t msb = _i2cPort->read(); // TODO - //uint8_t lsb = _i2cPort->read(); uint8_t msb = read_buffer[0]; uint8_t lsb = read_buffer[1]; - //printk("checkUbloxI2C, data_read_ok, MSB: 0x%x LSB: 0x%x\n", msb, lsb); - if (lsb == 0xFF) { //I believe this is a Ublox bug. Device should never present an 0xFF. @@ -443,15 +428,9 @@ bool SFE_UBLOX_GPS::checkUbloxI2C(ubxPacket *incomingUBX, uint8_t requestedClass while (bytesAvailable) { - /* - _i2cPort->beginTransmission(_gpsI2Caddress); // TODO - _i2cPort->write(0xFF); //0xFF is the register to read data from - if (_i2cPort->endTransmission(false) != 0) //Send a restart command. Do not release bus. - return (false); //Sensor did not ACK - */ u8_t data_buffer[1]; - data_buffer[0] = 0xFF; - err = transferWriteI2C(data_buffer, 1, false); + data_buffer[0] = 0xFF; //0xFF is the register to read data from + err = transferWriteI2C(data_buffer, 1, false); //Send a restart command. Do not release bus. if (err) { return (false); } @@ -462,8 +441,6 @@ bool SFE_UBLOX_GPS::checkUbloxI2C(ubxPacket *incomingUBX, uint8_t requestedClass bytesToRead = I2C_BUFFER_LENGTH; TRY_AGAIN: - //_i2cPort->requestFrom((uint8_t)_gpsI2Caddress, (uint8_t)bytesToRead); // TODO - //if (_i2cPort->available()) bool data_read_ok = true; u8_t read_buffer[(uint8_t)bytesToRead]; err = transferReadI2C(read_buffer, (uint8_t)bytesToRead); @@ -475,9 +452,7 @@ bool SFE_UBLOX_GPS::checkUbloxI2C(ubxPacket *incomingUBX, uint8_t requestedClass { for (uint16_t x = 0; x < bytesToRead; x++) { - //uint8_t incoming = _i2cPort->read(); //Grab the actual character // TODO - //i2c_read(_i2cPort, read_buffer, 1, _gpsI2Caddress); //Grab the actual character - uint8_t incoming = read_buffer[x]; + uint8_t incoming = read_buffer[x]; //Grab the actual character //Check to see if the first read is 0x7F. If it is, the module is not ready //to respond. Stop, wait, and try again @@ -1028,7 +1003,7 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg) highResModuleQueried.verticalAccuracy = true; moduleQueried.gpsiTOW = true; // this can arrive via HPPOS too. - if (_printDebug == true) // TODO check float prints + if (_printDebug == true) { printk("Sec: %f", ((float)extractLong(4)) / 1000.0f); printk("\n"); @@ -1115,32 +1090,14 @@ sfe_ublox_status_e SFE_UBLOX_GPS::sendI2cCommand(ubxPacket *outgoingUBX, uint16_ { int err; //Point at 0xFF data register - /* // TODO - _i2cPort->beginTransmission((uint8_t)_gpsI2Caddress); //There is no register to write to, we just begin writing data bytes - _i2cPort->write(0xFF); - if (_i2cPort->endTransmission() != 0) //Don't release bus - return (SFE_UBLOX_STATUS_I2C_COMM_FAILURE); //Sensor did not ACK - */ u8_t small_buffer[1]; small_buffer[0] = 0xFF; - //err = i2c_write(_i2cPort, small_buffer, 1, _gpsI2Caddress); - err = transferWriteI2C(small_buffer, 1); + err = transferWriteI2C(small_buffer, 1); //There is no register to write to, we just begin writing data bytes if (err) { return (SFE_UBLOX_STATUS_I2C_COMM_FAILURE); //Sensor did not ACK } //Write header bytes - /* // TODO - _i2cPort->beginTransmission((uint8_t)_gpsI2Caddress); //There is no register to write to, we just begin writing data bytes - _i2cPort->write(UBX_SYNCH_1); //μ - oh ublox, you're funny. I will call you micro-blox from now on. - _i2cPort->write(UBX_SYNCH_2); //b - _i2cPort->write(outgoingUBX->cls); - _i2cPort->write(outgoingUBX->id); - _i2cPort->write(outgoingUBX->len & 0xFF); //LSB - _i2cPort->write(outgoingUBX->len >> 8); //MSB - if (_i2cPort->endTransmission(false) != 0) //Do not release bus - return (SFE_UBLOX_STATUS_I2C_COMM_FAILURE); //Sensor did not ACK - */ u8_t header_buffer[6]; //There is no register to write to, we just begin writing data bytes header_buffer[0] = UBX_SYNCH_1; //μ - oh ublox, you're funny. I will call you micro-blox from now on. header_buffer[1] = UBX_SYNCH_2; //b @@ -1148,7 +1105,6 @@ sfe_ublox_status_e SFE_UBLOX_GPS::sendI2cCommand(ubxPacket *outgoingUBX, uint16_ header_buffer[3] = outgoingUBX->id; header_buffer[4] = outgoingUBX->len & 0xFF; //LSB header_buffer[5] = outgoingUBX->len >> 8; //MSB - //err = i2c_write(_i2cPort, header_buffer, 6, _gpsI2Caddress); err = transferWriteI2C(header_buffer, 6, false); k_msleep(1); // added this millisecond sleep to not progress too fast @@ -1168,22 +1124,12 @@ sfe_ublox_status_e SFE_UBLOX_GPS::sendI2cCommand(ubxPacket *outgoingUBX, uint16_ uint8_t len = bytesToSend; if (len > I2C_BUFFER_LENGTH) len = I2C_BUFFER_LENGTH; - /* // TODO - _i2cPort->beginTransmission((uint8_t)_gpsI2Caddress); - //_i2cPort->write(outgoingUBX->payload, len); //Write a portion of the payload to the bus - - for (uint16_t x = 0; x < len; x++) - _i2cPort->write(outgoingUBX->payload[startSpot + x]); //Write a portion of the payload to the bus - if (_i2cPort->endTransmission(false) != 0) //Don't release bus - return (SFE_UBLOX_STATUS_I2C_COMM_FAILURE); //Sensor did not ACK - */ u8_t data_buffer[len]; for (uint16_t x = 0; x < len; x++) { data_buffer[x] = outgoingUBX->payload[startSpot + x]; } - //err = i2c_write(_i2cPort, data_buffer, len, _gpsI2Caddress); - err = transferWriteI2C(data_buffer, len, false); + err = transferWriteI2C(data_buffer, len, false); //Write a portion of the payload to the bus if (err) { return (SFE_UBLOX_STATUS_I2C_COMM_FAILURE); //Sensor did not ACK } @@ -1194,11 +1140,7 @@ sfe_ublox_status_e SFE_UBLOX_GPS::sendI2cCommand(ubxPacket *outgoingUBX, uint16_ } //Write checksum - //_i2cPort->beginTransmission((uint8_t)_gpsI2Caddress); // TODO if (bytesToSend == 1) { - //_i2cPort->write(outgoingUBX->payload, 1); - - //i2c_write(_i2cPort, outgoingUBX->payload, 1, _gpsI2Caddress); u8_t checksum_buffer[3]; uint8_t payload = *outgoingUBX->payload; checksum_buffer[0] = payload; @@ -1212,10 +1154,6 @@ sfe_ublox_status_e SFE_UBLOX_GPS::sendI2cCommand(ubxPacket *outgoingUBX, uint16_ checksum_buffer[1] = outgoingUBX->checksumB; err = transferWriteI2C(checksum_buffer, 2, true); } - //_i2cPort->write(outgoingUBX->checksumA); // TODO - //_i2cPort->write(outgoingUBX->checksumB); - - //err = i2c_write(_i2cPort, checksum_buffer, 3, _gpsI2Caddress); if (err) { return (SFE_UBLOX_STATUS_I2C_COMM_FAILURE); //Sensor did not ACK } @@ -1253,20 +1191,12 @@ bool SFE_UBLOX_GPS::isConnected(uint16_t maxWait) { if (commType == COMM_TYPE_I2C) { - /* // TODO - _i2cPort->beginTransmission((uint8_t)_gpsI2Caddress); - if (_i2cPort->endTransmission() != 0) - return false; //Sensor did not ack - */ - u8_t buff[1]; buff[0] = 0x00; - //int err = i2c_write(_i2cPort, buff, 1, _gpsI2Caddress); int err = transferWriteI2C(buff, 1); if (err) { return false; //Sensor did not ack } - } // Query navigation rate to see whether we get a meaningful response @@ -1276,7 +1206,7 @@ bool SFE_UBLOX_GPS::isConnected(uint16_t maxWait) packetCfg.startingSpot = 0; //return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_RECEIVED); // We are polling the RATE so we expect data and an ACK - return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // data send is returned not data received - TODO library bug? + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // data send is returned, not data received - TODO library bug? } //Given a message, calc and store the two byte "8-Bit Fletcher" checksum over the entirety of the message diff --git a/src/SparkFun_Ublox_Zephyr_Library.h b/src/SparkFun_Ublox_Zephyr_Library.h index 76c6abb..0aede99 100644 --- a/src/SparkFun_Ublox_Zephyr_Library.h +++ b/src/SparkFun_Ublox_Zephyr_Library.h @@ -17,7 +17,9 @@ https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library Development environment specifics: - Arduino IDE 1.8.5 + NCS v1.0.3 release + + This port was made by Vid Rajtmajer , IRNAS d.o.o. SparkFun code, firmware, and software is released under the MIT License(http://opensource.org/licenses/MIT). The MIT License (MIT) @@ -437,16 +439,16 @@ class SFE_UBLOX_GPS // If you know you are only going to be using I2C / Qwiic communication, you can // safely reduce defaultMaxWait to 250. #ifndef defaultMaxWait // Let's allow the user to define their own value if they want to -#define defaultMaxWait 1100 // TODO +#define defaultMaxWait 250 // only I2C is used, so this has been reduced from 1100 #endif // Set gpio device, used by checksumFailurePin bool init_gpio_pins(struct device &gpio_dev); - //By default use the default I2C address, and use Wire port + //By default use the default I2C address bool begin(struct device &i2c_dev, uint8_t deviceAddress = 0x42); //Returns true if module is detected //serialPort needs to be perviously initialized to correct baud rate - //bool begin(Stream &serialPort); //Returns true if module is detected // TODO + //bool begin(Stream &serialPort); //Returns true if module is detected - function not ported //Returns true if device answers on _gpsI2Caddress address or via Serial //maxWait is only used for Serial @@ -896,4 +898,4 @@ class SFE_UBLOX_GPS uint16_t rtcmLen = 0; }; -#endif +#endif //SPARKFUN_UBLOX_ZEPHYR_LIBRARY_H diff --git a/src/ublox_lib_interface.cpp b/src/ublox_lib_interface.cpp index 62b1019..a521d93 100644 --- a/src/ublox_lib_interface.cpp +++ b/src/ublox_lib_interface.cpp @@ -1,57 +1,82 @@ +/* + This is an interface that connects the CPP Ublox library with the main C code. + Added to make it possible to run Ublox lib on Zephyr (NCS) + + This port was made by Vid Rajtmajer , www.irnas.eu +*/ #include "ublox_lib_interface.h" +#include +#include +#include +#include + #include "SparkFun_Ublox_Zephyr_Library.h" -SFE_UBLOX_GPS myGPS; -long lastTime = 0; //Simple local timer. Limits amount if I2C traffic to Ublox module. +SFE_UBLOX_GPS myGPS; // driver class instance +long lastTime = 0; // Simple local timer. Limits amount if I2C traffic to Ublox module. -uint8_t set_gpio_dev(struct device *gpio_dev) { - if (myGPS.init_gpio_pins(*gpio_dev) == false) { - return -1; +// init GPIO checksumFailurePin and load GPIO device pointer to the driver +uint8_t set_gpio_dev(struct device *gpio_dev, uint8_t enable_debug) +{ + if (myGPS.init_gpio_pins(*gpio_dev) == false) + { + return -EIO; + } + // turn on debugging if enable_debug is set + if (enable_debug) + { + myGPS.enableDebugging(); } - - // This will pipe all NMEA sentences to the serial port so we can see them - //myGPS.setNMEAOutputPort(); // uncomment this to see raw prints - // turn on debugging - myGPS.enableDebugging(); return 0; } -uint8_t gps_begin(struct device *i2c_dev) { - if (myGPS.begin(*i2c_dev) == false) { - return -1; +// initialize I2C and check if GPS device respons +uint8_t gps_begin(struct device *i2c_dev) +{ + if (myGPS.begin(*i2c_dev) == false) + { + return -EIO; } - - myGPS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise) - myGPS.saveConfiguration(); //Save the current settings to flash and BBR - return 0; } -void check_ublox(void) { - myGPS.checkUblox(); +// This will pipe all NMEA sentences to UART so we can see them +void pipe_nmea_sentences(void) +{ + myGPS.setNMEAOutputPort(); } -void get_position(void) +// Check for available bytes from the device +void check_ublox(void) { - //Query module only every second. Doing it more often will just cause I2C traffic. - //The module only responds when a new position is available - if (k_uptime_get_32() - lastTime > 1000) - { - lastTime = k_uptime_get_32(); //Update the timer + myGPS.checkUblox(); +} - long latitude = myGPS.getLatitude(); - long longitude = myGPS.getLongitude(); - long altitude = myGPS.getAltitude(); +// Get position information when requested, also display number of satellites used in the fix +int get_position(void) +{ + //Query module only every second. Doing it more often will just cause I2C traffic. + //The module only responds when a new position is available, print it to console + if (k_uptime_get_32() - lastTime > 1000) + { + lastTime = k_uptime_get_32(); //Update the timer - uint8_t SIV = myGPS.getSIV(); + long latitude = myGPS.getLatitude(); + long longitude = myGPS.getLongitude(); + long altitude = myGPS.getAltitude(); + uint8_t SIV = myGPS.getSIV(); - printk("Position: Lat: %ld, Lon: %ld, Alt: %ld, SIV: %d\n", latitude, longitude, altitude, SIV); - } + printk("Position: Lat: %ld, Lon: %ld, Alt: %ld, SIV: %d", latitude, longitude, altitude, SIV); + return 0; + } + return -EBUSY; } -void get_datetime(void) { +// Get date and time information when requested, check if they are valid and print info to console, it returns UNIX time +void get_datetime(void) +{ int year = myGPS.getYear(); int month = myGPS.getMonth(); int day = myGPS.getDay(); @@ -72,4 +97,4 @@ void get_datetime(void) { printk("not "); } printk("valid.\n"); -} \ No newline at end of file +} diff --git a/src/ublox_lib_interface.h b/src/ublox_lib_interface.h index 3f1935b..0af3d3c 100644 --- a/src/ublox_lib_interface.h +++ b/src/ublox_lib_interface.h @@ -1,3 +1,10 @@ +/* + This is an interface that connects the CPP Ublox library with the main C code. + Added to make it possible to run Ublox lib on Zephyr (NCS) + + This port was made by Vid Rajtmajer , www.irnas.eu +*/ +#include #include #ifndef _UBLOX_LIB_INTERFACE_H_ @@ -7,15 +14,16 @@ extern "C" { #endif - uint8_t set_gpio_dev(struct device *gpio_dev); + uint8_t set_gpio_dev(struct device *gpio_dev, uint8_t enable_debug); // init GPIO + uint8_t gps_begin(struct device *i2c_dev); // initialize I2C and check if GPS device respons + void pipe_nmea_sentences(void); // print NMEA sentences - uint8_t gps_begin(struct device *i2c_dev); - void check_ublox(void); - void get_position(void); - void get_datetime(void); + void check_ublox(void); // Check for available bytes from the device + int get_position(void); // Get position information + void get_datetime(void); // Get date and time information #ifdef __cplusplus } #endif -#endif // _UBLOX_LIB_INTERFACE_H_ \ No newline at end of file +#endif //UBLOX_LIB_INTERFACE_H_ \ No newline at end of file From 9ccdb9f7492ab65d1d36d4ef06a60d479cc6e634 Mon Sep 17 00:00:00 2001 From: vid Date: Mon, 2 Nov 2020 15:52:40 +0100 Subject: [PATCH 17/36] changed prints --- examples/Example_Zephyr/src/main.c | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/examples/Example_Zephyr/src/main.c b/examples/Example_Zephyr/src/main.c index e62adac..cc819d0 100644 --- a/examples/Example_Zephyr/src/main.c +++ b/examples/Example_Zephyr/src/main.c @@ -17,7 +17,6 @@ #include #include #include -#include #include #include @@ -26,8 +25,6 @@ #define I2C_DEV "I2C_0" -LOG_MODULE_REGISTER(zephyr_main); // init logging - struct device *gpio_dev; struct device *i2c_dev; /* I2C pins used are defaults for I2C_0 on nrf52840 @@ -39,7 +36,7 @@ uint8_t init_gpio(void) { const char* gpioName = "GPIO_0"; gpio_dev = device_get_binding(gpioName); if (gpio_dev == NULL) { - LOG_ERR("Could not get %s device", gpioName); + printk("Error: Could not get %s device\n", gpioName); return -EIO; } int err = set_gpio_dev(gpio_dev); @@ -53,12 +50,12 @@ uint8_t init_i2c(void) { i2c_dev = device_get_binding(I2C_DEV); if (!i2c_dev) { - LOG_ERR("I2C_0 error"); + printk("I2C_0 error\n"); return -1; } else { - LOG_INF("I2C_0 Init OK"); + printk("I2C_0 Init OK\n"); return 0; } } @@ -66,7 +63,7 @@ uint8_t init_i2c(void) { uint8_t init_gps(void) { if (gps_begin(i2c_dev) != 0) { - LOG_ERR("Ublox GPS init error!"); + printk("Ublox GPS init error!\n"); return -1; } return 0; @@ -74,7 +71,7 @@ uint8_t init_gps(void) { void main(void) { - LOG_INF("Ublox Zephyr example"); + printk("Ublox Zephyr example\n"); int err; err = init_gpio(); From 4c3482bf22b91d7ba942f101cc75db0a533de710 Mon Sep 17 00:00:00 2001 From: vid Date: Mon, 2 Nov 2020 15:55:17 +0100 Subject: [PATCH 18/36] add comment --- examples/Example_Zephyr/src/main.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/examples/Example_Zephyr/src/main.c b/examples/Example_Zephyr/src/main.c index cc819d0..2cc47af 100644 --- a/examples/Example_Zephyr/src/main.c +++ b/examples/Example_Zephyr/src/main.c @@ -13,6 +13,9 @@ I2C clock speed: 100 kHz Ported to Zephyr by Vid Rajtmajer , www.irnas.eu + + To build: west build -b -p + To flash: west flash --erase */ #include #include From bb615686d606913f92d0f4613847399c0e982688 Mon Sep 17 00:00:00 2001 From: vid Date: Mon, 2 Nov 2020 16:01:03 +0100 Subject: [PATCH 19/36] bug fix --- examples/Example_Zephyr/CMakeLists.txt | 2 ++ examples/Example_Zephyr/src/main.c | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/Example_Zephyr/CMakeLists.txt b/examples/Example_Zephyr/CMakeLists.txt index cd3ef07..91bf013 100644 --- a/examples/Example_Zephyr/CMakeLists.txt +++ b/examples/Example_Zephyr/CMakeLists.txt @@ -5,6 +5,8 @@ cmake_minimum_required(VERSION 3.13.1) find_package(Zephyr HINTS $ENV{ZEPHYR_BASE}) project(sparkfun_ublox_zephyr_library) +zephyr_compile_options(-fdiagnostics-color=always) + zephyr_include_directories(../../src/) target_sources(app PRIVATE ../../src/SparkFun_Ublox_Zephyr_Library.cpp) target_sources(app PRIVATE ../../src/ublox_lib_interface.cpp) diff --git a/examples/Example_Zephyr/src/main.c b/examples/Example_Zephyr/src/main.c index 2cc47af..f4bab02 100644 --- a/examples/Example_Zephyr/src/main.c +++ b/examples/Example_Zephyr/src/main.c @@ -42,7 +42,7 @@ uint8_t init_gpio(void) { printk("Error: Could not get %s device\n", gpioName); return -EIO; } - int err = set_gpio_dev(gpio_dev); + int err = set_gpio_dev(gpio_dev, true); // set GPIO_0 device and enable debugging if (err) { return -EIO; } From 16bea65553d60aaa16ca80baeb62e895fcede0ae Mon Sep 17 00:00:00 2001 From: vid Date: Mon, 2 Nov 2020 16:21:14 +0100 Subject: [PATCH 20/36] update comment --- src/SparkFun_Ublox_Zephyr_Library.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/SparkFun_Ublox_Zephyr_Library.h b/src/SparkFun_Ublox_Zephyr_Library.h index 0aede99..7bd3958 100644 --- a/src/SparkFun_Ublox_Zephyr_Library.h +++ b/src/SparkFun_Ublox_Zephyr_Library.h @@ -19,7 +19,7 @@ Development environment specifics: NCS v1.0.3 release - This port was made by Vid Rajtmajer , IRNAS d.o.o. + This port was made by Vid Rajtmajer , IRNAS www.irnas.eu SparkFun code, firmware, and software is released under the MIT License(http://opensource.org/licenses/MIT). The MIT License (MIT) From ec1661befca85fb0a37c872fe1690e80538ea178 Mon Sep 17 00:00:00 2001 From: Balamurugan Kandan Date: Tue, 3 Nov 2020 20:56:47 +0000 Subject: [PATCH 21/36] NAV-PVT velocity parameters parsed. --- src/SparkFun_Ublox_Arduino_Library.cpp | 65 +++++++++++++++++++++++++- src/SparkFun_Ublox_Arduino_Library.h | 19 ++++++++ 2 files changed, 83 insertions(+), 1 deletion(-) diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp index 81994a1..7011ca5 100644 --- a/src/SparkFun_Ublox_Arduino_Library.cpp +++ b/src/SparkFun_Ublox_Arduino_Library.cpp @@ -986,6 +986,12 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg) latitude = extractLong(28 - startingSpot); altitude = extractLong(32 - startingSpot); altitudeMSL = extractLong(36 - startingSpot); + horizontalAccEst = extractLong(40 - startingSpot); + verticalAccEst = extractLong(44 - startingSpot); + nedNorthVel = extractLong(48 - startingSpot); + nedEastVel = extractLong(52 - startingSpot); + nedDownVel = extractLong(56 - startingSpot); + groundSpeed = extractLong(60 - startingSpot); headingOfMotion = extractLong(64 - startingSpot); pDOP = extractInt(76 - startingSpot); @@ -1007,6 +1013,13 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg) moduleQueried.latitude = true; moduleQueried.altitude = true; moduleQueried.altitudeMSL = true; + + moduleQueried.horizontalAccEst = true; + moduleQueried.verticalAccEst = true; + moduleQueried.nedNorthVel = true; + moduleQueried.nedEastVel = true; + moduleQueried.nedDownVel = true; + moduleQueried.SIV = true; moduleQueried.fixType = true; moduleQueried.carrierSolution = true; @@ -3308,6 +3321,56 @@ int32_t SFE_UBLOX_GPS::getAltitudeMSL(uint16_t maxWait) return (altitudeMSL); } +int32_t SFE_UBLOX_GPS::getHorizontalAccEst(uint16_t maxWait) +{ + if (moduleQueried.horizontalAccEst == false) + getPVT(maxWait); + moduleQueried.horizontalAccEst = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (horizontalAccEst); +} + +int32_t SFE_UBLOX_GPS::getVerticalAccEst(uint16_t maxWait) +{ + if (moduleQueried.verticalAccEst == false) + getPVT(maxWait); + moduleQueried.verticalAccEst = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (verticalAccEst); +} + +int32_t SFE_UBLOX_GPS::getNedNorthVel(uint16_t maxWait) +{ + if (moduleQueried.nedNorthVel == false) + getPVT(maxWait); + moduleQueried.nedNorthVel = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (nedNorthVel); +} + +int32_t SFE_UBLOX_GPS::getNedEastVel(uint16_t maxWait) +{ + if (moduleQueried.nedEastVel == false) + getPVT(maxWait); + moduleQueried.nedEastVel = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (nedEastVel); +} + +int32_t SFE_UBLOX_GPS::getNedDownVel(uint16_t maxWait) +{ + if (moduleQueried.nedDownVel == false) + getPVT(maxWait); + moduleQueried.nedDownVel = false; //Since we are about to give this to user, mark this data as stale + moduleQueried.all = false; + + return (nedDownVel); +} + //Get the number of satellites used in fix uint8_t SFE_UBLOX_GPS::getSIV(uint16_t maxWait) { @@ -3800,4 +3863,4 @@ bool SFE_UBLOX_GPS::setStaticPosition(int32_t ecefXOrLat, int8_t ecefXOrLatHP, i bool SFE_UBLOX_GPS::setStaticPosition(int32_t ecefXOrLat, int32_t ecefYOrLon, int32_t ecefZOrAlt, bool latlong, uint16_t maxWait) { return (setStaticPosition(ecefXOrLat, 0, ecefYOrLon, 0, ecefZOrAlt, 0, latlong, maxWait)); -} \ No newline at end of file +} diff --git a/src/SparkFun_Ublox_Arduino_Library.h b/src/SparkFun_Ublox_Arduino_Library.h index 7405335..e84498a 100644 --- a/src/SparkFun_Ublox_Arduino_Library.h +++ b/src/SparkFun_Ublox_Arduino_Library.h @@ -508,6 +508,13 @@ class SFE_UBLOX_GPS int32_t getLongitude(uint16_t maxWait = getPVTmaxWait); //Returns the current longitude in degrees * 10-7. Auto selects between HighPrecision and Regular depending on ability of module. int32_t getAltitude(uint16_t maxWait = getPVTmaxWait); //Returns the current altitude in mm above ellipsoid int32_t getAltitudeMSL(uint16_t maxWait = getPVTmaxWait); //Returns the current altitude in mm above mean sea level + + int32_t getHorizontalAccEst(uint16_t maxWait = getPVTmaxWait); + int32_t getVerticalAccEst(uint16_t maxWait = getPVTmaxWait); + int32_t getNedNorthVel(uint16_t maxWait = getPVTmaxWait); + int32_t getNedEastVel(uint16_t maxWait = getPVTmaxWait); + int32_t getNedDownVel(uint16_t maxWait = getPVTmaxWait); + uint8_t getSIV(uint16_t maxWait = getPVTmaxWait); //Returns number of sats used in fix uint8_t getFixType(uint16_t maxWait = getPVTmaxWait); //Returns the type of fix: 0=no, 3=3D, 4=GNSS+Deadreckoning uint8_t getCarrierSolutionType(uint16_t maxWait = getPVTmaxWait); //Returns RTK solution: 0=no, 1=float solution, 2=fixed solution @@ -691,6 +698,11 @@ class SFE_UBLOX_GPS int32_t longitude; //Degrees * 10^-7 (more accurate than floats) int32_t altitude; //Number of mm above ellipsoid int32_t altitudeMSL; //Number of mm above Mean Sea Level + uint32_t horizontalAccEst; + uint32_t verticalAccEst; + int32_t nedNorthVel; + int32_t nedEastVel; + int32_t nedDownVel; uint8_t SIV; //Number of satellites used in position solution uint8_t fixType; //Tells us when we have a solution aka lock uint8_t carrierSolution; //Tells us when we have an RTK float/fixed solution @@ -877,6 +889,13 @@ class SFE_UBLOX_GPS uint32_t latitude : 1; uint32_t altitude : 1; uint32_t altitudeMSL : 1; + + uint32_t horizontalAccEst : 1; + uint32_t verticalAccEst : 1; + uint32_t nedNorthVel : 1; + uint32_t nedEastVel : 1; + uint32_t nedDownVel : 1; + uint32_t SIV : 1; uint32_t fixType : 1; uint32_t carrierSolution : 1; From 2f3493cd30783a0777ca3bc3f70caa9151b99b0f Mon Sep 17 00:00:00 2001 From: Balamurugan Kandan Date: Wed, 4 Nov 2020 09:53:25 +0000 Subject: [PATCH 22/36] keywords.txt file is updated for newly added functions. --- keywords.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/keywords.txt b/keywords.txt index 08b919c..66b7b65 100644 --- a/keywords.txt +++ b/keywords.txt @@ -53,6 +53,11 @@ getGroundSpeed KEYWORD2 getHeading KEYWORD2 getPDOP KEYWORD2 getTimeOfWeek KEYWORD2 +getHorizontalAccEst KEYWORD2 +getVerticalAccEst KEYWORD2 +getNedNorthVel KEYWORD2 +getNedEastVel KEYWORD2 +getNedDownVel KEYWORD2 setPortOutput KEYWORD2 setPortInput KEYWORD2 From 436486b84de1f90d62f225d5b90760363a8a6781 Mon Sep 17 00:00:00 2001 From: Balamurugan Kandan Date: Wed, 4 Nov 2020 10:18:36 +0000 Subject: [PATCH 23/36] NAV-PVT newly added functions demonstrated in example and keywords.txt corrected. --- .../Example1_AutoPVT/Example1_AutoPVT.ino | 27 ++++++++++++++++++- keywords.txt | 10 +++---- 2 files changed, 31 insertions(+), 6 deletions(-) diff --git a/examples/Example13_PVT/Example1_AutoPVT/Example1_AutoPVT.ino b/examples/Example13_PVT/Example1_AutoPVT/Example1_AutoPVT.ino index 28ffa26..b5dd97e 100644 --- a/examples/Example13_PVT/Example1_AutoPVT/Example1_AutoPVT.ino +++ b/examples/Example13_PVT/Example1_AutoPVT/Example1_AutoPVT.ino @@ -78,7 +78,32 @@ void loop() int PDOP = myGPS.getPDOP(); Serial.print(F(" PDOP: ")); Serial.print(PDOP); - Serial.print(F(" (10^-2)")); + Serial.print(F(" (10^-2)")); + + int nedNorthVel = myGPS.getNedNorthVel(); + Serial.print(F(" VelN: ")); + Serial.print(nedNorthVel); + Serial.print(F(" (mm/s)")); + + int nedEastVel = myGPS.getNedEastVel(); + Serial.print(F(" VelE: ")); + Serial.print(nedEastVel); + Serial.print(F(" (mm/s)")); + + int nedDownVel = myGPS.getNedDownVel(); + Serial.print(F(" VelD: ")); + Serial.print(nedDownVel); + Serial.print(F(" (mm/s)")); + + int verticalAccEst = myGPS.getVerticalAccEst(); + Serial.print(F(" VAccEst: ")); + Serial.print(verticalAccEst); + Serial.print(F(" (mm)")); + + int horizontalAccEst = myGPS.getHorizontalAccEst(); + Serial.print(F(" HAccEst: ")); + Serial.print(horizontalAccEst); + Serial.print(F(" (mm)")); Serial.println(); } else { diff --git a/keywords.txt b/keywords.txt index 66b7b65..f3f9dcf 100644 --- a/keywords.txt +++ b/keywords.txt @@ -53,11 +53,11 @@ getGroundSpeed KEYWORD2 getHeading KEYWORD2 getPDOP KEYWORD2 getTimeOfWeek KEYWORD2 -getHorizontalAccEst KEYWORD2 -getVerticalAccEst KEYWORD2 -getNedNorthVel KEYWORD2 -getNedEastVel KEYWORD2 -getNedDownVel KEYWORD2 +getHorizontalAccEst KEYWORD2 +getVerticalAccEst KEYWORD2 +getNedNorthVel KEYWORD2 +getNedEastVel KEYWORD2 +getNedDownVel KEYWORD2 setPortOutput KEYWORD2 setPortInput KEYWORD2 From a2ad45fa67fe97ad7544d87d20889c306c06b27f Mon Sep 17 00:00:00 2001 From: vid Date: Wed, 4 Nov 2020 14:22:27 +0100 Subject: [PATCH 24/36] directory reorganisation --- .gitignore | 2 +- .../CMakeLists.txt | 6 ++-- .../nrf52840dk_nrf52840.overlay | 0 .../prj.conf | 0 .../src/SparkFun_Ublox_Zephyr_Interface.cpp | 2 +- .../src/SparkFun_Ublox_Zephyr_Interface.h | 0 .../src}/SparkFun_Ublox_Zephyr_Library.cpp | 0 .../src}/SparkFun_Ublox_Zephyr_Library.h | 0 .../src/main.c | 36 ++++++++++--------- 9 files changed, 24 insertions(+), 22 deletions(-) rename examples/{Example_Zephyr => Zephyr/Example1_GetPositionAndTime_Zephyr}/CMakeLists.txt (62%) rename examples/{Example_Zephyr => Zephyr/Example1_GetPositionAndTime_Zephyr}/nrf52840dk_nrf52840.overlay (100%) rename examples/{Example_Zephyr => Zephyr/Example1_GetPositionAndTime_Zephyr}/prj.conf (100%) rename src/ublox_lib_interface.cpp => examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src/SparkFun_Ublox_Zephyr_Interface.cpp (98%) rename src/ublox_lib_interface.h => examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src/SparkFun_Ublox_Zephyr_Interface.h (100%) rename {src => examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src}/SparkFun_Ublox_Zephyr_Library.cpp (100%) rename {src => examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src}/SparkFun_Ublox_Zephyr_Library.h (100%) rename examples/{Example_Zephyr => Zephyr/Example1_GetPositionAndTime_Zephyr}/src/main.c (56%) diff --git a/.gitignore b/.gitignore index 5b0c1cd..58b4bef 100644 --- a/.gitignore +++ b/.gitignore @@ -55,4 +55,4 @@ Temporary Items *.swp # Zephyr build files -examples/Example_Zephyr/build/* \ No newline at end of file +examples/Zephyr/*/build/* \ No newline at end of file diff --git a/examples/Example_Zephyr/CMakeLists.txt b/examples/Zephyr/Example1_GetPositionAndTime_Zephyr/CMakeLists.txt similarity index 62% rename from examples/Example_Zephyr/CMakeLists.txt rename to examples/Zephyr/Example1_GetPositionAndTime_Zephyr/CMakeLists.txt index 91bf013..3dd6c0a 100644 --- a/examples/Example_Zephyr/CMakeLists.txt +++ b/examples/Zephyr/Example1_GetPositionAndTime_Zephyr/CMakeLists.txt @@ -7,8 +7,8 @@ project(sparkfun_ublox_zephyr_library) zephyr_compile_options(-fdiagnostics-color=always) -zephyr_include_directories(../../src/) -target_sources(app PRIVATE ../../src/SparkFun_Ublox_Zephyr_Library.cpp) -target_sources(app PRIVATE ../../src/ublox_lib_interface.cpp) +zephyr_include_directories(.) +target_sources(app PRIVATE src/SparkFun_Ublox_Zephyr_Library.cpp) +target_sources(app PRIVATE src/SparkFun_Ublox_Zephyr_Interface.cpp) target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src/main.c) diff --git a/examples/Example_Zephyr/nrf52840dk_nrf52840.overlay b/examples/Zephyr/Example1_GetPositionAndTime_Zephyr/nrf52840dk_nrf52840.overlay similarity index 100% rename from examples/Example_Zephyr/nrf52840dk_nrf52840.overlay rename to examples/Zephyr/Example1_GetPositionAndTime_Zephyr/nrf52840dk_nrf52840.overlay diff --git a/examples/Example_Zephyr/prj.conf b/examples/Zephyr/Example1_GetPositionAndTime_Zephyr/prj.conf similarity index 100% rename from examples/Example_Zephyr/prj.conf rename to examples/Zephyr/Example1_GetPositionAndTime_Zephyr/prj.conf diff --git a/src/ublox_lib_interface.cpp b/examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src/SparkFun_Ublox_Zephyr_Interface.cpp similarity index 98% rename from src/ublox_lib_interface.cpp rename to examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src/SparkFun_Ublox_Zephyr_Interface.cpp index a521d93..65505be 100644 --- a/src/ublox_lib_interface.cpp +++ b/examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src/SparkFun_Ublox_Zephyr_Interface.cpp @@ -4,7 +4,7 @@ This port was made by Vid Rajtmajer , www.irnas.eu */ -#include "ublox_lib_interface.h" +#include "SparkFun_Ublox_Zephyr_Interface.h" #include #include diff --git a/src/ublox_lib_interface.h b/examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src/SparkFun_Ublox_Zephyr_Interface.h similarity index 100% rename from src/ublox_lib_interface.h rename to examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src/SparkFun_Ublox_Zephyr_Interface.h diff --git a/src/SparkFun_Ublox_Zephyr_Library.cpp b/examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src/SparkFun_Ublox_Zephyr_Library.cpp similarity index 100% rename from src/SparkFun_Ublox_Zephyr_Library.cpp rename to examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src/SparkFun_Ublox_Zephyr_Library.cpp diff --git a/src/SparkFun_Ublox_Zephyr_Library.h b/examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src/SparkFun_Ublox_Zephyr_Library.h similarity index 100% rename from src/SparkFun_Ublox_Zephyr_Library.h rename to examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src/SparkFun_Ublox_Zephyr_Library.h diff --git a/examples/Example_Zephyr/src/main.c b/examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src/main.c similarity index 56% rename from examples/Example_Zephyr/src/main.c rename to examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src/main.c index f4bab02..32d2717 100644 --- a/examples/Example_Zephyr/src/main.c +++ b/examples/Zephyr/Example1_GetPositionAndTime_Zephyr/src/main.c @@ -1,21 +1,23 @@ /* - Read NMEA sentences over I2C using Ublox module SAM-M8Q, NEO-M8P, ZED-F9P, etc - By: Nathan Seidle - SparkFun Electronics - Date: August 22nd, 2018 - License: MIT. See license file for more information but you can - basically do whatever you want with this code. + Reading lat, long and UTC time via UBX binary commands + By: Nathan Seidle + SparkFun Electronics + Date: August 22nd, 2018 + License: MIT. See license file for more information but you can + basically do whatever you want with this code. - This example reads the NMEA setences from the Ublox module over I2c and outputs - them to the serial port + This example reads the NMEA setences from the Ublox module over I2c and outputs + them to the serial port - Open the serial monitor at 115200 baud to see the output - I2C clock speed: 100 kHz + Open the serial monitor at 115200 baud to see the output + I2C clock speed: 100 kHz - Ported to Zephyr by Vid Rajtmajer , www.irnas.eu + Ported to Zephyr by Vid Rajtmajer , www.irnas.eu - To build: west build -b -p - To flash: west flash --erase + Development environment specifics: NCS v1.0.3 release + + To build: west build -b -p Can also read NMEA sentences over I2C with check_ublox function + To flash: west flash --erase */ #include #include @@ -23,7 +25,7 @@ #include #include -#include "ublox_lib_interface.h" +#include "SparkFun_Ublox_Zephyr_Interface.h" #define I2C_DEV "I2C_0" @@ -74,7 +76,7 @@ uint8_t init_gps(void) { void main(void) { - printk("Ublox Zephyr example\n"); + printk("Zephyr Ublox example\n"); int err; err = init_gpio(); @@ -92,9 +94,9 @@ void main(void) { } while(1) { - //check_ublox(); // See if new data is available. Process bytes as they come in. + //check_ublox(); // See if new data is available. Process bytes as they come in. get_position(); get_datetime(); - k_msleep(250); // Don't pound too hard on the I2C bus + k_msleep(250); // Don't pound too hard on the I2C bus } } \ No newline at end of file From 04d8f4a2f70590d73335247d664f50282246ff12 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Tue, 17 Nov 2020 10:54:26 +0000 Subject: [PATCH 25/36] Please see description for full details: Corrected some bit fields and ANDs in the dead-reckoning functions. Added extractSignedLong to avoid casting ambiguity. Corrected getRELPOSNED for the M8. Function now supports both the M8 and F9. --- src/SparkFun_Ublox_Arduino_Library.cpp | 226 ++++++++++++++++--------- src/SparkFun_Ublox_Arduino_Library.h | 5 +- 2 files changed, 150 insertions(+), 81 deletions(-) diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp index 86a4387..4f82fd0 100644 --- a/src/SparkFun_Ublox_Arduino_Library.cpp +++ b/src/SparkFun_Ublox_Arduino_Library.cpp @@ -554,7 +554,7 @@ void SFE_UBLOX_GPS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t re //So let's check for an HPPOSLLH message arriving when we were expecting PVT and vice versa else if ((packetBuf.cls == requestedClass) && (((packetBuf.id == UBX_NAV_PVT) && (requestedID == UBX_NAV_HPPOSLLH || requestedID == UBX_NAV_DOP)) || - ((packetBuf.id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_DOP)) || + ((packetBuf.id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_DOP)) || ((packetBuf.id == UBX_NAV_DOP) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_HPPOSLLH)))) { //This is not the message we were expecting but we start diverting data into incomingUBX (usually packetCfg) and process it anyway @@ -827,7 +827,7 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t //So let's check for an HPPOSLLH message arriving when we were expecting PVT and vice versa else if ((incomingUBX->cls == requestedClass) && (((incomingUBX->id == UBX_NAV_PVT) && (requestedID == UBX_NAV_HPPOSLLH || requestedID == UBX_NAV_DOP)) || - ((incomingUBX->id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_DOP)) || + ((incomingUBX->id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_DOP)) || ((incomingUBX->id == UBX_NAV_DOP) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_HPPOSLLH)))) { // This isn't the message we are looking for... @@ -979,25 +979,25 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg) gpsSecond = extractByte(10); gpsDateValid = extractByte(11) & 0x01; gpsTimeValid = (extractByte(11) & 0x02) >> 1; - gpsNanosecond = extractLong(16); //Includes milliseconds + gpsNanosecond = extractSignedLong(16); //Includes milliseconds fixType = extractByte(20 - startingSpot); gnssFixOk = extractByte(21 - startingSpot) & 0x1; //Get the 1st bit diffSoln = extractByte(21 - startingSpot) >> 1 & 0x1; //Get the 2nd bit carrierSolution = extractByte(21 - startingSpot) >> 6; //Get 6th&7th bits of this byte SIV = extractByte(23 - startingSpot); - longitude = extractLong(24 - startingSpot); - latitude = extractLong(28 - startingSpot); - altitude = extractLong(32 - startingSpot); - altitudeMSL = extractLong(36 - startingSpot); + longitude = extractSignedLong(24 - startingSpot); + latitude = extractSignedLong(28 - startingSpot); + altitude = extractSignedLong(32 - startingSpot); + altitudeMSL = extractSignedLong(36 - startingSpot); horizontalAccEst = extractLong(40 - startingSpot); verticalAccEst = extractLong(44 - startingSpot); - nedNorthVel = extractLong(48 - startingSpot); - nedEastVel = extractLong(52 - startingSpot); - nedDownVel = extractLong(56 - startingSpot); + nedNorthVel = extractSignedLong(48 - startingSpot); + nedEastVel = extractSignedLong(52 - startingSpot); + nedDownVel = extractSignedLong(56 - startingSpot); - groundSpeed = extractLong(60 - startingSpot); - headingOfMotion = extractLong(64 - startingSpot); + groundSpeed = extractSignedLong(60 - startingSpot); + headingOfMotion = extractSignedLong(64 - startingSpot); pDOP = extractInt(76 - startingSpot); //Mark all datums as fresh (not read before) @@ -1036,10 +1036,10 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg) else if (msg->id == UBX_NAV_HPPOSLLH && msg->len == 36) { timeOfWeek = extractLong(4); - highResLongitude = extractLong(8); - highResLatitude = extractLong(12); - elipsoid = extractLong(16); - meanSeaLevel = extractLong(20); + highResLongitude = extractSignedLong(8); + highResLatitude = extractSignedLong(12); + elipsoid = extractSignedLong(16); + meanSeaLevel = extractSignedLong(20); highResLongitudeHp = extractSignedChar(24); highResLatitudeHp = extractSignedChar(25); elipsoidHp = extractSignedChar(26); @@ -2973,6 +2973,19 @@ uint32_t SFE_UBLOX_GPS::extractLong(uint8_t spotToStart) return (val); } +//Just so there is no ambiguity about whether a uint32_t will cast to a int32_t correctly... +int32_t SFE_UBLOX_GPS::extractSignedLong(uint8_t spotToStart) +{ + union // Use a union to convert from uint32_t to int32_t + { + uint32_t unsignedLong; + int32_t signedLong; + } unsignedSigned; + + unsignedSigned.unsignedLong = extractLong(spotToStart); + return (unsignedSigned.signedLong); +} + //Given a spot in the payload array, extract two bytes and build an int uint16_t SFE_UBLOX_GPS::extractInt(uint8_t spotToStart) { @@ -3444,7 +3457,7 @@ boolean SFE_UBLOX_GPS::getDOP(uint16_t maxWait) if (retVal == SFE_UBLOX_STATUS_DATA_RECEIVED) return (true); - + if ((retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN) && (packetCfg.id == UBX_NAV_PVT)) { if (_printDebug == true) @@ -3733,7 +3746,7 @@ boolean SFE_UBLOX_GPS::getProtocolVersion(uint16_t maxWait) for (uint8_t extensionNumber = 0; extensionNumber < 10; extensionNumber++) { //Now we need to find "PROTVER=18.00" in the incoming byte stream - if (payloadCfg[(30 * extensionNumber) + 0] == 'P' && payloadCfg[(30 * extensionNumber) + 6] == 'R') + if ((payloadCfg[(30 * extensionNumber) + 0] == 'P') && (payloadCfg[(30 * extensionNumber) + 6] == 'R')) { versionHigh = (payloadCfg[(30 * extensionNumber) + 8] - '0') * 10 + (payloadCfg[(30 * extensionNumber) + 9] - '0'); //Convert '18' to 18 versionLow = (payloadCfg[(30 * extensionNumber) + 11] - '0') * 10 + (payloadCfg[(30 * extensionNumber) + 12] - '0'); //Convert '00' to 00 @@ -3818,6 +3831,9 @@ void SFE_UBLOX_GPS::flushDOP() //Relative Positioning Information in NED frame //Returns true if commands was successful +//Note: +// RELPOSNED on the M8 is only 40 bytes long +// RELPOSNED on the F9 is 64 bytes long and contains much more information boolean SFE_UBLOX_GPS::getRELPOSNED(uint16_t maxWait) { packetCfg.cls = UBX_CLASS_NAV; @@ -3836,35 +3852,73 @@ boolean SFE_UBLOX_GPS::getRELPOSNED(uint16_t maxWait) int32_t tempRelPos; - tempRelPos = extractLong(8); - relPosInfo.relPosN = tempRelPos / 100.0; //Convert cm to m + tempRelPos = extractSignedLong(8); + relPosInfo.relPosN = ((float)tempRelPos) / 100.0; //Convert cm to m - tempRelPos = extractLong(12); - relPosInfo.relPosE = tempRelPos / 100.0; //Convert cm to m + tempRelPos = extractSignedLong(12); + relPosInfo.relPosE = ((float)tempRelPos) / 100.0; //Convert cm to m - tempRelPos = extractLong(16); - relPosInfo.relPosD = tempRelPos / 100.0; //Convert cm to m + tempRelPos = extractSignedLong(16); + relPosInfo.relPosD = ((float)tempRelPos) / 100.0; //Convert cm to m - relPosInfo.relPosLength = extractLong(20); - relPosInfo.relPosHeading = extractLong(24); + if (packetCfg.len == 40) + { + // The M8 version does not contain relPosLength or relPosHeading + relPosInfo.relPosLength = 0; + relPosInfo.relPosHeading = 0; + } + else + { + relPosInfo.relPosLength = extractSignedLong(20); + relPosInfo.relPosHeading = extractSignedLong(24); + } - relPosInfo.relPosHPN = payloadCfg[32]; - relPosInfo.relPosHPE = payloadCfg[33]; - relPosInfo.relPosHPD = payloadCfg[34]; - relPosInfo.relPosHPLength = payloadCfg[35]; + if (packetCfg.len == 40) + { + relPosInfo.relPosHPN = payloadCfg[20]; + relPosInfo.relPosHPE = payloadCfg[21]; + relPosInfo.relPosHPD = payloadCfg[22]; + relPosInfo.relPosHPLength = 0; // The M8 version does not contain relPosHPLength + } + else + { + relPosInfo.relPosHPN = payloadCfg[32]; + relPosInfo.relPosHPE = payloadCfg[33]; + relPosInfo.relPosHPD = payloadCfg[34]; + relPosInfo.relPosHPLength = payloadCfg[35]; + } uint32_t tempAcc; - tempAcc = extractLong(36); - relPosInfo.accN = tempAcc / 10000.0; //Convert 0.1 mm to m - - tempAcc = extractLong(40); - relPosInfo.accE = tempAcc / 10000.0; //Convert 0.1 mm to m + if (packetCfg.len == 40) + { + tempAcc = extractLong(24); + relPosInfo.accN = ((float)tempAcc) / 10000.0; //Convert 0.1 mm to m + tempAcc = extractLong(28); + relPosInfo.accE = ((float)tempAcc) / 10000.0; //Convert 0.1 mm to m + tempAcc = extractLong(32); + relPosInfo.accD = ((float)tempAcc) / 10000.0; //Convert 0.1 mm to m + } + else + { + tempAcc = extractLong(36); + relPosInfo.accN = ((float)tempAcc) / 10000.0; //Convert 0.1 mm to m + tempAcc = extractLong(40); + relPosInfo.accE = ((float)tempAcc) / 10000.0; //Convert 0.1 mm to m + tempAcc = extractLong(44); + relPosInfo.accD = ((float)tempAcc) / 10000.0; //Convert 0.1 mm to m + } - tempAcc = extractLong(44); - relPosInfo.accD = tempAcc / 10000.0; //Convert 0.1 mm to m + uint8_t flags; - uint8_t flags = payloadCfg[60]; + if (packetCfg.len == 40) + { + flags = payloadCfg[36]; + } + else + { + flags = payloadCfg[60]; + } relPosInfo.gnssFixOk = flags & (1 << 0); relPosInfo.diffSoln = flags & (1 << 1); @@ -3876,6 +3930,7 @@ boolean SFE_UBLOX_GPS::getRELPOSNED(uint16_t maxWait) return (true); } + boolean SFE_UBLOX_GPS::getEsfInfo(uint16_t maxWait) { // Requesting Data from the receiver @@ -3914,20 +3969,20 @@ boolean SFE_UBLOX_GPS::getEsfIns(uint16_t maxWait) // Validity of each sensor value below uint32_t validity = extractLong(0); - imuMeas.xAngRateVald = (validity && 0x0080) >> 8; - imuMeas.yAngRateVald = (validity && 0x0100) >> 9; - imuMeas.zAngRateVald = (validity && 0x0200) >> 10; - imuMeas.xAccelVald = (validity && 0x0400) >> 11; - imuMeas.yAccelVald = (validity && 0x0800) >> 12; - imuMeas.zAccelVald = (validity && 0x1000) >> 13; + imuMeas.xAngRateVald = (validity & 0x0100) >> 8; + imuMeas.yAngRateVald = (validity & 0x0200) >> 9; + imuMeas.zAngRateVald = (validity & 0x0400) >> 10; + imuMeas.xAccelVald = (validity & 0x0800) >> 11; + imuMeas.yAccelVald = (validity & 0x1000) >> 12; + imuMeas.zAccelVald = (validity & 0x2000) >> 13; - imuMeas.xAngRate = extractLong(12); // deg/s - imuMeas.yAngRate = extractLong(16); // deg/s - imuMeas.zAngRate = extractLong(20); // deg/s + imuMeas.xAngRate = extractSignedLong(12); // 0.001 deg/s + imuMeas.yAngRate = extractSignedLong(16); // 0.001 deg/s + imuMeas.zAngRate = extractSignedLong(20); // 0.001 deg/s - imuMeas.xAccel = extractLong(24); // m/s - imuMeas.yAccel = extractLong(28); // m/s - imuMeas.zAccel = extractLong(32); // m/s + imuMeas.xAccel = extractSignedLong(24); // 0.01 m/s^2 + imuMeas.yAccel = extractSignedLong(28); // 0.01 m/s^2 + imuMeas.zAccel = extractSignedLong(32); // 0.01 m/s^2 return (true); } @@ -3949,23 +4004,34 @@ boolean SFE_UBLOX_GPS::getEsfDataInfo(uint16_t maxWait) uint32_t timeStamp = extractLong(0); uint32_t flags = extractInt(4); - uint8_t timeSent = (flags && 0x01) >> 1; - uint8_t timeEdge = (flags && 0x02) >> 2; - uint8_t tagValid = (flags && 0x04) >> 3; - uint8_t numMeas = (flags && 0x1000) >> 15; + uint8_t timeSent = flags & 0x03; // timeSent is 2-bit: 0 = none, 1 = on Ext0, 2 = on Ext1 + uint8_t timeEdge = (flags & 0x04) >> 2; + uint8_t tagValid = (flags & 0x08) >> 3; + uint8_t numMeas = (flags & 0xF800) >> 11; - if (numMeas > DEF_NUM_SENS) + if (numMeas > DEF_NUM_SENS) // Truncate numMeas if required numMeas = DEF_NUM_SENS; uint8_t byteOffset = 4; for (uint8_t i = 0; i < numMeas; i++) { + uint32_t bitField = extractLong(8 + (byteOffset * i)); + imuMeas.dataType[i] = (bitField & 0x3F000000) >> 24; + imuMeas.data[i] = (bitField & 0xFFFFFF); + } + + numMeas = (flags & 0xF800) >> 11; // Restore numMeas - uint32_t bitField = extractLong(4 + byteOffset * i); - imuMeas.dataType[i] = (bitField && 0xFF000000) >> 23; - imuMeas.data[i] = (bitField && 0xFFFFFF); - imuMeas.dataTStamp[i] = extractLong(8 + byteOffset * i); + if (packetCfg.len > (8 + (4 * numMeas))) // The calibTtag is optional - only extract it if it is present + { + uint8_t startOfTtag = 8 + (4 * numMeas); // Calculate where the Ttag data starts + if (numMeas > DEF_NUM_SENS) // Truncate numMeas if required + numMeas = DEF_NUM_SENS; + for (uint8_t i = 0; i < numMeas; i++) + { + imuMeas.dataTStamp[i] = extractLong(startOfTtag + (byteOffset * i)); + } } return (true); @@ -3987,13 +4053,15 @@ boolean SFE_UBLOX_GPS::getEsfRawDataInfo(uint16_t maxWait) checkUblox(); uint32_t bitField = extractLong(4); - imuMeas.rawDataType = (bitField && 0xFF000000) >> 23; - imuMeas.rawData = (bitField && 0xFFFFFF); + imuMeas.rawDataType = (bitField & 0xFF000000) >> 24; + imuMeas.rawData = (bitField & 0xFFFFFF); + imuMeas.rawTStamp = extractLong(8); return (true); } +// Note: senor numbering starts at 1 (not 0) sfe_ublox_status_e SFE_UBLOX_GPS::getSensState(uint8_t sensor, uint16_t maxWait) { @@ -4018,22 +4086,22 @@ sfe_ublox_status_e SFE_UBLOX_GPS::getSensState(uint8_t sensor, uint16_t maxWait) for (uint8_t i = 0; i < sensor; i++) { - uint8_t sensorFieldOne = extractByte(16 + offset * i); - uint8_t sensorFieldTwo = extractByte(17 + offset * i); - ubloxSen.freq = extractByte(18 + offset * i); + uint8_t sensorFieldOne = extractByte(16 + (offset * i)); + uint8_t sensorFieldTwo = extractByte(17 + (offset * i)); + ubloxSen.freq = extractByte(18 + (offset * i)); uint8_t sensorFieldThr = extractByte(19 + offset * i); - ubloxSen.senType = (sensorFieldOne && 0x10) >> 5; - ubloxSen.isUsed = (sensorFieldOne && 0x20) >> 6; - ubloxSen.isReady = (sensorFieldOne && 0x30) >> 7; + ubloxSen.senType = (sensorFieldOne & 0x3F); + ubloxSen.isUsed = (sensorFieldOne & 0x40) >> 6; + ubloxSen.isReady = (sensorFieldOne & 0x80) >> 7; - ubloxSen.calibStatus = sensorFieldTwo && 0x03; - ubloxSen.timeStatus = (sensorFieldTwo && 0xC) >> 2; + ubloxSen.calibStatus = sensorFieldTwo & 0x03; + ubloxSen.timeStatus = (sensorFieldTwo & 0xC) >> 2; - ubloxSen.badMeas = (sensorFieldThr && 0x01); - ubloxSen.badTag = (sensorFieldThr && 0x02) >> 1; - ubloxSen.missMeas = (sensorFieldThr && 0x04) >> 2; - ubloxSen.noisyMeas = (sensorFieldThr && 0x08) >> 3; + ubloxSen.badMeas = (sensorFieldThr & 0x01); + ubloxSen.badTag = (sensorFieldThr & 0x02) >> 1; + ubloxSen.missMeas = (sensorFieldThr & 0x04) >> 2; + ubloxSen.noisyMeas = (sensorFieldThr & 0x08) >> 3; } return (SFE_UBLOX_STATUS_SUCCESS); @@ -4052,13 +4120,13 @@ boolean SFE_UBLOX_GPS::getVehAtt(uint16_t maxWait) checkUblox(); - vehAtt.roll = extractLong(8); - vehAtt.pitch = extractLong(12); - vehAtt.heading = extractLong(16); + vehAtt.roll = extractSignedLong(8); // 0.00001 deg + vehAtt.pitch = extractSignedLong(12); // 0.00001 deg + vehAtt.heading = extractSignedLong(16); // 0.00001 deg - vehAtt.accRoll = extractLong(20); - vehAtt.accPitch = extractLong(24); - vehAtt.accHeading = extractLong(28); + vehAtt.accRoll = extractLong(20); // 0.00001 deg + vehAtt.accPitch = extractLong(24); // 0.00001 deg + vehAtt.accHeading = extractLong(28); // 0.00001 deg return (true); } diff --git a/src/SparkFun_Ublox_Arduino_Library.h b/src/SparkFun_Ublox_Arduino_Library.h index 341083f..3dc0774 100644 --- a/src/SparkFun_Ublox_Arduino_Library.h +++ b/src/SparkFun_Ublox_Arduino_Library.h @@ -504,7 +504,7 @@ class SFE_UBLOX_GPS boolean assumeAutoDOP(boolean enabled, boolean implicitUpdate = true); //In case no config access to the GPS is possible and DOP is send cyclically already boolean setAutoDOP(boolean enabled, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic DOP reports at the navigation frequency boolean setAutoDOP(boolean enabled, boolean implicitUpdate, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic DOP reports at the navigation frequency, with implicitUpdate == false accessing stale data will not issue parsing of data in the rxbuffer of your interface, instead you have to call checkUblox when you want to perform an update - boolean getDOP(uint16_t maxWait = getDOPmaxWait); //Query module for latest dilution of precision values and load global vars:. If autoDOP is disabled, performs an explicit poll and waits, if enabled does not block. Returns true if new DOP is available. + boolean getDOP(uint16_t maxWait = getDOPmaxWait); //Query module for latest dilution of precision values and load global vars:. If autoDOP is disabled, performs an explicit poll and waits, if enabled does not block. Returns true if new DOP is available. void flushPVT(); //Mark all the PVT data as read/stale. This is handy to get data alignment after CRC failure void flushHPPOSLLH(); //Mark all the PVT data as read/stale. This is handy to get data alignment after CRC failure void flushDOP(); //Mark all the DOP data as read/stale. This is handy to get data alignment after CRC failure @@ -842,6 +842,7 @@ uint16_t eastingDOP; // Easting dilution of precision * 10^-2 //Functions boolean checkUbloxInternal(ubxPacket *incomingUBX, uint8_t requestedClass = 255, uint8_t requestedID = 255); //Checks module with user selected commType uint32_t extractLong(uint8_t spotToStart); //Combine four bytes from payload into long + int32_t extractSignedLong(uint8_t spotToStart); //Combine four bytes from payload into signed long (avoiding any ambiguity caused by casting) uint16_t extractInt(uint8_t spotToStart); //Combine two bytes from payload into int uint8_t extractByte(uint8_t spotToStart); //Get byte from payload int8_t extractSignedChar(uint8_t spotToStart); //Get signed 8-bit value from payload @@ -890,7 +891,7 @@ uint16_t eastingDOP; // Easting dilution of precision * 10^-2 boolean autoHPPOSLLHImplicitUpdate = true; // Whether autoHPPOSLLH is triggered by accessing stale data (=true) or by a call to checkUblox (=false) boolean autoDOP = false; //Whether autoDOP is enabled or not boolean autoDOPImplicitUpdate = true; // Whether autoDOP is triggered by accessing stale data (=true) or by a call to checkUblox (=false) - + uint16_t ubxFrameCounter; //It counts all UBX frame. [Fixed header(2bytes), CLS(1byte), ID(1byte), length(2bytes), payload(x bytes), checksums(2bytes)] uint8_t rollingChecksumA; //Rolls forward as we receive incoming bytes. Checked against the last two A/B checksum bytes From aac251eeb9bc4ce1f60008d2f124863c33071e4a Mon Sep 17 00:00:00 2001 From: PaulZC Date: Tue, 17 Nov 2020 12:14:59 +0000 Subject: [PATCH 26/36] Correcting change to getEsfDataInfo. calibTtag is only appended once. --- src/SparkFun_Ublox_Arduino_Library.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp index 4f82fd0..6c75c3b 100644 --- a/src/SparkFun_Ublox_Arduino_Library.cpp +++ b/src/SparkFun_Ublox_Arduino_Library.cpp @@ -4026,11 +4026,11 @@ boolean SFE_UBLOX_GPS::getEsfDataInfo(uint16_t maxWait) if (packetCfg.len > (8 + (4 * numMeas))) // The calibTtag is optional - only extract it if it is present { uint8_t startOfTtag = 8 + (4 * numMeas); // Calculate where the Ttag data starts - if (numMeas > DEF_NUM_SENS) // Truncate numMeas if required + if (numMeas > DEF_NUM_SENS) // Truncate numMeas again if required numMeas = DEF_NUM_SENS; for (uint8_t i = 0; i < numMeas; i++) { - imuMeas.dataTStamp[i] = extractLong(startOfTtag + (byteOffset * i)); + imuMeas.dataTStamp[i] = extractLong(startOfTtag); // calibTtag is only appended once } } From 5ecba8998aa6cf2825dffe7b1adad8c1821112f4 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Wed, 18 Nov 2020 07:00:49 +0000 Subject: [PATCH 27/36] Keeping clang happy... --- src/SparkFun_Ublox_Arduino_Library.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp index 6c75c3b..32f4249 100644 --- a/src/SparkFun_Ublox_Arduino_Library.cpp +++ b/src/SparkFun_Ublox_Arduino_Library.cpp @@ -2347,10 +2347,8 @@ uint8_t SFE_UBLOX_GPS::getNavigationFrequency(uint16_t maxWait) if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK return (false); //If command send fails then bail - uint16_t measurementRate = 0; - //payloadCfg is now loaded with current bytes. Get what we need - measurementRate = extractInt(0); //Pull from payloadCfg at measRate LSB + uint16_t measurementRate = extractInt(0); //Pull from payloadCfg at measRate LSB measurementRate = 1000 / measurementRate; //This may return an int when it's a float, but I'd rather not return 4 bytes return (measurementRate); From 75eedde568768ea967933cad40373dc9ec426f61 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Mon, 23 Nov 2020 09:09:21 +0000 Subject: [PATCH 28/36] Fix to avoid conflicting #defines for Serial --- src/SparkFun_Ublox_Arduino_Library.h | 34 +++++++++++++++++----------- 1 file changed, 21 insertions(+), 13 deletions(-) diff --git a/src/SparkFun_Ublox_Arduino_Library.h b/src/SparkFun_Ublox_Arduino_Library.h index 3dc0774..38432ee 100644 --- a/src/SparkFun_Ublox_Arduino_Library.h +++ b/src/SparkFun_Ublox_Arduino_Library.h @@ -52,19 +52,6 @@ #include "u-blox_config_keys.h" -// Define Serial for SparkFun SAMD based boards. -// Boards like the RedBoard Turbo use SerialUSB (not Serial). -// But other boards like the SAMD51 Thing Plus use Serial (not SerialUSB). -// The next nine lines let the code compile cleanly on as many SAMD boards as possible. -#if defined(ARDUINO_ARCH_SAMD) // Is this a SAMD board? -#if defined(USB_VID) // Is the USB Vendor ID defined? -#if (USB_VID == 0x1B4F) // Is this a SparkFun board? -#if !defined(ARDUINO_SAMD51_THING_PLUS) & !defined(ARDUINO_SAMD51_MICROMOD) // If it is not a SAMD51 Thing Plus or SAMD51 MicroMod -#define Serial SerialUSB // Define Serial as SerialUSB -#endif -#endif -#endif -#endif //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //Define a digital pin to aid checksum failure capture and analysis @@ -623,7 +610,28 @@ class SFE_UBLOX_GPS boolean getRELPOSNED(uint16_t maxWait = 1100); //Get Relative Positioning Information of the NED frame + // Enable debug messages using the chosen Serial port (Stream) + // Boards like the RedBoard Turbo use SerialUSB (not Serial). + // But other boards like the SAMD51 Thing Plus use Serial (not SerialUSB). + // These lines let the code compile cleanly on as many SAMD boards as possible. + #if defined(ARDUINO_ARCH_SAMD) // Is this a SAMD board? + #if defined(USB_VID) // Is the USB Vendor ID defined? + #if (USB_VID == 0x1B4F) // Is this a SparkFun board? + #if !defined(ARDUINO_SAMD51_THING_PLUS) & !defined(ARDUINO_SAMD51_MICROMOD) // If it is not a SAMD51 Thing Plus or SAMD51 MicroMod + void enableDebugging(Stream &debugPort = SerialUSB, boolean printLimitedDebug = false); //Given a port to print to, enable debug messages. Default to all, not limited. + #else void enableDebugging(Stream &debugPort = Serial, boolean printLimitedDebug = false); //Given a port to print to, enable debug messages. Default to all, not limited. + #endif + #else + void enableDebugging(Stream &debugPort = Serial, boolean printLimitedDebug = false); //Given a port to print to, enable debug messages. Default to all, not limited. + #endif + #else + void enableDebugging(Stream &debugPort = Serial, boolean printLimitedDebug = false); //Given a port to print to, enable debug messages. Default to all, not limited. + #endif + #else + void enableDebugging(Stream &debugPort = Serial, boolean printLimitedDebug = false); //Given a port to print to, enable debug messages. Default to all, not limited. + #endif + void disableDebugging(void); //Turn off debug statements void debugPrint(char *message); //Safely print debug statements void debugPrintln(char *message); //Safely print debug statements From c914451e2da68d0aefd0641c217da169b1dfaa94 Mon Sep 17 00:00:00 2001 From: nlarsen Date: Sat, 28 Nov 2020 17:04:15 +0100 Subject: [PATCH 29/36] preparation: swap 'if's leaving logic 100% as before --- src/SparkFun_Ublox_Arduino_Library.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp index 81994a1..4a263ba 100644 --- a/src/SparkFun_Ublox_Arduino_Library.cpp +++ b/src/SparkFun_Ublox_Arduino_Library.cpp @@ -924,14 +924,14 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t uint16_t startingSpot = incomingUBX->startingSpot; if (incomingUBX->cls == UBX_CLASS_NAV && incomingUBX->id == UBX_NAV_PVT) startingSpot = 0; - //Begin recording if counter goes past startingSpot - if ((incomingUBX->counter - 4) >= startingSpot) + // Check if this is payload data which should be ignored + if (ignoreThisPayload == false) { - //Check to see if we have room for this byte - if (((incomingUBX->counter - 4) - startingSpot) < MAX_PAYLOAD_SIZE) //If counter = 208, starting spot = 200, we're good to record. + //Begin recording if counter goes past startingSpot + if ((incomingUBX->counter - 4) >= startingSpot) { - // Check if this is payload data which should be ignored - if (ignoreThisPayload == false) + //Check to see if we have room for this byte + if (((incomingUBX->counter - 4) - startingSpot) < MAX_PAYLOAD_SIZE) //If counter = 208, starting spot = 200, we're good to record. { incomingUBX->payload[incomingUBX->counter - 4 - startingSpot] = incoming; //Store this byte into payload array } From 386e0de91e8c80f4cb65e262a7de50313edc73a2 Mon Sep 17 00:00:00 2001 From: nlarsen Date: Sat, 28 Nov 2020 17:30:28 +0100 Subject: [PATCH 30/36] fix critical array overrun bug (incomingUBX->payload[] index was only checked against MAX_PAYLOAD_SIZE, but incomingUBX can also be packetAck or packetBuf which have much smaller buffers of only 2 bytes) --- src/SparkFun_Ublox_Arduino_Library.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp index 4a263ba..3c31247 100644 --- a/src/SparkFun_Ublox_Arduino_Library.cpp +++ b/src/SparkFun_Ublox_Arduino_Library.cpp @@ -760,6 +760,9 @@ void SFE_UBLOX_GPS::processRTCM(uint8_t incoming) //a subset of bytes within a larger packet. void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID) { + size_t max_payload_size = (activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG) ? MAX_PAYLOAD_SIZE : 2; + bool overrun = false; + //Add all incoming bytes to the rolling checksum //Stop at len+4 as this is the checksum bytes to that should not be added to the rolling checksum if (incomingUBX->counter < incomingUBX->len + 4) @@ -931,10 +934,14 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t if ((incomingUBX->counter - 4) >= startingSpot) { //Check to see if we have room for this byte - if (((incomingUBX->counter - 4) - startingSpot) < MAX_PAYLOAD_SIZE) //If counter = 208, starting spot = 200, we're good to record. + if (((incomingUBX->counter - 4) - startingSpot) < max_payload_size) //If counter = 208, starting spot = 200, we're good to record. { incomingUBX->payload[incomingUBX->counter - 4 - startingSpot] = incoming; //Store this byte into payload array } + else + { + overrun = true; + } } } } @@ -942,7 +949,7 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t //Increment the counter incomingUBX->counter++; - if (incomingUBX->counter == MAX_PAYLOAD_SIZE) + if (overrun or incomingUBX->counter == MAX_PAYLOAD_SIZE) { //Something has gone very wrong currentSentence = NONE; //Reset the sentence to being looking for a new start char From f6a1fe0bfdd5b54d3b4186941adc66bb80777962 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Mon, 30 Nov 2020 07:29:01 +0000 Subject: [PATCH 31/36] Adding PR #154 (u-blox 7 series) --- .../SparkFun_Ublox_Arduino_Library_Series_6_7.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/examples/Series_6_7/Example1_GetPositionAndTime_Series_6_7/SparkFun_Ublox_Arduino_Library_Series_6_7.cpp b/examples/Series_6_7/Example1_GetPositionAndTime_Series_6_7/SparkFun_Ublox_Arduino_Library_Series_6_7.cpp index dca258c..a6c04b6 100644 --- a/examples/Series_6_7/Example1_GetPositionAndTime_Series_6_7/SparkFun_Ublox_Arduino_Library_Series_6_7.cpp +++ b/examples/Series_6_7/Example1_GetPositionAndTime_Series_6_7/SparkFun_Ublox_Arduino_Library_Series_6_7.cpp @@ -899,7 +899,9 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg) switch (msg->cls) { case UBX_CLASS_NAV: - if (msg->id == UBX_NAV_PVT && msg->len == 92) + //u-blox8 length == 92 + //u-blox7 length == 84 + if ((msg->id == UBX_NAV_PVT) && ((msg->len == 92) || (msg->len == 84))) { //Parse various byte fields into global vars constexpr int startingSpot = 0; //fixed value used in processUBX @@ -915,6 +917,7 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg) gpsNanosecond = extractLong(16); //Includes milliseconds fixType = extractByte(20 - startingSpot); + //Note: the u-blox7 does not support carrSoln. carrierSolution will be zero. carrierSolution = extractByte(21 - startingSpot) >> 6; //Get 6th&7th bits of this byte SIV = extractByte(23 - startingSpot); longitude = extractLong(24 - startingSpot); From 864a3a55f910fc1fc0031d8e0c7aa9dff4f8fcb7 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Tue, 1 Dec 2020 12:56:05 +0000 Subject: [PATCH 32/36] Adding an extra debug print for overrun --- .../Example13_autoHPPOSLLH.ino | 25 ++++++++++--------- src/SparkFun_Ublox_Arduino_Library.cpp | 9 ++++--- 2 files changed, 19 insertions(+), 15 deletions(-) diff --git a/examples/ZED-F9P/Example13_autoHPPOSLLH/Example13_autoHPPOSLLH.ino b/examples/ZED-F9P/Example13_autoHPPOSLLH/Example13_autoHPPOSLLH.ino index a07827f..66fb3ec 100644 --- a/examples/ZED-F9P/Example13_autoHPPOSLLH/Example13_autoHPPOSLLH.ino +++ b/examples/ZED-F9P/Example13_autoHPPOSLLH/Example13_autoHPPOSLLH.ino @@ -43,6 +43,7 @@ void setup() Wire.begin(); //myGPS.enableDebugging(); // Uncomment this line to enable lots of helpful debug messages + //myGPS.enableDebugging(Serial, true); // Uncomment this line to enable the minimum of helpful debug messages if (myGPS.begin() == false) //Connect to the Ublox module using Wire port { @@ -55,21 +56,21 @@ void setup() myGPS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise) myGPS.saveConfigSelective(VAL_CFG_SUBSEC_IOPORT); //Save the communications port settings to flash and BBR - + myGPS.setNavigationFrequency(1); //Produce one solution per second - + // The acid test: all four of these combinations should work seamlessly :-) - + //myGPS.setAutoPVT(false); // Library will poll each reading //myGPS.setAutoHPPOSLLH(false); // Library will poll each reading - + //myGPS.setAutoPVT(true); // Tell the GPS to "send" each solution automatically //myGPS.setAutoHPPOSLLH(false); // Library will poll each reading //myGPS.setAutoPVT(false); // Library will poll each reading //myGPS.setAutoHPPOSLLH(true); // Tell the GPS to "send" each hi res solution automatically - + myGPS.setAutoPVT(true); // Tell the GPS to "send" each solution automatically myGPS.setAutoHPPOSLLH(true); // Tell the GPS to "send" each hi res solution automatically } @@ -81,31 +82,31 @@ void loop() if ((myGPS.getHPPOSLLH()) || (myGPS.getPVT())) { Serial.println(); - + long highResLatitude = myGPS.getHighResLatitude(); Serial.print(F("Hi Res Lat: ")); Serial.print(highResLatitude); - + int highResLatitudeHp = myGPS.getHighResLatitudeHp(); Serial.print(F(" ")); Serial.print(highResLatitudeHp); - + long highResLongitude = myGPS.getHighResLongitude(); Serial.print(F(" Hi Res Long: ")); Serial.print(highResLongitude); - + int highResLongitudeHp = myGPS.getHighResLongitudeHp(); Serial.print(F(" ")); Serial.print(highResLongitudeHp); - + unsigned long horizAccuracy = myGPS.getHorizontalAccuracy(); Serial.print(F(" Horiz accuracy: ")); Serial.print(horizAccuracy); - + long latitude = myGPS.getLatitude(); Serial.print(F(" Lat: ")); Serial.print(latitude); - + long longitude = myGPS.getLongitude(); Serial.print(F(" Long: ")); Serial.println(longitude); diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp index a9683e8..7787e07 100644 --- a/src/SparkFun_Ublox_Arduino_Library.cpp +++ b/src/SparkFun_Ublox_Arduino_Library.cpp @@ -951,13 +951,16 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t //Increment the counter incomingUBX->counter++; - if (overrun or incomingUBX->counter == MAX_PAYLOAD_SIZE) + if (overrun || (incomingUBX->counter == MAX_PAYLOAD_SIZE)) { //Something has gone very wrong currentSentence = NONE; //Reset the sentence to being looking for a new start char - if (_printDebug == true) + if ((_printDebug == true) || (_printLimitedDebug == true)) // Print this if doing limited debugging { - _debugSerial->println(F("processUBX: counter hit MAX_PAYLOAD_SIZE")); + if (overrun) + _debugSerial->println(F("processUBX: buffer overrun detected")); + else + _debugSerial->println(F("processUBX: counter hit MAX_PAYLOAD_SIZE")); } } } From b7e7d8aabf539c0af49cec7416b1585991b28695 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Tue, 1 Dec 2020 15:07:37 +0000 Subject: [PATCH 33/36] Adding pushRawData: pushRawData allows (e.g.) RTCM data to be pushed directly to the module Tested with updated versions of the ZED-F9P Example3 (RTCM base) and Example5 (Relative Positioning) --- .../Example3_StartRTCMBase.ino | 19 +++++++- ...xample5_RelativePositioningInformation.ino | 38 ++++++++++++++- keywords.txt | 2 + src/SparkFun_Ublox_Arduino_Library.cpp | 48 +++++++++++++++++++ src/SparkFun_Ublox_Arduino_Library.h | 4 ++ 5 files changed, 108 insertions(+), 3 deletions(-) diff --git a/examples/ZED-F9P/Example3_StartRTCMBase/Example3_StartRTCMBase.ino b/examples/ZED-F9P/Example3_StartRTCMBase/Example3_StartRTCMBase.ino index ddff4c2..83d825c 100644 --- a/examples/ZED-F9P/Example3_StartRTCMBase/Example3_StartRTCMBase.ino +++ b/examples/ZED-F9P/Example3_StartRTCMBase/Example3_StartRTCMBase.ino @@ -29,12 +29,19 @@ #include "SparkFun_Ublox_Arduino_Library.h" //http://librarymanager/All#SparkFun_Ublox_GPS SFE_UBLOX_GPS myGPS; +//#define USE_SERIAL1 // Uncomment this line to push the RTCM data to Serial1 + void setup() { Serial.begin(115200); while (!Serial); //Wait for user to open terminal Serial.println("Ublox Base station example"); +#ifdef USE_SERIAL1 + // If our board supports it, we can output the RTCM data on Serial1 + Serial1.begin(115200); +#endif + Wire.begin(); Wire.setClock(400000); //Increase I2C clock speed to 400kHz @@ -44,8 +51,11 @@ void setup() while (1); } + // Uncomment the next line if you want to reset your module back to the default settings with 1Hz navigation rate + //myGPS.factoryDefault(); delay(5000); + myGPS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise) - myGPS.saveConfiguration(); //Save the current settings to flash and BBR + myGPS.saveConfigSelective(VAL_CFG_SUBSEC_IOPORT); //Save the communications port settings to flash and BBR while (Serial.available()) Serial.read(); //Clear any latent chars in serial buffer Serial.println("Press any key to send commands to begin Survey-In"); @@ -153,7 +163,12 @@ void loop() //Useful for passing the RTCM correction data to a radio, Ntrip broadcaster, etc. void SFE_UBLOX_GPS::processRTCM(uint8_t incoming) { - //Let's just pretty-print the HEX values for now +#ifdef USE_SERIAL1 + //Push the RTCM data to Serial1 + Serial1.write(incoming); +#endif + + //Pretty-print the HEX values to Serial if (myGPS.rtcmFrameCounter % 16 == 0) Serial.println(); Serial.print(" "); if (incoming < 0x10) Serial.print("0"); diff --git a/examples/ZED-F9P/Example5_RelativePositioningInformation/Example5_RelativePositioningInformation.ino b/examples/ZED-F9P/Example5_RelativePositioningInformation/Example5_RelativePositioningInformation.ino index 82b0525..6d4d86c 100644 --- a/examples/ZED-F9P/Example5_RelativePositioningInformation/Example5_RelativePositioningInformation.ino +++ b/examples/ZED-F9P/Example5_RelativePositioningInformation/Example5_RelativePositioningInformation.ino @@ -26,12 +26,21 @@ #include "SparkFun_Ublox_Arduino_Library.h" //http://librarymanager/All#SparkFun_Ublox_GPS SFE_UBLOX_GPS myGPS; +//#define USE_SERIAL1 // Uncomment this line to push the RTCM data from Serial1 to the module via I2C + +size_t numBytes = 0; // Record the number os bytes received from Serial1 + void setup() { Serial.begin(115200); while (!Serial); //Wait for user to open terminal Serial.println("Ublox Base station example"); +#ifdef USE_SERIAL1 + // If our board supports it, we can receive the RTCM data on Serial1 + Serial1.begin(115200); +#endif + Wire.begin(); Wire.setClock(400000); //Increase I2C clock speed to 400kHz @@ -40,6 +49,15 @@ void setup() Serial.println(F("Ublox GPS not detected at default I2C address. Please check wiring. Freezing.")); while (1); } + + // Uncomment the next line if you want to reset your module back to the default settings with 1Hz navigation rate + //myGPS.factoryDefault(); delay(5000); + +#ifdef USE_SERIAL1 + Serial.print(F("Enabling UBX and RTCM input on I2C. Result: ")); + Serial.print(myGPS.setPortInput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_RTCM3)); //Enable UBX and RTCM input on I2C + myGPS.saveConfigSelective(VAL_CFG_SUBSEC_IOPORT); //Save the communications port settings to flash and BBR +#endif } void loop() @@ -121,5 +139,23 @@ void loop() else Serial.println("RELPOS request failed"); - delay(4000); + for (int i = 0; i < 500; i++) + { +#ifdef USE_SERIAL1 + uint8_t store[256]; + while ((Serial1.available()) && (numBytes < 256)) // Check if data has been received + { + store[numBytes++] = Serial1.read(); // Read a byte from Serial1 and store it + } + if (numBytes > 0) // Check if data was received + { + //Serial.print("Pushing "); + //Serial.print(numBytes); + //Serial.println(" bytes via I2C"); + myGPS.pushRawData(((uint8_t *)&store), numBytes); // Push the RTCM data via I2C + numBytes = 0; // Reset numBytes + } +#endif + delay(10); + } } diff --git a/keywords.txt b/keywords.txt index f3f9dcf..39981df 100644 --- a/keywords.txt +++ b/keywords.txt @@ -173,6 +173,8 @@ getI2CTransactionSize KEYWORD2 setStaticPosition KEYWORD2 +pushRawData KEYWORD2 + ####################################### # Constants (LITERAL1) ####################################### diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp index 7787e07..c8d4395 100644 --- a/src/SparkFun_Ublox_Arduino_Library.cpp +++ b/src/SparkFun_Ublox_Arduino_Library.cpp @@ -4197,3 +4197,51 @@ bool SFE_UBLOX_GPS::setStaticPosition(int32_t ecefXOrLat, int32_t ecefYOrLon, in { return (setStaticPosition(ecefXOrLat, 0, ecefYOrLon, 0, ecefZOrAlt, 0, latlong, maxWait)); } + +// Push (e.g.) RTCM data directly to the module +// Returns true if all numDataBytes were pushed successfully +// Warning: this function does not check that the data is valid. It is the user's responsibility to ensure the data is valid before pushing. +boolean SFE_UBLOX_GPS::pushRawData(uint8_t *dataBytes, size_t numDataBytes) +{ + if (commType == COMM_TYPE_SERIAL) + { + // Serial: write all the bytes in one go + size_t bytesWritten = _serialPort->write(dataBytes, numDataBytes); + return (bytesWritten == numDataBytes); + } + else + { + // I2C: split the data up into packets of i2cTransactionSize + size_t bytesLeftToWrite = numDataBytes; + size_t bytesWrittenTotal = 0; + + while (bytesLeftToWrite > 0) + { + size_t bytesToWrite; // Limit bytesToWrite to i2cTransactionSize + if (bytesLeftToWrite > i2cTransactionSize) + bytesToWrite = i2cTransactionSize; + else + bytesToWrite = bytesLeftToWrite; + + _i2cPort->beginTransmission(_gpsI2Caddress); + size_t bytesWritten = _i2cPort->write(dataBytes, bytesToWrite); // Write the bytes + + bytesWrittenTotal += bytesWritten; // Update the totals + bytesLeftToWrite -= bytesToWrite; + dataBytes += bytesToWrite; // Point to fresh data + + if (bytesLeftToWrite > 0) + { + if (_i2cPort->endTransmission(false) != 0) //Send a restart command. Do not release bus. + return (false); //Sensor did not ACK + } + else + { + if (_i2cPort->endTransmission() != 0) //We're done. Release bus. + return (false); //Sensor did not ACK + } + } + + return (bytesWrittenTotal == numDataBytes); + } +} diff --git a/src/SparkFun_Ublox_Arduino_Library.h b/src/SparkFun_Ublox_Arduino_Library.h index 38432ee..75ce1dd 100644 --- a/src/SparkFun_Ublox_Arduino_Library.h +++ b/src/SparkFun_Ublox_Arduino_Library.h @@ -666,6 +666,10 @@ class SFE_UBLOX_GPS bool setStaticPosition(int32_t ecefXOrLat, int8_t ecefXOrLatHP, int32_t ecefYOrLon, int8_t ecefYOrLonHP, int32_t ecefZOrAlt, int8_t ecefZOrAltHP, bool latLong = false, uint16_t maxWait = 250); bool setStaticPosition(int32_t ecefXOrLat, int32_t ecefYOrLon, int32_t ecefZOrAlt, bool latLong = false, uint16_t maxWait = 250); + // Push (e.g.) RTCM data directly to the module + // Warning: this function does not check that the data is valid. It is the user's responsibility to ensure the data is valid before pushing. + boolean pushRawData(uint8_t *dataBytes, size_t numDataBytes); + //Survey-in specific controls struct svinStructure { From 946ef6886e46dabf2743478a78f95c5c96f81aa6 Mon Sep 17 00:00:00 2001 From: PaulZC Date: Tue, 1 Dec 2020 15:09:01 +0000 Subject: [PATCH 34/36] Adding a missing endTransmission(false) --- src/SparkFun_Ublox_Arduino_Library.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp index c8d4395..a42bed3 100644 --- a/src/SparkFun_Ublox_Arduino_Library.cpp +++ b/src/SparkFun_Ublox_Arduino_Library.cpp @@ -1190,7 +1190,7 @@ sfe_ublox_status_e SFE_UBLOX_GPS::sendI2cCommand(ubxPacket *outgoingUBX, uint16_ //Point at 0xFF data register _i2cPort->beginTransmission((uint8_t)_gpsI2Caddress); //There is no register to write to, we just begin writing data bytes _i2cPort->write(0xFF); - if (_i2cPort->endTransmission() != 0) //Don't release bus + if (_i2cPort->endTransmission(false) != 0) //Don't release bus return (SFE_UBLOX_STATUS_I2C_COMM_FAILURE); //Sensor did not ACK //Write header bytes From c357aeb6873d8ec96fb185ba3c6da1a2048f6c6e Mon Sep 17 00:00:00 2001 From: PaulZC Date: Tue, 1 Dec 2020 15:25:44 +0000 Subject: [PATCH 35/36] v1.8.8 --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index 9c2a287..4773b05 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=SparkFun u-blox Arduino Library -version=1.8.7 +version=1.8.8 author=SparkFun Electronics maintainer=SparkFun Electronics sentence=Library for I2C and Serial Communication with u-blox modules From 0d34d128b46efe6efc3b695952395254127921cf Mon Sep 17 00:00:00 2001 From: PaulZC Date: Tue, 1 Dec 2020 15:54:38 +0000 Subject: [PATCH 36/36] Updating Thanks for v1.8.8 --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 7e4504f..be11801 100644 --- a/README.md +++ b/README.md @@ -49,6 +49,12 @@ Thanks to: * [blazczak](https://github.com/blazczak) and [geeksville](https://github.com/geeksville) for adding support for the series 6 and 7 modules. * [bjorn@unsurv](https://github.com/unsurv) for adding powerOff and powerOffWithInterrupt. * [dotMorten](https://github.com/dotMorten) for the MSGOUT keys, autoHPPOSLLH, autoDOP and upgrades to autoPVT. +* [markuckermann](https://github.com/markuckermann) for spotting the config layer gremlins +* [vid553](https://github.com/vid553) for the Zephyr port +* [balamuruganky](https://github.com/balamuruganky) for the NAV-PVT velocity parameters +* [nelarsen](https://github.com/nelarsen) for the buffer overrun improvements +* [mstranne](https://github.com/mstranne) and [shaneperera](https://github.com/shaneperera) for the pushRawData suggestion +* [rubienr](https://github.com/rubienr) for spotting the logical AND issues Need a Python version for Raspberry Pi? Checkout the [Qwiic Ublox GPS Py module](https://github.com/sparkfun/Qwiic_Ublox_Gps_Py).