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

Compass calibration improvements

Browse Source 
- fix orientation vectors based on acceleration from calibration
- re-zero calibration data after running calibration to start fresh on next calibration
This commit is contained in:
Dan Welch 2024-12-26 10:34:29 -07:00
parent ba0b98b371
commit 3d92c379f6
3 changed files with 85 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/motion/BMX160Sensor.cpp
View File

@ -16,12 +16,22 @@ bool BMX160Sensor::init()
if (sensor.begin()) { if (sensor.begin()) {
// set output data rate // set output data rate
sensor.ODR_Config(BMX160_ACCEL_ODR_100HZ, BMX160_GYRO_ODR_100HZ); 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(); loadState();
LOG_INFO("BMX160 load calibration min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", LOG_INFO("BMX160 MAG calibration center_x: %.4f, center_Y: %.4f, center_Z: %.4f", sensorConfig.mAccel.x,
sensorConfig.mAccel.min.x, sensorConfig.mAccel.max.x, sensorConfig.mAccel.min.y, sensorConfig.mAccel.max.y, sensorConfig.mAccel.y, sensorConfig.mAccel.z);
sensorConfig.mAccel.min.z, sensorConfig.mAccel.max.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; return true;
} }
@ -47,24 +57,26 @@ int32_t BMX160Sensor::runOnce()
getGyroCalibrationData(gyroAccel.x, gyroAccel.y, gyroAccel.z, gAccel.x, gAccel.y, gAccel.z); 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; 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.y = gAccel.y * sensorConfig.orientation.y;
ga.axis.z = gAccel.z * sensorConfig.orientation.z; ga.axis.z = gAccel.z * sensorConfig.orientation.z;
ma.axis.x = magAccel.x * sensorConfig.orientation.x; ma.axis.x = magAccel.x * sensorConfig.orientation.x;
ma.axis.y = magAccel.y * sensorConfig.orientation.y; ma.axis.y = magAccel.y * sensorConfig.orientation.y;
ma.axis.z = magAccel.z * sensorConfig.orientation.z * 3; 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 we're set to one of the inverted positions
if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) { // if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ); // ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ);
ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ); // ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
} // }
float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma); float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma);
@ -86,6 +98,11 @@ int32_t BMX160Sensor::runOnce()
break; 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); screen->setHeading(heading);
#endif #endif

83
src/motion/MotionSensor.cpp
View File

