From 4b1db13f258910ae265b9ef107511be92d00f665 Mon Sep 17 00:00:00 2001 From: Dan Welch Date: Thu, 12 Dec 2024 10:46:33 -0700 Subject: [PATCH 01/11] Initiate magenetometer based compass calibration from button presses - only active for BMX160 accelerometers on RAK_4631 - replace automatic calibration on power on with button triggered calibration - set 5 presses to trigger 30s calibration - set 6 presses to trigger 60s calibration (useful if unit is not handheld, ie vehicle mounted) - show calibration time remaining on calibration alert screen --- src/ButtonThread.cpp | 12 ++++++++++++ src/graphics/Screen.h | 6 ++++++ src/motion/AccelerometerThread.h | 7 +++++++ src/motion/BMX160Sensor.cpp | 33 ++++++++++++++++++++++++++------ src/motion/BMX160Sensor.h | 1 + src/motion/MotionSensor.cpp | 8 ++++++++ src/motion/MotionSensor.h | 6 ++++++ 7 files changed, 67 insertions(+), 6 deletions(-) diff --git a/src/ButtonThread.cpp b/src/ButtonThread.cpp index 3f64b3b3e2..664c3560f8 100644 --- a/src/ButtonThread.cpp +++ b/src/ButtonThread.cpp @@ -191,6 +191,18 @@ int32_t ButtonThread::runOnce() digitalWrite(PIN_EINK_EN, digitalRead(PIN_EINK_EN) == LOW); break; #endif + // 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; // No valid multipress action default: break; diff --git a/src/graphics/Screen.h b/src/graphics/Screen.h index 00884c5af9..95516bead2 100644 --- a/src/graphics/Screen.h +++ b/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(); @@ -593,6 +597,8 @@ class Screen : public concurrency::OSThread bool hasCompass = false; float compassHeading; + uint32_t endCalibrationAt; + /// Holds state for debug information DebugInfo debugInfo; diff --git a/src/motion/AccelerometerThread.h b/src/motion/AccelerometerThread.h index 95f09910fd..6e517d6b0f 100755 --- a/src/motion/AccelerometerThread.h +++ b/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 { diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp index 6562a651c6..8e483fc532 100755 --- a/src/motion/BMX160Sensor.cpp +++ b/src/motion/BMX160Sensor.cpp @@ -31,13 +31,14 @@ int32_t BMX160Sensor::runOnce() /* 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 (doCalibration) { + if (!showingScreen) { + powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration showingScreen = true; screen->startAlert((FrameCallback)drawFrameCalibration); } + if (magAccel.x > highestX) highestX = magAccel.x; if (magAccel.x < lowestX) @@ -50,11 +51,20 @@ int32_t BMX160Sensor::runOnce() highestZ = magAccel.z; if (magAccel.z < lowestZ) lowestZ = magAccel.z; - } else if (showingScreen && millis() >= 30 * 1000) { - showingScreen = false; - screen->endAlert(); + + uint32_t now = millis(); + if (now > endCalibrationAt) { + doCalibration = false; + endCalibrationAt = 0; + showingScreen = false; + screen->endAlert(); + } + + // LOG_DEBUG("BMX160 min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", lowestX, highestX, + // lowestY, highestY, lowestZ, highestZ); } +#if !defined(MESHTASTIC_EXCLUDE_SCREEN) int highestRealX = highestX - (highestX + lowestX) / 2; magAccel.x -= (highestX + lowestX) / 2; @@ -99,6 +109,17 @@ int32_t BMX160Sensor::runOnce() return MOTION_SENSOR_CHECK_INTERVAL_MS; } +void BMX160Sensor::calibrate(uint16_t forSeconds) +{ + + LOG_DEBUG("BMX160 calibration started for %is", forSeconds); + + doCalibration = true; + uint16_t calibrateFor = forSeconds * 1000; // calibrate for seconds provided + endCalibrationAt = millis() + calibrateFor; + screen->setEndCalibration(endCalibrationAt); +} + #endif #endif \ No newline at end of file diff --git a/src/motion/BMX160Sensor.h b/src/motion/BMX160Sensor.h index 26f4772719..9031b45042 100755 --- a/src/motion/BMX160Sensor.h +++ b/src/motion/BMX160Sensor.h @@ -23,6 +23,7 @@ class BMX160Sensor : public MotionSensor explicit BMX160Sensor(ScanI2C::FoundDevice foundDevice); virtual bool init() override; virtual int32_t runOnce() override; + virtual void calibrate(uint16_t forSeconds) override; }; #else diff --git a/src/motion/MotionSensor.cpp b/src/motion/MotionSensor.cpp index 242e3709f8..d87380085b 100755 --- a/src/motion/MotionSensor.cpp +++ b/src/motion/MotionSensor.cpp @@ -2,6 +2,8 @@ #if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C +char timeRemainingBuffer[12]; + // screen is defined in main.cpp extern graphics::Screen *screen; @@ -37,6 +39,12 @@ void MotionSensor::drawFrameCalibration(OLEDDisplay *display, OLEDDisplayUiState display->setTextAlignment(TEXT_ALIGN_LEFT); display->setFont(FONT_MEDIUM); display->drawString(x, y, "Calibrating\nCompass"); + + uint8_t timeRemaining = (screen->getEndCalibration() - millis()) / 1000; + sprintf(timeRemainingBuffer, "( %02d )", timeRemaining); + display->setFont(FONT_SMALL); + display->drawString(x, y + 40, timeRemainingBuffer); + int16_t compassX = 0, compassY = 0; uint16_t compassDiam = graphics::Screen::getCompassDiam(display->getWidth(), display->getHeight()); diff --git a/src/motion/MotionSensor.h b/src/motion/MotionSensor.h index 78eec54cec..1848c472a1 100755 --- a/src/motion/MotionSensor.h +++ b/src/motion/MotionSensor.h @@ -40,6 +40,8 @@ class MotionSensor // Refer to /src/concurrency/OSThread.h for more information inline virtual int32_t runOnce() { return MOTION_SENSOR_CHECK_INTERVAL_MS; }; + virtual void calibrate(uint16_t forSeconds){}; + protected: // Turn on the screen when a tap or motion is detected virtual void wakeScreen(); @@ -53,6 +55,10 @@ class MotionSensor #endif ScanI2C::FoundDevice device; + + // Do calibration if true + bool doCalibration = false; + uint32_t endCalibrationAt; }; namespace MotionSensorI2C From 8db96c17ea83adc7bc4ad2e80f5ab1cc9c0c09dd Mon Sep 17 00:00:00 2001 From: Dan Welch Date: Thu, 12 Dec 2024 14:17:54 -0700 Subject: [PATCH 02/11] Fix non RAK 4631 builds - exclude changes from non RAK 4631 builds - remove calls to screen when not present --- src/ButtonThread.cpp | 2 ++ src/motion/BMX160Sensor.cpp | 5 +++++ src/motion/MotionSensor.h | 2 +- 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/ButtonThread.cpp b/src/ButtonThread.cpp index 664c3560f8..5175a26809 100644 --- a/src/ButtonThread.cpp +++ b/src/ButtonThread.cpp @@ -191,6 +191,7 @@ int32_t ButtonThread::runOnce() 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) { @@ -203,6 +204,7 @@ int32_t ButtonThread::runOnce() accelerometerThread->calibrate(60); } break; +#endif // No valid multipress action default: break; diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp index 8e483fc532..69485cedd4 100755 --- a/src/motion/BMX160Sensor.cpp +++ b/src/motion/BMX160Sensor.cpp @@ -33,11 +33,13 @@ int32_t BMX160Sensor::runOnce() if (doCalibration) { +#if !defined(MESHTASTIC_EXCLUDE_SCREEN) if (!showingScreen) { powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration showingScreen = true; screen->startAlert((FrameCallback)drawFrameCalibration); } +#endif if (magAccel.x > highestX) highestX = magAccel.x; @@ -103,6 +105,7 @@ int32_t BMX160Sensor::runOnce() heading += 270; break; } + screen->setHeading(heading); #endif @@ -117,7 +120,9 @@ void BMX160Sensor::calibrate(uint16_t forSeconds) doCalibration = true; uint16_t calibrateFor = forSeconds * 1000; // calibrate for seconds provided endCalibrationAt = millis() + calibrateFor; +#if !defined(MESHTASTIC_EXCLUDE_SCREEN) screen->setEndCalibration(endCalibrationAt); +#endif } #endif diff --git a/src/motion/MotionSensor.h b/src/motion/MotionSensor.h index 1848c472a1..1f4d093bfc 100755 --- a/src/motion/MotionSensor.h +++ b/src/motion/MotionSensor.h @@ -58,7 +58,7 @@ class MotionSensor // Do calibration if true bool doCalibration = false; - uint32_t endCalibrationAt; + uint32_t endCalibrationAt = 0; }; namespace MotionSensorI2C From 686f56954099beb1b68464ab0de9a3360e17ddce Mon Sep 17 00:00:00 2001 From: Dan Welch Date: Thu, 12 Dec 2024 14:22:53 -0700 Subject: [PATCH 03/11] Fix build on RAK4631_eth_gw - exclude all compass heading updates on variant without screen --- src/motion/BMX160Sensor.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp index 69485cedd4..06cea32297 100755 --- a/src/motion/BMX160Sensor.cpp +++ b/src/motion/BMX160Sensor.cpp @@ -25,6 +25,7 @@ bool BMX160Sensor::init() int32_t BMX160Sensor::runOnce() { +#if !defined(MESHTASTIC_EXCLUDE_SCREEN) sBmx160SensorData_t magAccel; sBmx160SensorData_t gAccel; @@ -33,13 +34,11 @@ int32_t BMX160Sensor::runOnce() if (doCalibration) { -#if !defined(MESHTASTIC_EXCLUDE_SCREEN) if (!showingScreen) { powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration showingScreen = true; screen->startAlert((FrameCallback)drawFrameCalibration); } -#endif if (magAccel.x > highestX) highestX = magAccel.x; @@ -66,7 +65,6 @@ int32_t BMX160Sensor::runOnce() // lowestY, highestY, lowestZ, highestZ); } -#if !defined(MESHTASTIC_EXCLUDE_SCREEN) int highestRealX = highestX - (highestX + lowestX) / 2; magAccel.x -= (highestX + lowestX) / 2; @@ -114,13 +112,12 @@ int32_t BMX160Sensor::runOnce() void BMX160Sensor::calibrate(uint16_t forSeconds) { - +#if !defined(MESHTASTIC_EXCLUDE_SCREEN) LOG_DEBUG("BMX160 calibration started for %is", forSeconds); doCalibration = true; uint16_t calibrateFor = forSeconds * 1000; // calibrate for seconds provided endCalibrationAt = millis() + calibrateFor; -#if !defined(MESHTASTIC_EXCLUDE_SCREEN) screen->setEndCalibration(endCalibrationAt); #endif } From 14561f63a879f078a2ea385a02a72ca030a5e3c4 Mon Sep 17 00:00:00 2001 From: Dan Welch Date: Fri, 13 Dec 2024 16:20:14 -0700 Subject: [PATCH 04/11] Persist BMX160 compass calibration - move compass calibration highest/lowest XYZ to struct for easier load/save - implement littleFS load/save of compass calibration values --- src/motion/BMX160Sensor.cpp | 88 ++++++++++++++++++++++++++++++------- src/motion/BMX160Sensor.h | 23 +++++++++- 2 files changed, 93 insertions(+), 18 deletions(-) diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp index 06cea32297..3db308c6b9 100755 --- a/src/motion/BMX160Sensor.cpp +++ b/src/motion/BMX160Sensor.cpp @@ -1,4 +1,5 @@ #include "BMX160Sensor.h" +#include "FSCommon.h" #if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C @@ -16,7 +17,13 @@ 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"); + + loadState(); + + LOG_DEBUG("BMX160 min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", + bmx160Config.mAccel.min.x, bmx160Config.mAccel.max.x, bmx160Config.mAccel.min.y, bmx160Config.mAccel.max.y, + bmx160Config.mAccel.min.z, bmx160Config.mAccel.max.z); + return true; } LOG_DEBUG("BMX160 init failed"); @@ -40,18 +47,18 @@ int32_t BMX160Sensor::runOnce() 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; + if (magAccel.x > bmx160Config.mAccel.max.x) + bmx160Config.mAccel.max.x = magAccel.x; + if (magAccel.x < bmx160Config.mAccel.min.x) + bmx160Config.mAccel.min.x = magAccel.x; + if (magAccel.y > bmx160Config.mAccel.max.y) + bmx160Config.mAccel.max.y = magAccel.y; + if (magAccel.y < bmx160Config.mAccel.min.y) + bmx160Config.mAccel.min.y = magAccel.y; + if (magAccel.z > bmx160Config.mAccel.max.z) + bmx160Config.mAccel.max.z = magAccel.z; + if (magAccel.z < bmx160Config.mAccel.min.z) + bmx160Config.mAccel.min.z = magAccel.z; uint32_t now = millis(); if (now > endCalibrationAt) { @@ -59,17 +66,19 @@ int32_t BMX160Sensor::runOnce() endCalibrationAt = 0; showingScreen = false; screen->endAlert(); + + updateState(); } // LOG_DEBUG("BMX160 min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", lowestX, highestX, // lowestY, highestY, lowestZ, highestZ); } - int highestRealX = highestX - (highestX + lowestX) / 2; + int highestRealX = bmx160Config.mAccel.max.x - (bmx160Config.mAccel.max.x + bmx160Config.mAccel.min.x) / 2; - magAccel.x -= (highestX + lowestX) / 2; - magAccel.y -= (highestY + lowestY) / 2; - magAccel.z -= (highestZ + lowestZ) / 2; + magAccel.x -= (bmx160Config.mAccel.max.x + bmx160Config.mAccel.min.x) / 2; + magAccel.y -= (bmx160Config.mAccel.max.y + bmx160Config.mAccel.min.y) / 2; + magAccel.z -= (bmx160Config.mAccel.max.z + bmx160Config.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.y = -gAccel.y; @@ -122,6 +131,51 @@ void BMX160Sensor::calibrate(uint16_t forSeconds) #endif } +void BMX160Sensor::loadState() +{ +#ifdef FSCom + auto file = FSCom.open(bmx160ConfigFileName, FILE_O_READ); + if (file) { + file.read((uint8_t *)&bmx160State, BMX160_MAX_STATE_BLOB_SIZE); + file.close(); + + memcpy(&bmx160Config, &bmx160State, sizeof(BMX160Config)); + + LOG_INFO("BMX160 config state read from %s", bmx160ConfigFileName); + } else { + LOG_INFO("No BMX160 config state found (File: %s)", bmx160ConfigFileName); + } +#else + LOG_ERROR("ERROR: Filesystem not implemented"); +#endif +} + +void BMX160Sensor::updateState() +{ +#ifdef FSCom + memcpy(&bmx160State, &bmx160Config, sizeof(BMX160Config)); + + LOG_DEBUG("BMX160 min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", bmx160Config.mAccel.min.x, + bmx160Config.mAccel.max.x, bmx160Config.mAccel.min.y, bmx160Config.mAccel.max.y, bmx160Config.mAccel.min.z, + bmx160Config.mAccel.max.z); + + if (FSCom.exists(bmx160ConfigFileName) && !FSCom.remove(bmx160ConfigFileName)) { + LOG_WARN("Can't remove old state file"); + } + auto file = FSCom.open(bmx160ConfigFileName, FILE_O_WRITE); + if (file) { + LOG_INFO("Write BMX160 config state to %s", bmx160ConfigFileName); + file.write((uint8_t *)&bmx160State, BMX160_MAX_STATE_BLOB_SIZE); + file.flush(); + file.close(); + } else { + LOG_INFO("Can't write BMX160 config state (File: %s)", bmx160ConfigFileName); + } +#else + LOG_ERROR("ERROR: Filesystem not implemented"); +#endif +} + #endif #endif \ No newline at end of file diff --git a/src/motion/BMX160Sensor.h b/src/motion/BMX160Sensor.h index 9031b45042..a03155531b 100755 --- a/src/motion/BMX160Sensor.h +++ b/src/motion/BMX160Sensor.h @@ -12,12 +12,33 @@ #include "Fusion/Fusion.h" #include +#define BMX160_MAX_STATE_BLOB_SIZE (144) // pad size to allow for additional saved config parameters (accel, gyro, etc) + +struct xyzFloat { + float x; + float y; + float z; +}; +struct minMaxXYZ { + xyzFloat min; + xyzFloat max; +}; +struct BMX160Config { + minMaxXYZ mAccel; +}; + class BMX160Sensor : public MotionSensor { private: RAK_BMX160 sensor; bool showingScreen = false; - float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0; + BMX160Config bmx160Config; + + protected: + const char *bmx160ConfigFileName = "/prefs/bmx160.dat"; + uint8_t bmx160State[BMX160_MAX_STATE_BLOB_SIZE] = {0}; + void loadState(); + void updateState(); public: explicit BMX160Sensor(ScanI2C::FoundDevice foundDevice); From bff3cc183d7407c77e66054c385306ac01ef61d8 Mon Sep 17 00:00:00 2001 From: Dan Welch Date: Fri, 13 Dec 2024 16:23:14 -0700 Subject: [PATCH 05/11] Persist BMX160 compass calibration - change updateState function to saveState function for clarity - increase max save size --- src/motion/BMX160Sensor.cpp | 4 ++-- src/motion/BMX160Sensor.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp index 3db308c6b9..3e11b9eaf1 100755 --- a/src/motion/BMX160Sensor.cpp +++ b/src/motion/BMX160Sensor.cpp @@ -67,7 +67,7 @@ int32_t BMX160Sensor::runOnce() showingScreen = false; screen->endAlert(); - updateState(); + saveState(); } // LOG_DEBUG("BMX160 min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", lowestX, highestX, @@ -150,7 +150,7 @@ void BMX160Sensor::loadState() #endif } -void BMX160Sensor::updateState() +void BMX160Sensor::saveState() { #ifdef FSCom memcpy(&bmx160State, &bmx160Config, sizeof(BMX160Config)); diff --git a/src/motion/BMX160Sensor.h b/src/motion/BMX160Sensor.h index a03155531b..24a194a88c 100755 --- a/src/motion/BMX160Sensor.h +++ b/src/motion/BMX160Sensor.h @@ -12,7 +12,7 @@ #include "Fusion/Fusion.h" #include -#define BMX160_MAX_STATE_BLOB_SIZE (144) // pad size to allow for additional saved config parameters (accel, gyro, etc) +#define BMX160_MAX_STATE_BLOB_SIZE (256) // pad size to allow for additional saved config parameters (accel, gyro, etc) struct xyzFloat { float x; @@ -38,7 +38,7 @@ class BMX160Sensor : public MotionSensor const char *bmx160ConfigFileName = "/prefs/bmx160.dat"; uint8_t bmx160State[BMX160_MAX_STATE_BLOB_SIZE] = {0}; void loadState(); - void updateState(); + void saveState(); public: explicit BMX160Sensor(ScanI2C::FoundDevice foundDevice); From 148ceac296dc679070f0516253ae03eaef92bed2 Mon Sep 17 00:00:00 2001 From: Dan Welch Date: Sat, 14 Dec 2024 09:46:26 -0700 Subject: [PATCH 06/11] Improve compass calibration method - initiate calibration with first read values to account for min values > 0 --- src/motion/BMX160Sensor.cpp | 23 +++++++++++++++++------ src/motion/MotionSensor.h | 1 + 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp index 06cea32297..14a2571d63 100755 --- a/src/motion/BMX160Sensor.cpp +++ b/src/motion/BMX160Sensor.cpp @@ -40,18 +40,28 @@ int32_t BMX160Sensor::runOnce() screen->startAlert((FrameCallback)drawFrameCalibration); } - if (magAccel.x > highestX) + if (firstCalibrationRead) { 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; + firstCalibrationRead = false; + } else { + 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; + } uint32_t now = millis(); if (now > endCalibrationAt) { @@ -116,6 +126,7 @@ void BMX160Sensor::calibrate(uint16_t forSeconds) LOG_DEBUG("BMX160 calibration started for %is", forSeconds); doCalibration = true; + firstCalibrationRead = true; uint16_t calibrateFor = forSeconds * 1000; // calibrate for seconds provided endCalibrationAt = millis() + calibrateFor; screen->setEndCalibration(endCalibrationAt); diff --git a/src/motion/MotionSensor.h b/src/motion/MotionSensor.h index 1f4d093bfc..6026033bc2 100755 --- a/src/motion/MotionSensor.h +++ b/src/motion/MotionSensor.h @@ -58,6 +58,7 @@ class MotionSensor // Do calibration if true bool doCalibration = false; + bool firstCalibrationRead = false; uint32_t endCalibrationAt = 0; }; From c2a6e725f59d92798f5c6a36c9af62eb5e9e4c04 Mon Sep 17 00:00:00 2001 From: Dan Welch Date: Sun, 15 Dec 2024 09:46:40 -0700 Subject: [PATCH 07/11] WIP improved compass calibration --- src/motion/BMX160Sensor.cpp | 105 ++++-------------------------------- src/motion/BMX160Sensor.h | 23 -------- src/motion/MotionSensor.cpp | 87 ++++++++++++++++++++++++++++++ src/motion/MotionSensor.h | 26 ++++++++- 4 files changed, 121 insertions(+), 120 deletions(-) diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp index 6ab3fc905f..e084a4a28c 100755 --- a/src/motion/BMX160Sensor.cpp +++ b/src/motion/BMX160Sensor.cpp @@ -1,5 +1,4 @@ #include "BMX160Sensor.h" -#include "FSCommon.h" #if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C @@ -20,9 +19,9 @@ bool BMX160Sensor::init() loadState(); - LOG_DEBUG("BMX160 min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", - bmx160Config.mAccel.min.x, bmx160Config.mAccel.max.x, bmx160Config.mAccel.min.y, bmx160Config.mAccel.max.y, - bmx160Config.mAccel.min.z, bmx160Config.mAccel.max.z); + 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); return true; } @@ -40,55 +39,14 @@ int32_t BMX160Sensor::runOnce() sensor.getAllData(&magAccel, NULL, &gAccel); if (doCalibration) { - - if (!showingScreen) { - powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration - showingScreen = true; - screen->startAlert((FrameCallback)drawFrameCalibration); - } - - if (firstCalibrationRead) { - bmx160Config.mAccel.min.x = magAccel.x; - bmx160Config.mAccel.max.x = magAccel.x; - bmx160Config.mAccel.min.y = magAccel.y; - bmx160Config.mAccel.max.y = magAccel.y; - bmx160Config.mAccel.min.z = magAccel.z; - bmx160Config.mAccel.max.z = magAccel.z; - firstCalibrationRead = false; - } else { - if (magAccel.x > bmx160Config.mAccel.max.x) - bmx160Config.mAccel.max.x = magAccel.x; - if (magAccel.x < bmx160Config.mAccel.min.x) - bmx160Config.mAccel.min.x = magAccel.x; - if (magAccel.y > bmx160Config.mAccel.max.y) - bmx160Config.mAccel.max.y = magAccel.y; - if (magAccel.y < bmx160Config.mAccel.min.y) - bmx160Config.mAccel.min.y = magAccel.y; - if (magAccel.z > bmx160Config.mAccel.max.z) - bmx160Config.mAccel.max.z = magAccel.z; - if (magAccel.z < bmx160Config.mAccel.min.z) - bmx160Config.mAccel.min.z = magAccel.z; - } - - uint32_t now = millis(); - if (now > endCalibrationAt) { - doCalibration = false; - endCalibrationAt = 0; - showingScreen = false; - screen->endAlert(); - - saveState(); - } - - // LOG_DEBUG("BMX160 min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", lowestX, highestX, - // lowestY, highestY, lowestZ, highestZ); + getMagCalibrationData(magAccel.x, magAccel.y, magAccel.z); } - int highestRealX = bmx160Config.mAccel.max.x - (bmx160Config.mAccel.max.x + bmx160Config.mAccel.min.x) / 2; + int highestRealX = sensorConfig.mAccel.max.x - (sensorConfig.mAccel.max.x + sensorConfig.mAccel.min.x) / 2; - magAccel.x -= (bmx160Config.mAccel.max.x + bmx160Config.mAccel.min.x) / 2; - magAccel.y -= (bmx160Config.mAccel.max.y + bmx160Config.mAccel.min.y) / 2; - magAccel.z -= (bmx160Config.mAccel.max.z + bmx160Config.mAccel.min.z) / 2; + 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; // default location for the BMX160 is on the rear of the board ga.axis.y = -gAccel.y; @@ -132,7 +90,7 @@ int32_t BMX160Sensor::runOnce() void BMX160Sensor::calibrate(uint16_t forSeconds) { #if !defined(MESHTASTIC_EXCLUDE_SCREEN) - LOG_DEBUG("BMX160 calibration started for %is", forSeconds); + LOG_INFO("BMX160 calibration started for %is", forSeconds); doCalibration = true; firstCalibrationRead = true; @@ -142,51 +100,6 @@ void BMX160Sensor::calibrate(uint16_t forSeconds) #endif } -void BMX160Sensor::loadState() -{ -#ifdef FSCom - auto file = FSCom.open(bmx160ConfigFileName, FILE_O_READ); - if (file) { - file.read((uint8_t *)&bmx160State, BMX160_MAX_STATE_BLOB_SIZE); - file.close(); - - memcpy(&bmx160Config, &bmx160State, sizeof(BMX160Config)); - - LOG_INFO("BMX160 config state read from %s", bmx160ConfigFileName); - } else { - LOG_INFO("No BMX160 config state found (File: %s)", bmx160ConfigFileName); - } -#else - LOG_ERROR("ERROR: Filesystem not implemented"); -#endif -} - -void BMX160Sensor::saveState() -{ -#ifdef FSCom - memcpy(&bmx160State, &bmx160Config, sizeof(BMX160Config)); - - LOG_DEBUG("BMX160 min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", bmx160Config.mAccel.min.x, - bmx160Config.mAccel.max.x, bmx160Config.mAccel.min.y, bmx160Config.mAccel.max.y, bmx160Config.mAccel.min.z, - bmx160Config.mAccel.max.z); - - if (FSCom.exists(bmx160ConfigFileName) && !FSCom.remove(bmx160ConfigFileName)) { - LOG_WARN("Can't remove old state file"); - } - auto file = FSCom.open(bmx160ConfigFileName, FILE_O_WRITE); - if (file) { - LOG_INFO("Write BMX160 config state to %s", bmx160ConfigFileName); - file.write((uint8_t *)&bmx160State, BMX160_MAX_STATE_BLOB_SIZE); - file.flush(); - file.close(); - } else { - LOG_INFO("Can't write BMX160 config state (File: %s)", bmx160ConfigFileName); - } -#else - LOG_ERROR("ERROR: Filesystem not implemented"); -#endif -} - #endif #endif \ No newline at end of file diff --git a/src/motion/BMX160Sensor.h b/src/motion/BMX160Sensor.h index 24a194a88c..4cd4d5d1a4 100755 --- a/src/motion/BMX160Sensor.h +++ b/src/motion/BMX160Sensor.h @@ -12,33 +12,10 @@ #include "Fusion/Fusion.h" #include -#define BMX160_MAX_STATE_BLOB_SIZE (256) // pad size to allow for additional saved config parameters (accel, gyro, etc) - -struct xyzFloat { - float x; - float y; - float z; -}; -struct minMaxXYZ { - xyzFloat min; - xyzFloat max; -}; -struct BMX160Config { - minMaxXYZ mAccel; -}; - class BMX160Sensor : public MotionSensor { private: RAK_BMX160 sensor; - bool showingScreen = false; - BMX160Config bmx160Config; - - protected: - const char *bmx160ConfigFileName = "/prefs/bmx160.dat"; - uint8_t bmx160State[BMX160_MAX_STATE_BLOB_SIZE] = {0}; - void loadState(); - void saveState(); public: explicit BMX160Sensor(ScanI2C::FoundDevice foundDevice); diff --git a/src/motion/MotionSensor.cpp b/src/motion/MotionSensor.cpp index d87380085b..9503d04cc0 100755 --- a/src/motion/MotionSensor.cpp +++ b/src/motion/MotionSensor.cpp @@ -84,4 +84,91 @@ void MotionSensor::buttonPress() {} #endif +void MotionSensor::getMagCalibrationData(float x, float y, float z) +{ + if (!showingScreen) { + powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration + showingScreen = true; + screen->startAlert((FrameCallback)drawFrameCalibration); + } + + 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; + 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; + } + + uint32_t now = millis(); + if (now > endCalibrationAt) { + doCalibration = false; + endCalibrationAt = 0; + showingScreen = false; + screen->endAlert(); + + saveState(); + } +} + +void MotionSensor::loadState() +{ +#ifdef FSCom + auto file = FSCom.open(configFileName, FILE_O_READ); + if (file) { + file.read((uint8_t *)&sensorState, MAX_STATE_BLOB_SIZE); + file.close(); + + memcpy(&sensorConfig, &sensorState, sizeof(SensorConfig)); + + LOG_INFO("Motion Sensor config state read from %s", configFileName); + } else { + LOG_INFO("No Motion Sensor config state found (File: %s)", configFileName); + } +#else + LOG_ERROR("ERROR: Filesystem not implemented"); +#endif +} + +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); + + if (FSCom.exists(configFileName) && !FSCom.remove(configFileName)) { + LOG_WARN("Can't remove old Motion Sensor config state file"); + } + auto file = FSCom.open(configFileName, FILE_O_WRITE); + if (file) { + LOG_INFO("Write Motion Sensor config state to %s", configFileName); + file.write((uint8_t *)&sensorState, MAX_STATE_BLOB_SIZE); + file.flush(); + file.close(); + } else { + LOG_INFO("Can't write Motion Sensor config state (File: %s)", configFileName); + } +#else + LOG_ERROR("ERROR: Filesystem not implemented"); +#endif +} + #endif \ No newline at end of file diff --git a/src/motion/MotionSensor.h b/src/motion/MotionSensor.h index 6026033bc2..b55364d7c7 100755 --- a/src/motion/MotionSensor.h +++ b/src/motion/MotionSensor.h @@ -14,8 +14,24 @@ #include "../graphics/Screen.h" #include "../graphics/ScreenFonts.h" #include "../power.h" +#include "FSCommon.h" #include "Wire.h" +#define MAX_STATE_BLOB_SIZE (256) // pad size to allow for additional saved config parameters (accel, gyro, etc) + +struct xyzFloat { + float x; + float y; + float z; +}; +struct minMaxXYZ { + xyzFloat min; + xyzFloat max; +}; +struct SensorConfig { + minMaxXYZ mAccel; +}; + // Base class for motion processing class MotionSensor { @@ -56,10 +72,18 @@ class MotionSensor ScanI2C::FoundDevice device; - // Do calibration if true + SensorConfig sensorConfig; + bool showingScreen = false; bool doCalibration = false; bool firstCalibrationRead = false; uint32_t endCalibrationAt = 0; + + void getMagCalibrationData(float x, float y, float z); + + const char *configFileName = "/prefs/motionSensor.dat"; + uint8_t sensorState[MAX_STATE_BLOB_SIZE] = {0}; + void loadState(); + void saveState(); }; namespace MotionSensorI2C From ba0b98b371f9d499a901a660802b901803bdb2df Mon Sep 17 00:00:00 2001 From: Dan Welch Date: Mon, 16 Dec 2024 09:47:26 -0700 Subject: [PATCH 08/11] WIP improved compass calibration --- src/motion/BMX160Sensor.cpp | 27 ++++--- src/motion/MotionSensor.cpp | 146 +++++++++++++++++++++++++++++++++++- src/motion/MotionSensor.h | 29 +++++-- 3 files changed, 183 insertions(+), 19 deletions(-) diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp index e084a4a28c..0d8a8ab33f 100755 --- a/src/motion/BMX160Sensor.cpp +++ b/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.y = -gAccel.y; - ga.axis.z = gAccel.z; - ma.axis.x = -magAccel.x; - ma.axis.y = -magAccel.y; - ma.axis.z = magAccel.z * 3; + 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 * 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; // 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; - screen->setEndCalibration(endCalibrationAt); + endMagCalibrationAt = millis() + calibrateFor; + screen->setEndCalibration(endMagCalibrationAt); #endif } diff --git a/src/motion/MotionSensor.cpp b/src/motion/MotionSensor.cpp index 9503d04cc0..1e270cdc6b 100755 --- a/src/motion/MotionSensor.cpp +++ b/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) { - doCalibration = false; - endCalibrationAt = 0; + if (now > endMagCalibrationAt) { + doMagCalibration = false; + 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(); } } diff --git a/src/motion/MotionSensor.h b/src/motion/MotionSensor.h index b55364d7c7..43a7d519e9 100755 --- a/src/motion/MotionSensor.h +++ b/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 y; - float z; + float x = 0.0; + float y = 0.0; + 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}; From 3d92c379f643b68cf44aaa25364e83c3144a6a3b Mon Sep 17 00:00:00 2001 From: Dan Welch Date: Thu, 26 Dec 2024 10:34:29 -0700 Subject: [PATCH 09/11] 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 0d8a8ab33f..d88ccefc2d 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 1e270cdc6b..b047537471 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 43a7d519e9..5b142b21ad 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); From 459bf65648bbdaa76ef636c5f38e5c81ed07dfea Mon Sep 17 00:00:00 2001 From: Dan Welch Date: Thu, 26 Dec 2024 11:05:27 -0700 Subject: [PATCH 10/11] fix timeRemainingBuffer size --- src/motion/MotionSensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/motion/MotionSensor.cpp b/src/motion/MotionSensor.cpp index b047537471..db679f4a12 100755 --- a/src/motion/MotionSensor.cpp +++ b/src/motion/MotionSensor.cpp @@ -2,7 +2,7 @@ #if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C -char timeRemainingBuffer[12]; +char timeRemainingBuffer[20]; // screen is defined in main.cpp extern graphics::Screen *screen; From 962ca6d524b8efad9338e34f4ca241025add545e Mon Sep 17 00:00:00 2001 From: Dan Welch Date: Thu, 26 Dec 2024 11:28:43 -0700 Subject: [PATCH 11/11] fix functions that were defined outside of RAK4631 to build on other variants --- src/motion/MotionSensor.cpp | 49 +++++++++++++++++++------------------ src/motion/MotionSensor.h | 20 +++++++-------- 2 files changed, 35 insertions(+), 34 deletions(-) diff --git a/src/motion/MotionSensor.cpp b/src/motion/MotionSensor.cpp index db679f4a12..f69389c66f 100755 --- a/src/motion/MotionSensor.cpp +++ b/src/motion/MotionSensor.cpp @@ -31,6 +31,29 @@ ScanI2C::I2CPort MotionSensor::devicePort() return device.address.port; } +#if !MESHTASTIC_EXCLUDE_POWER_FSM +void MotionSensor::wakeScreen() +{ + if (powerFSM.getState() == &stateDARK) { + LOG_DEBUG("Motion wakeScreen detected"); + powerFSM.trigger(EVENT_INPUT); + } +} + +void MotionSensor::buttonPress() +{ + LOG_DEBUG("Motion buttonPress detected"); + powerFSM.trigger(EVENT_PRESS); +} + +#else + +void MotionSensor::wakeScreen() {} + +void MotionSensor::buttonPress() {} + +#endif + #if defined(RAK_4631) & !MESHTASTIC_EXCLUDE_SCREEN void MotionSensor::drawFrameCalibration(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y) { @@ -86,30 +109,6 @@ void MotionSensor::drawFrameGyroCalibration(OLEDDisplay *display, OLEDDisplayUiS display->setFont(FONT_SMALL); display->drawString(x, y + 40, timeRemainingBuffer); } -#endif - -#if !MESHTASTIC_EXCLUDE_POWER_FSM -void MotionSensor::wakeScreen() -{ - if (powerFSM.getState() == &stateDARK) { - LOG_DEBUG("Motion wakeScreen detected"); - powerFSM.trigger(EVENT_INPUT); - } -} - -void MotionSensor::buttonPress() -{ - LOG_DEBUG("Motion buttonPress detected"); - powerFSM.trigger(EVENT_PRESS); -} - -#else - -void MotionSensor::wakeScreen() {} - -void MotionSensor::buttonPress() {} - -#endif void MotionSensor::getMagCalibrationData(float x, float y, float z) { @@ -336,4 +335,6 @@ void MotionSensor::saveState() #endif } +#endif + #endif \ No newline at end of file diff --git a/src/motion/MotionSensor.h b/src/motion/MotionSensor.h index 5b142b21ad..5f5c7b81d8 100755 --- a/src/motion/MotionSensor.h +++ b/src/motion/MotionSensor.h @@ -73,13 +73,6 @@ class MotionSensor // Register a button press when a double-tap is detected virtual void buttonPress(); -#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; @@ -96,14 +89,21 @@ class MotionSensor minMaxXYZ magCalibrationMinMax; uint16_t calibrationCount = 0; + const char *configFileName = "/prefs/motionSensor.dat"; + uint8_t sensorState[MAX_STATE_BLOB_SIZE] = {0}; + +#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); + 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}; void loadState(); void saveState(); +#endif }; namespace MotionSensorI2C