From c2a6e725f59d92798f5c6a36c9af62eb5e9e4c04 Mon Sep 17 00:00:00 2001 From: Dan Welch Date: Sun, 15 Dec 2024 09:46:40 -0700 Subject: [PATCH] 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 6ab3fc905..e084a4a28 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 24a194a88..4cd4d5d1a 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 d87380085..9503d04cc 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 6026033bc..b55364d7c 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