WIP improved compass calibration

src/motion/BMX160Sensor.cpp

@@ -33,13 +33,18 @@ 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);
+    sensor.getAllData(&magAccel, &gyroAccel, &gAccel);
 
-    if (doCalibration) {
+    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 = sensorConfig.mAccel.max.x - (sensorConfig.mAccel.max.x + sensorConfig.mAccel.min.x) / 2;
@@ -48,12 +53,12 @@ int32_t BMX160Sensor::runOnce()
     magAccel.y -= (sensorConfig.mAccel.max.y + sensorConfig.mAccel.min.y) / 2;
     magAccel.z -= (sensorConfig.mAccel.max.z + sensorConfig.mAccel.min.z) / 2;
     FusionVector ga, ma;
-    ga.axis.x = -gAccel.x; // default location for the BMX160 is on the rear of the board
+    ga.axis.x = gAccel.x * sensorConfig.orientation.x; // default location for the BMX160 is on the rear of the board
-    ga.axis.y = -gAccel.y;
+    ga.axis.y = gAccel.y * sensorConfig.orientation.y;
-    ga.axis.z = gAccel.z;
+    ga.axis.z = gAccel.z * sensorConfig.orientation.z;
-    ma.axis.x = -magAccel.x;
+    ma.axis.x = magAccel.x * sensorConfig.orientation.x;
-    ma.axis.y = -magAccel.y;
+    ma.axis.y = magAccel.y * sensorConfig.orientation.y;
-    ma.axis.z = magAccel.z * 3;
+    ma.axis.z = magAccel.z * sensorConfig.orientation.z * 3;
 
     // If we're set to one of the inverted positions
     if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
@@ -92,11 +97,11 @@ void BMX160Sensor::calibrate(uint16_t forSeconds)
 #if !defined(MESHTASTIC_EXCLUDE_SCREEN)
     LOG_INFO("BMX160 calibration started for %is", forSeconds);
 
-    doCalibration = true;
+    doMagCalibration = true;
     firstCalibrationRead = true;
     uint16_t calibrateFor = forSeconds * 1000; // calibrate for seconds provided
-    endCalibrationAt = millis() + calibrateFor;
+    endMagCalibrationAt = millis() + calibrateFor;
-    screen->setEndCalibration(endCalibrationAt);
+    screen->setEndCalibration(endMagCalibrationAt);
 #endif
 }
 

src/motion/MotionSensor.cpp

@@ -59,6 +59,33 @@ void MotionSensor::drawFrameCalibration(OLEDDisplay *display, OLEDDisplayUiState
     display->drawCircle(compassX, compassY, compassDiam / 2);
     screen->drawCompassNorth(display, compassX, compassY, screen->getHeading() * PI / 180);
 }
