Skip to content

Commit ff90a03

Browse files
nseidlenseidle
committed
Add support for Auto IMU alignment
1 parent ff75d2a commit ff90a03

File tree

4 files changed

+189
-0
lines changed

4 files changed

+189
-0
lines changed

Diff for: ‎examples/Dead_Reckoning/Example7_setESFAutoAlignment/Example7_setESFAutoAlignment.ino

+116
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
/*
2+
By: Nathan Seidle
3+
SparkFun Electronics
4+
Date: August, 2021
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 AutoAlignment option for the IMU.
9+
The ZED-F9R Integration guide recommends enabling Auto Alignment once
10+
the device has been attached to the vehicle's frame.
11+
Enabling auto-alignment will cause the the sensor fusion status
12+
to begin initialization. After driving around a few turns, the sensors
13+
should enter 'Calibrated' state. See example 1 for fusion state or
14+
monitor UBX-ESF-STATUS.
15+
16+
As of writing the ZED-F9R is using HPS v1.2 firmware. Please update using u-center if necessary.
17+
18+
Feel like supporting open source hardware?
19+
Buy a board from SparkFun!
20+
ZED-F9R: https://www.sparkfun.com/products/16344
21+
ZED-F9R pHat: https://www.sparkfun.com/products/16475
22+
NEO-M8U: https://www.sparkfun.com/products/16329
23+
24+
Hardware Connections:
25+
Plug a Qwiic cable into the GPS and a Redboard Qwiic
26+
If you don't have a platform with a Qwiic connection use the
27+
SparkFun Qwiic Breadboard Jumper (https://www.sparkfun.com/products/17912)
28+
Open the serial monitor at 115200 baud to see the output
29+
30+
*/
31+
32+
#include <Wire.h> //Needed for I2C to GPS
33+
34+
#include <SparkFun_u-blox_GNSS_Arduino_Library.h> //http://librarymanager/All#SparkFun_u-blox_GNSS
35+
SFE_UBLOX_GNSS myGNSS;
36+
37+
void setup()
38+
{
39+
Serial.begin(115200);
40+
while (!Serial); //Wait for user to open terminal
41+
Serial.println(F("SparkFun u-blox Example"));
42+
43+
Wire.begin();
44+
45+
//myGNSS.enableDebugging(); // Uncomment this line to enable debug messages on Serial
46+
47+
if (myGNSS.begin() == false) //Connect to the u-blox module using Wire port
48+
{
49+
Serial.println(F("Warning! u-blox GPS did not begin correctly."));
50+
Serial.println(F("(This may be because the I2C port is busy with HNR messages.)"));
51+
}
52+
53+
bool esfAutoAlignment = myGNSS.getESFAutoAlignment();
54+
Serial.print(F("esfAutoAlignment: "));
55+
if (esfAutoAlignment == true)
56+
Serial.println(F("True"));
57+
else
58+
Serial.println(F("False"));
59+
60+
myGNSS.setESFAutoAlignment(true); //Enable UBX-CFG-ESFALG Automatic IMU-mount Alignment
61+
62+
myGNSS.setAutoHNRATT(false); //Make sure auto HNR attitude messages are disabled
63+
myGNSS.setAutoHNRINS(false); //Make sure auto HNR vehicle dynamics messages are disabled
64+
myGNSS.setAutoHNRPVT(false); //Make sure auto HNR PVT messages are disabled
65+
66+
myGNSS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise)
67+
myGNSS.saveConfigSelective(VAL_CFG_SUBSEC_IOPORT); //Save (only) the communications port settings to flash and BBR
68+
}
69+
70+
void loop()
71+
{
72+
// ESF data is produced at the navigation rate, so by default we'll get fresh data once per second
73+
if (myGNSS.getEsfInfo()) // Poll new ESF STATUS data
74+
{
75+
Serial.print(F("Fusion Mode: "));
76+
Serial.print(myGNSS.packetUBXESFSTATUS->data.fusionMode);
77+
if (myGNSS.packetUBXESFSTATUS->data.fusionMode == 0)
78+
Serial.println(F(" Sensor is initializing..."));
79+
else if (myGNSS.packetUBXESFSTATUS->data.fusionMode == 1)
80+
Serial.println(F(" Sensor is calibrated!"));
81+
else if (myGNSS.packetUBXESFSTATUS->data.fusionMode == 2)
82+
Serial.println(F(" Sensor fusion is suspended!"));
83+
else if (myGNSS.packetUBXESFSTATUS->data.fusionMode == 3)
84+
Serial.println(F(" Sensor fusion is disabled!"));
85+
}
86+
87+
// Poll and print selected HNR data
88+
if (myGNSS.getHNRAtt(125) == true) // Request HNR Att data using a 125ms timeout
89+
{
90+
Serial.print(F("Roll: "));
91+
Serial.print(myGNSS.getHNRroll(), 2); // Use the helper function to get the roll in degrees
92+
Serial.print(F(" Pitch: "));
93+
Serial.print(myGNSS.getHNRpitch(), 2); // Use the helper function to get the pitch in degrees
94+
Serial.print(F(" Heading: "));
95+
Serial.println(myGNSS.getHNRheading(), 2); // Use the helper function to get the heading in degrees
96+
}
97+
if (myGNSS.getHNRDyn(125) == true) // Request HNR Dyn data using a 125ms timeout
98+
{
99+
Serial.print(F("xAccel: "));
100+
Serial.print(myGNSS.packetUBXHNRINS->data.xAccel);
101+
Serial.print(F(" yAccel: "));
102+
Serial.print(myGNSS.packetUBXHNRINS->data.yAccel);
103+
Serial.print(F(" zAccel: "));
104+
Serial.println(myGNSS.packetUBXHNRINS->data.zAccel);
105+
}
106+
if (myGNSS.getHNRPVT(125) == true) // Request HNR PVT data using a 125ms timeout
107+
{
108+
Serial.print(F("ns: "));
109+
Serial.print(myGNSS.packetUBXHNRPVT->data.nano);
110+
Serial.print(F(" Lat: "));
111+
Serial.print(myGNSS.packetUBXHNRPVT->data.lat);
112+
Serial.print(F(" Lon: "));
113+
Serial.println(myGNSS.packetUBXHNRPVT->data.lon);
114+
}
115+
116+
}

