Add support for Auto IMU alignment

examples/Dead_Reckoning/Example7_setESFAutoAlignment/Example7_setESFAutoAlignment.ino

@@ -0,0 +1,116 @@
+/*
+  By: Nathan Seidle
+  SparkFun Electronics
+  Date: August, 2021
+  License: MIT. See license file for more information but you can
+  basically do whatever you want with this code.
+
+  This example configures the AutoAlignment option for the IMU.
+  The ZED-F9R Integration guide recommends enabling Auto Alignment once
+  the device has been attached to the vehicle's frame.
+  Enabling auto-alignment will cause the the sensor fusion status
+  to begin initialization. After driving around a few turns, the sensors
+  should enter 'Calibrated' state. See example 1 for fusion state or
+  monitor UBX-ESF-STATUS.
+
+  As of writing the ZED-F9R is using HPS v1.2 firmware. Please update using u-center if necessary.
+
+  Feel like supporting open source hardware?
+  Buy a board from SparkFun!
+  ZED-F9R: https://www.sparkfun.com/products/16344
+  ZED-F9R pHat: https://www.sparkfun.com/products/16475
+  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/17912)
+  Open the serial monitor at 115200 baud to see the output
+
+*/
+
+#include <Wire.h> //Needed for I2C to GPS
+
+#include <SparkFun_u-blox_GNSS_Arduino_Library.h> //http://librarymanager/All#SparkFun_u-blox_GNSS
+SFE_UBLOX_GNSS myGNSS;
+
+void setup()
+{
+  Serial.begin(115200);
+  while (!Serial); //Wait for user to open terminal
+  Serial.println(F("SparkFun u-blox Example"));
+
+  Wire.begin();
+
+  //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("Warning! u-blox GPS did not begin correctly."));
+    Serial.println(F("(This may be because the I2C port is busy with HNR messages.)"));
+  }
+
+  bool esfAutoAlignment = myGNSS.getESFAutoAlignment();
+  Serial.print(F("esfAutoAlignment: "));
+  if (esfAutoAlignment == true)
+    Serial.println(F("True"));
+  else
+    Serial.println(F("False"));
+
+  myGNSS.setESFAutoAlignment(true); //Enable UBX-CFG-ESFALG Automatic IMU-mount Alignment
+
+  myGNSS.setAutoHNRATT(false); //Make sure auto HNR attitude messages are disabled
+  myGNSS.setAutoHNRINS(false); //Make sure auto HNR vehicle dynamics messages are disabled
+  myGNSS.setAutoHNRPVT(false); //Make sure auto HNR PVT messages are disabled
+
+  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
+}
+
+void loop()
+{
+  // ESF data is produced at the navigation rate, so by default we'll get fresh data once per second
+  if (myGNSS.getEsfInfo()) // Poll new ESF STATUS data
+  {
+    Serial.print(F("Fusion Mode: "));
+    Serial.print(myGNSS.packetUBXESFSTATUS->data.fusionMode);
+    if (myGNSS.packetUBXESFSTATUS->data.fusionMode == 0)
+      Serial.println(F("  Sensor is initializing..."));
+    else if (myGNSS.packetUBXESFSTATUS->data.fusionMode == 1)
+      Serial.println(F("  Sensor is calibrated!"));
+    else if (myGNSS.packetUBXESFSTATUS->data.fusionMode == 2)
+      Serial.println(F("  Sensor fusion is suspended!"));
+    else if (myGNSS.packetUBXESFSTATUS->data.fusionMode == 3)
+      Serial.println(F("  Sensor fusion is disabled!"));
+  }
+
+  // Poll and print selected HNR data
+  if (myGNSS.getHNRAtt(125) == true) // Request HNR Att data using a 125ms timeout
+  {
+    Serial.print(F("Roll: "));
+    Serial.print(myGNSS.getHNRroll(), 2); // Use the helper function to get the roll in degrees
+    Serial.print(F(" Pitch: "));
+    Serial.print(myGNSS.getHNRpitch(), 2); // Use the helper function to get the pitch in degrees
+    Serial.print(F(" Heading: "));
+    Serial.println(myGNSS.getHNRheading(), 2); // Use the helper function to get the heading in degrees
+  }
+  if (myGNSS.getHNRDyn(125) == true) // Request HNR Dyn data using a 125ms timeout
+  {
+    Serial.print(F("xAccel: "));
+    Serial.print(myGNSS.packetUBXHNRINS->data.xAccel);
+    Serial.print(F(" yAccel: "));
+    Serial.print(myGNSS.packetUBXHNRINS->data.yAccel);
+    Serial.print(F(" zAccel: "));
+    Serial.println(myGNSS.packetUBXHNRINS->data.zAccel);
+  }
+  if (myGNSS.getHNRPVT(125) == true) // Request HNR PVT data using a 125ms timeout
+  {
+    Serial.print(F("ns: "));
+    Serial.print(myGNSS.packetUBXHNRPVT->data.nano);
+    Serial.print(F(" Lat: "));
+    Serial.print(myGNSS.packetUBXHNRPVT->data.lat);
+    Serial.print(F(" Lon: "));
+    Serial.println(myGNSS.packetUBXHNRPVT->data.lon);
+  }
+
+}