+
+void MotionSensor::drawFrameGyroWarning(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
+{
+    // int x_offset = display->width() / 2;
+    // int y_offset = display->height() <= 80 ? 0 : 32;
+    display->setTextAlignment(TEXT_ALIGN_LEFT);
+    display->setFont(FONT_SMALL);
+    display->drawString(x, y, "Place Screen Face Up\nPointing North\nKeep Still");
+
+    uint8_t timeRemaining = (screen->getEndCalibration() - millis()) / 1000;
+    sprintf(timeRemainingBuffer, "Starting in ( %02d )", timeRemaining);
+    display->drawString(x, y + 40, timeRemainingBuffer);
+}
+
+void MotionSensor::drawFrameGyroCalibration(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
+{
+    // int x_offset = display->width() / 2;
+    // int y_offset = display->height() <= 80 ? 0 : 32;
+    display->setTextAlignment(TEXT_ALIGN_LEFT);
+    display->setFont(FONT_MEDIUM);
+    display->drawString(x, y, "Calibrating\nGyroscope");
+
+    uint8_t timeRemaining = (screen->getEndCalibration() - millis()) / 1000;
+    sprintf(timeRemainingBuffer, "Keep Still ( %02d )", timeRemaining);
+    display->setFont(FONT_SMALL);
+    display->drawString(x, y + 40, timeRemainingBuffer);
+}
 #endif
 
 #if !MESHTASTIC_EXCLUDE_POWER_FSM
@@ -116,13 +143,126 @@ void MotionSensor::getMagCalibrationData(float x, float y, float z)
     }
 
     uint32_t now = millis();
-    if (now > endCalibrationAt) {
+    if (now > endMagCalibrationAt) {
-        doCalibration = false;
+        doMagCalibration = false;
-        endCalibrationAt = 0;
+        endMagCalibrationAt = 0;
         showingScreen = false;
         screen->endAlert();
 
+        doGyroWarning = true;
+        endGyroWarningAt = now + 10000;
+        screen->setEndCalibration(endGyroWarningAt);
+    }
+}
+
+void MotionSensor::gyroCalibrationWarning()
+{
+    if (!showingScreen) {
+        powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration
+        showingScreen = true;
+        screen->startAlert((FrameCallback)drawFrameGyroWarning);
+    }
+
+    uint32_t now = millis();
+    if (now > endGyroWarningAt) {
+        doGyroWarning = false;
+        endGyroWarningAt = 0;
+        showingScreen = false;
+        screen->endAlert();
+
+        doGyroCalibration = true;
+        endGyroCalibrationAt = now + 10000;
+        screen->setEndCalibration(endGyroCalibrationAt);
+    }
+}
+
+void MotionSensor::getGyroCalibrationData(float g_x, float g_y, float g_z, float a_x, float a_y, float a_z)
+{
+    if (!showingScreen) {
+        powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration
+        showingScreen = true;
+        screen->startAlert((FrameCallback)drawFrameGyroCalibration);
+    }
+
+    gyroCalibrationSum.x += g_x;
+    gyroCalibrationSum.y += g_y;
+    gyroCalibrationSum.z += g_z;
+
+    // increment x, y, or z based on greatest accel vector to identify down direction
+    if (abs(a_x) > abs(a_y) && abs(a_x) > abs(a_z)) {
+        if (a_x >= 0) {
+            accelCalibrationSum.x += 1;
+        } else {
+            accelCalibrationSum.x += -1;
+        }
+    } else if (abs(a_y) > abs(a_x) && abs(a_y) > abs(a_z)) {
+        if (a_y >= 0) {
+            accelCalibrationSum.y += 1;
+        } else {
+            accelCalibrationSum.y += -1;
+        }
+    } else if (abs(a_z) > abs(a_x) && abs(a_z) > abs(a_y)) {
+        if (a_z >= 0) {
+            accelCalibrationSum.z += 1;
+        } else {
+            accelCalibrationSum.z += -1;
+        }
+    }
+    calibrationCount++;
+
+    LOG_DEBUG("Accel calibration x: %i, y: %i, z: %i", accelCalibrationSum.x, accelCalibrationSum.y, accelCalibrationSum.z);
+
+    uint32_t now = millis();
+    if (now > endGyroCalibrationAt) {
+        sensorConfig.gyroAccel.x = gyroCalibrationSum.x / calibrationCount;
+        sensorConfig.gyroAccel.y = gyroCalibrationSum.y / calibrationCount;
+        sensorConfig.gyroAccel.z = gyroCalibrationSum.z / calibrationCount;
+
+        // determine orientation multipliers based on down direction
+        if (abs(accelCalibrationSum.x) > abs(accelCalibrationSum.y) && abs(accelCalibrationSum.x) > abs(accelCalibrationSum.z)) {
+            if (accelCalibrationSum.x >= 0) {
+                sensorConfig.orientation.x = 1;
+                sensorConfig.orientation.y = 1;
+                sensorConfig.orientation.z = 1;
+            } else {
+                sensorConfig.orientation.x = 1;
+                sensorConfig.orientation.y = 1;
+                sensorConfig.orientation.z = 1;
+            }
+        } else if (abs(accelCalibrationSum.y) > abs(accelCalibrationSum.x) &&
+                   abs(accelCalibrationSum.y) > abs(accelCalibrationSum.z)) {
+            if (accelCalibrationSum.y >= 0) {
+                sensorConfig.orientation.x = 1;
+                sensorConfig.orientation.y = 1;
+                sensorConfig.orientation.z = 1;
+            } else {
+                sensorConfig.orientation.x = 1;
+                sensorConfig.orientation.y = 1;
+                sensorConfig.orientation.z = 1;
+            }
+        } else if (abs(accelCalibrationSum.z) > abs(accelCalibrationSum.x) &&
+                   abs(accelCalibrationSum.z) > abs(accelCalibrationSum.y)) {
+            if (accelCalibrationSum.z >= 0) {
+                sensorConfig.orientation.x = -1;
+                sensorConfig.orientation.y = -1;
+                sensorConfig.orientation.z = -1;
+            } else {
+                sensorConfig.orientation.x = -1;
+                sensorConfig.orientation.y = -1;
+                sensorConfig.orientation.z = 1;
+            }
+        }
+
+        LOG_INFO("Gyro center x: %.4f, y: %.4f, z: %.4f", sensorConfig.gyroAccel.x, sensorConfig.gyroAccel.y,
+                 sensorConfig.gyroAccel.z);
+        LOG_INFO("Orientation vector x: %i, y: %i, z: %i", sensorConfig.orientation.x, sensorConfig.orientation.y,
+                 sensorConfig.orientation.z);
+
         saveState();
+        doGyroCalibration = false;
+        endGyroCalibrationAt = 0;
+        showingScreen = false;
+        screen->endAlert();
     }
 }
 

src/motion/MotionSensor.h

@@ -19,10 +19,15 @@
 
 #define MAX_STATE_BLOB_SIZE (256) // pad size to allow for additional saved config parameters (accel, gyro, etc)
 
+struct xyzInt {
+    int x = 0;
+    int y = 0;
+    int z = 0;
+};
 struct xyzFloat {
-    float x;
+    float x = 0.0;
-    float y;
+    float y = 0.0;
-    float z;
+    float z = 0.0;
 };
 struct minMaxXYZ {
     xyzFloat min;
@@ -30,6 +35,9 @@ struct minMaxXYZ {
 };
 struct SensorConfig {
     minMaxXYZ mAccel;
+    xyzFloat gyroAccel;
+    // xyzFloat gAccel;
+    xyzInt orientation;
 };
 
 // Base class for motion processing
@@ -68,17 +76,28 @@ class MotionSensor
 #if defined(RAK_4631) & !MESHTASTIC_EXCLUDE_SCREEN
     // draw an OLED frame (currently only used by the RAK4631 BMX160 sensor)
     static void drawFrameCalibration(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y);
+    static void drawFrameGyroWarning(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y);
+    static void drawFrameGyroCalibration(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y);
 #endif
 
     ScanI2C::FoundDevice device;
 
     SensorConfig sensorConfig;
     bool showingScreen = false;
-    bool doCalibration = false;
+    bool doMagCalibration = false;
+    bool doGyroWarning = false;
+    bool doGyroCalibration = false;
     bool firstCalibrationRead = false;
-    uint32_t endCalibrationAt = 0;
+    uint32_t endMagCalibrationAt = 0;
+    uint32_t endGyroWarningAt = 0;
+    uint32_t endGyroCalibrationAt = 0;
+    xyzFloat gyroCalibrationSum;
+    xyzInt accelCalibrationSum;
+    uint16_t calibrationCount = 0;
 
     void getMagCalibrationData(float x, float y, float z);
+    void gyroCalibrationWarning();
+    void getGyroCalibrationData(float g_x, float g_y, float g_z, float a_x, float a_y, float a_z);
 
     const char *configFileName = "/prefs/motionSensor.dat";
     uint8_t sensorState[MAX_STATE_BLOB_SIZE] = {0};