From 3d92c379f643b68cf44aaa25364e83c3144a6a3b Mon Sep 17 00:00:00 2001 From: Dan Welch Date: Thu, 26 Dec 2024 10:34:29 -0700 Subject: [PATCH] Compass calibration improvements - fix orientation vectors based on acceleration from calibration - re-zero calibration data after running calibration to start fresh on next calibration --- src/motion/BMX160Sensor.cpp | 41 ++++++++++++------ src/motion/MotionSensor.cpp | 83 ++++++++++++++++++++++++------------- src/motion/MotionSensor.h | 3 +- 3 files changed, 85 insertions(+), 42 deletions(-) diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp index 0d8a8ab33..d88ccefc2 100755 --- a/src/motion/BMX160Sensor.cpp +++ b/src/motion/BMX160Sensor.cpp @@ -16,12 +16,22 @@ bool BMX160Sensor::init() if (sensor.begin()) { // set output data rate sensor.ODR_Config(BMX160_ACCEL_ODR_100HZ, BMX160_GYRO_ODR_100HZ); + 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 load calibration min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", - sensorConfig.mAccel.min.x, sensorConfig.mAccel.max.x, sensorConfig.mAccel.min.y, sensorConfig.mAccel.max.y, - sensorConfig.mAccel.min.z, sensorConfig.mAccel.max.z); + 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; } @@ -47,24 +57,26 @@ int32_t BMX160Sensor::runOnce() 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; + // int highestRealX = sensorConfig.mAccel.max.x - (sensorConfig.mAccel.max.x + sensorConfig.mAccel.min.x) / 2; + + magAccel.x -= sensorConfig.mAccel.x; + magAccel.y -= sensorConfig.mAccel.y; + magAccel.z -= sensorConfig.mAccel.z; - magAccel.x -= (sensorConfig.mAccel.max.x + sensorConfig.mAccel.min.x) / 2; - 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 * sensorConfig.orientation.x; // default location for the BMX160 is on the rear of the board + 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); @@ -86,6 +98,11 @@ int32_t BMX160Sensor::runOnce() 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 diff --git a/src/motion/MotionSensor.cpp b/src/motion/MotionSensor.cpp index 1e270cdc6..b04753747 100755 --- a/src/motion/MotionSensor.cpp +++ b/src/motion/MotionSensor.cpp @@ -66,7 +66,7 @@ void MotionSensor::drawFrameGyroWarning(OLEDDisplay *display, OLEDDisplayUiState // 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"); + display->drawString(x, y, "Place Screen Face Up\n& Keep Still"); uint8_t timeRemaining = (screen->getEndCalibration() - millis()) / 1000; sprintf(timeRemainingBuffer, "Starting in ( %02d )", timeRemaining); @@ -120,32 +120,42 @@ void MotionSensor::getMagCalibrationData(float x, float y, float z) } if (firstCalibrationRead) { - sensorConfig.mAccel.min.x = x; - sensorConfig.mAccel.max.x = x; - sensorConfig.mAccel.min.y = y; - sensorConfig.mAccel.max.y = y; - sensorConfig.mAccel.min.z = z; - sensorConfig.mAccel.max.z = z; + magCalibrationMinMax.min.x = x; + magCalibrationMinMax.max.x = x; + magCalibrationMinMax.min.y = y; + magCalibrationMinMax.max.y = y; + magCalibrationMinMax.min.z = z; + magCalibrationMinMax.max.z = z; firstCalibrationRead = false; } else { - if (x > sensorConfig.mAccel.max.x) - sensorConfig.mAccel.max.x = x; - if (x < sensorConfig.mAccel.min.x) - sensorConfig.mAccel.min.x = x; - if (y > sensorConfig.mAccel.max.y) - sensorConfig.mAccel.max.y = y; - if (y < sensorConfig.mAccel.min.y) - sensorConfig.mAccel.min.y = y; - if (z > sensorConfig.mAccel.max.z) - sensorConfig.mAccel.max.z = z; - if (z < sensorConfig.mAccel.min.z) - sensorConfig.mAccel.min.z = z; + if (x > magCalibrationMinMax.max.x) + magCalibrationMinMax.max.x = x; + if (x < magCalibrationMinMax.min.x) + magCalibrationMinMax.min.x = x; + if (y > magCalibrationMinMax.max.y) + magCalibrationMinMax.max.y = y; + if (y < magCalibrationMinMax.min.y) + magCalibrationMinMax.min.y = y; + if (z > magCalibrationMinMax.max.z) + magCalibrationMinMax.max.z = z; + if (z < magCalibrationMinMax.min.z) + magCalibrationMinMax.min.z = z; } uint32_t now = millis(); if (now > endMagCalibrationAt) { + sensorConfig.mAccel.x = (magCalibrationMinMax.max.x + magCalibrationMinMax.min.x) / 2; + sensorConfig.mAccel.y = (magCalibrationMinMax.max.y + magCalibrationMinMax.min.y) / 2; + sensorConfig.mAccel.z = (magCalibrationMinMax.max.z + magCalibrationMinMax.min.z) / 2; + doMagCalibration = false; endMagCalibrationAt = 0; + magCalibrationMinMax.min.x = 0; + magCalibrationMinMax.max.x = 0; + magCalibrationMinMax.min.y = 0; + magCalibrationMinMax.max.y = 0; + magCalibrationMinMax.min.z = 0; + magCalibrationMinMax.max.z = 0; showingScreen = false; screen->endAlert(); @@ -221,32 +231,38 @@ void MotionSensor::getGyroCalibrationData(float g_x, float g_y, float g_z, float // determine orientation multipliers based on down direction if (abs(accelCalibrationSum.x) > abs(accelCalibrationSum.y) && abs(accelCalibrationSum.x) > abs(accelCalibrationSum.z)) { if (accelCalibrationSum.x >= 0) { + // X axis oriented with down positive sensorConfig.orientation.x = 1; sensorConfig.orientation.y = 1; sensorConfig.orientation.z = 1; } else { + // X axis oriented with down negative sensorConfig.orientation.x = 1; - sensorConfig.orientation.y = 1; - sensorConfig.orientation.z = 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) { + // Y axis oriented with down positive sensorConfig.orientation.x = 1; sensorConfig.orientation.y = 1; sensorConfig.orientation.z = 1; } else { - sensorConfig.orientation.x = 1; + // Y axis oriented with down negative + sensorConfig.orientation.x = -1; sensorConfig.orientation.y = 1; - sensorConfig.orientation.z = 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; + // Z axis oriented with down positive + sensorConfig.orientation.x = 1; + sensorConfig.orientation.y = 1; + sensorConfig.orientation.z = 1; } else { + // Z axis oriented with down negative sensorConfig.orientation.x = -1; sensorConfig.orientation.y = -1; sensorConfig.orientation.z = 1; @@ -261,6 +277,12 @@ void MotionSensor::getGyroCalibrationData(float g_x, float g_y, float g_z, float saveState(); doGyroCalibration = false; endGyroCalibrationAt = 0; + accelCalibrationSum.x = 0; + accelCalibrationSum.y = 0; + accelCalibrationSum.z = 0; + gyroCalibrationSum.x = 0; + gyroCalibrationSum.y = 0; + gyroCalibrationSum.z = 0; showingScreen = false; screen->endAlert(); } @@ -290,9 +312,12 @@ void MotionSensor::saveState() #ifdef FSCom memcpy(&sensorState, &sensorConfig, sizeof(SensorConfig)); - LOG_INFO("Motion Sensor save calibration min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", - sensorConfig.mAccel.min.x, sensorConfig.mAccel.max.x, sensorConfig.mAccel.min.y, sensorConfig.mAccel.max.y, - sensorConfig.mAccel.min.z, sensorConfig.mAccel.max.z); + LOG_INFO("Save MAG calibration center_x: %.4f, center_Y: %.4f, center_Z: %.4f", sensorConfig.mAccel.x, sensorConfig.mAccel.y, + sensorConfig.mAccel.z); + LOG_INFO("Save GYRO calibration center_x: %.4f, center_Y: %.4f, center_Z: %.4f", sensorConfig.gyroAccel.x, + sensorConfig.gyroAccel.y, sensorConfig.gyroAccel.z); + LOG_INFO("Save ORIENT calibration: x=%i, y=%i, z=%i", sensorConfig.orientation.x, sensorConfig.orientation.y, + sensorConfig.orientation.z); if (FSCom.exists(configFileName) && !FSCom.remove(configFileName)) { LOG_WARN("Can't remove old Motion Sensor config state file"); diff --git a/src/motion/MotionSensor.h b/src/motion/MotionSensor.h index 43a7d519e..5b142b21a 100755 --- a/src/motion/MotionSensor.h +++ b/src/motion/MotionSensor.h @@ -34,7 +34,7 @@ struct minMaxXYZ { xyzFloat max; }; struct SensorConfig { - minMaxXYZ mAccel; + xyzFloat mAccel; xyzFloat gyroAccel; // xyzFloat gAccel; xyzInt orientation; @@ -93,6 +93,7 @@ class MotionSensor uint32_t endGyroCalibrationAt = 0; xyzFloat gyroCalibrationSum; xyzInt accelCalibrationSum; + minMaxXYZ magCalibrationMinMax; uint16_t calibrationCount = 0; void getMagCalibrationData(float x, float y, float z);