Skip to content

Commit 1593dbc

Browse files
PaulZCPaulZC
committed
Correct support for ESF RAW
1 parent e988f05 commit 1593dbc

File tree

5 files changed

+181
-123
lines changed

5 files changed

+181
-123
lines changed

‎examples/Callbacks/CallbackExample10_ESF_RAW/CallbackExample10_ESF_RAW.ino

+160
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,160 @@
1+
/*
2+
By: Paul Clark
3+
SparkFun Electronics
4+
Date: September 8th, 2022
5+
License: MIT. See license file for more information but you can
6+
basically do whatever you want with this code.
7+
8+
This example configures the External Sensor Fusion RAW IMU sensor messages on the NEO-M8U/ZED-F9R and
9+
uses callbacks to process and display the ESF data automatically. No more polling!
10+
11+
Notes:
12+
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).
13+
However, on the NEO-M8U, each message contains _ten_ sets of IMU sensor data, seventy readings in total.
14+
The NEO-M8U data is all timestamped and it is possible to reconstruct the full data stream, you just need to do it
15+
ten at a time...
16+
Also, note that the sensor data is 24-bit signed (two's complement). You need to be careful when converting to int32_t.
17+
Data will arrive at 100Hz. 10Hz x 10 on the NEO-M8U.
18+
400kHz I2C is essential.
19+
Serial printing needs to be kept short and the baud rate needs to be around 500000.
20+
21+
Please make sure your NEO-M8U is running UDR firmware >= 1.31. Please update using u-center if necessary:
22+
https://www.u-blox.com/en/product/neo-m8u-module#tab-documentation-resources
23+
24+
Feel like supporting open source hardware?
25+
Buy a board from SparkFun!
26+
NEO-M8U: https://www.sparkfun.com/products/16329
27+
28+
Hardware Connections:
29+
Plug a Qwiic cable into the GPS and a Redboard Qwiic
30+
If you don't have a platform with a Qwiic connection use the
31+
SparkFun Qwiic Breadboard Jumper (https://www.sparkfun.com/products/14425)
32+
Open the serial monitor at 115200 baud to see the output
33+
34+
*/
35+
36+
#include <Wire.h> //Needed for I2C to GPS
37+
38+
#include <SparkFun_u-blox_GNSS_Arduino_Library.h> //http://librarymanager/All#SparkFun_u-blox_GNSS
39+
SFE_UBLOX_GNSS myGNSS;
40+
41+
// Callback: printESFRAWdata will be called when new ESF RAW data arrives
42+
// See u-blox_structs.h for the full definition of UBX_ESF_RAW_data_t
43+
// _____ You can use any name you like for the callback. Use the same name when you call setAutoESFRAWcallback
44+
// / _____ This _must_ be UBX_ESF_RAW_data_t
45+
// | / _____ You can use any name you like for the struct
46+
// | | /
47+
// | | |
48+
void printESFRAWdata(UBX_ESF_RAW_data_t *ubxDataStruct)
49+
{
50+
Serial.print(F("New ESF RAW data received. Number of sensor readings is: "));
51+
Serial.print(ubxDataStruct->numEsfRawBlocks);
52+
if (ubxDataStruct->numEsfRawBlocks > 7)
53+
Serial.println(F(". (Only the first 7 will be printed.)"));
54+
else
55+
Serial.println(F("."));
56+
57+
for (uint8_t i = 0; (i < ubxDataStruct->numEsfRawBlocks) && (i < 7); i++)
58+
{
59+
switch (ubxDataStruct->data[i].data.bits.dataType)
60+
{
61+
case 5:
62+
Serial.print(F("z-axis gyro: "));
63+
break;
64+
case 12:
65+
Serial.print(F("gyro temperature: "));
66+
break;
67+
case 13:
68+
Serial.print(F("y-axis gyro: "));
69+
break;
70+
case 14:
71+
Serial.print(F("x-axis gyro: "));
72+
break;
73+
case 16:
74+
Serial.print(F("x-axis accel: "));
75+
break;
76+
case 17:
77+
Serial.print(F("y-axis accel: "));
78+
break;
79+
case 18:
80+
Serial.print(F("z-axis accel: "));
81+
break;
82+
default:
83+
break;
84+
}
85+
if ((ubxDataStruct->data[i].data.bits.dataType == 5) || (ubxDataStruct->data[i].data.bits.dataType == 13) || (ubxDataStruct->data[i].data.bits.dataType == 14))
86+
{
87+
union
88+
{
89+
int32_t signed32;
90+
uint32_t unsigned32;
91+
} signedUnsigned; // Avoid any ambiguity casting uint32_t to int32_t
92+
// The dataField is 24-bit signed, stored in the 24 LSBs of a uint32_t
93+
signedUnsigned.unsigned32 = ubxDataStruct->data[i].data.bits.dataField << 8; // Shift left by 8 bits to correctly align the data
94+
float rate = signedUnsigned.signed32; // Extract the signed data. Convert to float
95+
rate /= 256.0; // Divide by 256 to undo the shift
96+
rate *= 0.000244140625; // Convert from deg/s*2^-12 to deg/s
97+
Serial.println(rate);
98+
}
99+
else if ((ubxDataStruct->data[i].data.bits.dataType == 16) || (ubxDataStruct->data[i].data.bits.dataType == 17) || (ubxDataStruct->data[i].data.bits.dataType == 18))
100+
{
101+
union
102+
{
103+
int32_t signed32;
104+
uint32_t unsigned32;
105+
} signedUnsigned; // Avoid any ambiguity casting uint32_t to int32_t
106+
// The dataField is 24-bit signed, stored in the 24 LSBs of a uint32_t
107+
signedUnsigned.unsigned32 = ubxDataStruct->data[i].data.bits.dataField << 8; // Shift left by 8 bits to correctly align the data
108+
float force = signedUnsigned.signed32; // Extract the signed data. Convert to float
109+
force /= 256.0; // Divide by 256 to undo the shift
110+
force *= 0.0009765625; // Convert from m/s*2^-10 to m/s
111+
Serial.println(force);
112+
}
113+
else if (ubxDataStruct->data[i].data.bits.dataType == 12)
114+
{
115+
union
116+
{
117+
int32_t signed32;
118+
uint32_t unsigned32;
119+
} signedUnsigned; // Avoid any ambiguity casting uint32_t to int32_t
120+
// The dataField is 24-bit signed, stored in the 24 LSBs of a uint32_t
121+
signedUnsigned.unsigned32 = ubxDataStruct->data[i].data.bits.dataField << 8; // Shift left by 8 bits to correctly align the data
122+
float temperature = signedUnsigned.signed32; // Extract the signed data. Convert to float
123+
temperature /= 256.0; // Divide by 256 to undo the shift
124+
temperature *= 0.01; // Convert from C*1e-2 to C
125+
Serial.println(temperature);
126+
}
127+
}
128+
}
129+
130+
void setup()
131+
{
132+
Serial.begin(500000);
133+
while (!Serial); //Wait for user to open terminal
134+
Serial.println(F("SparkFun u-blox Example"));
135+
136+
Wire.begin();
137+
Wire.setClock(400000); // Use 400kHz I2C
138+
139+
//myGNSS.enableDebugging(); // Uncomment this line to enable debug messages on Serial
140+
141+
if (myGNSS.begin() == false) //Connect to the u-blox module using Wire port
142+
{
143+
Serial.println(F("u-blox GNSS not detected at default I2C address. Please check wiring. Freezing."));
144+
while (1);
145+
}
146+
147+
myGNSS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise)
148+
myGNSS.saveConfigSelective(VAL_CFG_SUBSEC_IOPORT); //Save (only) the communications port settings to flash and BBR
149+
150+
myGNSS.setI2CpollingWait(5); //Allow checkUblox to poll I2C data every 5ms to keep up with the ESF RAW messages
151+
152+
if (myGNSS.setAutoESFRAWcallbackPtr(&printESFRAWdata) == true) // Enable automatic ESF RAW messages with callback to printESFRAWdata
153+
Serial.println(F("setAutoESFRAWcallback successful"));
154+
}
155+
156+
void loop()
157+
{
158+
myGNSS.checkUblox(); // Check for the arrival of new data and process it.
159+
myGNSS.checkCallbacks(); // Check if any callbacks are waiting to be processed.
160+
}

