pohui_pohui/firmware
1
0
Fork 0
mirror of https://github.com/meshtastic/firmware.git synced 2025-09-12 22:13:06 +00:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

WIP improved compass calibration

Browse Source
This commit is contained in:
Dan Welch 2024-12-16 09:47:26 -07:00
parent c2a6e725f5
commit ba0b98b371
3 changed files with 183 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/motion/BMX160Sensor.cpp
View File

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

146
src/motion/MotionSensor.cpp
View File

@ -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();
}
}

29
src/motion/MotionSensor.h
View File

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