Mesh node t1 fixes (#10602)

* Fixes * Remove BATTERY_LPCOMP_THRESHOLD BATTERY_LPCOMP_THRESHOLD is dead code — in main-nrf52.cpp it's inside #ifdef BATTERY_LPCOMP_INPUT, which this board intentionally doesn't define. The threshold value is never reached. * Trunk fix * Update MotionSensor.cpp * fix --------- Co-authored-by: Ben Meadors <benmmeadors@gmail.com> Co-authored-by: Jonathan Bennett <jbennett@incomsystems.biz>
2026-06-16 12:40:45 -04:00 · 2026-06-13 20:57:28 -04:00
parent b938b63e8a
commit 745b53698a
8 changed files with 181 additions and 99 deletions
--- a/src/graphics/Screen.cpp
+++ b/src/graphics/Screen.cpp
@@ -233,6 +233,16 @@ static inline float wrapHeading360(float heading)
    return heading;
 }

+static inline float wrapDelta180(float delta)
+{
+    if (delta > 180.0f) {
+        delta -= 360.0f;
+    } else if (delta < -180.0f) {
+        delta += 360.0f;
+    }
+    return delta;
+}
+
 void Screen::setHeading(float heading)
 {
    const float wrappedHeading = wrapHeading360(heading);
@@ -244,37 +254,30 @@ void Screen::setHeading(float heading)
    }

    // Interpolate using shortest-path angular delta to avoid jumps around 0/360.
-    float delta = wrappedHeading - compassHeading;
-    if (delta > 180.0f) {
-        delta -= 360.0f;
-    } else if (delta < -180.0f) {
-        delta += 360.0f;
-    }
+    float delta = wrapDelta180(wrappedHeading - compassHeading);

    // Adaptive filtering:
    // - Strong damping for tiny deltas (jitter)
    // - Faster response for larger turns
    const float absDelta = (delta >= 0.0f) ? delta : -delta;
-    if (absDelta < 1.0f) {
-        return;
-    }
+    if (absDelta >= 1.0f) {
+        float alpha = 0.35f;
+        if (absDelta > 25.0f) {
+            alpha = 0.85f;
+        } else if (absDelta > 10.0f) {
+            alpha = 0.65f;
+        }

-    float alpha = 0.35f;
-    if (absDelta > 25.0f) {
-        alpha = 0.85f;
-    } else if (absDelta > 10.0f) {
-        alpha = 0.65f;
-    }
+        float step = delta * alpha;
+        const float maxStep = 12.0f;
+        if (step > maxStep) {
+            step = maxStep;
+        } else if (step < -maxStep) {
+            step = -maxStep;
+        }

-    float step = delta * alpha;
-    const float maxStep = 12.0f;
-    if (step > maxStep) {
-        step = maxStep;
-    } else if (step < -maxStep) {
-        step = -maxStep;
+        compassHeading = wrapHeading360(compassHeading + step);
    }
-
-    compassHeading = wrapHeading360(compassHeading + step);
 }

 // ==============================
--- a/src/motion/ICM42607PSensor.cpp
+++ b/src/motion/ICM42607PSensor.cpp
@@ -4,10 +4,16 @@

 #include "detect/ScanI2CTwoWire.h"
 #include <ICM42670P.h>
+#include <math.h>

 static constexpr uint16_t ICM42607P_ACCEL_ODR_HZ = 50;
 static constexpr uint16_t ICM42607P_ACCEL_FSR_G = 2;
-static constexpr float ICM42607P_COUNTS_PER_G = 32768.0f / ICM42607P_ACCEL_FSR_G;
+static constexpr float ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG_VALUE =
+#ifdef ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG
+    ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG;
+#else
+    0.0f;
+#endif

 #ifdef ICM_42607P_INT_PIN
 volatile static bool ICM42607P_IRQ = false;
@@ -18,10 +24,7 @@ void ICM42607PSetInterrupt()
 }
 #endif

-ICM42607PSensor::ICM42607PSensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice)
-{
-    wire = ScanI2CTwoWire::fetchI2CBus(foundDevice.address);
-}
+ICM42607PSensor::ICM42607PSensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {}

 ICM42607PSensor::~ICM42607PSensor() = default;

@@ -30,6 +33,7 @@ bool ICM42607PSensor::init()
    bool addressLsb = deviceAddress() == ICM42607P_ADDR_ALT;

    LOG_DEBUG("ICM-42607-P begin on addr 0x%02X (port=%d)", deviceAddress(), devicePort());