@ -66,7 +66,7 @@ void MotionSensor::drawFrameGyroWarning(OLEDDisplay *display, OLEDDisplayUiState
// int y_offset = display->height() <= 80 ? 0 : 32; // int y_offset = display->height() <= 80 ? 0 : 32;
display->setTextAlignment(TEXT_ALIGN_LEFT); display->setTextAlignment(TEXT_ALIGN_LEFT);
display->setFont(FONT_SMALL); 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; uint8_t timeRemaining = (screen->getEndCalibration() - millis()) / 1000;
sprintf(timeRemainingBuffer, "Starting in ( %02d )", timeRemaining); sprintf(timeRemainingBuffer, "Starting in ( %02d )", timeRemaining);
@ -120,32 +120,42 @@ void MotionSensor::getMagCalibrationData(float x, float y, float z)
} }
if (firstCalibrationRead) { if (firstCalibrationRead) {
sensorConfig.mAccel.min.x = x; magCalibrationMinMax.min.x = x;
sensorConfig.mAccel.max.x = x; magCalibrationMinMax.max.x = x;
sensorConfig.mAccel.min.y = y; magCalibrationMinMax.min.y = y;
sensorConfig.mAccel.max.y = y; magCalibrationMinMax.max.y = y;
sensorConfig.mAccel.min.z = z; magCalibrationMinMax.min.z = z;
sensorConfig.mAccel.max.z = z; magCalibrationMinMax.max.z = z;
firstCalibrationRead = false; firstCalibrationRead = false;
} else { } else {
if (x > sensorConfig.mAccel.max.x) if (x > magCalibrationMinMax.max.x)
sensorConfig.mAccel.max.x = x; magCalibrationMinMax.max.x = x;
if (x < sensorConfig.mAccel.min.x) if (x < magCalibrationMinMax.min.x)
sensorConfig.mAccel.min.x = x; magCalibrationMinMax.min.x = x;
if (y > sensorConfig.mAccel.max.y) if (y > magCalibrationMinMax.max.y)
sensorConfig.mAccel.max.y = y; magCalibrationMinMax.max.y = y;
if (y < sensorConfig.mAccel.min.y) if (y < magCalibrationMinMax.min.y)
sensorConfig.mAccel.min.y = y; magCalibrationMinMax.min.y = y;
if (z > sensorConfig.mAccel.max.z) if (z > magCalibrationMinMax.max.z)
sensorConfig.mAccel.max.z = z; magCalibrationMinMax.max.z = z;
if (z < sensorConfig.mAccel.min.z) if (z < magCalibrationMinMax.min.z)
sensorConfig.mAccel.min.z = z; magCalibrationMinMax.min.z = z;
} }
uint32_t now = millis(); uint32_t now = millis();
if (now > endMagCalibrationAt) { 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; doMagCalibration = false;
endMagCalibrationAt = 0; 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; showingScreen = false;
screen->endAlert(); 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 // determine orientation multipliers based on down direction
if (abs(accelCalibrationSum.x) > abs(accelCalibrationSum.y) && abs(accelCalibrationSum.x) > abs(accelCalibrationSum.z)) { if (abs(accelCalibrationSum.x) > abs(accelCalibrationSum.y) && abs(accelCalibrationSum.x) > abs(accelCalibrationSum.z)) {
if (accelCalibrationSum.x >= 0) { if (accelCalibrationSum.x >= 0) {
// X axis oriented with down positive
sensorConfig.orientation.x = 1; sensorConfig.orientation.x = 1;
sensorConfig.orientation.y = 1; sensorConfig.orientation.y = 1;
sensorConfig.orientation.z = 1; sensorConfig.orientation.z = 1;
} else { } else {
// X axis oriented with down negative
sensorConfig.orientation.x = 1; sensorConfig.orientation.x = 1;
sensorConfig.orientation.y = 1; sensorConfig.orientation.y = -1;
sensorConfig.orientation.z = 1; sensorConfig.orientation.z = -1;
} }
} else if (abs(accelCalibrationSum.y) > abs(accelCalibrationSum.x) && } else if (abs(accelCalibrationSum.y) > abs(accelCalibrationSum.x) &&
abs(accelCalibrationSum.y) > abs(accelCalibrationSum.z)) { abs(accelCalibrationSum.y) > abs(accelCalibrationSum.z)) {
if (accelCalibrationSum.y >= 0) { if (accelCalibrationSum.y >= 0) {
// Y axis oriented with down positive
sensorConfig.orientation.x = 1; sensorConfig.orientation.x = 1;
sensorConfig.orientation.y = 1; sensorConfig.orientation.y = 1;
sensorConfig.orientation.z = 1; sensorConfig.orientation.z = 1;
} else { } else {
sensorConfig.orientation.x = 1; // Y axis oriented with down negative
sensorConfig.orientation.x = -1;
sensorConfig.orientation.y = 1; sensorConfig.orientation.y = 1;
sensorConfig.orientation.z = 1; sensorConfig.orientation.z = -1;
} }
} else if (abs(accelCalibrationSum.z) > abs(accelCalibrationSum.x) && } else if (abs(accelCalibrationSum.z) > abs(accelCalibrationSum.x) &&
abs(accelCalibrationSum.z) > abs(accelCalibrationSum.y)) { abs(accelCalibrationSum.z) > abs(accelCalibrationSum.y)) {
if (accelCalibrationSum.z >= 0) { if (accelCalibrationSum.z >= 0) {
sensorConfig.orientation.x = -1; // Z axis oriented with down positive
sensorConfig.orientation.y = -1; sensorConfig.orientation.x = 1;
sensorConfig.orientation.z = -1; sensorConfig.orientation.y = 1;
sensorConfig.orientation.z = 1;
} else { } else {
// Z axis oriented with down negative
sensorConfig.orientation.x = -1; sensorConfig.orientation.x = -1;
sensorConfig.orientation.y = -1; sensorConfig.orientation.y = -1;
sensorConfig.orientation.z = 1; sensorConfig.orientation.z = 1;
@ -261,6 +277,12 @@ void MotionSensor::getGyroCalibrationData(float g_x, float g_y, float g_z, float
saveState(); saveState();
doGyroCalibration = false; doGyroCalibration = false;
endGyroCalibrationAt = 0; endGyroCalibrationAt = 0;
accelCalibrationSum.x = 0;
accelCalibrationSum.y = 0;
accelCalibrationSum.z = 0;
gyroCalibrationSum.x = 0;
gyroCalibrationSum.y = 0;
gyroCalibrationSum.z = 0;
showingScreen = false; showingScreen = false;
screen->endAlert(); screen->endAlert();
} }
@ -290,9 +312,12 @@ void MotionSensor::saveState()
#ifdef FSCom #ifdef FSCom
memcpy(&sensorState, &sensorConfig, sizeof(SensorConfig)); 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", LOG_INFO("Save MAG calibration center_x: %.4f, center_Y: %.4f, center_Z: %.4f", sensorConfig.mAccel.x, sensorConfig.mAccel.y,
sensorConfig.mAccel.min.x, sensorConfig.mAccel.max.x, sensorConfig.mAccel.min.y, sensorConfig.mAccel.max.y, sensorConfig.mAccel.z);
sensorConfig.mAccel.min.z, sensorConfig.mAccel.max.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)) { if (FSCom.exists(configFileName) && !FSCom.remove(configFileName)) {
LOG_WARN("Can't remove old Motion Sensor config state file"); LOG_WARN("Can't remove old Motion Sensor config state file");

3
src/motion/MotionSensor.h
View File

@ -34,7 +34,7 @@ struct minMaxXYZ {
xyzFloat max; xyzFloat max;
}; };
struct SensorConfig { struct SensorConfig {
minMaxXYZ mAccel; xyzFloat mAccel;
xyzFloat gyroAccel; xyzFloat gyroAccel;
// xyzFloat gAccel; // xyzFloat gAccel;
xyzInt orientation; xyzInt orientation;
@ -93,6 +93,7 @@ class MotionSensor
uint32_t endGyroCalibrationAt = 0; uint32_t endGyroCalibrationAt = 0;
xyzFloat gyroCalibrationSum; xyzFloat gyroCalibrationSum;
xyzInt accelCalibrationSum; xyzInt accelCalibrationSum;
minMaxXYZ magCalibrationMinMax;
uint16_t calibrationCount = 0; uint16_t calibrationCount = 0;
void getMagCalibrationData(float x, float y, float z); void getMagCalibrationData(float x, float y, float z);