Feature persist compass calibration

src/ButtonThread.cpp

@@ -190,6 +190,20 @@ int32_t ButtonThread::runOnce()
             case 4:
                 digitalWrite(PIN_EINK_EN, digitalRead(PIN_EINK_EN) == LOW);
                 break;
+#endif
+#if defined(RAK_4631)
+            // 5 clicks: start accelerometer/magenetometer calibration for 30 seconds
+            case 5:
+                if (accelerometerThread) {
+                    accelerometerThread->calibrate(30);
+                }
+                break;
+            // 6 clicks: start accelerometer/magenetometer calibration for 60 seconds
+            case 6:
+                if (accelerometerThread) {
+                    accelerometerThread->calibrate(60);
+                }
+                break;
 #endif
             // No valid multipress action
             default:

src/graphics/Screen.h

@@ -278,6 +278,10 @@ class Screen : public concurrency::OSThread
     bool hasHeading() { return hasCompass; }
 
     long getHeading() { return compassHeading; }
+
+    void setEndCalibration(uint32_t _endCalibrationAt) { endCalibrationAt = _endCalibrationAt; }
+    uint32_t getEndCalibration() { return endCalibrationAt; }
+
     // functions for display brightness
     void increaseBrightness();
     void decreaseBrightness();
@@ -673,6 +677,8 @@ class Screen : public concurrency::OSThread
 
     bool hasCompass = false;
     float compassHeading;
+    uint32_t endCalibrationAt;
+
     /// Holds state for debug information
     DebugInfo debugInfo;
 

src/motion/AccelerometerThread.h

@@ -48,6 +48,13 @@ class AccelerometerThread : public concurrency::OSThread
         setIntervalFromNow(0);
     };
 
+    void calibrate(uint16_t forSeconds)
+    {
+        if (sensor) {
+            sensor->calibrate(forSeconds);
+        }
+    }
+
   protected:
     int32_t runOnce() override
     {

src/motion/BMX160Sensor.cpp

@@ -16,7 +16,23 @@ bool BMX160Sensor::init()
     if (sensor.begin()) {
         // set output data rate
         sensor.ODR_Config(BMX160_ACCEL_ODR_100HZ, BMX160_GYRO_ODR_100HZ);
-        LOG_DEBUG("BMX160 init ok");
+        sensor.setGyroRange(eGyroRange_500DPS);
+        sensor.setAccelRange(eAccelRange_2G);
+
+        // default location for the BMX160 is on the rear of the board with Z negative
+        sensorConfig.orientation.x = -1;
+        sensorConfig.orientation.y = -1;
+        sensorConfig.orientation.z = 1;
+
+        loadState();
+
+        LOG_INFO("BMX160 MAG calibration center_x: %.4f, center_Y: %.4f, center_Z: %.4f", sensorConfig.mAccel.x,
+                 sensorConfig.mAccel.y, sensorConfig.mAccel.z);
+        LOG_INFO("BMX160 GYRO calibration center_x: %.4f, center_Y: %.4f, center_Z: %.4f", sensorConfig.gyroAccel.x,
+                 sensorConfig.gyroAccel.y, sensorConfig.gyroAccel.z);
+        LOG_INFO("BMX160 ORIENT calibration: x=%i, y=%i, z=%i", sensorConfig.orientation.x, sensorConfig.orientation.y,
+                 sensorConfig.orientation.z);
+
         return true;
     }
     LOG_DEBUG("BMX160 init failed");
@@ -25,54 +41,42 @@ bool BMX160Sensor::init()
 
 int32_t BMX160Sensor::runOnce()
 {
+#if !defined(MESHTASTIC_EXCLUDE_SCREEN)
     sBmx160SensorData_t magAccel;
+    sBmx160SensorData_t gyroAccel;
     sBmx160SensorData_t gAccel;
 
     /* Get a new sensor event */
-    sensor.getAllData(&magAccel, NULL, &gAccel);
-
-#if !defined(MESHTASTIC_EXCLUDE_SCREEN)
-    // experimental calibrate routine. Limited to between 10 and 30 seconds after boot
-    if (millis() > 12 * 1000 && millis() < 30 * 1000) {
-        if (!showingScreen) {
-            showingScreen = true;
-            screen->startAlert((FrameCallback)drawFrameCalibration);
-        }
-        if (magAccel.x > highestX)
-            highestX = magAccel.x;
-        if (magAccel.x < lowestX)
-            lowestX = magAccel.x;
-        if (magAccel.y > highestY)
-            highestY = magAccel.y;
-        if (magAccel.y < lowestY)
-            lowestY = magAccel.y;
-        if (magAccel.z > highestZ)
-            highestZ = magAccel.z;
-        if (magAccel.z < lowestZ)
-            lowestZ = magAccel.z;
-    } else if (showingScreen && millis() >= 30 * 1000) {
-        showingScreen = false;
-        screen->endAlert();
+    sensor.getAllData(&magAccel, &gyroAccel, &gAccel);
+
+    if (doMagCalibration) {
+        getMagCalibrationData(magAccel.x, magAccel.y, magAccel.z);
+    } else if (doGyroWarning) {
+        gyroCalibrationWarning();
+    } else if (doGyroCalibration) {
+        getGyroCalibrationData(gyroAccel.x, gyroAccel.y, gyroAccel.z, gAccel.x, gAccel.y, gAccel.z);
     }
 
-    int highestRealX = highestX - (highestX + lowestX) / 2;
+    // int highestRealX = sensorConfig.mAccel.max.x - (sensorConfig.mAccel.max.x + sensorConfig.mAccel.min.x) / 2;
 
-    magAccel.x -= (highestX + lowestX) / 2;
-    magAccel.y -= (highestY + lowestY) / 2;
-    magAccel.z -= (highestZ + lowestZ) / 2;
-    FusionVector ga, ma;
-    ga.axis.x = -gAccel.x; // default location for the BMX160 is on the rear of the board
-    ga.axis.y = -gAccel.y;
-    ga.axis.z = gAccel.z;
-    ma.axis.x = -magAccel.x;
-    ma.axis.y = -magAccel.y;
-    ma.axis.z = magAccel.z * 3;
+    magAccel.x -= sensorConfig.mAccel.x;
+    magAccel.y -= sensorConfig.mAccel.y;
+    magAccel.z -= sensorConfig.mAccel.z;
 
+    FusionVector ga, ma;
+    ga.axis.x = gAccel.x * sensorConfig.orientation.x;
+    ga.axis.y = gAccel.y * sensorConfig.orientation.y;
+    ga.axis.z = gAccel.z * sensorConfig.orientation.z;
+    ma.axis.x = magAccel.x * sensorConfig.orientation.x;
+    ma.axis.y = magAccel.y * sensorConfig.orientation.y;
+    ma.axis.z = magAccel.z * sensorConfig.orientation.z * 3;
+
+    // Use calibration orientation instead of swap based on CompassOrientation definition
     // If we're set to one of the inverted positions
-    if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
-        ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ);
-        ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
-    }
+    // if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
+    //     ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ);
+    //     ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
+    // }
 
     float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma);
 