+    TwoWire *wire = ScanI2CTwoWire::fetchI2CBus(device.address);
    sensor.reset();
    auto newSensor = std::make_unique<ICM42670>(*wire, addressLsb);

@@ -82,8 +86,22 @@ int32_t ICM42607PSensor::runOnce()
        return MOTION_SENSOR_CHECK_INTERVAL_MS;
    }

-    // LOG_DEBUG("ICM-42607-P accel read x=%.3fg y=%.3fg z=%.3fg", (float)event.accel[0] / ICM42607P_COUNTS_PER_G,
-    //           (float)event.accel[1] / ICM42607P_COUNTS_PER_G, (float)event.accel[2] / ICM42607P_COUNTS_PER_G);
+    float ax = static_cast<float>(event.accel[0]);
+    float ay = static_cast<float>(event.accel[1]);
+    const float az = static_cast<float>(event.accel[2]);
+
+    if (ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG_VALUE != 0.0f) {
+        static const float rotRad = ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG_VALUE * DEG_TO_RAD;
+        static const float cosTheta = cosf(rotRad);
+        static const float sinTheta = sinf(rotRad);
+        const float rotatedX = (ax * cosTheta) - (ay * sinTheta);
+        const float rotatedY = (ax * sinTheta) + (ay * cosTheta);
+        ax = rotatedX;
+        ay = rotatedY;
+    }
+
+    // Match the accel sign convention used by other FusionCompass sensor paths.
+    publishCompassAccelSample(ax, -ay, -az);

    return MOTION_SENSOR_CHECK_INTERVAL_MS;
 #endif
--- a/src/motion/ICM42607PSensor.h
+++ b/src/motion/ICM42607PSensor.h
@@ -14,7 +14,6 @@ class ICM42607PSensor : public MotionSensor
 {
  private:
    std::unique_ptr<ICM42670> sensor;
-    TwoWire *wire = nullptr;

  public:
    explicit ICM42607PSensor(ScanI2C::FoundDevice foundDevice);
--- a/src/motion/MMC5983MASensor.cpp
+++ b/src/motion/MMC5983MASensor.cpp
@@ -2,6 +2,7 @@

 #if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C && __has_include(<SparkFun_MMC5983MA_Arduino_Library.h>)

+#include "Fusion/Fusion.h"
 #include "detect/ScanI2CTwoWire.h"

 #if !defined(MESHTASTIC_EXCLUDE_SCREEN)
@@ -10,8 +11,11 @@ extern graphics::Screen *screen;

 static constexpr float MMC5983MA_ZERO_FIELD = 131072.0f;
 static constexpr float MMC5983MA_COUNTS_PER_GAUSS = 16384.0f;
-static constexpr uint16_t MMC5983MA_CONTINUOUS_FREQUENCY_HZ = 10;
+static constexpr uint16_t MMC5983MA_CONTINUOUS_FREQUENCY_HZ = 50;
+static constexpr int32_t MMC5983MA_UPDATE_INTERVAL_MS = 20;
 static constexpr float MMC5983MA_HEADING_OFFSET_DEG = 180.0f;
+static constexpr uint32_t MMC5983MA_ACCEL_STALE_MS = 300;
+static constexpr float MMC5983MA_MIN_AXIS_RADIUS = 1e-4f;

 MMC5983MASensor::MMC5983MASensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {}

@@ -62,7 +66,7 @@ int32_t MMC5983MASensor::runOnce()
 {
    float magX = 0, magY = 0, magZ = 0;
    if (!readMagnetometer(magX, magY, magZ)) {
-        return MOTION_SENSOR_CHECK_INTERVAL_MS;
+        return MMC5983MA_UPDATE_INTERVAL_MS;
    }

 #if !defined(MESHTASTIC_EXCLUDE_SCREEN)
@@ -74,25 +78,53 @@ int32_t MMC5983MASensor::runOnce()
    }
 #endif

-    magX -= (highestX + lowestX) / 2;
-    magY -= (highestY + lowestY) / 2;
-    magZ -= (highestZ + lowestZ) / 2;
+    // Hard-iron bias removal.
+    magX -= (highestX + lowestX) * 0.5f;
+    magY -= (highestY + lowestY) * 0.5f;
+    magZ -= (highestZ + lowestZ) * 0.5f;
+
+    // Soft-iron diagonal scaling from calibration extrema.
+    const float radiusX = (highestX - lowestX) * 0.5f;
+    const float radiusY = (highestY - lowestY) * 0.5f;
+    const float radiusZ = (highestZ - lowestZ) * 0.5f;
+    const float avgRadius = (radiusX + radiusY + radiusZ) / 3.0f;
+    magX *= (radiusX > MMC5983MA_MIN_AXIS_RADIUS) ? (avgRadius / radiusX) : 1.0f;
+    magY *= (radiusY > MMC5983MA_MIN_AXIS_RADIUS) ? (avgRadius / radiusY) : 1.0f;
+    magZ *= (radiusZ > MMC5983MA_MIN_AXIS_RADIUS) ? (avgRadius / radiusZ) : 1.0f;

 #if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
-    float heading = atan2f(magY, magX) * RAD_TO_DEG + MMC5983MA_HEADING_OFFSET_DEG;
-    if (heading < 0.0f) {
-        heading += 360.0f;
-    } else if (heading >= 360.0f) {
-        heading -= 360.0f;
+    float heading;
+    float accelX = 0.0f;
+    float accelY = 0.0f;
+    float accelZ = 0.0f;
+    uint32_t accelAgeMs = 0;
+
+    if (getLatestCompassAccelSample(accelX, accelY, accelZ, accelAgeMs) && accelAgeMs <= MMC5983MA_ACCEL_STALE_MS) {
+        FusionVector ga = {.axis = {accelX, accelY, accelZ}};
+        FusionVector ma = {.axis = {magX, magY, magZ}};
+        if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
+            ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ);
+            ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
+        }
+        heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma) + MMC5983MA_HEADING_OFFSET_DEG;
+    } else {
+        heading = atan2f(magY, magX) * RAD_TO_DEG + MMC5983MA_HEADING_OFFSET_DEG;
    }

