Add menu config of SensorFusion.

Author: nseidle

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("enableSensorFusion=" + (String)settings.enableSensorFusion);
       settingsFile.println("autoIMUmountAlignment=" + (String)settings.autoIMUmountAlignment);
       settingsFile.println("enableResetDisplay=" + (String)settings.enableResetDisplay);
       settingsFile.println("resetCount=" + (String)settings.resetCount);
@@ -404,6 +405,8 @@ bool parseLine(char* str) {
     settings.lastState = (SystemState)d;
   else if (strcmp(settingName, "throttleDuringSPPCongestion") == 0)
     settings.throttleDuringSPPCongestion = d;
+  else if (strcmp(settingName, "enableSensorFusion") == 0)
+    settings.enableSensorFusion = d;
   else if (strcmp(settingName, "autoIMUmountAlignment") == 0)
     settings.autoIMUmountAlignment = d;
   else if (strcmp(settingName, "enableResetDisplay") == 0)

Firmware/RTK_Surveyor/menuBase.ino

@@ -237,7 +237,14 @@ void menuSensorFusion()
     else if (i2cGNSS.packetUBXESFSTATUS->data.fusionMode == 3)
       Serial.println(F("Disabled"));
 
-    Serial.print(F("1) Toggle Automatic IMU-mount Alignment: "));
+    if (settings.enableSensorFusion == true && settings.dynamicModel != DYN_MODEL_AUTOMOTIVE)
+      Serial.println(F("Warning: Dynamic Model not set to Automotive. Sensor Fusion is best used with the Automotive Dynamic Model."));
+
+    Serial.print(F("1) Toggle Sensor Fusion: "));
+    if (settings.enableSensorFusion == true) Serial.println(F("Enabled"));
+    else Serial.println(F("Disabled"));
+
+    Serial.print(F("2) Toggle Automatic IMU-mount Alignment: "));
     if (settings.autoIMUmountAlignment == true) Serial.println(F("Enabled"));
     else Serial.println(F("Disabled"));
 
@@ -246,6 +253,11 @@ void menuSensorFusion()
     int incoming = getNumber(menuTimeout); //Timeout after x seconds
 
     if (incoming == 1)
+    {
+      settings.enableSensorFusion ^= 1;
+      setSensorFusion(settings.enableSensorFusion); //Enable/disable sensor fusion
+    }
+    else if (incoming == 2)
     {
       settings.autoIMUmountAlignment ^= 1;
     }
@@ -262,3 +274,15 @@ void menuSensorFusion()
 
   while (Serial.available()) Serial.read(); //Empty buffer of any newline chars
 }
+
+//Enable or disable sensor fusion using keys
+void setSensorFusion(bool enable)
+{
+  if (getSensorFusion() != enable)
+    i2cGNSS.setVal8(UBLOX_CFG_SFCORE_USE_SF, enable, VAL_LAYER_ALL);
+}
+
+bool getSensorFusion()
+{
+  return (i2cGNSS.getVal8(UBLOX_CFG_SFCORE_USE_SF, VAL_LAYER_RAM, 1200));
+}

Firmware/RTK_Surveyor/settings.h

@@ -332,7 +332,8 @@ 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;
+  bool enableSensorFusion = false; //If IMU is available, avoid using it unless user specifically selects automotive
+  bool autoIMUmountAlignment = true; //Allows unit to automatically establish device orientation in vehicle
   bool enableResetDisplay = false;
   uint8_t resetCount = 0;
 } settings;