diff --git a/src/graphics/Screen.cpp b/src/graphics/Screen.cpp index 0f189a269..ce0e600c4 100644 --- a/src/graphics/Screen.cpp +++ b/src/graphics/Screen.cpp @@ -1015,6 +1015,15 @@ void Screen::setFrames(FrameFocus focus) normalFrames[numframes++] = graphics::UIRenderer::drawCompassAndLocationScreen; indicatorIcons.push_back(icon_compass); } +#endif + // After GPS, add QMC6310 and QMI8658 debug screens +#if !MESHTASTIC_EXCLUDE_I2C + normalFrames[numframes++] = graphics::DebugRenderer::drawQMC6310Screen; + indicatorIcons.push_back(icon_compass); +#endif +#if defined(IMU_CS) + normalFrames[numframes++] = graphics::DebugRenderer::drawQMI8658Screen; + indicatorIcons.push_back(icon_system); #endif if (RadioLibInterface::instance && !hiddenFrames.lora) { fsi.positions.lora = numframes; diff --git a/src/graphics/draw/DebugRenderer.cpp b/src/graphics/draw/DebugRenderer.cpp index 5fe10a1b8..85d9b1c59 100644 --- a/src/graphics/draw/DebugRenderer.cpp +++ b/src/graphics/draw/DebugRenderer.cpp @@ -30,6 +30,8 @@ #include #include #include +#include +#include "motion/SensorLiveData.h" using namespace meshtastic; @@ -693,6 +695,48 @@ void drawChirpy(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int1 display->drawString(textX, getTextPositions(display)[line++], "World!"); } +// ---------------- Additional IMU/Magnetometer debug screens ---------------- +void drawQMC6310Screen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y) +{ + static uint32_t last = 0; + display->clear(); + display->setTextAlignment(TEXT_ALIGN_LEFT); + display->setFont(FONT_SMALL); + graphics::drawCommonHeader(display, x, y, "QMC6310"); + int line = 1; + if (g_qmc6310Live.initialized) { + if (!Throttle::isWithinTimespanMs(last, 1000)) { + last = millis(); + char buf[64]; + snprintf(buf, sizeof(buf), "Head %3.0f offX %.0f offY %.0f", g_qmc6310Live.heading, g_qmc6310Live.offX, + g_qmc6310Live.offY); + display->drawString(x, getTextPositions(display)[line++], buf); + snprintf(buf, sizeof(buf), "rawX %d rawY %d", g_qmc6310Live.rawX, g_qmc6310Live.rawY); + display->drawString(x, getTextPositions(display)[line++], buf); + } + } else { + display->drawString(x, getTextPositions(display)[line++], "No data"); + } +} + +void drawQMI8658Screen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y) +{ + (void)state; (void)x; (void)y; + display->clear(); + display->setTextAlignment(TEXT_ALIGN_LEFT); + display->setFont(FONT_SMALL); + graphics::drawCommonHeader(display, x, y, "QMI8658"); + int line = 1; + if (g_qmi8658Live.initialized) { + char buf[64]; + snprintf(buf, sizeof(buf), "ACC %.2f %.2f %.2f", g_qmi8658Live.acc.x, g_qmi8658Live.acc.y, g_qmi8658Live.acc.z); + display->drawString(x, getTextPositions(display)[line++], buf); + snprintf(buf, sizeof(buf), "GYR %.2f %.2f %.2f", g_qmi8658Live.gyr.x, g_qmi8658Live.gyr.y, g_qmi8658Live.gyr.z); + display->drawString(x, getTextPositions(display)[line++], buf); + } else { + display->drawString(x, getTextPositions(display)[line++], "No data"); + } +} } // namespace DebugRenderer } // namespace graphics -#endif \ No newline at end of file +#endif diff --git a/src/graphics/draw/DebugRenderer.h b/src/graphics/draw/DebugRenderer.h index 563a6c1ce..14ac1263a 100644 --- a/src/graphics/draw/DebugRenderer.h +++ b/src/graphics/draw/DebugRenderer.h @@ -36,6 +36,9 @@ void drawSystemScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x // Chirpy screen display void drawChirpy(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y); +// IMU/Magnetometer debug screens +void drawQMI8658Screen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y); +void drawQMC6310Screen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y); } // namespace DebugRenderer } // namespace graphics diff --git a/src/motion/QMC6310Sensor.cpp b/src/motion/QMC6310Sensor.cpp index 23f38af0e..bdb6ace1e 100644 --- a/src/motion/QMC6310Sensor.cpp +++ b/src/motion/QMC6310Sensor.cpp @@ -1,5 +1,6 @@ #include "QMC6310Sensor.h" #include +#include "SensorLiveData.h" #if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C && __has_include() @@ -54,9 +55,18 @@ int32_t QMC6310Sensor::runOnce() int16_t ry = sensor.getRawY(); int16_t rz = sensor.getRawZ(); - if (rx < minX) minX = rx; if (rx > maxX) maxX = rx; - if (ry < minY) minY = ry; if (ry > maxY) maxY = ry; - if (rz < minZ) minZ = rz; if (rz > maxZ) maxZ = rz; + if (rx < minX) + minX = rx; + if (rx > maxX) + maxX = rx; + if (ry < minY) + minY = ry; + if (ry > maxY) + maxY = ry; + if (rz < minZ) + minZ = rz; + if (rz > maxZ) + maxZ = rz; offsetX = (maxX + minX) * 0.5f; offsetY = (maxY + minY) * 0.5f; @@ -71,6 +81,16 @@ int32_t QMC6310Sensor::runOnce() while (heading < 0.0f) heading += 360.0f; while (heading >= 360.0f) heading -= 360.0f; + g_qmc6310Live.initialized = true; + g_qmc6310Live.rawX = rx; + g_qmc6310Live.rawY = ry; + g_qmc6310Live.rawZ = rz; + g_qmc6310Live.offX = offsetX; + g_qmc6310Live.offY = offsetY; + g_qmc6310Live.offZ = offsetZ; + g_qmc6310Live.heading = heading; + g_qmc6310Live.last_ms = millis(); + #if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN switch (config.display.compass_orientation) { case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0_INVERTED: diff --git a/src/motion/QMI8658Sensor.cpp b/src/motion/QMI8658Sensor.cpp index a77e6030c..3fbb63db5 100644 --- a/src/motion/QMI8658Sensor.cpp +++ b/src/motion/QMI8658Sensor.cpp @@ -1,5 +1,6 @@ #include "QMI8658Sensor.h" #include "NodeDB.h" +#include "SensorLiveData.h" #if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C && __has_include() @@ -46,7 +47,8 @@ bool QMI8658Sensor::init() uint8_t id = qmi.getChipID(); LOG_DEBUG("QMI8658: chip id=0x%02x", id); #ifdef QMI8658_DEBUG_STREAM - LOG_INFO("QMI8658 debug stream enabled (10 Hz)"); + LOG_INFO("QMI8658 debug stream enabled (1 Hz)"); + lastLogMs = 0; #endif // Basic configuration similar to lewisxhe examples @@ -78,21 +80,35 @@ bool QMI8658Sensor::init() LOG_DEBUG("QMI8658: dump control registers ->"); qmi.dumpCtrlRegister(); LOG_DEBUG("QMI8658: init ok"); + g_qmi8658Live.initialized = true; return true; } int32_t QMI8658Sensor::runOnce() { #ifdef QMI8658_DEBUG_STREAM - // Always sample/log when debug stream is enabled + // Always sample/log when debug stream is enabled (throttle to ~1 Hz) IMUdata acc = {0}; IMUdata gyr = {0}; bool ready = qmi.getDataReady(); - bool gotAcc = qmi.getAccelerometer(acc.x, acc.y, acc.z); - bool gotGyr = qmi.getGyroscope(gyr.x, gyr.y, gyr.z); - LOG_DEBUG("QMI8658: ready=%d ACC[x=%.3f y=%.3f z=%.3f] m/s^2 GYR[x=%.3f y=%.3f z=%.3f] dps", - (int)ready, acc.x, acc.y, acc.z, gyr.x, gyr.y, gyr.z); - return 100; // ~10 Hz + qmi.getAccelerometer(acc.x, acc.y, acc.z); + qmi.getGyroscope(gyr.x, gyr.y, gyr.z); + + uint32_t now = millis(); + g_qmi8658Live.ready = ready; + g_qmi8658Live.acc.x = acc.x; + g_qmi8658Live.acc.y = acc.y; + g_qmi8658Live.acc.z = acc.z; + g_qmi8658Live.gyr.x = gyr.x; + g_qmi8658Live.gyr.y = gyr.y; + g_qmi8658Live.gyr.z = gyr.z; + g_qmi8658Live.last_ms = now; + if (now - lastLogMs > 1000) { + lastLogMs = now; + LOG_DEBUG("QMI8658: ready=%d ACC[x=%.3f y=%.3f z=%.3f] m/s^2 GYR[x=%.3f y=%.3f z=%.3f] dps", + (int)ready, acc.x, acc.y, acc.z, gyr.x, gyr.y, gyr.z); + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; #endif if (!config.display.wake_on_tap_or_motion) diff --git a/src/motion/QMI8658Sensor.h b/src/motion/QMI8658Sensor.h index 8a123c5a9..405f28e84 100644 --- a/src/motion/QMI8658Sensor.h +++ b/src/motion/QMI8658Sensor.h @@ -13,6 +13,7 @@ class QMI8658Sensor : public MotionSensor { private: SensorQMI8658 qmi; + uint32_t lastLogMs = 0; // Simple motion threshold in Gs above steady 1g static constexpr float MOTION_THRESHOLD_G = 0.15f; // ~0.15 g diff --git a/src/motion/SensorLiveData.cpp b/src/motion/SensorLiveData.cpp new file mode 100644 index 000000000..364686104 --- /dev/null +++ b/src/motion/SensorLiveData.cpp @@ -0,0 +1,5 @@ +#include "SensorLiveData.h" + +QMI8658LiveData g_qmi8658Live; +QMC6310LiveData g_qmc6310Live; + diff --git a/src/motion/SensorLiveData.h b/src/motion/SensorLiveData.h new file mode 100644 index 000000000..7e0609b57 --- /dev/null +++ b/src/motion/SensorLiveData.h @@ -0,0 +1,29 @@ +#pragma once + +#include + +struct Vec3f { + float x = 0; + float y = 0; + float z = 0; +}; + +struct QMI8658LiveData { + bool initialized = false; + bool ready = false; + Vec3f acc; // m/s^2 + Vec3f gyr; // dps + uint32_t last_ms = 0; +}; + +struct QMC6310LiveData { + bool initialized = false; + int16_t rawX = 0, rawY = 0, rawZ = 0; + float offX = 0, offY = 0, offZ = 0; + float heading = 0; // degrees 0..360 + uint32_t last_ms = 0; +}; + +extern QMI8658LiveData g_qmi8658Live; +extern QMC6310LiveData g_qmc6310Live; +