@@ -93,12 +97,31 @@ int32_t BMX160Sensor::runOnce()
         heading += 270;
         break;
     }
+
+    // LOG_DEBUG("MAG Sensor Data: X=%.4f, Y=%.4f, Z=%.4f", magAccel.x, magAccel.y, magAccel.z);
+    // LOG_DEBUG("ACCEL Sensor Data: X=%.4f, Y=%.4f, Z=%.4f", gAccel.x, gAccel.y, gAccel.z);
+    // LOG_DEBUG("HEADING Sensor Data: %.1f deg", heading);
+    // LOG_DEBUG("Gyro Sensor Data: X=%.4f, Y=%.4f, Z=%.4f", gyroAccel.x, gyroAccel.y, gyroAccel.z);
+
     screen->setHeading(heading);
 #endif
 
     return MOTION_SENSOR_CHECK_INTERVAL_MS;
 }
 
+void BMX160Sensor::calibrate(uint16_t forSeconds)
+{
+#if !defined(MESHTASTIC_EXCLUDE_SCREEN)
+    LOG_INFO("BMX160 calibration started for %is", forSeconds);
+
+    doMagCalibration = true;
+    firstCalibrationRead = true;
+    uint16_t calibrateFor = forSeconds * 1000; // calibrate for seconds provided
+    endMagCalibrationAt = millis() + calibrateFor;
+    screen->setEndCalibration(endMagCalibrationAt);
+#endif
+}
+
 #endif
 
 #endif

src/motion/BMX160Sensor.h

@@ -16,13 +16,12 @@ class BMX160Sensor : public MotionSensor
 {
   private:
     RAK_BMX160 sensor;
-    bool showingScreen = false;
-    float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0;
 
   public:
     explicit BMX160Sensor(ScanI2C::FoundDevice foundDevice);
     virtual bool init() override;
     virtual int32_t runOnce() override;
+    virtual void calibrate(uint16_t forSeconds) override;
 };
 
 #else