keywords.txt

@@ -365,6 +365,9 @@ initPacketUBXESFRAW	KEYWORD2
 flushESFRAW	KEYWORD2
 logESFRAW	KEYWORD2
 
+getESFAutoAlignment	KEYWORD2
+setESFAutoAlignment	KEYWORD2
+
 getHNRAtt	KEYWORD2
 getHNRATT	KEYWORD2
 setAutoHNRATT	KEYWORD2

src/SparkFun_u-blox_GNSS_Arduino_Library.cpp

@@ -5074,6 +5074,72 @@ boolean SFE_UBLOX_GNSS::resetIMUalignment(uint16_t maxWait)
   return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
+//UBX-CFG-ESFALG is not documented. This was found using u-center.
+//Returns the state of the UBX-CFG-ESFALG 'Automatic IMU-mount Alignment' flag
+bool SFE_UBLOX_GNSS::getESFAutoAlignment(uint16_t maxWait)
+{
+  packetCfg.cls = UBX_CLASS_CFG;
+  packetCfg.id = UBX_CFG_ESFALG;
+  packetCfg.len = 0;
+  packetCfg.startingSpot = 0;
+
+  if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED)
+  {
+  #ifndef SFE_UBLOX_REDUCED_PROG_MEM
+    if (_printDebug == true)
+    {
+      _debugSerial->println(F("getESFAutoAlignment failed"));
+    }
+  #endif
+
+    return (false); //If command send fails then bail
+  }
+
+  return (payloadCfg[1] & 0b1); //Return Bit 0
+}
+
+//Set the state of the UBX-CFG-ESFALG 'Automatic IMU-mount Alignment' flag
+bool SFE_UBLOX_GNSS::setESFAutoAlignment(bool enable, uint16_t maxWait)
+{
+  packetCfg.cls = UBX_CLASS_CFG;
+  packetCfg.id = UBX_CFG_ESFALG;
+  packetCfg.len = 0;
+  packetCfg.startingSpot = 0;
+
+  if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED)
+  {
+    #ifndef SFE_UBLOX_REDUCED_PROG_MEM
+      if (_printDebug == true)
+      {
+        _debugSerial->println(F("getESFAutoAlignment failed"));
+      }
+    #endif
+
+    return (false); //If command send fails then bail
+  }
+
+  //payloadCfg is now filled
+
+  if (enable)
+    payloadCfg[1] |= 0b1;
+  else
+    payloadCfg[1] &= ~(0b1);
+
+  if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_SENT) // This time we are only expecting an ACK
+  {
+    #ifndef SFE_UBLOX_REDUCED_PROG_MEM
+      if (_printDebug == true)
+      {
+        _debugSerial->println(F("setESFAutoAlignment failed"));
+      }
+    #endif
+    return (false);
+  }
+
+  return (true);
+}
+
+
 //Get the time pulse parameters using UBX_CFG_TP5
 boolean SFE_UBLOX_GNSS::getTimePulseParameters(UBX_CFG_TP5_data_t *data, uint16_t maxWait)
 {

src/SparkFun_u-blox_GNSS_Arduino_Library.h

@@ -766,6 +766,10 @@ public:
 	//Reset ESF automatic IMU-mount alignment
 	boolean resetIMUalignment(uint16_t maxWait = defaultMaxWait);
 
+	//Enable/disable esfAutoAlignment
+	bool getESFAutoAlignment(uint16_t maxWait = defaultMaxWait);
+	bool setESFAutoAlignment(bool enable, uint16_t maxWait = defaultMaxWait);
+
 	//Configure Time Pulse Parameters
 	boolean getTimePulseParameters(UBX_CFG_TP5_data_t *data = NULL, uint16_t maxWait = defaultMaxWait); // Get the time pulse parameters using UBX_CFG_TP5
 	boolean setTimePulseParameters(UBX_CFG_TP5_data_t *data = NULL, uint16_t maxWait = defaultMaxWait); // Set the time pulse parameters using UBX_CFG_TP5