pohui_pohui/firmware
1
0
Fork 0
mirror of https://github.com/meshtastic/firmware.git synced 2025-09-12 05:51:29 +00:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

WIP improved compass calibration

Browse Source
This commit is contained in:
Dan Welch 2024-12-15 09:46:40 -07:00
parent 66769527be
commit c2a6e725f5
4 changed files with 121 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
src/motion/BMX160Sensor.cpp
View File

@ -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

23
src/motion/BMX160Sensor.h
View File

@ -12,33 +12,10 @@
#include "Fusion/Fusion.h"
#include <Rak_BMX160.h>
#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);

87
src/motion/MotionSensor.cpp
View File

@ -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

26
src/motion/MotionSensor.h
View File

@ -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