Diff for: ‎keywords.txt

+3
Original file line numberDiff line numberDiff line change
@@ -365,6 +365,9 @@ initPacketUBXESFRAW KEYWORD2
365365
flushESFRAW KEYWORD2
366366
logESFRAW KEYWORD2
367367

368+
getESFAutoAlignment KEYWORD2
369+
setESFAutoAlignment KEYWORD2
370+
368371
getHNRAtt KEYWORD2
369372
getHNRATT KEYWORD2
370373
setAutoHNRATT KEYWORD2

Diff for: ‎src/SparkFun_u-blox_GNSS_Arduino_Library.cpp

+66
Original file line numberDiff line numberDiff line change
@@ -5074,6 +5074,72 @@ boolean SFE_UBLOX_GNSS::resetIMUalignment(uint16_t maxWait)
50745074
return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
50755075
}
50765076

5077+
//UBX-CFG-ESFALG is not documented. This was found using u-center.
5078+
//Returns the state of the UBX-CFG-ESFALG 'Automatic IMU-mount Alignment' flag
5079+
bool SFE_UBLOX_GNSS::getESFAutoAlignment(uint16_t maxWait)
5080+
{
5081+
packetCfg.cls = UBX_CLASS_CFG;
5082+
packetCfg.id = UBX_CFG_ESFALG;
5083+
packetCfg.len = 0;
5084+
packetCfg.startingSpot = 0;
5085+
5086+
if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED)
5087+
{
5088+
#ifndef SFE_UBLOX_REDUCED_PROG_MEM
5089+
if (_printDebug == true)
5090+
{
5091+
_debugSerial->println(F("getESFAutoAlignment failed"));
5092+
}
5093+
#endif
5094+
5095+
return (false); //If command send fails then bail
5096+
}
5097+
5098+
return (payloadCfg[1] & 0b1); //Return Bit 0
5099+
}
5100+
5101+
//Set the state of the UBX-CFG-ESFALG 'Automatic IMU-mount Alignment' flag
5102+
bool SFE_UBLOX_GNSS::setESFAutoAlignment(bool enable, uint16_t maxWait)
5103+
{
5104+
packetCfg.cls = UBX_CLASS_CFG;
5105+
packetCfg.id = UBX_CFG_ESFALG;
5106+
packetCfg.len = 0;
5107+
packetCfg.startingSpot = 0;
5108+
5109+
if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED)
5110+
{
5111+
#ifndef SFE_UBLOX_REDUCED_PROG_MEM
5112+
if (_printDebug == true)
5113+
{
5114+
_debugSerial->println(F("getESFAutoAlignment failed"));
5115+
}
5116+
#endif
5117+
5118+
return (false); //If command send fails then bail
5119+
}
5120+
5121+
//payloadCfg is now filled
5122+
5123+
if (enable)
5124+
payloadCfg[1] |= 0b1;
5125+
else
5126+
payloadCfg[1] &= ~(0b1);
5127+
5128+
if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_SENT) // This time we are only expecting an ACK
5129+
{
5130+
#ifndef SFE_UBLOX_REDUCED_PROG_MEM
5131+
if (_printDebug == true)
5132+
{
5133+
_debugSerial->println(F("setESFAutoAlignment failed"));
5134+
}
5135+
#endif
5136+
return (false);
5137+
}
5138+
5139+
return (true);
5140+
}
5141+
5142+
50775143
//Get the time pulse parameters using UBX_CFG_TP5
50785144
boolean SFE_UBLOX_GNSS::getTimePulseParameters(UBX_CFG_TP5_data_t *data, uint16_t maxWait)
50795145
{

Diff for: ‎src/SparkFun_u-blox_GNSS_Arduino_Library.h

+4
Original file line numberDiff line numberDiff line change
@@ -766,6 +766,10 @@ class SFE_UBLOX_GNSS
766766
//Reset ESF automatic IMU-mount alignment
767767
boolean resetIMUalignment(uint16_t maxWait = defaultMaxWait);
768768

769+
//Enable/disable esfAutoAlignment
770+
bool getESFAutoAlignment(uint16_t maxWait = defaultMaxWait);
771+
bool setESFAutoAlignment(bool enable, uint16_t maxWait = defaultMaxWait);
772+
769773
//Configure Time Pulse Parameters
770774
boolean getTimePulseParameters(UBX_CFG_TP5_data_t *data = NULL, uint16_t maxWait = defaultMaxWait); // Get the time pulse parameters using UBX_CFG_TP5
771775
boolean setTimePulseParameters(UBX_CFG_TP5_data_t *data = NULL, uint16_t maxWait = defaultMaxWait); // Set the time pulse parameters using UBX_CFG_TP5

0 commit comments

Comments
 (0)