diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp index e084a4a28..0d8a8ab33 100755 --- a/src/motion/BMX160Sensor.cpp +++ b/src/motion/BMX160Sensor.cpp @@ -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 } diff --git a/src/motion/MotionSensor.cpp b/src/motion/MotionSensor.cpp index 9503d04cc..1e270cdc6 100755 --- a/src/motion/MotionSensor.cpp +++ b/src/motion/MotionSensor.cpp @@ -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(); } } diff --git a/src/motion/MotionSensor.h b/src/motion/MotionSensor.h index b55364d7c..43a7d519e 100755 --- a/src/motion/MotionSensor.h +++ b/src/motion/MotionSensor.h @@ -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};