diff --git a/src/graphics/Screen.cpp b/src/graphics/Screen.cpp index 1f7e8c079..bc65a08c8 100644 --- 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); } // ============================== diff --git a/src/motion/ICM42607PSensor.cpp b/src/motion/ICM42607PSensor.cpp index 5cd565fea..4d3f54c99 100644 --- a/src/motion/ICM42607PSensor.cpp +++ b/src/motion/ICM42607PSensor.cpp @@ -4,10 +4,16 @@ #include "detect/ScanI2CTwoWire.h" #include +#include 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(*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(event.accel[0]); + float ay = static_cast(event.accel[1]); + const float az = static_cast(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 diff --git a/src/motion/ICM42607PSensor.h b/src/motion/ICM42607PSensor.h index 370fef276..c4725d827 100644 --- a/src/motion/ICM42607PSensor.h +++ b/src/motion/ICM42607PSensor.h @@ -14,7 +14,6 @@ class ICM42607PSensor : public MotionSensor { private: std::unique_ptr sensor; - TwoWire *wire = nullptr; public: explicit ICM42607PSensor(ScanI2C::FoundDevice foundDevice); diff --git a/src/motion/MMC5983MASensor.cpp b/src/motion/MMC5983MASensor.cpp index ba09f4648..3789c6136 100644 --- a/src/motion/MMC5983MASensor.cpp +++ b/src/motion/MMC5983MASensor.cpp @@ -2,6 +2,7 @@ #if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C && __has_include() +#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) diff --git a/src/motion/MagnetometerThread.h b/src/motion/MagnetometerThread.h index affd99e94..1f558eb57 100644 --- 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; } diff --git a/src/motion/MotionSensor.cpp b/src/motion/MotionSensor.cpp index 83231aea9..878b4dd50 100755 --- 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) { diff --git a/src/motion/MotionSensor.h b/src/motion/MotionSensor.h index 71b71f73a..c898faf51 100755 --- 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; diff --git a/variants/nrf52840/heltec_mesh_node_t1/variant.h b/variants/nrf52840/heltec_mesh_node_t1/variant.h index dc84a0487..c2808ad54 100644 --- 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