+    if (heading >= 360.0f)
+        heading -= 360.0f;
+    else if (heading < 0.0f)
+        heading += 360.0f;
+    heading = 360.0f - heading;
+    if (heading >= 360.0f)
+        heading -= 360.0f;
+
    heading = applyCompassOrientation(heading);
-    if (screen) {
+    if (screen)
        screen->setHeading(heading);
-    }
 #endif

-    return MOTION_SENSOR_CHECK_INTERVAL_MS;
+    return MMC5983MA_UPDATE_INTERVAL_MS;
 }

 void MMC5983MASensor::calibrate(uint16_t forSeconds)
--- a/src/motion/MagnetometerThread.h
+++ b/src/motion/MagnetometerThread.h
@@ -47,9 +47,8 @@ class MagnetometerThread : public concurrency::OSThread
    {
        canSleep = true;

-        if (isInitialised) {
+        if (isInitialised)
            return sensor->runOnce();
-        }

        return MOTION_SENSOR_CHECK_INTERVAL_MS;
    }
--- a/src/motion/MotionSensor.cpp
+++ b/src/motion/MotionSensor.cpp
@@ -2,6 +2,7 @@
 #include "FSCommon.h"
 #include "SPILock.h"
 #include "SafeFile.h"
+#include "concurrency/LockGuard.h"
 #include "graphics/draw/CompassRenderer.h"

 #if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C
@@ -30,6 +31,17 @@ bool isRangeValid(float highest, float lowest)
    // NaN/Inf guard without pulling in extra math helpers.
    return (highest == highest) && (lowest == lowest) && (highest > lowest);
 }
+
+struct CompassAccelSample {
+    float x = 0.0f;
+    float y = 0.0f;
+    float z = 0.0f;
+    uint32_t sampledAtMs = 0;
+    bool valid = false;
+};
+
+concurrency::Lock latestCompassAccelLock;
+CompassAccelSample latestCompassAccelSample;
 } // namespace

 // screen is defined in main.cpp
@@ -204,6 +216,35 @@ float MotionSensor::applyCompassOrientation(float heading)
    }
 }

