From 1593dbc1a6dce36386269e01464f89ddce4a4baf Mon Sep 17 00:00:00 2001 From: PaulZC Date: Fri, 9 Sep 2022 01:58:53 +0100 Subject: [PATCH] Correct support for ESF RAW --- .../CallbackExample10_ESF_RAW.ino | 160 ++++++++++++++++++ keywords.txt | 2 - src/SparkFun_u-blox_GNSS_Arduino_Library.cpp | 112 ++---------- src/SparkFun_u-blox_GNSS_Arduino_Library.h | 3 - src/u-blox_structs.h | 27 ++- 5 files changed, 181 insertions(+), 123 deletions(-) create mode 100644 examples/Callbacks/CallbackExample10_ESF_RAW/CallbackExample10_ESF_RAW.ino diff --git a/examples/Callbacks/CallbackExample10_ESF_RAW/CallbackExample10_ESF_RAW.ino b/examples/Callbacks/CallbackExample10_ESF_RAW/CallbackExample10_ESF_RAW.ino new file mode 100644 index 0000000..ef58d40 --- /dev/null +++ b/examples/Callbacks/CallbackExample10_ESF_RAW/CallbackExample10_ESF_RAW.ino @@ -0,0 +1,160 @@ +/* + By: Paul Clark + SparkFun Electronics + Date: September 8th, 2022 + License: MIT. See license file for more information but you can + basically do whatever you want with this code. + + This example configures the External Sensor Fusion RAW IMU sensor messages on the NEO-M8U/ZED-F9R and + uses callbacks to process and display the ESF data automatically. No more polling! + + Notes: + On the ZED-F9R, each ESF RAW message contains _one_ set of IMU sensor data, seven readings in total (3 x Accel, 3 x Gyro, 1 x Temperature). + However, on the NEO-M8U, each message contains _ten_ sets of IMU sensor data, seventy readings in total. + The NEO-M8U data is all timestamped and it is possible to reconstruct the full data stream, you just need to do it + ten at a time... + Also, note that the sensor data is 24-bit signed (two's complement). You need to be careful when converting to int32_t. + Data will arrive at 100Hz. 10Hz x 10 on the NEO-M8U. + 400kHz I2C is essential. + Serial printing needs to be kept short and the baud rate needs to be around 500000. + + Please make sure your NEO-M8U is running UDR firmware >= 1.31. Please update using u-center if necessary: + https://www.u-blox.com/en/product/neo-m8u-module#tab-documentation-resources + + Feel like supporting open source hardware? + Buy a board from SparkFun! + NEO-M8U: https://www.sparkfun.com/products/16329 + + Hardware Connections: + Plug a Qwiic cable into the GPS and a Redboard Qwiic + If you don't have a platform with a Qwiic connection use the + SparkFun Qwiic Breadboard Jumper (https://www.sparkfun.com/products/14425) + Open the serial monitor at 115200 baud to see the output + +*/ + +#include //Needed for I2C to GPS + +#include //http://librarymanager/All#SparkFun_u-blox_GNSS +SFE_UBLOX_GNSS myGNSS; + +// Callback: printESFRAWdata will be called when new ESF RAW data arrives +// See u-blox_structs.h for the full definition of UBX_ESF_RAW_data_t +// _____ You can use any name you like for the callback. Use the same name when you call setAutoESFRAWcallback +// / _____ This _must_ be UBX_ESF_RAW_data_t +// | / _____ You can use any name you like for the struct +// | | / +// | | | +void printESFRAWdata(UBX_ESF_RAW_data_t *ubxDataStruct) +{ + Serial.print(F("New ESF RAW data received. Number of sensor readings is: ")); + Serial.print(ubxDataStruct->numEsfRawBlocks); + if (ubxDataStruct->numEsfRawBlocks > 7) + Serial.println(F(". (Only the first 7 will be printed.)")); + else + Serial.println(F(".")); + + for (uint8_t i = 0; (i < ubxDataStruct->numEsfRawBlocks) && (i < 7); i++) + { + switch (ubxDataStruct->data[i].data.bits.dataType) + { + case 5: + Serial.print(F("z-axis gyro: ")); + break; + case 12: + Serial.print(F("gyro temperature: ")); + break; + case 13: + Serial.print(F("y-axis gyro: ")); + break; + case 14: + Serial.print(F("x-axis gyro: ")); + break; + case 16: + Serial.print(F("x-axis accel: ")); + break; + case 17: + Serial.print(F("y-axis accel: ")); + break; + case 18: + Serial.print(F("z-axis accel: ")); + break; + default: + break; + } + if ((ubxDataStruct->data[i].data.bits.dataType == 5) || (ubxDataStruct->data[i].data.bits.dataType == 13) || (ubxDataStruct->data[i].data.bits.dataType == 14)) + { + union + { + int32_t signed32; + uint32_t unsigned32; + } signedUnsigned; // Avoid any ambiguity casting uint32_t to int32_t + // The dataField is 24-bit signed, stored in the 24 LSBs of a uint32_t + signedUnsigned.unsigned32 = ubxDataStruct->data[i].data.bits.dataField << 8; // Shift left by 8 bits to correctly align the data + float rate = signedUnsigned.signed32; // Extract the signed data. Convert to float + rate /= 256.0; // Divide by 256 to undo the shift + rate *= 0.000244140625; // Convert from deg/s*2^-12 to deg/s + Serial.println(rate); + } + else if ((ubxDataStruct->data[i].data.bits.dataType == 16) || (ubxDataStruct->data[i].data.bits.dataType == 17) || (ubxDataStruct->data[i].data.bits.dataType == 18)) + { + union + { + int32_t signed32; + uint32_t unsigned32; + } signedUnsigned; // Avoid any ambiguity casting uint32_t to int32_t + // The dataField is 24-bit signed, stored in the 24 LSBs of a uint32_t + signedUnsigned.unsigned32 = ubxDataStruct->data[i].data.bits.dataField << 8; // Shift left by 8 bits to correctly align the data + float force = signedUnsigned.signed32; // Extract the signed data. Convert to float + force /= 256.0; // Divide by 256 to undo the shift + force *= 0.0009765625; // Convert from m/s*2^-10 to m/s + Serial.println(force); + } + else if (ubxDataStruct->data[i].data.bits.dataType == 12) + { + union + { + int32_t signed32; + uint32_t unsigned32; + } signedUnsigned; // Avoid any ambiguity casting uint32_t to int32_t + // The dataField is 24-bit signed, stored in the 24 LSBs of a uint32_t + signedUnsigned.unsigned32 = ubxDataStruct->data[i].data.bits.dataField << 8; // Shift left by 8 bits to correctly align the data + float temperature = signedUnsigned.signed32; // Extract the signed data. Convert to float + temperature /= 256.0; // Divide by 256 to undo the shift + temperature *= 0.01; // Convert from C*1e-2 to C + Serial.println(temperature); + } + } +} + +void setup() +{ + Serial.begin(500000); + while (!Serial); //Wait for user to open terminal + Serial.println(F("SparkFun u-blox Example")); + + Wire.begin(); + Wire.setClock(400000); // Use 400kHz I2C + + //myGNSS.enableDebugging(); // Uncomment this line to enable debug messages on Serial + + if (myGNSS.begin() == false) //Connect to the u-blox module using Wire port + { + Serial.println(F("u-blox GNSS not detected at default I2C address. Please check wiring. Freezing.")); + while (1); + } + + myGNSS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise) + myGNSS.saveConfigSelective(VAL_CFG_SUBSEC_IOPORT); //Save (only) the communications port settings to flash and BBR + + myGNSS.setI2CpollingWait(5); //Allow checkUblox to poll I2C data every 5ms to keep up with the ESF RAW messages + + if (myGNSS.setAutoESFRAWcallbackPtr(&printESFRAWdata) == true) // Enable automatic ESF RAW messages with callback to printESFRAWdata + Serial.println(F("setAutoESFRAWcallback successful")); +} + +void loop() +{ + myGNSS.checkUblox(); // Check for the arrival of new data and process it. + myGNSS.checkCallbacks(); // Check if any callbacks are waiting to be processed. +} diff --git a/keywords.txt b/keywords.txt index 3c1f60d..752e31d 100644 --- a/keywords.txt +++ b/keywords.txt @@ -484,8 +484,6 @@ assumeAutoESFMEAS KEYWORD2 flushESFMEAS KEYWORD2 logESFMEAS KEYWORD2 -getEsfRawDataInfo KEYWORD2 -getESFRAW KEYWORD2 setAutoESFRAW KEYWORD2 setAutoESFRAWrate KEYWORD2 setAutoESFRAWcallback KEYWORD2 diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp index 0377cf7..a7e3791 100644 --- a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp +++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp @@ -4246,15 +4246,13 @@ void SFE_UBLOX_GNSS::processUBXpacket(ubxPacket *msg) // Parse various byte fields into storage - but only if we have memory allocated for it if (packetUBXESFRAW != NULL) { - for (uint16_t i = 0; (i < DEF_NUM_SENS) && ((i * 8) < (msg->len - 4)); i++) + packetUBXESFRAW->data.numEsfRawBlocks = (msg->len - 4) / 8; // Record how many blocks were received. Could be 7 or 70 (ZED-F9R vs. NEO-M8U) + for (uint16_t i = 0; (i < (DEF_NUM_SENS * DEF_MAX_NUM_ESF_RAW_REPEATS)) && ((i * 8) < (msg->len - 4)); i++) { packetUBXESFRAW->data.data[i].data.all = extractLong(msg, 4 + (i * 8)); packetUBXESFRAW->data.data[i].sTag = extractLong(msg, 8 + (i * 8)); } - // Mark all datums as fresh (not read before) - packetUBXESFRAW->moduleQueried.moduleQueried.all = 0xFFFFFFFF; - // Check if we need to copy the data for the callback if ((packetUBXESFRAW->callbackData != NULL) // If RAM has been allocated for the copy of the data && (packetUBXESFRAW->automaticFlags.flags.bits.callbackCopyValid == false)) // AND the data is stale @@ -10233,7 +10231,7 @@ bool SFE_UBLOX_GNSS::getVehAtt(uint16_t maxWait) bool SFE_UBLOX_GNSS::getNAVATT(uint16_t maxWait) { if (packetUBXNAVATT == NULL) - initPacketUBXNAVATT(); // Check that RAM has been allocated for the ESF RAW data + initPacketUBXNAVATT(); // Check that RAM has been allocated for the NAV ATT data if (packetUBXNAVATT == NULL) // Only attempt this if RAM allocation was successful return false; @@ -10373,7 +10371,7 @@ bool SFE_UBLOX_GNSS::setAutoNAVATTcallbackPtr(void (*callbackPointerPtr)(UBX_NAV bool SFE_UBLOX_GNSS::assumeAutoNAVATT(bool enabled, bool implicitUpdate) { if (packetUBXNAVATT == NULL) - initPacketUBXNAVATT(); // Check that RAM has been allocated for the ESF RAW data + initPacketUBXNAVATT(); // Check that RAM has been allocated for the NAV ATT data if (packetUBXNAVATT == NULL) // Only attempt this if RAM allocation was successful return false; @@ -14738,92 +14736,23 @@ void SFE_UBLOX_GNSS::logESFMEAS(bool enabled) // ***** ESF RAW automatic support -bool SFE_UBLOX_GNSS::getEsfRawDataInfo(uint16_t maxWait) -{ - return (getESFRAW(maxWait)); -} +// ESF RAW messages are output only. They cannot be polled. -bool SFE_UBLOX_GNSS::getESFRAW(uint16_t maxWait) -{ - if (packetUBXESFRAW == NULL) - initPacketUBXESFRAW(); // Check that RAM has been allocated for the ESF RAW data - if (packetUBXESFRAW == NULL) // Only attempt this if RAM allocation was successful - return false; - - if (packetUBXESFRAW->automaticFlags.flags.bits.automatic && packetUBXESFRAW->automaticFlags.flags.bits.implicitUpdate) - { - // The GPS is automatically reporting, we just check whether we got unread data - // if (_printDebug == true) - // { - // _debugSerial->println(F("getEsfRawDataInfo: Autoreporting")); - // } - checkUbloxInternal(&packetCfg, UBX_CLASS_ESF, UBX_ESF_RAW); - return packetUBXESFRAW->moduleQueried.moduleQueried.bits.all; - } - else if (packetUBXESFRAW->automaticFlags.flags.bits.automatic && !packetUBXESFRAW->automaticFlags.flags.bits.implicitUpdate) - { - // Someone else has to call checkUblox for us... - // if (_printDebug == true) - // { - // _debugSerial->println(F("getEsfRawDataInfo: Exit immediately")); - // } - return (false); - } - else - { - // if (_printDebug == true) - // { - // _debugSerial->println(F("getEsfRawDataInfo: Polling")); - // } - - // The GPS is not automatically reporting HNR PVT so we have to poll explicitly - packetCfg.cls = UBX_CLASS_ESF; - packetCfg.id = UBX_ESF_RAW; - packetCfg.len = 0; - packetCfg.startingSpot = 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) - { - // if (_printDebug == true) - // { - // _debugSerial->println(F("getEsfRawDataInfo: data in packetCfg was OVERWRITTEN by another message (but that's OK)")); - // } - return (true); - } - - // if (_printDebug == true) - // { - // _debugSerial->print(F("getEsfRawDataInfo retVal: ")); - // _debugSerial->println(statusString(retVal)); - // } - return (false); - } - - return (false); // Trap. We should never get here... -} - -// Enable or disable automatic ESF RAW message generation by the GNSS. This changes the way getESFRawDataInfo -// works. +// Enable or disable automatic ESF RAW message generation by the GNSS. bool SFE_UBLOX_GNSS::setAutoESFRAW(bool enable, uint16_t maxWait) { return setAutoESFRAWrate(enable ? 1 : 0, true, maxWait); } -// Enable or disable automatic ESF RAW message generation by the GNSS. This changes the way getESFRawDataInfo -// works. +// Enable or disable automatic ESF RAW message generation by the GNSS. bool SFE_UBLOX_GNSS::setAutoESFRAW(bool enable, bool implicitUpdate, uint16_t maxWait) { return setAutoESFRAWrate(enable ? 1 : 0, implicitUpdate, maxWait); } -// Enable or disable automatic ESF RAW message generation by the GNSS. This changes the way getESFRawDataInfo -// works. +// Enable or disable automatic ESF RAW message generation by the GNSS. +// Note: this function can only be used to enable or disable the messages. A rate of zero disables the messages. +// A rate of 1 or more causes the messages to be generated at the full 100Hz. bool SFE_UBLOX_GNSS::setAutoESFRAWrate(uint8_t rate, bool implicitUpdate, uint16_t maxWait) { if (packetUBXESFRAW == NULL) @@ -14848,11 +14777,10 @@ bool SFE_UBLOX_GNSS::setAutoESFRAWrate(uint8_t rate, bool implicitUpdate, uint16 packetUBXESFRAW->automaticFlags.flags.bits.automatic = (rate > 0); packetUBXESFRAW->automaticFlags.flags.bits.implicitUpdate = implicitUpdate; } - packetUBXESFRAW->moduleQueried.moduleQueried.bits.all = false; // Mark data as stale return ok; } -// Enable automatic navigation message generation by the GNSS. +// Enable automatic message generation by the GNSS. bool SFE_UBLOX_GNSS::setAutoESFRAWcallback(void (*callbackPointer)(UBX_ESF_RAW_data_t), uint16_t maxWait) { // Enable auto messages. Set implicitUpdate to false as we expect the user to call checkUblox manually. @@ -14937,7 +14865,6 @@ bool SFE_UBLOX_GNSS::initPacketUBXESFRAW() packetUBXESFRAW->callbackPointer = NULL; packetUBXESFRAW->callbackPointerPtr = NULL; packetUBXESFRAW->callbackData = NULL; - packetUBXESFRAW->moduleQueried.moduleQueried.all = 0; return (true); } @@ -14946,7 +14873,6 @@ void SFE_UBLOX_GNSS::flushESFRAW() { if (packetUBXESFRAW == NULL) return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!) - packetUBXESFRAW->moduleQueried.moduleQueried.all = 0; // Mark all datums as stale (read before) } // Log this data in file buffer @@ -18141,22 +18067,6 @@ bool SFE_UBLOX_GNSS::getSensorFusionMeasurement(UBX_ESF_MEAS_sensorData_t *senso return (true); } -bool SFE_UBLOX_GNSS::getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, uint8_t sensor, uint16_t maxWait) -{ - if (packetUBXESFRAW == NULL) - initPacketUBXESFRAW(); // Check that RAM has been allocated for the ESF RAW data - if (packetUBXESFRAW == NULL) // Bail if the RAM allocation failed - return (false); - - if ((packetUBXESFRAW->moduleQueried.moduleQueried.bits.data & (1 << sensor)) == 0) - getESFRAW(maxWait); - packetUBXESFRAW->moduleQueried.moduleQueried.bits.data &= ~(1 << sensor); // Since we are about to give this to user, mark this data as stale - packetUBXESFRAW->moduleQueried.moduleQueried.bits.all = false; - sensorData->data.all = packetUBXESFRAW->data.data[sensor].data.all; - sensorData->sTag = packetUBXESFRAW->data.data[sensor].sTag; - return (true); -} - bool SFE_UBLOX_GNSS::getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, UBX_ESF_RAW_data_t ubxDataStruct, uint8_t sensor) { sensorData->data.all = ubxDataStruct.data[sensor].data.all; diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.h b/src/SparkFun_u-blox_GNSS_Arduino_Library.h index d405b08..e608fe8 100644 --- a/src/SparkFun_u-blox_GNSS_Arduino_Library.h +++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.h @@ -1283,8 +1283,6 @@ public: void flushESFMEAS(); // Mark all the data as read/stale void logESFMEAS(bool enabled = true); // Log data to file buffer - bool getEsfRawDataInfo(uint16_t maxWait = defaultMaxWait); // ESF RAW Helper - bool getESFRAW(uint16_t maxWait = defaultMaxWait); // ESF RAW bool setAutoESFRAW(bool enabled, uint16_t maxWait = defaultMaxWait); // Enable/disable automatic ESF RAW reports bool setAutoESFRAW(bool enabled, bool implicitUpdate, uint16_t maxWait = defaultMaxWait); // Enable/disable automatic ESF RAW reports, 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 setAutoESFRAWrate(uint8_t rate, bool implicitUpdate = true, uint16_t maxWait = defaultMaxWait); // Set the rate for automatic RAW reports @@ -1470,7 +1468,6 @@ public: float getESFyaw(uint16_t maxWait = defaultMaxWait); // Returned as degrees bool getSensorFusionMeasurement(UBX_ESF_MEAS_sensorData_t *sensorData, uint8_t sensor, uint16_t maxWait = defaultMaxWait); bool getSensorFusionMeasurement(UBX_ESF_MEAS_sensorData_t *sensorData, UBX_ESF_MEAS_data_t ubxDataStruct, uint8_t sensor); - bool getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, uint8_t sensor, uint16_t maxWait = defaultMaxWait); bool getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, UBX_ESF_RAW_data_t ubxDataStruct, uint8_t sensor); bool getSensorFusionStatus(UBX_ESF_STATUS_sensorStatus_t *sensorStatus, uint8_t sensor, uint16_t maxWait = defaultMaxWait); bool getSensorFusionStatus(UBX_ESF_STATUS_sensorStatus_t *sensorStatus, UBX_ESF_STATUS_data_t ubxDataStruct, uint8_t sensor); diff --git a/src/u-blox_structs.h b/src/u-blox_structs.h index 80ca38a..bd7c875 100644 --- a/src/u-blox_structs.h +++ b/src/u-blox_structs.h @@ -49,6 +49,10 @@ #define DEF_NUM_SENS 7 // The maximum number of ESF sensors #endif +#ifndef DEF_MAX_NUM_ESF_RAW_REPEATS +#define DEF_MAX_NUM_ESF_RAW_REPEATS 10 // The NEO-M8U sends ESF RAW data in blocks / sets of ten readings. (The ZED-F9R sends them one at a time.) +#endif + // Additional flags and pointers that need to be stored with each message type struct ubxAutomaticFlags { @@ -2257,7 +2261,10 @@ typedef struct // UBX-ESF-RAW (0x10 0x03): Raw sensor measurements // Note: length is variable -const uint16_t UBX_ESF_RAW_MAX_LEN = 4 + (8 * DEF_NUM_SENS); +// Note: The ZED-F9R sends sets of seven sensor readings one at a time +// But the NEO-M8U sends them in sets of ten (i.e. seventy readings per message) +// Note: ESF RAW data cannot be polled. It is "Output" only +const uint16_t UBX_ESF_RAW_MAX_LEN = 4 + (8 * DEF_NUM_SENS * DEF_MAX_NUM_ESF_RAW_REPEATS); typedef struct { @@ -2276,28 +2283,14 @@ typedef struct typedef struct { uint8_t reserved1[4]; - UBX_ESF_RAW_sensorData_t data[DEF_NUM_SENS]; + UBX_ESF_RAW_sensorData_t data[DEF_NUM_SENS * DEF_MAX_NUM_ESF_RAW_REPEATS]; + uint8_t numEsfRawBlocks; // Note: this is not contained in the ESF RAW message. It is calculated from the message length. } UBX_ESF_RAW_data_t; -typedef struct -{ - union - { - uint32_t all; - struct - { - uint32_t all : 1; - - uint32_t data : DEF_NUM_SENS; - } bits; - } moduleQueried; -} UBX_ESF_RAW_moduleQueried_t; - typedef struct { ubxAutomaticFlags automaticFlags; UBX_ESF_RAW_data_t data; - UBX_ESF_RAW_moduleQueried_t moduleQueried; void (*callbackPointer)(UBX_ESF_RAW_data_t); void (*callbackPointerPtr)(UBX_ESF_RAW_data_t *); UBX_ESF_RAW_data_t *callbackData;