‎keywords.txt

-2
Original file line numberDiff line numberDiff line change
@@ -484,8 +484,6 @@ assumeAutoESFMEAS KEYWORD2
484484
flushESFMEAS KEYWORD2
485485
logESFMEAS KEYWORD2
486486

487-
getEsfRawDataInfo KEYWORD2
488-
getESFRAW KEYWORD2
489487
setAutoESFRAW KEYWORD2
490488
setAutoESFRAWrate KEYWORD2
491489
setAutoESFRAWcallback KEYWORD2

‎src/SparkFun_u-blox_GNSS_Arduino_Library.cpp

+11-101
Original file line numberDiff line numberDiff line change
@@ -4246,15 +4246,13 @@ void SFE_UBLOX_GNSS::processUBXpacket(ubxPacket *msg)
42464246
// Parse various byte fields into storage - but only if we have memory allocated for it
42474247
if (packetUBXESFRAW != NULL)
42484248
{
4249-
for (uint16_t i = 0; (i < DEF_NUM_SENS) && ((i * 8) < (msg->len - 4)); i++)
4249+
packetUBXESFRAW->data.numEsfRawBlocks = (msg->len - 4) / 8; // Record how many blocks were received. Could be 7 or 70 (ZED-F9R vs. NEO-M8U)
4250+
for (uint16_t i = 0; (i < (DEF_NUM_SENS * DEF_MAX_NUM_ESF_RAW_REPEATS)) && ((i * 8) < (msg->len - 4)); i++)
42504251
{
42514252
packetUBXESFRAW->data.data[i].data.all = extractLong(msg, 4 + (i * 8));
42524253
packetUBXESFRAW->data.data[i].sTag = extractLong(msg, 8 + (i * 8));
42534254
}
42544255

4255-
// Mark all datums as fresh (not read before)
4256-
packetUBXESFRAW->moduleQueried.moduleQueried.all = 0xFFFFFFFF;
4257-
42584256
// Check if we need to copy the data for the callback
42594257
if ((packetUBXESFRAW->callbackData != NULL) // If RAM has been allocated for the copy of the data
42604258
&& (packetUBXESFRAW->automaticFlags.flags.bits.callbackCopyValid == false)) // AND the data is stale
@@ -10233,7 +10231,7 @@ bool SFE_UBLOX_GNSS::getVehAtt(uint16_t maxWait)
1023310231
bool SFE_UBLOX_GNSS::getNAVATT(uint16_t maxWait)
1023410232
{
1023510233
if (packetUBXNAVATT == NULL)
10236-
initPacketUBXNAVATT(); // Check that RAM has been allocated for the ESF RAW data
10234+
initPacketUBXNAVATT(); // Check that RAM has been allocated for the NAV ATT data
1023710235
if (packetUBXNAVATT == NULL) // Only attempt this if RAM allocation was successful
1023810236
return false;
1023910237

@@ -10373,7 +10371,7 @@ bool SFE_UBLOX_GNSS::setAutoNAVATTcallbackPtr(void (*callbackPointerPtr)(UBX_NAV
1037310371
bool SFE_UBLOX_GNSS::assumeAutoNAVATT(bool enabled, bool implicitUpdate)
1037410372
{
1037510373
if (packetUBXNAVATT == NULL)
10376-
initPacketUBXNAVATT(); // Check that RAM has been allocated for the ESF RAW data
10374+
initPacketUBXNAVATT(); // Check that RAM has been allocated for the NAV ATT data
1037710375
if (packetUBXNAVATT == NULL) // Only attempt this if RAM allocation was successful
1037810376
return false;
1037910377

@@ -14738,92 +14736,23 @@ void SFE_UBLOX_GNSS::logESFMEAS(bool enabled)
1473814736

1473914737
// ***** ESF RAW automatic support
1474014738

14741-
bool SFE_UBLOX_GNSS::getEsfRawDataInfo(uint16_t maxWait)
14742-
{
14743-
return (getESFRAW(maxWait));
14744-
}
14745-
14746-
bool SFE_UBLOX_GNSS::getESFRAW(uint16_t maxWait)
14747-
{
14748-
if (packetUBXESFRAW == NULL)
14749-
initPacketUBXESFRAW(); // Check that RAM has been allocated for the ESF RAW data
14750-
if (packetUBXESFRAW == NULL) // Only attempt this if RAM allocation was successful
14751-
return false;
14752-
14753-
if (packetUBXESFRAW->automaticFlags.flags.bits.automatic && packetUBXESFRAW->automaticFlags.flags.bits.implicitUpdate)
14754-
{
14755-
// The GPS is automatically reporting, we just check whether we got unread data
14756-
// if (_printDebug == true)
14757-
// {
14758-
// _debugSerial->println(F("getEsfRawDataInfo: Autoreporting"));
14759-
// }
14760-
checkUbloxInternal(&packetCfg, UBX_CLASS_ESF, UBX_ESF_RAW);
14761-
return packetUBXESFRAW->moduleQueried.moduleQueried.bits.all;
14762-
}
14763-
else if (packetUBXESFRAW->automaticFlags.flags.bits.automatic && !packetUBXESFRAW->automaticFlags.flags.bits.implicitUpdate)
14764-
{
14765-
// Someone else has to call checkUblox for us...
14766-
// if (_printDebug == true)
14767-
// {
14768-
// _debugSerial->println(F("getEsfRawDataInfo: Exit immediately"));
14769-
// }
14770-
return (false);
14771-
}
14772-
else
14773-
{
14774-
// if (_printDebug == true)
14775-
// {
14776-
// _debugSerial->println(F("getEsfRawDataInfo: Polling"));
14777-
// }
14778-
14779-
// The GPS is not automatically reporting HNR PVT so we have to poll explicitly
14780-
packetCfg.cls = UBX_CLASS_ESF;
14781-
packetCfg.id = UBX_ESF_RAW;
14782-
packetCfg.len = 0;
14783-
packetCfg.startingSpot = 0;
14784-
14785-
// The data is parsed as part of processing the response
14786-
sfe_ublox_status_e retVal = sendCommand(&packetCfg, maxWait);
14787-
14788-
if (retVal == SFE_UBLOX_STATUS_DATA_RECEIVED)
14789-
return (true);
14790-
14791-
if (retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN)
14792-
{
14793-
// if (_printDebug == true)
14794-
// {
14795-
// _debugSerial->println(F("getEsfRawDataInfo: data in packetCfg was OVERWRITTEN by another message (but that's OK)"));
14796-
// }
14797-
return (true);
14798-
}
14799-
14800-
// if (_printDebug == true)
14801-
// {
14802-
// _debugSerial->print(F("getEsfRawDataInfo retVal: "));
14803-
// _debugSerial->println(statusString(retVal));
14804-
// }
14805-
return (false);
14806-
}
14807-
14808-
return (false); // Trap. We should never get here...
14809-
}
14739+
// ESF RAW messages are output only. They cannot be polled.
1481014740

14811-
// Enable or disable automatic ESF RAW message generation by the GNSS. This changes the way getESFRawDataInfo
14812-
// works.
14741+
// Enable or disable automatic ESF RAW message generation by the GNSS.
1481314742
bool SFE_UBLOX_GNSS::setAutoESFRAW(bool enable, uint16_t maxWait)
1481414743
{
1481514744
return setAutoESFRAWrate(enable ? 1 : 0, true, maxWait);
1481614745
}
1481714746

14818-
// Enable or disable automatic ESF RAW message generation by the GNSS. This changes the way getESFRawDataInfo
14819-
// works.
14747+
// Enable or disable automatic ESF RAW message generation by the GNSS.
1482014748
bool SFE_UBLOX_GNSS::setAutoESFRAW(bool enable, bool implicitUpdate, uint16_t maxWait)
1482114749
{
1482214750
return setAutoESFRAWrate(enable ? 1 : 0, implicitUpdate, maxWait);
1482314751
}
1482414752

14825-
// Enable or disable automatic ESF RAW message generation by the GNSS. This changes the way getESFRawDataInfo
14826-
// works.
14753+
// Enable or disable automatic ESF RAW message generation by the GNSS.
14754+
// Note: this function can only be used to enable or disable the messages. A rate of zero disables the messages.
14755+
// A rate of 1 or more causes the messages to be generated at the full 100Hz.
1482714756
bool SFE_UBLOX_GNSS::setAutoESFRAWrate(uint8_t rate, bool implicitUpdate, uint16_t maxWait)
1482814757
{
1482914758
if (packetUBXESFRAW == NULL)
@@ -14848,11 +14777,10 @@ bool SFE_UBLOX_GNSS::setAutoESFRAWrate(uint8_t rate, bool implicitUpdate, uint16
1484814777
packetUBXESFRAW->automaticFlags.flags.bits.automatic = (rate > 0);
1484914778
packetUBXESFRAW->automaticFlags.flags.bits.implicitUpdate = implicitUpdate;
1485014779
}
14851-
packetUBXESFRAW->moduleQueried.moduleQueried.bits.all = false; // Mark data as stale
1485214780
return ok;
1485314781
}
1485414782

14855-
// Enable automatic navigation message generation by the GNSS.
14783+
// Enable automatic message generation by the GNSS.
1485614784
bool SFE_UBLOX_GNSS::setAutoESFRAWcallback(void (*callbackPointer)(UBX_ESF_RAW_data_t), uint16_t maxWait)
1485714785
{
1485814786
// 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()
1493714865
packetUBXESFRAW->callbackPointer = NULL;
1493814866
packetUBXESFRAW->callbackPointerPtr = NULL;
1493914867
packetUBXESFRAW->callbackData = NULL;
14940-
packetUBXESFRAW->moduleQueried.moduleQueried.all = 0;
1494114868
return (true);
1494214869
}
1494314870

@@ -14946,7 +14873,6 @@ void SFE_UBLOX_GNSS::flushESFRAW()
1494614873
{
1494714874
if (packetUBXESFRAW == NULL)
1494814875
return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!)
14949-
packetUBXESFRAW->moduleQueried.moduleQueried.all = 0; // Mark all datums as stale (read before)
1495014876
}
1495114877

1495214878
// Log this data in file buffer
@@ -18141,22 +18067,6 @@ bool SFE_UBLOX_GNSS::getSensorFusionMeasurement(UBX_ESF_MEAS_sensorData_t *senso
1814118067
return (true);
1814218068
}
1814318069

18144-
bool SFE_UBLOX_GNSS::getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, uint8_t sensor, uint16_t maxWait)
18145-
{
18146-
if (packetUBXESFRAW == NULL)
18147-
initPacketUBXESFRAW(); // Check that RAM has been allocated for the ESF RAW data
18148-
if (packetUBXESFRAW == NULL) // Bail if the RAM allocation failed
18149-
return (false);
18150-
18151-
if ((packetUBXESFRAW->moduleQueried.moduleQueried.bits.data & (1 << sensor)) == 0)
18152-
getESFRAW(maxWait);
18153-
packetUBXESFRAW->moduleQueried.moduleQueried.bits.data &= ~(1 << sensor); // Since we are about to give this to user, mark this data as stale
18154-
packetUBXESFRAW->moduleQueried.moduleQueried.bits.all = false;
18155-
sensorData->data.all = packetUBXESFRAW->data.data[sensor].data.all;
18156-
sensorData->sTag = packetUBXESFRAW->data.data[sensor].sTag;
18157-
return (true);
18158-
}
18159-
1816018070
bool SFE_UBLOX_GNSS::getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, UBX_ESF_RAW_data_t ubxDataStruct, uint8_t sensor)
1816118071
{
1816218072
sensorData->data.all = ubxDataStruct.data[sensor].data.all;

‎src/SparkFun_u-blox_GNSS_Arduino_Library.h

-3
Original file line numberDiff line numberDiff line change
@@ -1283,8 +1283,6 @@ class SFE_UBLOX_GNSS
12831283
void flushESFMEAS(); // Mark all the data as read/stale
12841284
void logESFMEAS(bool enabled = true); // Log data to file buffer
12851285

1286-
bool getEsfRawDataInfo(uint16_t maxWait = defaultMaxWait); // ESF RAW Helper
1287-
bool getESFRAW(uint16_t maxWait = defaultMaxWait); // ESF RAW
12881286
bool setAutoESFRAW(bool enabled, uint16_t maxWait = defaultMaxWait); // Enable/disable automatic ESF RAW reports
12891287
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
12901288
bool setAutoESFRAWrate(uint8_t rate, bool implicitUpdate = true, uint16_t maxWait = defaultMaxWait); // Set the rate for automatic RAW reports
@@ -1470,7 +1468,6 @@ class SFE_UBLOX_GNSS
14701468
float getESFyaw(uint16_t maxWait = defaultMaxWait); // Returned as degrees
14711469
bool getSensorFusionMeasurement(UBX_ESF_MEAS_sensorData_t *sensorData, uint8_t sensor, uint16_t maxWait = defaultMaxWait);
14721470
bool getSensorFusionMeasurement(UBX_ESF_MEAS_sensorData_t *sensorData, UBX_ESF_MEAS_data_t ubxDataStruct, uint8_t sensor);
1473-
bool getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, uint8_t sensor, uint16_t maxWait = defaultMaxWait);
14741471
bool getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, UBX_ESF_RAW_data_t ubxDataStruct, uint8_t sensor);
14751472
bool getSensorFusionStatus(UBX_ESF_STATUS_sensorStatus_t *sensorStatus, uint8_t sensor, uint16_t maxWait = defaultMaxWait);
14761473
bool getSensorFusionStatus(UBX_ESF_STATUS_sensorStatus_t *sensorStatus, UBX_ESF_STATUS_data_t ubxDataStruct, uint8_t sensor);

0 commit comments

Comments
 (0)