+void MotionSensor::publishCompassAccelSample(float x, float y, float z)
+{
+    concurrency::LockGuard guard(&latestCompassAccelLock);
+    latestCompassAccelSample.x = x;
+    latestCompassAccelSample.y = y;
+    latestCompassAccelSample.z = z;
+    latestCompassAccelSample.sampledAtMs = millis();
+    latestCompassAccelSample.valid = true;
+}
+
+bool MotionSensor::getLatestCompassAccelSample(float &x, float &y, float &z, uint32_t &ageMs)
+{
+    uint32_t sampledAtMs = 0;
+    {
+        concurrency::LockGuard guard(&latestCompassAccelLock);
+        if (!latestCompassAccelSample.valid) {
+            return false;
+        }
+
+        x = latestCompassAccelSample.x;
+        y = latestCompassAccelSample.y;
+        z = latestCompassAccelSample.z;
+        sampledAtMs = latestCompassAccelSample.sampledAtMs;
+    }
+
+    ageMs = millis() - sampledAtMs;
+    return true;
+}
+
 #if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
 void MotionSensor::drawFrameCalibration(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
 {
--- a/src/motion/MotionSensor.h
+++ b/src/motion/MotionSensor.h
@@ -67,6 +67,8 @@ class MotionSensor
    static void updateCalibrationExtrema(float x, float y, float z, float &highestX, float &lowestX, float &highestY,
                                         float &lowestY, float &highestZ, float &lowestZ);
    static float applyCompassOrientation(float heading);
+    static void publishCompassAccelSample(float x, float y, float z);
+    static bool getLatestCompassAccelSample(float &x, float &y, float &z, uint32_t &ageMs);

    ScanI2C::FoundDevice device;

--- a/variants/nrf52840/heltec_mesh_node_t1/variant.h
+++ b/variants/nrf52840/heltec_mesh_node_t1/variant.h
@@ -18,21 +18,20 @@

 #ifndef _VARIANT_HELTEC_MESH_NODE_T1_
 #define _VARIANT_HELTEC_MESH_NODE_T1_
-/** Master clock frequency */
+
 #define VARIANT_MCK (64000000ul)
-
 #define USE_LFXO // Board uses 32khz crystal for LF

-/*----------------------------------------------------------------------------
- *        Headers
- *----------------------------------------------------------------------------*/
-
 #include "WVariant.h"

 #ifdef __cplusplus
 extern "C" {
 #endif // __cplusplus

+#define HELTEC_MESH_NODE_T1
+
+// Display (ST7735, 80x160 TFT via SPI1)
+
 #define ST7735_CS (0 + 12)
 #define ST7735_RS (0 + 22) // DC
 #define ST7735_SDA (0 + 24)
@@ -41,7 +40,7 @@ extern "C" {
 #define ST7735_MISO -1
 #define ST7735_BUSY -1
 #define ST7735_BL (0 + 15)
-#define VTFT_CTRL (0 + 13)  // Active HIGH, powers the ST7735 display
+#define VTFT_CTRL (0 + 13) // Active HIGH, powers the ST7735 display
 #define SPI_FREQUENCY 80000000
 #define SPI_READ_FREQUENCY 16000000
 #define SCREEN_ROTATE
@@ -50,51 +49,47 @@ extern "C" {
 #define TFT_OFFSET_X 24
 #define TFT_OFFSET_Y 0
 #define TFT_INVERT false
-#define SCREEN_TRANSITION_FRAMERATE 3 // fps
 #define DISPLAY_FORCE_SMALL_FONTS
+#define FORCE_LOW_RES 1 // 80px-wide panel causes artifacts with full-res UI elements
+
+// Pins

-// Number of pins defined in PinDescription array
 #define PINS_COUNT (48)
 #define NUM_DIGITAL_PINS (48)
 #define NUM_ANALOG_INPUTS (1)
 #define NUM_ANALOG_OUTPUTS (0)

 // LEDs
+
 #define PIN_LED1 (0 + 16)
 #define LED_BLUE PIN_LED1 // fake for bluefruit library
 #define LED_GREEN PIN_LED1
 #define LED_STATE_ON 0 // State when LED is lit

-/*
- * Buttons
- */
+// Buttons
+
 #define PIN_BUTTON1 (32 + 10)
 #define PIN_BUTTON2 (0 + 14)

-/*
-No longer populated on PCB
-*/
+// Serial (unused, not populated on PCB)
+
 #define PIN_SERIAL2_RX (-1)
 #define PIN_SERIAL2_TX (-1)

-/*
- * I2C
- */
+// I2C (ICM42607P IMU and MMC5983MA compass)

 #define WIRE_INTERFACES_COUNT 1
-
-// I2C bus 1
 #define PIN_WIRE_SDA (32 + 3)
 #define PIN_WIRE_SCL (0 + 10)

-#define PIN_SENSOR_EN (32 + 6)   // Power control pin for sensors
-#define PIN_SENSOR_EN_ACTIVE LOW // Power control active state
-// #define ICM_42607P_INT_PIN (32 + 1)  // ICM42607P INT1, Arduino pin 33 / nRF P1.01
-// #define ICM_42607P_INT2_PIN (32 + 7) // ICM42607P INT2, Arduino pin 39 / nRF P1.07
+#define PIN_SENSOR_EN (32 + 6) // Active LOW — controls IMU and compass VDD
+#define PIN_SENSOR_EN_ACTIVE LOW

-/*
- * Lora radio
- */
+// ICM42607P interrupt pins — populated on PCB, not yet used in firmware
+// #define ICM_42607P_INT_PIN  (32 + 1) // INT1 — P1.01
+// #define ICM_42607P_INT2_PIN (32 + 7) // INT2 — P1.07
+
+// LoRa (SX1262)

 #define USE_SX1262
 #define SX126X_CS (32 + 11) // FIXME - we really should define LORA_CS instead
@@ -105,46 +100,48 @@ No longer populated on PCB
 #define SX126X_DIO2_AS_RF_SWITCH
 #define SX126X_DIO3_TCXO_VOLTAGE 1.8

-/*
- * SPI Interfaces
- */
+// SPI
+
 #define SPI_INTERFACES_COUNT 2

-// For LORA, spi 0
+// SPI0 — LoRa
 #define PIN_SPI_MISO (0 + 3)
 #define PIN_SPI_MOSI (32 + 14)
 #define PIN_SPI_SCK (32 + 13)

-#define PIN_SPI1_MISO ST7735_MISO 
+// SPI1 — Display (ST7735, write-only)
+#define PIN_SPI1_MISO ST7735_MISO
 #define PIN_SPI1_MOSI ST7735_SDA
-#define PIN_SPI1_SCK  ST7735_SCK
+#define PIN_SPI1_SCK ST7735_SCK
+
+// GPS (UC6580)

-/*
- * GPS pins
- */
 #define GPS_UC6580
 #define GPS_BAUDRATE 115200
 #define PIN_GPS_RESET (0 + 26)
 #define GPS_RESET_MODE LOW
 #define PIN_GPS_EN (0 + 4)
 #define GPS_EN_ACTIVE LOW
-#define PERIPHERAL_WARMUP_MS 1000 // Make sure I2C QuickLink has stable power before continuing
+#define PERIPHERAL_WARMUP_MS 1000 // Allow I2C bus to stabilise after sensor power-on
 #define PIN_GPS_PPS (32 + 9)      // Pulse per second input from the GPS
 #define GPS_TX_PIN (0 + 7)
 #define GPS_RX_PIN (0 + 8)
-
 #define GPS_THREAD_INTERVAL 50

 #define PIN_SERIAL1_RX GPS_RX_PIN
 #define PIN_SERIAL1_TX GPS_TX_PIN

+// Buzzer
+
 #define PIN_BUZZER (0 + 9)
 #define PIN_BUZZER_VOLTAGE_MULTIPLIER_1 (32 + 2)
 #define PIN_BUZZER_VOLTAGE_MULTIPLIER_2 (32 + 5)

+// Battery / ADC
+
 #define ADC_CTRL 11
 #define ADC_CTRL_ENABLED HIGH
-#define BATTERY_PIN 5
+#define BATTERY_PIN 5 // nRF52840 AIN3
 #define ADC_RESOLUTION 14

 #define BATTERY_SENSE_RESOLUTION_BITS 12
@@ -154,24 +151,15 @@ No longer populated on PCB
 #define VBAT_AR_INTERNAL AR_INTERNAL_3_0
 #define ADC_MULTIPLIER (4.916F)

-// nrf52840 AIN3 = Pin 5
-// commented out due to power leakage of 2.9mA in shutdown state see reported issue #8801
-#define BATTERY_LPCOMP_INPUT NRF_LPCOMP_INPUT_3
+// #define BATTERY_LPCOMP_INPUT NRF_LPCOMP_INPUT_3 // UNSAFE: causes 2.9 mA deep-sleep leakage (issue #8801)

-// We have AIN3 with a VBAT divider so AIN3 = VBAT * (100/490)
-// We have the device going deep sleep under 3.1V, which is AIN3 = 0.63V
-// So we can wake up when VBAT>=VDD is restored to 3.3V, where AIN3 = 0.67V
-// Ratio 0.67/3.3 = 0.20, so we can pick a bit higher, 2/8 VDD, which means
-// VBAT=4.04V
-#define BATTERY_LPCOMP_THRESHOLD NRF_LPCOMP_REF_SUPPLY_2_8
+// Power / USB
+
+#define NRF_APM   // USB VBUS detection via nrfx_power_usbstatus_get() — no dedicated charging IC on this board
+#define HAS_RTC 0 // No external RTC fitted

-#define HAS_RTC 0
 #ifdef __cplusplus
 }
 #endif

-/*----------------------------------------------------------------------------
- *        Arduino objects - C++ only
- *----------------------------------------------------------------------------*/
-
 #endif