Add text menus for Sensor Fusion config. Add icon for Fusion state.

Author: nseidle

Firmware/RTK_Surveyor/Display.ino

@@ -321,7 +321,43 @@ void paintBaseState()
         systemState == STATE_ROVER_RTK_FLOAT ||
         systemState == STATE_ROVER_RTK_FIX)
     {
-      oled.drawIcon(27, 3, Rover_Width, Rover_Height, Rover, sizeof(Rover), true);
+      //Normal rover for ZED-F9P, fusion rover for ZED-F9R
+      if (zedModuleType == PLATFORM_F9P)
+      {
+        oled.drawIcon(27, 3, Rover_Width, Rover_Height, Rover, sizeof(Rover), true);
+      }
+      else if (zedModuleType == PLATFORM_F9R)
+      {
+        //Blink fusion rover until we have calibration
+        i2cGNSS.getEsfInfo(); // Poll new ESF STATUS data
+        if (i2cGNSS.packetUBXESFSTATUS->data.fusionMode == 0) //Initializing
+        {
+          //Blink Fusion Rover icon until sensor calibration is complete
+          if (millis() - lastBaseIconUpdate > 500)
+          {
+            lastBaseIconUpdate = millis();
+            if (baseIconDisplayed == false)
+            {
+              baseIconDisplayed = true;
+
+              //Draw the icon
+              oled.drawIcon(27, 2, Rover_Fusion_Width, Rover_Fusion_Height, Rover_Fusion, sizeof(Rover_Fusion), true);
+            }
+            else
+              baseIconDisplayed = false;
+          }
+        }
+        else if (i2cGNSS.packetUBXESFSTATUS->data.fusionMode == 1) //Calibrated
+        {
+          //Solid fusion rover
+          oled.drawIcon(27, 2, Rover_Fusion_Width, Rover_Fusion_Height, Rover_Fusion, sizeof(Rover_Fusion), true);
+        }
+        else if (i2cGNSS.packetUBXESFSTATUS->data.fusionMode == 2 || i2cGNSS.packetUBXESFSTATUS->data.fusionMode == 3) //Suspended or disabled
+        {
+          //Empty rover
+          oled.drawIcon(27, 2, Rover_Fusion_Empty_Width, Rover_Fusion_Empty_Height, Rover_Fusion_Empty, sizeof(Rover_Fusion_Empty), true);
+        }
+      }
     }
     else if (systemState == STATE_BASE_TEMP_SETTLE ||
              systemState == STATE_BASE_TEMP_SURVEY_STARTED //Turn on base icon solid (blink crosshair in paintHorzAcc)
@@ -370,7 +406,7 @@ void paintSIV()
   if (online.display == true)
   {
     //Blink satellite dish icon if we don't have a fix
-    if (i2cGNSS.getFixType() == 3 || i2cGNSS.getFixType() == 5) //3D or Time
+    if (i2cGNSS.getFixType() == 3 || i2cGNSS.getFixType() == 4 || i2cGNSS.getFixType() == 5) //3D, 3D+DR, or Time
     {
       //Fix, turn on icon
       oled.drawIcon(2, 35, Antenna_Width, Antenna_Height, Antenna, sizeof(Antenna), true);

Firmware/RTK_Surveyor/NVM.ino

@@ -139,6 +139,7 @@ void recordSystemSettingsToFile()
       settingsFile.println("dynamicModel=" + (String)settings.dynamicModel);
       settingsFile.println("lastState=" + (String)settings.lastState);
       settingsFile.println("throttleDuringSPPCongestion=" + (String)settings.throttleDuringSPPCongestion);
+      settingsFile.println("autoIMUmountAlignment=" + (String)settings.autoIMUmountAlignment);
 
       //Record constellation settings
       for (int x = 0 ; x < MAX_CONSTELLATIONS ; x++)
@@ -402,6 +403,8 @@ bool parseLine(char* str) {
     settings.lastState = (SystemState)d;
   else if (strcmp(settingName, "throttleDuringSPPCongestion") == 0)
     settings.throttleDuringSPPCongestion = d;
+  else if (strcmp(settingName, "autoIMUmountAlignment") == 0)
+    settings.autoIMUmountAlignment = d;
 
   //Check for bulk settings (constellations and message rates)
   //Must be last on else list

Firmware/RTK_Surveyor/Rover.ino

@@ -42,6 +42,10 @@ bool configureUbloxModuleRover()
     Serial.println(F("setNMEASettings failed"));
 
   response = true; //Reset
+  if (zedModuleType == PLATFORM_F9R)
+  {
+    i2cGNSS.setESFAutoAlignment(settings.autoIMUmountAlignment); //Configure UBX-CFG-ESFALG Automatic IMU-mount Alignment
+  }
 
   //The last thing we do is set output rate.
   response = true; //Reset

Firmware/RTK_Surveyor/icons.h

@@ -48,6 +48,20 @@ uint8_t Rover [] = {
 int Rover_Height = 8;
 int Rover_Width = 15;
 
+uint8_t Rover_Fusion [] = {
+0x3E, 0xC1, 0x21, 0x21, 0xC1, 0x7D, 0x55, 0x55, 0x45, 0x41, 0xC2, 0x24, 0x24, 0xC4, 0x3C, 0x00,
+0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x00, 0x00, 
+};
+int Rover_Fusion_Height = 9;
+int Rover_Fusion_Width = 15;
+
+uint8_t Rover_Fusion_Empty [] = {
+0x3E, 0xC1, 0x21, 0x21, 0xC1, 0x41, 0x41, 0x41, 0x41, 0x41, 0xC2, 0x24, 0x24, 0xC4, 0x3C, 0x00,
+0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x00, 0x00, 
+};
+int Rover_Fusion_Empty_Height = 9;
+int Rover_Fusion_Empty_Width = 15;
+
 uint8_t BaseTemporary [] = {
 0x00, 0xFF, 0x99, 0x99, 0xE7, 0xCE, 0x32, 0x32, 0xE7, 0xE7, 0x99, 0x32, 0xFE, 0x00, 0x00, 0x1F,
 0x00, 0x00, 0x00, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x01, 0x01, 0x00, 

Firmware/RTK_Surveyor/menuBase.ino

@@ -3,7 +3,7 @@
 void menuBase()
 {
   int menuTimeoutExtended = 30; //Increase time needed for complex data entry (mount point ID, ECEF coords, etc).
-  
+
   while (1)
   {
     Serial.println();
@@ -216,3 +216,51 @@ void menuBase()
 
   while (Serial.available()) Serial.read(); //Empty buffer of any newline chars
 }
+
+//Configure ESF settings
+void menuSensorFusion()
+{
+  while (1)
+  {
+    Serial.println();
+    Serial.println(F("Menu: Sensor Fusion Menu"));
+
+    if (i2cGNSS.getEsfInfo()) // Poll new ESF STATUS data
+    {
+      Serial.print(F("Fusion Mode: "));
+      Serial.print(i2cGNSS.packetUBXESFSTATUS->data.fusionMode);
+      if (i2cGNSS.packetUBXESFSTATUS->data.fusionMode == 0)
+        Serial.println(F("Initializing"));
+      else if (i2cGNSS.packetUBXESFSTATUS->data.fusionMode == 1)
+        Serial.println(F("Calibrated"));
+      else if (i2cGNSS.packetUBXESFSTATUS->data.fusionMode == 2)
+        Serial.println(F("Suspended"));
+      else if (i2cGNSS.packetUBXESFSTATUS->data.fusionMode == 3)
+        Serial.println(F("Disabled"));
+    }
+
+    Serial.print(F("1) Toggle Automatic IMU-mount Alignment: "));
+    if (settings.autoIMUmountAlignment == true) Serial.println(F("Enabled"));
+    else Serial.println(F("Disabled"));
+
+    Serial.println(F("x) Exit"));
+
+    int incoming = getNumber(menuTimeout); //Timeout after x seconds
+
+    if (incoming == 1)
+    {
+      settings.autoIMUmountAlignment ^= 1;
+    }
+
+    else if (incoming == STATUS_PRESSED_X)
+      break;
+    else if (incoming == STATUS_GETNUMBER_TIMEOUT)
+      break;
+    else
+      printUnknown(incoming);
+  }
+
+  i2cGNSS.setESFAutoAlignment(settings.autoIMUmountAlignment); //Configure UBX-CFG-ESFALG Automatic IMU-mount Alignment
+
+  while (Serial.available()) Serial.read(); //Empty buffer of any newline chars
+}

Firmware/RTK_Surveyor/menuMain.ino

@@ -21,6 +21,8 @@ void menuMain()
 
     if (zedModuleType == PLATFORM_F9P)
       Serial.println(F("3) Configure Base"));
+    else if (zedModuleType == PLATFORM_F9R)
+      Serial.println(F("3) Configure Sensor Fusion"));
 
     Serial.println(F("4) Configure Ports"));
 
@@ -50,6 +52,8 @@ void menuMain()
       menuMessages();
     else if (incoming == '3' && zedModuleType == PLATFORM_F9P)
       menuBase();
+    else if (incoming == '3' && zedModuleType == PLATFORM_F9R)
+      menuSensorFusion();
     else if (incoming == '4')
       menuPorts();
     else if (incoming == '5')

Firmware/RTK_Surveyor/menuMessages.ino

@@ -77,6 +77,8 @@ void menuMessages()
     Serial.println(F("1) Set NMEA Messages"));
     if (zedModuleType == PLATFORM_F9P)
       Serial.println(F("2) Set RTCM Messages"));
+    else if (zedModuleType == PLATFORM_F9R)
+      Serial.println(F("2) Set ESF Messages"));
     Serial.println(F("3) Set RXM Messages"));
     Serial.println(F("4) Set NAV Messages"));
     Serial.println(F("5) Set MON Messages"));
@@ -94,6 +96,8 @@ void menuMessages()
       menuMessagesSubtype("NMEA");
     else if (incoming == 2 && zedModuleType == PLATFORM_F9P)
       menuMessagesSubtype("RTCM");
+    else if (incoming == 2 && zedModuleType == PLATFORM_F9R)
+      menuMessagesSubtype("ESF");
     else if (incoming == 3)
       menuMessagesSubtype("RXM");
     else if (incoming == 4)

Firmware/RTK_Surveyor/menuPorts.ino

@@ -1,8 +1,8 @@
 void menuPorts()
 {
-  if(productVariant == RTK_SURVEYOR)
+  if (productVariant == RTK_SURVEYOR)
     menuPortsSurveyor();
-  else if(productVariant == RTK_EXPRESS || productVariant == RTK_EXPRESS_PLUS)
+  else if (productVariant == RTK_EXPRESS || productVariant == RTK_EXPRESS_PLUS)
     menuPortsExpress();
 }
 
@@ -83,8 +83,13 @@ void menuPortsExpress()
       Serial.println(F("NMEA TX Out/RX In"));
     else if (settings.dataPortChannel == MUX_PPS_EVENTTRIGGER)
       Serial.println(F("PPS OUT/Event Trigger In"));
-    else if (settings.dataPortChannel == MUX_I2C)
-      Serial.println(F("I2C SDA/SCL"));
+    else if (settings.dataPortChannel == MUX_I2C_WT)
+    {
+      if (zedModuleType == PLATFORM_F9P)
+        Serial.println(F("I2C SDA/SCL"));
+      else if (zedModuleType == PLATFORM_F9R)
+        Serial.println(F("Wheel Tick/Direction"));
+    }
     else if (settings.dataPortChannel == MUX_ADC_DAC)
       Serial.println(F("ESP32 DAC Out/ADC In"));
 
@@ -119,9 +124,12 @@ void menuPortsExpress()
       Serial.println(F("\n\rEnter the pin connection to use (1 to 4) for Data Port: "));
       Serial.println(F("1) NMEA TX Out/RX In"));
       Serial.println(F("2) PPS OUT/Event Trigger In"));
-      Serial.println(F("3) I2C SDA/SCL"));
+      if (zedModuleType == PLATFORM_F9P)
+        Serial.println(F("3) I2C SDA/SCL"));
+      else if (zedModuleType == PLATFORM_F9R)
+        Serial.println(F("3) Wheel Tick/Direction"));
       Serial.println(F("4) ESP32 DAC Out/ADC In"));
-      
+
       int muxPort = getNumber(menuTimeout); //Timeout after x seconds
       if (muxPort < 1 || muxPort > 4)
       {

Firmware/RTK_Surveyor/settings.h

@@ -59,7 +59,7 @@ typedef enum muxConnectionType_e
 {
   MUX_UBLOX_NMEA = 0,
   MUX_PPS_EVENTTRIGGER,
-  MUX_I2C,
+  MUX_I2C_WT,
   MUX_ADC_DAC,
 } muxConnectionType_e;
 
@@ -332,6 +332,7 @@ struct struct_settings {
   SystemState lastState = STATE_ROVER_NOT_STARTED; //For Express, start unit in last known state
   bool throttleDuringSPPCongestion = true;
   ubxConstellation ubxConstellations; //Constellations monitored/used for fix
+  bool autoIMUmountAlignment = true;
 } settings;
 
 //These are the devices on board RTK Surveyor that may be on or offline.