diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp index 56f794306..5888c20be 100755 --- a/src/motion/BMX160Sensor.cpp +++ b/src/motion/BMX160Sensor.cpp @@ -115,8 +115,13 @@ int32_t BMX160Sensor::runOnce() void BMX160Sensor::calibrate(uint16_t forSeconds) { #if !defined(MESHTASTIC_EXCLUDE_SCREEN) + sBmx160SensorData_t magAccel; + sBmx160SensorData_t gAccel; LOG_DEBUG("BMX160 calibration started for %is", forSeconds); - highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0; + sensor.getAllData(&magAccel, NULL, &gAccel); + highestX = magAccel.x, lowestX = magAccel.x; + highestY = magAccel.y, lowestY = magAccel.y; + highestZ = magAccel.z, lowestZ = magAccel.z; doCalibration = true; uint16_t calibrateFor = forSeconds * 1000; // calibrate for seconds provided diff --git a/src/motion/ICM20948Sensor.cpp b/src/motion/ICM20948Sensor.cpp index ebb0f7b66..10918eb7d 100755 --- a/src/motion/ICM20948Sensor.cpp +++ b/src/motion/ICM20948Sensor.cpp @@ -157,7 +157,17 @@ void ICM20948Sensor::calibrate(uint16_t forSeconds) { #if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN LOG_DEBUG("BMX160 calibration started for %is", forSeconds); - highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0; + if (sensor->dataReady()) { + sensor->getAGMT(); + highestX = sensor->agmt.mag.axes.x; + lowestX = sensor->agmt.mag.axes.x; + highestY = sensor->agmt.mag.axes.y; + lowestY = sensor->agmt.mag.axes.y; + highestZ = sensor->agmt.mag.axes.z; + lowestZ = sensor->agmt.mag.axes.z; + } else { + highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0; + } doCalibration = true; uint16_t calibrateFor = forSeconds * 1000; // calibrate for seconds provided