From 5002139635d336f3c7cf0242582a96322c8bfffd Mon Sep 17 00:00:00 2001 From: David <2941290+dhskinner@users.noreply.github.com> Date: Mon, 23 Sep 2024 23:06:39 +1000 Subject: [PATCH 1/8] Initial upload --- src/AccelerometerThread.h | 319 ------------------------------- src/configuration.h | 2 + src/detect/ScanI2C.cpp | 4 +- src/detect/ScanI2C.h | 10 +- src/detect/ScanI2CTwoWire.cpp | 21 +- src/main.cpp | 7 +- src/main.h | 6 +- src/modules/AdminModule.cpp | 2 +- src/motion/AccelerometerThread.h | 136 +++++++++++++ src/motion/BMA423Sensor.cpp | 62 ++++++ src/motion/BMA423Sensor.h | 26 +++ src/motion/BMX160Sensor.cpp | 100 ++++++++++ src/motion/BMX160Sensor.h | 41 ++++ src/motion/ICM20948Sensor.cpp | 190 ++++++++++++++++++ src/motion/ICM20948Sensor.h | 96 ++++++++++ src/motion/LIS3DHSensor.cpp | 36 ++++ src/motion/LIS3DHSensor.h | 24 +++ src/motion/LSM6DS3Sensor.cpp | 33 ++++ src/motion/LSM6DS3Sensor.h | 28 +++ src/motion/MPU6050Sensor.cpp | 31 +++ src/motion/MPU6050Sensor.h | 24 +++ src/motion/MotionSensor.cpp | 79 ++++++++ src/motion/MotionSensor.h | 85 ++++++++ src/motion/STK8XXXSensor.cpp | 40 ++++ src/motion/STK8XXXSensor.h | 37 ++++ 25 files changed, 1104 insertions(+), 335 deletions(-) delete mode 100644 src/AccelerometerThread.h create mode 100755 src/motion/AccelerometerThread.h create mode 100755 src/motion/BMA423Sensor.cpp create mode 100755 src/motion/BMA423Sensor.h create mode 100755 src/motion/BMX160Sensor.cpp create mode 100755 src/motion/BMX160Sensor.h create mode 100755 src/motion/ICM20948Sensor.cpp create mode 100755 src/motion/ICM20948Sensor.h create mode 100755 src/motion/LIS3DHSensor.cpp create mode 100755 src/motion/LIS3DHSensor.h create mode 100755 src/motion/LSM6DS3Sensor.cpp create mode 100755 src/motion/LSM6DS3Sensor.h create mode 100755 src/motion/MPU6050Sensor.cpp create mode 100755 src/motion/MPU6050Sensor.h create mode 100755 src/motion/MotionSensor.cpp create mode 100755 src/motion/MotionSensor.h create mode 100755 src/motion/STK8XXXSensor.cpp create mode 100755 src/motion/STK8XXXSensor.h diff --git a/src/AccelerometerThread.h b/src/AccelerometerThread.h deleted file mode 100644 index 629d63c6a5..0000000000 --- a/src/AccelerometerThread.h +++ /dev/null @@ -1,319 +0,0 @@ -#pragma once -#include "configuration.h" - -#if !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR - -#include "PowerFSM.h" -#include "concurrency/OSThread.h" -#include "main.h" -#include "power.h" - -#include -#include -#include -#ifdef STK8XXX_INT -#include -#endif -#include -#include -#include -#ifdef RAK_4631 -#include "Fusion/Fusion.h" -#include "graphics/Screen.h" -#include "graphics/ScreenFonts.h" -#include -#endif - -#define ACCELEROMETER_CHECK_INTERVAL_MS 100 -#define ACCELEROMETER_CLICK_THRESHOLD 40 - -volatile static bool STK_IRQ; - -static inline int readRegister(uint8_t address, uint8_t reg, uint8_t *data, uint8_t len) -{ - Wire.beginTransmission(address); - Wire.write(reg); - Wire.endTransmission(); - Wire.requestFrom((uint8_t)address, (uint8_t)len); - uint8_t i = 0; - while (Wire.available()) { - data[i++] = Wire.read(); - } - return 0; // Pass -} - -static inline int writeRegister(uint8_t address, uint8_t reg, uint8_t *data, uint8_t len) -{ - Wire.beginTransmission(address); - Wire.write(reg); - Wire.write(data, len); - return (0 != Wire.endTransmission()); -} - -class AccelerometerThread : public concurrency::OSThread -{ - public: - explicit AccelerometerThread(ScanI2C::DeviceType type) : OSThread("AccelerometerThread") - { - if (accelerometer_found.port == ScanI2C::I2CPort::NO_I2C) { - LOG_DEBUG("AccelerometerThread disabling due to no sensors found\n"); - disable(); - return; - } - acceleremoter_type = type; -#ifndef RAK_4631 - if (!config.display.wake_on_tap_or_motion && !config.device.double_tap_as_button_press) { - LOG_DEBUG("AccelerometerThread disabling due to no interested configurations\n"); - disable(); - return; - } -#endif - init(); - } - - void start() - { - init(); - setIntervalFromNow(0); - }; - - protected: - int32_t runOnce() override - { - canSleep = true; // Assume we should not keep the board awake - - if (acceleremoter_type == ScanI2C::DeviceType::MPU6050 && mpu.getMotionInterruptStatus()) { - wakeScreen(); - } else if (acceleremoter_type == ScanI2C::DeviceType::STK8BAXX && STK_IRQ) { - STK_IRQ = false; - if (config.display.wake_on_tap_or_motion) { - wakeScreen(); - } - } else if (acceleremoter_type == ScanI2C::DeviceType::LIS3DH && lis.getClick() > 0) { - uint8_t click = lis.getClick(); - if (!config.device.double_tap_as_button_press) { - wakeScreen(); - } - - if (config.device.double_tap_as_button_press && (click & 0x20)) { - buttonPress(); - return 500; - } - } else if (acceleremoter_type == ScanI2C::DeviceType::BMA423 && bmaSensor.readIrqStatus() != DEV_WIRE_NONE) { - if (bmaSensor.isTilt() || bmaSensor.isDoubleTap()) { - wakeScreen(); - return 500; - } -#ifdef RAK_4631 - } else if (acceleremoter_type == ScanI2C::DeviceType::BMX160) { - sBmx160SensorData_t magAccel; - sBmx160SensorData_t gAccel; - - /* Get a new sensor event */ - bmx160.getAllData(&magAccel, NULL, &gAccel); - - // expirimental calibrate routine. Limited to between 10 and 30 seconds after boot - if (millis() > 12 * 1000 && millis() < 30 * 1000) { - if (!showingScreen) { - showingScreen = true; - screen->startAlert((FrameCallback)drawFrameCalibration); - } - if (magAccel.x > highestX) - highestX = magAccel.x; - if (magAccel.x < lowestX) - lowestX = magAccel.x; - if (magAccel.y > highestY) - highestY = magAccel.y; - if (magAccel.y < lowestY) - lowestY = magAccel.y; - if (magAccel.z > highestZ) - highestZ = magAccel.z; - if (magAccel.z < lowestZ) - lowestZ = magAccel.z; - } else if (showingScreen && millis() >= 30 * 1000) { - showingScreen = false; - screen->endAlert(); - } - - int highestRealX = highestX - (highestX + lowestX) / 2; - - magAccel.x -= (highestX + lowestX) / 2; - magAccel.y -= (highestY + lowestY) / 2; - magAccel.z -= (highestZ + lowestZ) / 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; - - // If we're set to one of the inverted positions - if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) { - ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ); - ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ); - } - - float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma); - - switch (config.display.compass_orientation) { - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0_INVERTED: - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0: - break; - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90: - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90_INVERTED: - heading += 90; - break; - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180: - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180_INVERTED: - heading += 180; - break; - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270: - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED: - heading += 270; - break; - } - - screen->setHeading(heading); - -#endif - } else if (acceleremoter_type == ScanI2C::DeviceType::LSM6DS3 && lsm.shake()) { - wakeScreen(); - return 500; - } - - return ACCELEROMETER_CHECK_INTERVAL_MS; - } - - private: - void init() - { - LOG_DEBUG("AccelerometerThread initializing\n"); - - if (acceleremoter_type == ScanI2C::DeviceType::MPU6050 && mpu.begin(accelerometer_found.address)) { - LOG_DEBUG("MPU6050 initializing\n"); - // setup motion detection - mpu.setHighPassFilter(MPU6050_HIGHPASS_0_63_HZ); - mpu.setMotionDetectionThreshold(1); - mpu.setMotionDetectionDuration(20); - mpu.setInterruptPinLatch(true); // Keep it latched. Will turn off when reinitialized. - mpu.setInterruptPinPolarity(true); -#ifdef STK8XXX_INT - } else if (acceleremoter_type == ScanI2C::DeviceType::STK8BAXX && stk8baxx.STK8xxx_Initialization(STK8xxx_VAL_RANGE_2G)) { - STK_IRQ = false; - LOG_DEBUG("STX8BAxx initialized\n"); - stk8baxx.STK8xxx_Anymotion_init(); - pinMode(STK8XXX_INT, INPUT_PULLUP); - attachInterrupt( - digitalPinToInterrupt(STK8XXX_INT), [] { STK_IRQ = true; }, RISING); -#endif - } else if (acceleremoter_type == ScanI2C::DeviceType::LIS3DH && lis.begin(accelerometer_found.address)) { - LOG_DEBUG("LIS3DH initializing\n"); - lis.setRange(LIS3DH_RANGE_2_G); - // Adjust threshold, higher numbers are less sensitive - lis.setClick(config.device.double_tap_as_button_press ? 2 : 1, ACCELEROMETER_CLICK_THRESHOLD); - } else if (acceleremoter_type == ScanI2C::DeviceType::BMA423 && - bmaSensor.begin(accelerometer_found.address, &readRegister, &writeRegister)) { - LOG_DEBUG("BMA423 initializing\n"); - bmaSensor.configAccelerometer(bmaSensor.RANGE_2G, bmaSensor.ODR_100HZ, bmaSensor.BW_NORMAL_AVG4, - bmaSensor.PERF_CONTINUOUS_MODE); - bmaSensor.enableAccelerometer(); - bmaSensor.configInterrupt(BMA4_LEVEL_TRIGGER, BMA4_ACTIVE_HIGH, BMA4_PUSH_PULL, BMA4_OUTPUT_ENABLE, - BMA4_INPUT_DISABLE); - -#ifdef BMA423_INT - pinMode(BMA4XX_INT, INPUT); - attachInterrupt( - BMA4XX_INT, - [] { - // Set interrupt to set irq value to true - BMA_IRQ = true; - }, - RISING); // Select the interrupt mode according to the actual circuit -#endif - -#ifdef T_WATCH_S3 - // Need to raise the wrist function, need to set the correct axis - bmaSensor.setReampAxes(bmaSensor.REMAP_TOP_LAYER_RIGHT_CORNER); -#else - bmaSensor.setReampAxes(bmaSensor.REMAP_BOTTOM_LAYER_BOTTOM_LEFT_CORNER); -#endif - // bmaSensor.enableFeature(bmaSensor.FEATURE_STEP_CNTR, true); - bmaSensor.enableFeature(bmaSensor.FEATURE_TILT, true); - bmaSensor.enableFeature(bmaSensor.FEATURE_WAKEUP, true); - // bmaSensor.resetPedometer(); - - // Turn on feature interrupt - bmaSensor.enablePedometerIRQ(); - bmaSensor.enableTiltIRQ(); - // It corresponds to isDoubleClick interrupt - bmaSensor.enableWakeupIRQ(); -#ifdef RAK_4631 - } else if (acceleremoter_type == ScanI2C::DeviceType::BMX160 && bmx160.begin()) { - bmx160.ODR_Config(BMX160_ACCEL_ODR_100HZ, BMX160_GYRO_ODR_100HZ); // set output data rate - -#endif - } else if (acceleremoter_type == ScanI2C::DeviceType::LSM6DS3 && lsm.begin_I2C(accelerometer_found.address)) { - LOG_DEBUG("LSM6DS3 initializing\n"); - // Default threshold of 2G, less sensitive options are 4, 8 or 16G - lsm.setAccelRange(LSM6DS_ACCEL_RANGE_2_G); -#ifndef LSM6DS3_WAKE_THRESH -#define LSM6DS3_WAKE_THRESH 20 -#endif - lsm.enableWakeup(config.display.wake_on_tap_or_motion, 1, LSM6DS3_WAKE_THRESH); - // Duration is number of occurances needed to trigger, higher threshold is less sensitive - } - } - void wakeScreen() - { - if (powerFSM.getState() == &stateDARK) { - LOG_INFO("Tap or motion detected. Turning on screen\n"); - powerFSM.trigger(EVENT_INPUT); - } - } - - void buttonPress() - { - LOG_DEBUG("Double-tap detected. Firing button press\n"); - powerFSM.trigger(EVENT_PRESS); - } - - ScanI2C::DeviceType acceleremoter_type; - Adafruit_MPU6050 mpu; - Adafruit_LIS3DH lis; -#ifdef STK8XXX_INT - STK8xxx stk8baxx; -#endif - Adafruit_LSM6DS3TRC lsm; - SensorBMA423 bmaSensor; - bool BMA_IRQ = false; -#ifdef RAK_4631 - bool showingScreen = false; - RAK_BMX160 bmx160; - float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0; - - static void drawFrameCalibration(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\nCompass"); - int16_t compassX = 0, compassY = 0; - uint16_t compassDiam = graphics::Screen::getCompassDiam(display->getWidth(), display->getHeight()); - - // coordinates for the center of the compass/circle - if (config.display.displaymode == meshtastic_Config_DisplayConfig_DisplayMode_DEFAULT) { - compassX = x + display->getWidth() - compassDiam / 2 - 5; - compassY = y + display->getHeight() / 2; - } else { - compassX = x + display->getWidth() - compassDiam / 2 - 5; - compassY = y + FONT_HEIGHT_SMALL + (display->getHeight() - FONT_HEIGHT_SMALL) / 2; - } - display->drawCircle(compassX, compassY, compassDiam / 2); - screen->drawCompassNorth(display, compassX, compassY, screen->getHeading() * PI / 180); - } -#endif -}; - -#endif \ No newline at end of file diff --git a/src/configuration.h b/src/configuration.h index 349bd2870f..4a3deaca44 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -150,6 +150,8 @@ along with this program. If not, see . #define BMA423_ADDR 0x19 #define LSM6DS3_ADDR 0x6A #define BMX160_ADDR 0x69 +#define ICM20948_ADDR 0x69 +#define ICM20948_ADDR_ALT 0x68 // ----------------------------------------------------------------------------- // LED diff --git a/src/detect/ScanI2C.cpp b/src/detect/ScanI2C.cpp index eaba62a78a..a9d70edaa9 100644 --- a/src/detect/ScanI2C.cpp +++ b/src/detect/ScanI2C.cpp @@ -37,8 +37,8 @@ ScanI2C::FoundDevice ScanI2C::firstKeyboard() const ScanI2C::FoundDevice ScanI2C::firstAccelerometer() const { - ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3, BMX160, STK8BAXX}; - return firstOfOrNONE(6, types); + ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3, BMX160, STK8BAXX, ICM20948}; + return firstOfOrNONE(7, types); } ScanI2C::FoundDevice ScanI2C::find(ScanI2C::DeviceType) const diff --git a/src/detect/ScanI2C.h b/src/detect/ScanI2C.h index 090b1a9682..5e8c9824da 100644 --- a/src/detect/ScanI2C.h +++ b/src/detect/ScanI2C.h @@ -56,7 +56,8 @@ class ScanI2C DFROBOT_LARK, NAU7802, FT6336U, - STK8BAXX + STK8BAXX, + ICM20948 } DeviceType; // typedef uint8_t DeviceAddress; @@ -67,8 +68,9 @@ class ScanI2C } I2CPort; typedef struct DeviceAddress { - I2CPort port; - uint8_t address; + // set default values for ADDRESS_NONE + I2CPort port = I2CPort::NO_I2C; + uint8_t address = 0; explicit DeviceAddress(I2CPort port, uint8_t address); DeviceAddress(); @@ -119,4 +121,4 @@ class ScanI2C private: bool shouldSuppressScreen = false; -}; +}; \ No newline at end of file diff --git a/src/detect/ScanI2CTwoWire.cpp b/src/detect/ScanI2CTwoWire.cpp index f09eb3b95c..cd43960d1c 100644 --- a/src/detect/ScanI2CTwoWire.cpp +++ b/src/detect/ScanI2CTwoWire.cpp @@ -383,10 +383,7 @@ void ScanI2CTwoWire::scanPort(I2CPort port, uint8_t *address, uint8_t asize) SCAN_SIMPLE_CASE(QMC5883L_ADDR, QMC5883L, "QMC5883L Highrate 3-Axis magnetic sensor found\n") SCAN_SIMPLE_CASE(HMC5883L_ADDR, HMC5883L, "HMC5883L 3-Axis digital compass found\n") - SCAN_SIMPLE_CASE(PMSA0031_ADDR, PMSA0031, "PMSA0031 air quality sensor found\n") - SCAN_SIMPLE_CASE(MPU6050_ADDR, MPU6050, "MPU6050 accelerometer found\n"); - SCAN_SIMPLE_CASE(BMX160_ADDR, BMX160, "BMX160 accelerometer found\n"); SCAN_SIMPLE_CASE(BMA423_ADDR, BMA423, "BMA423 accelerometer found\n"); SCAN_SIMPLE_CASE(LSM6DS3_ADDR, LSM6DS3, "LSM6DS3 accelerometer found at address 0x%x\n", (uint8_t)addr.address); SCAN_SIMPLE_CASE(TCA9535_ADDR, TCA9535, "TCA9535 I2C expander found\n"); @@ -398,6 +395,24 @@ void ScanI2CTwoWire::scanPort(I2CPort port, uint8_t *address, uint8_t asize) SCAN_SIMPLE_CASE(NAU7802_ADDR, NAU7802, "NAU7802 based scale found\n"); SCAN_SIMPLE_CASE(FT6336U_ADDR, FT6336U, "FT6336U touchscreen found\n"); + case ICM20948_ADDR: // same as BMX160_ADDR + case ICM20948_ADDR_ALT: // same as MPU6050_ADDR + registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x00), 1); + if (registerValue == 0xEA) { + type = ICM20948; + LOG_INFO("ICM20948 9-dof motion processor found\n"); + break; + } else if (addr.address == BMX160_ADDR) { + type = BMX160; + LOG_INFO("BMX160 accelerometer found\n"); + break; + } else { + type = MPU6050; + LOG_INFO("MPU6050 accelerometer found\n"); + break; + } + break; + default: LOG_INFO("Device found at address 0x%x was not able to be enumerated\n", addr.address); } diff --git a/src/main.cpp b/src/main.cpp index e24ba68b35..a6bffcd01f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -102,8 +102,8 @@ NRF52Bluetooth *nrf52Bluetooth = nullptr; #include "AmbientLightingThread.h" #include "PowerFSMThread.h" -#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR -#include "AccelerometerThread.h" +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C +#include "motion/AccelerometerThread.h" AccelerometerThread *accelerometerThread = nullptr; #endif @@ -570,6 +570,7 @@ void setup() SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::SHT4X, meshtastic_TelemetrySensorType_SHT4X) SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::AHT10, meshtastic_TelemetrySensorType_AHT10) SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::DFROBOT_LARK, meshtastic_TelemetrySensorType_DFROBOT_LARK) + SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::ICM20948, meshtastic_TelemetrySensorType_ICM20948) i2cScanner.reset(); #endif @@ -637,7 +638,7 @@ void setup() #endif #if !MESHTASTIC_EXCLUDE_I2C -#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) if (acc_info.type != ScanI2C::DeviceType::NONE) { accelerometerThread = new AccelerometerThread(acc_info.type); } diff --git a/src/main.h b/src/main.h index 52a3fff1f8..5722f7cf0c 100644 --- a/src/main.h +++ b/src/main.h @@ -56,8 +56,8 @@ extern AudioThread *audioThread; // Global Screen singleton. extern graphics::Screen *screen; -#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR -#include "AccelerometerThread.h" +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C +#include "motion/AccelerometerThread.h" extern AccelerometerThread *accelerometerThread; #endif @@ -86,4 +86,4 @@ void nrf52Setup(), esp32Setup(), nrf52Loop(), esp32Loop(), rp2040Setup(), clearB meshtastic_DeviceMetadata getDeviceMetadata(); // We default to 4MHz SPI, SPI mode 0 -extern SPISettings spiSettings; +extern SPISettings spiSettings; \ No newline at end of file diff --git a/src/modules/AdminModule.cpp b/src/modules/AdminModule.cpp index 48048b0547..64cf12aa43 100644 --- a/src/modules/AdminModule.cpp +++ b/src/modules/AdminModule.cpp @@ -35,7 +35,7 @@ #endif #if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR -#include "AccelerometerThread.h" +#include "motion/AccelerometerThread.h" #endif AdminModule *adminModule; diff --git a/src/motion/AccelerometerThread.h b/src/motion/AccelerometerThread.h new file mode 100755 index 0000000000..a4daf345e4 --- /dev/null +++ b/src/motion/AccelerometerThread.h @@ -0,0 +1,136 @@ +#pragma once +#ifndef _ACCELEROMETER_H_ +#define _ACCELEROMETER_H_ + +#include "configuration.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#include "../concurrency/OSThread.h" +#include "BMA423Sensor.h" +#include "BMX160Sensor.h" +#include "ICM20948Sensor.h" +#include "LIS3DHSensor.h" +#include "LSM6DS3Sensor.h" +#include "MPU6050Sensor.h" +#include "MotionSensor.h" +#include "STK8XXXSensor.h" + +extern ScanI2C::DeviceAddress accelerometer_found; + +class AccelerometerThread : public concurrency::OSThread +{ + private: + MotionSensor *sensor = nullptr; + bool isInitialised = false; + + public: + explicit AccelerometerThread(ScanI2C::FoundDevice foundDevice) : OSThread("AccelerometerThread") + { + device = foundDevice; + init(); + } + + explicit AccelerometerThread(ScanI2C::DeviceType type) : AccelerometerThread(ScanI2C::FoundDevice{type, accelerometer_found}) + { + } + + void start() + { + init(); + setIntervalFromNow(0); + }; + + protected: + int32_t runOnce() override + { + // Assume we should not keep the board awake + canSleep = true; + + if (isInitialised) + return sensor->runOnce(); + + return MOTION_SENSOR_CHECK_INTERVAL_MS; + } + + private: + ScanI2C::FoundDevice device; + + void init() + { + if (isInitialised) + return; + + if (device.address.port == ScanI2C::I2CPort::NO_I2C || device.address.address == 0 || device.type == ScanI2C::NONE) { + LOG_DEBUG("AccelerometerThread disabling due to no sensors found\n"); + disable(); + return; + } + +#ifndef RAK_4631 + if (!config.display.wake_on_tap_or_motion && !config.device.double_tap_as_button_press) { + LOG_DEBUG("AccelerometerThread disabling due to no interested configurations\n"); + disable(); + return; + } +#endif + + switch (device.type) { + case ScanI2C::DeviceType::BMA423: + sensor = new BMA423Sensor(device); + break; + case ScanI2C::DeviceType::MPU6050: + sensor = new MPU6050Sensor(device); + break; + case ScanI2C::DeviceType::BMX160: + sensor = new BMX160Sensor(device); + break; + case ScanI2C::DeviceType::LIS3DH: + sensor = new LIS3DHSensor(device); + break; + case ScanI2C::DeviceType::LSM6DS3: + sensor = new LSM6DS3Sensor(device); + break; + case ScanI2C::DeviceType::STK8BAXX: + sensor = new STK8XXXSensor(device); + break; +#ifndef ARCH_NRF52 + case ScanI2C::DeviceType::ICM20948: + sensor = new ICM20948Sensor(device); + break; +#endif + default: + disable(); + return; + } + + isInitialised = sensor->init(); + if (!isInitialised) { + clean(); + } + LOG_DEBUG("AccelerometerThread::init %s\n", isInitialised ? "ok" : "failed"); + } + + // Copy constructor (not implemented / included to avoid cppcheck warnings) + AccelerometerThread(const AccelerometerThread &other) : OSThread::OSThread("AccelerometerThread") {} + + // Destructor (included to avoid cppcheck warnings) + virtual ~AccelerometerThread() { clean(); } + + // Copy assignment (not implemented / included to avoid cppcheck warnings) + AccelerometerThread &operator=(const AccelerometerThread &other) { return *this; } + + // Cleanup resources + void clean() + { + isInitialised = false; + if (sensor != nullptr) { + delete sensor; + sensor = nullptr; + } + } +}; + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/BMA423Sensor.cpp b/src/motion/BMA423Sensor.cpp new file mode 100755 index 0000000000..ec07b7c40c --- /dev/null +++ b/src/motion/BMA423Sensor.cpp @@ -0,0 +1,62 @@ +#include "BMA423Sensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +using namespace MotionSensorI2C; + +BMA423Sensor::BMA423Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {} + +bool BMA423Sensor::init() +{ + if (sensor.begin(deviceAddress(), &MotionSensorI2C::readRegister, &MotionSensorI2C::writeRegister)) { + sensor.configAccelerometer(sensor.RANGE_2G, sensor.ODR_100HZ, sensor.BW_NORMAL_AVG4, sensor.PERF_CONTINUOUS_MODE); + sensor.enableAccelerometer(); + sensor.configInterrupt(BMA4_LEVEL_TRIGGER, BMA4_ACTIVE_HIGH, BMA4_PUSH_PULL, BMA4_OUTPUT_ENABLE, BMA4_INPUT_DISABLE); + +#ifdef BMA423_INT + pinMode(BMA4XX_INT, INPUT); + attachInterrupt( + BMA4XX_INT, + [] { + // Set interrupt to set irq value to true + BMA_IRQ = true; + }, + RISING); // Select the interrupt mode according to the actual circuit +#endif + +#ifdef T_WATCH_S3 + // Need to raise the wrist function, need to set the correct axis + sensor.setReampAxes(sensor.REMAP_TOP_LAYER_RIGHT_CORNER); +#else + sensor.setReampAxes(sensor.REMAP_BOTTOM_LAYER_BOTTOM_LEFT_CORNER); +#endif + // sensor.enableFeature(sensor.FEATURE_STEP_CNTR, true); + sensor.enableFeature(sensor.FEATURE_TILT, true); + sensor.enableFeature(sensor.FEATURE_WAKEUP, true); + // sensor.resetPedometer(); + + // Turn on feature interrupt + sensor.enablePedometerIRQ(); + sensor.enableTiltIRQ(); + + // It corresponds to isDoubleClick interrupt + sensor.enableWakeupIRQ(); + LOG_DEBUG("BMA423Sensor::init ok\n"); + return true; + } + LOG_DEBUG("BMA423Sensor::init failed\n"); + return false; +} + +int32_t BMA423Sensor::runOnce() +{ + if (sensor.readIrqStatus() != DEV_WIRE_NONE) { + if (sensor.isTilt() || sensor.isDoubleTap()) { + wakeScreen(); + return 500; + } + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#endif \ No newline at end of file diff --git a/src/motion/BMA423Sensor.h b/src/motion/BMA423Sensor.h new file mode 100755 index 0000000000..0bc0ddf8cd --- /dev/null +++ b/src/motion/BMA423Sensor.h @@ -0,0 +1,26 @@ +#pragma once +#ifndef _BMA423_SENSOR_H_ +#define _BMA423_SENSOR_H_ + +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#include +#include + +class BMA423Sensor : public MotionSensor +{ + private: + SensorBMA423 sensor; + volatile bool BMA_IRQ = false; + + public: + explicit BMA423Sensor(ScanI2C::FoundDevice foundDevice); + virtual bool init() override; + virtual int32_t runOnce() override; +}; + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp new file mode 100755 index 0000000000..f18c4b586f --- /dev/null +++ b/src/motion/BMX160Sensor.cpp @@ -0,0 +1,100 @@ +#include "BMX160Sensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +BMX160Sensor::BMX160Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {} + +#ifdef RAK_4631 + +// screen is defined in main.cpp +extern graphics::Screen *screen; + +bool BMX160Sensor::init() +{ + if (sensor.begin()) { + // set output data rate + sensor.ODR_Config(BMX160_ACCEL_ODR_100HZ, BMX160_GYRO_ODR_100HZ); + LOG_DEBUG("BMX160Sensor::init ok\n"); + return true; + } + LOG_DEBUG("BMX160Sensor::init failed\n"); + return false; +} + +int32_t BMX160Sensor::runOnce() +{ + sBmx160SensorData_t magAccel; + sBmx160SensorData_t gAccel; + + /* Get a new sensor event */ + sensor.getAllData(&magAccel, NULL, &gAccel); + + // experimental calibrate routine. Limited to between 10 and 30 seconds after boot + if (millis() > 12 * 1000 && millis() < 30 * 1000) { + if (!showingScreen) { + showingScreen = true; + screen->startAlert((FrameCallback)drawFrameCalibration); + } + if (magAccel.x > highestX) + highestX = magAccel.x; + if (magAccel.x < lowestX) + lowestX = magAccel.x; + if (magAccel.y > highestY) + highestY = magAccel.y; + if (magAccel.y < lowestY) + lowestY = magAccel.y; + if (magAccel.z > highestZ) + highestZ = magAccel.z; + if (magAccel.z < lowestZ) + lowestZ = magAccel.z; + } else if (showingScreen && millis() >= 30 * 1000) { + showingScreen = false; + screen->endAlert(); + } + + int highestRealX = highestX - (highestX + lowestX) / 2; + + magAccel.x -= (highestX + lowestX) / 2; + magAccel.y -= (highestY + lowestY) / 2; + magAccel.z -= (highestZ + lowestZ) / 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; + + // If we're set to one of the inverted positions + if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) { + ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ); + ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ); + } + + float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma); + + switch (config.display.compass_orientation) { + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0_INVERTED: + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0: + break; + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90: + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90_INVERTED: + heading += 90; + break; + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180: + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180_INVERTED: + heading += 180; + break; + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270: + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED: + heading += 270; + break; + } + screen->setHeading(heading); + + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/BMX160Sensor.h b/src/motion/BMX160Sensor.h new file mode 100755 index 0000000000..26f4772719 --- /dev/null +++ b/src/motion/BMX160Sensor.h @@ -0,0 +1,41 @@ +#pragma once + +#ifndef _BMX160_SENSOR_H_ +#define _BMX160_SENSOR_H_ + +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#ifdef RAK_4631 + +#include "Fusion/Fusion.h" +#include + +class BMX160Sensor : public MotionSensor +{ + private: + RAK_BMX160 sensor; + bool showingScreen = false; + float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0; + + public: + explicit BMX160Sensor(ScanI2C::FoundDevice foundDevice); + virtual bool init() override; + virtual int32_t runOnce() override; +}; + +#else + +// Stub +class BMX160Sensor : public MotionSensor +{ + public: + explicit BMX160Sensor(ScanI2C::FoundDevice foundDevice); +}; + +#endif + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/ICM20948Sensor.cpp b/src/motion/ICM20948Sensor.cpp new file mode 100755 index 0000000000..bd524dbf5e --- /dev/null +++ b/src/motion/ICM20948Sensor.cpp @@ -0,0 +1,190 @@ +#include "ICM20948Sensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !defined(ARCH_NRF52) && !MESHTASTIC_EXCLUDE_I2C + +// Flag when an interrupt has been detected +volatile static bool ICM20948_IRQ = false; + +// Interrupt service routine +void ICM20948SetInterrupt() +{ + ICM20948_IRQ = true; +} + +ICM20948Sensor::ICM20948Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {} + +bool ICM20948Sensor::init() +{ + // Initialise the sensor + sensor = ICM20948Singleton::GetInstance(); + if (!sensor->init(device)) + return false; + + // Enable simple Wake on Motion + return sensor->setWakeOnMotion(); +} + +#ifdef ICM_20948_INT_PIN + +int32_t ICM20948Sensor::runOnce() +{ + // Wake on motion using hardware interrupts - this is the most efficient way to check for motion + if (ICM20948_IRQ) { + ICM20948_IRQ = false; + sensor->clearInterrupts(); + wakeScreen(); + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#else + +int32_t ICM20948Sensor::runOnce() +{ + // Wake on motion using polling - this is not as efficient as using hardware interrupt pin (see above) + auto status = sensor->setBank(0); + if (sensor->status != ICM_20948_Stat_Ok) { + LOG_DEBUG("ICM20948Sensor::isWakeOnMotion failed to set bank - %s\n", sensor->statusString()); + return MOTION_SENSOR_CHECK_INTERVAL_MS; + } + + ICM_20948_INT_STATUS_t int_stat; + status = sensor->read(AGB0_REG_INT_STATUS, (uint8_t *)&int_stat, sizeof(ICM_20948_INT_STATUS_t)); + if (status != ICM_20948_Stat_Ok) { + LOG_DEBUG("ICM20948Sensor::isWakeOnMotion failed to read interrupts - %s\n", sensor->statusString()); + return MOTION_SENSOR_CHECK_INTERVAL_MS; + } + + if (int_stat.WOM_INT != 0) { + // Wake up! + wakeScreen(); + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#endif + +#endif + +// ---------------------------------------------------------------------- +// ICM20948Singleton +// ---------------------------------------------------------------------- + +#ifndef ARCH_NRF52 + +// Get a singleton wrapper for an Sparkfun ICM_20948_I2C +ICM20948Singleton *ICM20948Singleton::GetInstance() +{ + if (pinstance == nullptr) { + pinstance = new ICM20948Singleton(); + } + return pinstance; +} + +ICM20948Singleton::ICM20948Singleton() {} + +ICM20948Singleton::~ICM20948Singleton() {} + +ICM20948Singleton *ICM20948Singleton::pinstance{nullptr}; + +// Initialise the ICM20948 Sensor +bool ICM20948Singleton::init(ScanI2C::FoundDevice device) +{ +#ifdef ICM_20948_DEBUG + // Set ICM_20948_DEBUG to enable helpful debug messages on Serial + enableDebugging(); +#endif + +// startup +#ifdef Wire1 + ICM_20948_Status_e status = + begin(device.address.port == ScanI2C::I2CPort::WIRE1 ? Wire1 : Wire, device.address.address == ICM20948_ADDR ? 1 : 0); +#else + ICM_20948_Status_e status = begin(Wire, device.address.address == ICM20948_ADDR ? 1 : 0); +#endif + if (status != ICM_20948_Stat_Ok) { + LOG_DEBUG("ICM20948Sensor::init begin - %s\n", statusString()); + return false; + } + + // SW reset to make sure the device starts in a known state + if (swReset() != ICM_20948_Stat_Ok) { + LOG_DEBUG("ICM20948Sensor::init reset - %s\n", statusString()); + return false; + } + delay(200); + + // Now wake the sensor up + if (sleep(false) != ICM_20948_Stat_Ok) { + LOG_DEBUG("ICM20948Sensor::init wake - %s\n", statusString()); + return false; + } + + if (lowPower(false) != ICM_20948_Stat_Ok) { + LOG_DEBUG("ICM20948Sensor::init high power - %s\n", statusString()); + return false; + } + +#ifdef ICM_20948_INT_PIN + + // Active low + cfgIntActiveLow(true); + LOG_DEBUG("ICM20948Sensor::init set cfgIntActiveLow - %s\n", statusString()); + + // Push-pull + cfgIntOpenDrain(false); + LOG_DEBUG("ICM20948Sensor::init set cfgIntOpenDrain - %s\n", statusString()); + + // If enabled, *ANY* read will clear the INT_STATUS register. + cfgIntAnyReadToClear(true); + LOG_DEBUG("ICM20948Sensor::init set cfgIntAnyReadToClear - %s\n", statusString()); + + // Latch the interrupt until cleared + cfgIntLatch(true); + LOG_DEBUG("ICM20948Sensor::init set cfgIntLatch - %s\n", statusString()); + + // Set up an interrupt pin with an internal pullup for active low + pinMode(ICM_20948_INT_PIN, INPUT_PULLUP); + + // Set up an interrupt service routine + attachInterrupt(ICM_20948_INT_PIN, ICM20948SetInterrupt, FALLING); + +#endif + return true; +} + +#ifdef ICM_20948_DMP_IS_ENABLED + +// Stub +bool ICM20948Sensor::initDMP() +{ + return false; +} + +#endif + +bool ICM20948Singleton::setWakeOnMotion() +{ + // Set WoM threshold in milli G's + auto status = WOMThreshold(ICM_20948_WOM_THRESHOLD); + if (status != ICM_20948_Stat_Ok) + return false; + + // Enable WoM Logic mode 1 = Compare the current sample with the previous sample + status = WOMLogic(true, 1); + LOG_DEBUG("ICM20948Sensor::init set WOMLogic - %s\n", statusString()); + if (status != ICM_20948_Stat_Ok) + return false; + + // Enable interrupts on WakeOnMotion + status = intEnableWOM(true); + LOG_DEBUG("ICM20948Sensor::init set intEnableWOM - %s\n", statusString()); + return status == ICM_20948_Stat_Ok; + + // Clear any current interrupts + ICM20948_IRQ = false; + clearInterrupts(); + return true; +} + +#endif \ No newline at end of file diff --git a/src/motion/ICM20948Sensor.h b/src/motion/ICM20948Sensor.h new file mode 100755 index 0000000000..373c1164f1 --- /dev/null +++ b/src/motion/ICM20948Sensor.h @@ -0,0 +1,96 @@ +#pragma once +#ifndef _ICM_20948_SENSOR_H_ +#define _ICM_20948_SENSOR_H_ + +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !defined(ARCH_NRF52) && !MESHTASTIC_EXCLUDE_I2C + +#include + +// Set the default gyro scale - dps250, dps500, dps1000, dps2000 +#ifndef ICM_20948_MPU_GYRO_SCALE +#define ICM_20948_MPU_GYRO_SCALE dps250 +#endif + +// Set the default accelerometer scale - gpm2, gpm4, gpm8, gpm16 +#ifndef ICM_20948_MPU_ACCEL_SCALE +#define ICM_20948_MPU_ACCEL_SCALE gpm2 +#endif + +// Define a threshold for Wake on Motion Sensing (0mg to 1020mg) +#ifndef ICM_20948_WOM_THRESHOLD +#define ICM_20948_WOM_THRESHOLD 16U +#endif + +// Define a pin in variant.h to use interrupts to read the ICM-20948 +#ifndef ICM_20948_WOM_THRESHOLD +#define ICM_20948_INT_PIN 255 +#endif + +// Uncomment this line to enable helpful debug messages on Serial +// #define ICM_20948_DEBUG 1 + +// Uncomment this line to enable the onboard digital motion processor (to be added in a future PR) +// #define ICM_20948_DMP_IS_ENABLED 1 + +// Check for a mandatory compiler flag to use the DMP (to be added in a future PR) +#ifdef ICM_20948_DMP_IS_ENABLED +#ifndef ICM_20948_USE_DMP +#error To use the digital motion processor, please either set the compiler flag ICM_20948_USE_DMP or uncomment line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h +#endif +#endif + +// The I2C address of the Accelerometer (if found) from main.cpp +extern ScanI2C::DeviceAddress accelerometer_found; + +// Singleton wrapper for the Sparkfun ICM_20948_I2C class +class ICM20948Singleton : public ICM_20948_I2C +{ + private: + static ICM20948Singleton *pinstance; + + protected: + ICM20948Singleton(); + ~ICM20948Singleton(); + + public: + // Create a singleton instance (not thread safe) + static ICM20948Singleton *GetInstance(); + + // Singletons should not be cloneable. + ICM20948Singleton(ICM20948Singleton &other) = delete; + + // Singletons should not be assignable. + void operator=(const ICM20948Singleton &) = delete; + + // Initialise the motion sensor singleton for normal operation + bool init(ScanI2C::FoundDevice device); + + // Enable Wake on Motion interrupts (sensor must be initialised first) + bool setWakeOnMotion(); + +#ifdef ICM_20948_DMP_IS_ENABLED + // Initialise the motion sensor singleton for digital motion processing + bool initDMP(); +#endif +}; + +class ICM20948Sensor : public MotionSensor +{ + private: + ICM20948Singleton *sensor = nullptr; + + public: + explicit ICM20948Sensor(ScanI2C::FoundDevice foundDevice); + + // Initialise the motion sensor + virtual bool init() override; + + // Called each time our sensor gets a chance to run + virtual int32_t runOnce() override; +}; + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/LIS3DHSensor.cpp b/src/motion/LIS3DHSensor.cpp new file mode 100755 index 0000000000..e2df60b1ce --- /dev/null +++ b/src/motion/LIS3DHSensor.cpp @@ -0,0 +1,36 @@ +#include "LIS3DHSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +LIS3DHSensor::LIS3DHSensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {} + +bool LIS3DHSensor::init() +{ + if (sensor.begin(deviceAddress())) { + sensor.setRange(LIS3DH_RANGE_2_G); + // Adjust threshold, higher numbers are less sensitive + sensor.setClick(config.device.double_tap_as_button_press ? 2 : 1, MOTION_SENSOR_CHECK_INTERVAL_MS); + LOG_DEBUG("LIS3DHSensor::init ok\n"); + return true; + } + LOG_DEBUG("LIS3DHSensor::init failed\n"); + return false; +} + +int32_t LIS3DHSensor::runOnce() +{ + if (sensor.getClick() > 0) { + uint8_t click = sensor.getClick(); + if (!config.device.double_tap_as_button_press) { + wakeScreen(); + } + + if (config.device.double_tap_as_button_press && (click & 0x20)) { + buttonPress(); + return 500; + } + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#endif \ No newline at end of file diff --git a/src/motion/LIS3DHSensor.h b/src/motion/LIS3DHSensor.h new file mode 100755 index 0000000000..603d195a80 --- /dev/null +++ b/src/motion/LIS3DHSensor.h @@ -0,0 +1,24 @@ +#pragma once +#ifndef _LIS3DH_SENSOR_H_ +#define _LIS3DH_SENSOR_H_ + +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#include + +class LIS3DHSensor : public MotionSensor +{ + private: + Adafruit_LIS3DH sensor; + + public: + explicit LIS3DHSensor(ScanI2C::FoundDevice foundDevice); + virtual bool init() override; + virtual int32_t runOnce() override; +}; + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/LSM6DS3Sensor.cpp b/src/motion/LSM6DS3Sensor.cpp new file mode 100755 index 0000000000..64ef9a23b1 --- /dev/null +++ b/src/motion/LSM6DS3Sensor.cpp @@ -0,0 +1,33 @@ +#include "LSM6DS3Sensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +LSM6DS3Sensor::LSM6DS3Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {} + +bool LSM6DS3Sensor::init() +{ + if (sensor.begin_I2C(deviceAddress())) { + + // Default threshold of 2G, less sensitive options are 4, 8 or 16G + sensor.setAccelRange(LSM6DS_ACCEL_RANGE_2_G); + + // Duration is number of occurances needed to trigger, higher threshold is less sensitive + sensor.enableWakeup(config.display.wake_on_tap_or_motion, 1, LSM6DS3_WAKE_THRESH); + + LOG_DEBUG("LSM6DS3Sensor::init ok\n"); + return true; + } + LOG_DEBUG("LSM6DS3Sensor::init failed\n"); + return false; +} + +int32_t LSM6DS3Sensor::runOnce() +{ + if (sensor.shake()) { + wakeScreen(); + return 500; + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#endif \ No newline at end of file diff --git a/src/motion/LSM6DS3Sensor.h b/src/motion/LSM6DS3Sensor.h new file mode 100755 index 0000000000..77069ef3c3 --- /dev/null +++ b/src/motion/LSM6DS3Sensor.h @@ -0,0 +1,28 @@ +#pragma once +#ifndef _LSM6DS3_SENSOR_H_ +#define _LSM6DS3_SENSOR_H_ + +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#ifndef LSM6DS3_WAKE_THRESH +#define LSM6DS3_WAKE_THRESH 20 +#endif + +#include + +class LSM6DS3Sensor : public MotionSensor +{ + private: + Adafruit_LSM6DS3TRC sensor; + + public: + explicit LSM6DS3Sensor(ScanI2C::FoundDevice foundDevice); + virtual bool init() override; + virtual int32_t runOnce() override; +}; + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/MPU6050Sensor.cpp b/src/motion/MPU6050Sensor.cpp new file mode 100755 index 0000000000..77aaca46d6 --- /dev/null +++ b/src/motion/MPU6050Sensor.cpp @@ -0,0 +1,31 @@ +#include "MPU6050Sensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +MPU6050Sensor::MPU6050Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {} + +bool MPU6050Sensor::init() +{ + if (sensor.begin(deviceAddress())) { + // setup motion detection + sensor.setHighPassFilter(MPU6050_HIGHPASS_0_63_HZ); + sensor.setMotionDetectionThreshold(1); + sensor.setMotionDetectionDuration(20); + sensor.setInterruptPinLatch(true); // Keep it latched. Will turn off when reinitialized. + sensor.setInterruptPinPolarity(true); + LOG_DEBUG("MPU6050Sensor::init ok\n"); + return true; + } + LOG_DEBUG("MPU6050Sensor::init failed\n"); + return false; +} + +int32_t MPU6050Sensor::runOnce() +{ + if (sensor.getMotionInterruptStatus()) { + wakeScreen(); + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#endif \ No newline at end of file diff --git a/src/motion/MPU6050Sensor.h b/src/motion/MPU6050Sensor.h new file mode 100755 index 0000000000..2e6eafecd4 --- /dev/null +++ b/src/motion/MPU6050Sensor.h @@ -0,0 +1,24 @@ +#pragma once +#ifndef _MPU6050_SENSOR_H_ +#define _MPU6050_SENSOR_H_ + +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#include + +class MPU6050Sensor : public MotionSensor +{ + private: + Adafruit_MPU6050 sensor; + + public: + explicit MPU6050Sensor(ScanI2C::FoundDevice foundDevice); + virtual bool init() override; + virtual int32_t runOnce() override; +}; + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/MotionSensor.cpp b/src/motion/MotionSensor.cpp new file mode 100755 index 0000000000..20e396bba5 --- /dev/null +++ b/src/motion/MotionSensor.cpp @@ -0,0 +1,79 @@ +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +// screen is defined in main.cpp +extern graphics::Screen *screen; + +MotionSensor::MotionSensor(ScanI2C::FoundDevice foundDevice) +{ + device.address.address = foundDevice.address.address; + device.address.port = foundDevice.address.port; + device.type = foundDevice.type; + LOG_DEBUG("MotionSensor::MotionSensor port: %s address: 0x%x type: %d\n", + devicePort() == ScanI2C::I2CPort::WIRE1 ? "Wire1" : "Wire", (uint8_t)deviceAddress(), deviceType()); +} + +ScanI2C::DeviceType MotionSensor::deviceType() +{ + return device.type; +} + +uint8_t MotionSensor::deviceAddress() +{ + return device.address.address; +} + +ScanI2C::I2CPort MotionSensor::devicePort() +{ + return device.address.port; +} + +#ifdef RAK_4631 +void MotionSensor::drawFrameCalibration(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\nCompass"); + int16_t compassX = 0, compassY = 0; + uint16_t compassDiam = graphics::Screen::getCompassDiam(display->getWidth(), display->getHeight()); + + // coordinates for the center of the compass/circle + if (config.display.displaymode == meshtastic_Config_DisplayConfig_DisplayMode_DEFAULT) { + compassX = x + display->getWidth() - compassDiam / 2 - 5; + compassY = y + display->getHeight() / 2; + } else { + compassX = x + display->getWidth() - compassDiam / 2 - 5; + compassY = y + FONT_HEIGHT_SMALL + (display->getHeight() - FONT_HEIGHT_SMALL) / 2; + } + display->drawCircle(compassX, compassY, compassDiam / 2); + screen->drawCompassNorth(display, compassX, compassY, screen->getHeading() * PI / 180); +} +#endif + +#if !MESHTASTIC_EXCLUDE_POWER_FSM +void MotionSensor::wakeScreen() +{ + if (powerFSM.getState() == &stateDARK) { + LOG_DEBUG("MotionSensor::wakeScreen detected\n"); + powerFSM.trigger(EVENT_INPUT); + } +} + +void MotionSensor::buttonPress() +{ + LOG_DEBUG("MotionSensor::buttonPress detected\n"); + powerFSM.trigger(EVENT_PRESS); +} + +#else + +void MotionSensor::wakeScreen() {} + +void MotionSensor::buttonPress() {} + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/MotionSensor.h b/src/motion/MotionSensor.h new file mode 100755 index 0000000000..0f7f3479b2 --- /dev/null +++ b/src/motion/MotionSensor.h @@ -0,0 +1,85 @@ +#pragma once +#ifndef _MOTION_SENSOR_H_ +#define _MOTION_SENSOR_H_ + +#define MOTION_SENSOR_CHECK_INTERVAL_MS 100 +#define MOTION_SENSOR_CLICK_THRESHOLD 40 + +#include "../configuration.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#include "../PowerFSM.h" +#include "../detect/ScanI2C.h" +#include "../graphics/Screen.h" +#include "../graphics/ScreenFonts.h" +#include "../power.h" + +// Base class for motion processing +class MotionSensor +{ + public: + explicit MotionSensor(ScanI2C::FoundDevice foundDevice); + virtual ~MotionSensor(){}; + + // Get the device type + ScanI2C::DeviceType deviceType(); + + // Get the device address + uint8_t deviceAddress(); + + // Get the device port + ScanI2C::I2CPort devicePort(); + + // Initialise the motion sensor + inline virtual bool init() { return false; }; + + // The method that will be called each time our sensor gets a chance to run + // Returns the desired period for next invocation (or RUN_SAME for no change) + // Refer to /src/concurrency/OSThread.h for more information + inline virtual int32_t runOnce() { return MOTION_SENSOR_CHECK_INTERVAL_MS; }; + + protected: + // Turn on the screen when a tap or motion is detected + virtual void wakeScreen(); + + // Register a button press when a double-tap is detected + virtual void buttonPress(); + +#ifdef RAK_4631 + // 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); +#endif + + ScanI2C::FoundDevice device; +}; + +namespace MotionSensorI2C +{ + +static inline int readRegister(uint8_t address, uint8_t reg, uint8_t *data, uint8_t len) +{ + Wire.beginTransmission(address); + Wire.write(reg); + Wire.endTransmission(); + Wire.requestFrom((uint8_t)address, (uint8_t)len); + uint8_t i = 0; + while (Wire.available()) { + data[i++] = Wire.read(); + } + return 0; // Pass +} + +static inline int writeRegister(uint8_t address, uint8_t reg, uint8_t *data, uint8_t len) +{ + Wire.beginTransmission(address); + Wire.write(reg); + Wire.write(data, len); + return (0 != Wire.endTransmission()); +} + +} // namespace MotionSensorI2C + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/STK8XXXSensor.cpp b/src/motion/STK8XXXSensor.cpp new file mode 100755 index 0000000000..d4d69ef992 --- /dev/null +++ b/src/motion/STK8XXXSensor.cpp @@ -0,0 +1,40 @@ +#include "STK8XXXSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +STK8XXXSensor::STK8XXXSensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {} + +#ifdef STK8XXX_INT + +volatile static bool STK_IRQ; + +bool STK8XXXSensor::init() +{ + if (sensor.STK8xxx_Initialization(STK8xxx_VAL_RANGE_2G)) { + STK_IRQ = false; + sensor.STK8xxx_Anymotion_init(); + pinMode(STK8XXX_INT, INPUT_PULLUP); + attachInterrupt( + digitalPinToInterrupt(STK8XXX_INT), [] { STK_IRQ = true; }, RISING); + + LOG_DEBUG("STK8XXXSensor::init ok\n"); + return true; + } + LOG_DEBUG("STK8XXXSensor::init failed\n"); + return false; +} + +int32_t STK8XXXSensor::runOnce() +{ + if (STK_IRQ) { + STK_IRQ = false; + if (config.display.wake_on_tap_or_motion) { + wakeScreen(); + } + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/STK8XXXSensor.h b/src/motion/STK8XXXSensor.h new file mode 100755 index 0000000000..190b916b45 --- /dev/null +++ b/src/motion/STK8XXXSensor.h @@ -0,0 +1,37 @@ +#pragma once +#ifndef _STK8XXX_SENSOR_H_ +#define _STK8XXX_SENSOR_H_ + +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#ifdef STK8XXX_INT + +#include + +class STK8XXXSensor : public MotionSensor +{ + private: + STK8xxx sensor; + + public: + explicit STK8XXXSensor(ScanI2C::FoundDevice foundDevice); + virtual bool init() override; + virtual int32_t runOnce() override; +}; + +#else + +// Stub +class STK8XXXSensor : public MotionSensor +{ + public: + explicit STK8XXXSensor(ScanI2C::FoundDevice foundDevice); +}; + +#endif + +#endif + +#endif \ No newline at end of file From 821d6e444f187bd9cc9e9e3c44e3ffc573e9d561 Mon Sep 17 00:00:00 2001 From: David <2941290+dhskinner@users.noreply.github.com> Date: Mon, 23 Sep 2024 23:28:30 +1000 Subject: [PATCH 2/8] Tidy up --- platformio.ini | 3 ++- src/modules/AdminModule.cpp | 2 +- src/motion/AccelerometerThread.h | 2 -- src/motion/ICM20948Sensor.cpp | 8 ++------ src/motion/ICM20948Sensor.h | 2 +- 5 files changed, 6 insertions(+), 11 deletions(-) diff --git a/platformio.ini b/platformio.ini index 9b869a0364..01d0416ca1 100644 --- a/platformio.ini +++ b/platformio.ini @@ -148,6 +148,7 @@ lib_deps = adafruit/Adafruit SHT4x Library@^1.0.4 adafruit/Adafruit TSL2591 Library@^1.4.5 sparkfun/SparkFun Qwiic Scale NAU7802 Arduino Library@^1.0.5 + sparkfun/SparkFun 9DoF IMU Breakout - ICM 20948 - Arduino Library@^1.2.13 ClosedCube OPT3001@^1.1.2 emotibit/EmotiBit MLX90632@^1.0.8 dfrobot/DFRobot_RTU@^1.0.3 @@ -161,4 +162,4 @@ lib_deps = https://github.com/meshtastic/DFRobot_LarkWeatherStation#dee914270dc7cb3e43fbf034edd85a63a16a12ee - https://github.com/gjelsoe/STK8xxx-Accelerometer.git#v0.1.1 + https://github.com/gjelsoe/STK8xxx-Accelerometer.git#v0.1.1 \ No newline at end of file diff --git a/src/modules/AdminModule.cpp b/src/modules/AdminModule.cpp index 64cf12aa43..389a0db8d8 100644 --- a/src/modules/AdminModule.cpp +++ b/src/modules/AdminModule.cpp @@ -34,7 +34,7 @@ #include "modules/PositionModule.h" #endif -#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C #include "motion/AccelerometerThread.h" #endif diff --git a/src/motion/AccelerometerThread.h b/src/motion/AccelerometerThread.h index a4daf345e4..7344bdfcbf 100755 --- a/src/motion/AccelerometerThread.h +++ b/src/motion/AccelerometerThread.h @@ -94,11 +94,9 @@ class AccelerometerThread : public concurrency::OSThread case ScanI2C::DeviceType::STK8BAXX: sensor = new STK8XXXSensor(device); break; -#ifndef ARCH_NRF52 case ScanI2C::DeviceType::ICM20948: sensor = new ICM20948Sensor(device); break; -#endif default: disable(); return; diff --git a/src/motion/ICM20948Sensor.cpp b/src/motion/ICM20948Sensor.cpp index bd524dbf5e..f4ac2bee49 100755 --- a/src/motion/ICM20948Sensor.cpp +++ b/src/motion/ICM20948Sensor.cpp @@ -1,6 +1,6 @@ #include "ICM20948Sensor.h" -#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !defined(ARCH_NRF52) && !MESHTASTIC_EXCLUDE_I2C +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C // Flag when an interrupt has been detected volatile static bool ICM20948_IRQ = false; @@ -70,8 +70,6 @@ int32_t ICM20948Sensor::runOnce() // ICM20948Singleton // ---------------------------------------------------------------------- -#ifndef ARCH_NRF52 - // Get a singleton wrapper for an Sparkfun ICM_20948_I2C ICM20948Singleton *ICM20948Singleton::GetInstance() { @@ -185,6 +183,4 @@ bool ICM20948Singleton::setWakeOnMotion() ICM20948_IRQ = false; clearInterrupts(); return true; -} - -#endif \ No newline at end of file +} \ No newline at end of file diff --git a/src/motion/ICM20948Sensor.h b/src/motion/ICM20948Sensor.h index 373c1164f1..d5e246c8de 100755 --- a/src/motion/ICM20948Sensor.h +++ b/src/motion/ICM20948Sensor.h @@ -4,7 +4,7 @@ #include "MotionSensor.h" -#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !defined(ARCH_NRF52) && !MESHTASTIC_EXCLUDE_I2C +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C #include From 2c9e75768a5b9df5090fa5be019adcb8c11cf2e6 Mon Sep 17 00:00:00 2001 From: David <2941290+dhskinner@users.noreply.github.com> Date: Mon, 23 Sep 2024 23:48:52 +1000 Subject: [PATCH 3/8] Update ICM20948Sensor.cpp --- src/motion/ICM20948Sensor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/motion/ICM20948Sensor.cpp b/src/motion/ICM20948Sensor.cpp index f4ac2bee49..d16968e06d 100755 --- a/src/motion/ICM20948Sensor.cpp +++ b/src/motion/ICM20948Sensor.cpp @@ -64,8 +64,6 @@ int32_t ICM20948Sensor::runOnce() #endif -#endif - // ---------------------------------------------------------------------- // ICM20948Singleton // ---------------------------------------------------------------------- @@ -183,4 +181,6 @@ bool ICM20948Singleton::setWakeOnMotion() ICM20948_IRQ = false; clearInterrupts(); return true; -} \ No newline at end of file +} + +#endif \ No newline at end of file From e427456319965570962e41dec822c3cd32685f47 Mon Sep 17 00:00:00 2001 From: David <2941290+dhskinner@users.noreply.github.com> Date: Tue, 24 Sep 2024 08:14:46 +1000 Subject: [PATCH 4/8] Update AccelerometerThread.h --- src/motion/AccelerometerThread.h | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/src/motion/AccelerometerThread.h b/src/motion/AccelerometerThread.h index 7344bdfcbf..f9cbb49a19 100755 --- a/src/motion/AccelerometerThread.h +++ b/src/motion/AccelerometerThread.h @@ -110,13 +110,28 @@ class AccelerometerThread : public concurrency::OSThread } // Copy constructor (not implemented / included to avoid cppcheck warnings) - AccelerometerThread(const AccelerometerThread &other) : OSThread::OSThread("AccelerometerThread") {} + AccelerometerThread(const AccelerometerThread &other) : OSThread::OSThread("AccelerometerThread") { this->copy(other); } // Destructor (included to avoid cppcheck warnings) virtual ~AccelerometerThread() { clean(); } // Copy assignment (not implemented / included to avoid cppcheck warnings) - AccelerometerThread &operator=(const AccelerometerThread &other) { return *this; } + AccelerometerThread &operator=(const AccelerometerThread &other) + { + this->copy(other); + return *this; + } + + // Take a very shallow copy (does not copy OSThread state nor the sensor object) + // If for some reason this is ever used, make sure to call init() after any copy + void copy(const AccelerometerThread &other) + { + if (this != &other) { + clean(); + this->device = ScanI2C::FoundDevice(other.device.type, + ScanI2C::DeviceAddress(other.device.address.port, other.device.address.address)); + } + } // Cleanup resources void clean() From e06e59feeac8b441b06a9b2323a9ee9a12a0b43c Mon Sep 17 00:00:00 2001 From: David <2941290+dhskinner@users.noreply.github.com> Date: Mon, 23 Sep 2024 23:06:39 +1000 Subject: [PATCH 5/8] Initial upload --- src/AccelerometerThread.h | 319 ------------------------------- src/configuration.h | 2 + src/detect/ScanI2C.cpp | 4 +- src/detect/ScanI2C.h | 10 +- src/detect/ScanI2CTwoWire.cpp | 21 +- src/main.cpp | 7 +- src/main.h | 6 +- src/modules/AdminModule.cpp | 2 +- src/motion/AccelerometerThread.h | 136 +++++++++++++ src/motion/BMA423Sensor.cpp | 62 ++++++ src/motion/BMA423Sensor.h | 26 +++ src/motion/BMX160Sensor.cpp | 100 ++++++++++ src/motion/BMX160Sensor.h | 41 ++++ src/motion/ICM20948Sensor.cpp | 190 ++++++++++++++++++ src/motion/ICM20948Sensor.h | 96 ++++++++++ src/motion/LIS3DHSensor.cpp | 36 ++++ src/motion/LIS3DHSensor.h | 24 +++ src/motion/LSM6DS3Sensor.cpp | 33 ++++ src/motion/LSM6DS3Sensor.h | 28 +++ src/motion/MPU6050Sensor.cpp | 31 +++ src/motion/MPU6050Sensor.h | 24 +++ src/motion/MotionSensor.cpp | 79 ++++++++ src/motion/MotionSensor.h | 85 ++++++++ src/motion/STK8XXXSensor.cpp | 40 ++++ src/motion/STK8XXXSensor.h | 37 ++++ 25 files changed, 1104 insertions(+), 335 deletions(-) delete mode 100644 src/AccelerometerThread.h create mode 100755 src/motion/AccelerometerThread.h create mode 100755 src/motion/BMA423Sensor.cpp create mode 100755 src/motion/BMA423Sensor.h create mode 100755 src/motion/BMX160Sensor.cpp create mode 100755 src/motion/BMX160Sensor.h create mode 100755 src/motion/ICM20948Sensor.cpp create mode 100755 src/motion/ICM20948Sensor.h create mode 100755 src/motion/LIS3DHSensor.cpp create mode 100755 src/motion/LIS3DHSensor.h create mode 100755 src/motion/LSM6DS3Sensor.cpp create mode 100755 src/motion/LSM6DS3Sensor.h create mode 100755 src/motion/MPU6050Sensor.cpp create mode 100755 src/motion/MPU6050Sensor.h create mode 100755 src/motion/MotionSensor.cpp create mode 100755 src/motion/MotionSensor.h create mode 100755 src/motion/STK8XXXSensor.cpp create mode 100755 src/motion/STK8XXXSensor.h diff --git a/src/AccelerometerThread.h b/src/AccelerometerThread.h deleted file mode 100644 index 629d63c6a5..0000000000 --- a/src/AccelerometerThread.h +++ /dev/null @@ -1,319 +0,0 @@ -#pragma once -#include "configuration.h" - -#if !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR - -#include "PowerFSM.h" -#include "concurrency/OSThread.h" -#include "main.h" -#include "power.h" - -#include -#include -#include -#ifdef STK8XXX_INT -#include -#endif -#include -#include -#include -#ifdef RAK_4631 -#include "Fusion/Fusion.h" -#include "graphics/Screen.h" -#include "graphics/ScreenFonts.h" -#include -#endif - -#define ACCELEROMETER_CHECK_INTERVAL_MS 100 -#define ACCELEROMETER_CLICK_THRESHOLD 40 - -volatile static bool STK_IRQ; - -static inline int readRegister(uint8_t address, uint8_t reg, uint8_t *data, uint8_t len) -{ - Wire.beginTransmission(address); - Wire.write(reg); - Wire.endTransmission(); - Wire.requestFrom((uint8_t)address, (uint8_t)len); - uint8_t i = 0; - while (Wire.available()) { - data[i++] = Wire.read(); - } - return 0; // Pass -} - -static inline int writeRegister(uint8_t address, uint8_t reg, uint8_t *data, uint8_t len) -{ - Wire.beginTransmission(address); - Wire.write(reg); - Wire.write(data, len); - return (0 != Wire.endTransmission()); -} - -class AccelerometerThread : public concurrency::OSThread -{ - public: - explicit AccelerometerThread(ScanI2C::DeviceType type) : OSThread("AccelerometerThread") - { - if (accelerometer_found.port == ScanI2C::I2CPort::NO_I2C) { - LOG_DEBUG("AccelerometerThread disabling due to no sensors found\n"); - disable(); - return; - } - acceleremoter_type = type; -#ifndef RAK_4631 - if (!config.display.wake_on_tap_or_motion && !config.device.double_tap_as_button_press) { - LOG_DEBUG("AccelerometerThread disabling due to no interested configurations\n"); - disable(); - return; - } -#endif - init(); - } - - void start() - { - init(); - setIntervalFromNow(0); - }; - - protected: - int32_t runOnce() override - { - canSleep = true; // Assume we should not keep the board awake - - if (acceleremoter_type == ScanI2C::DeviceType::MPU6050 && mpu.getMotionInterruptStatus()) { - wakeScreen(); - } else if (acceleremoter_type == ScanI2C::DeviceType::STK8BAXX && STK_IRQ) { - STK_IRQ = false; - if (config.display.wake_on_tap_or_motion) { - wakeScreen(); - } - } else if (acceleremoter_type == ScanI2C::DeviceType::LIS3DH && lis.getClick() > 0) { - uint8_t click = lis.getClick(); - if (!config.device.double_tap_as_button_press) { - wakeScreen(); - } - - if (config.device.double_tap_as_button_press && (click & 0x20)) { - buttonPress(); - return 500; - } - } else if (acceleremoter_type == ScanI2C::DeviceType::BMA423 && bmaSensor.readIrqStatus() != DEV_WIRE_NONE) { - if (bmaSensor.isTilt() || bmaSensor.isDoubleTap()) { - wakeScreen(); - return 500; - } -#ifdef RAK_4631 - } else if (acceleremoter_type == ScanI2C::DeviceType::BMX160) { - sBmx160SensorData_t magAccel; - sBmx160SensorData_t gAccel; - - /* Get a new sensor event */ - bmx160.getAllData(&magAccel, NULL, &gAccel); - - // expirimental calibrate routine. Limited to between 10 and 30 seconds after boot - if (millis() > 12 * 1000 && millis() < 30 * 1000) { - if (!showingScreen) { - showingScreen = true; - screen->startAlert((FrameCallback)drawFrameCalibration); - } - if (magAccel.x > highestX) - highestX = magAccel.x; - if (magAccel.x < lowestX) - lowestX = magAccel.x; - if (magAccel.y > highestY) - highestY = magAccel.y; - if (magAccel.y < lowestY) - lowestY = magAccel.y; - if (magAccel.z > highestZ) - highestZ = magAccel.z; - if (magAccel.z < lowestZ) - lowestZ = magAccel.z; - } else if (showingScreen && millis() >= 30 * 1000) { - showingScreen = false; - screen->endAlert(); - } - - int highestRealX = highestX - (highestX + lowestX) / 2; - - magAccel.x -= (highestX + lowestX) / 2; - magAccel.y -= (highestY + lowestY) / 2; - magAccel.z -= (highestZ + lowestZ) / 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; - - // If we're set to one of the inverted positions - if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) { - ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ); - ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ); - } - - float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma); - - switch (config.display.compass_orientation) { - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0_INVERTED: - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0: - break; - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90: - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90_INVERTED: - heading += 90; - break; - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180: - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180_INVERTED: - heading += 180; - break; - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270: - case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED: - heading += 270; - break; - } - - screen->setHeading(heading); - -#endif - } else if (acceleremoter_type == ScanI2C::DeviceType::LSM6DS3 && lsm.shake()) { - wakeScreen(); - return 500; - } - - return ACCELEROMETER_CHECK_INTERVAL_MS; - } - - private: - void init() - { - LOG_DEBUG("AccelerometerThread initializing\n"); - - if (acceleremoter_type == ScanI2C::DeviceType::MPU6050 && mpu.begin(accelerometer_found.address)) { - LOG_DEBUG("MPU6050 initializing\n"); - // setup motion detection - mpu.setHighPassFilter(MPU6050_HIGHPASS_0_63_HZ); - mpu.setMotionDetectionThreshold(1); - mpu.setMotionDetectionDuration(20); - mpu.setInterruptPinLatch(true); // Keep it latched. Will turn off when reinitialized. - mpu.setInterruptPinPolarity(true); -#ifdef STK8XXX_INT - } else if (acceleremoter_type == ScanI2C::DeviceType::STK8BAXX && stk8baxx.STK8xxx_Initialization(STK8xxx_VAL_RANGE_2G)) { - STK_IRQ = false; - LOG_DEBUG("STX8BAxx initialized\n"); - stk8baxx.STK8xxx_Anymotion_init(); - pinMode(STK8XXX_INT, INPUT_PULLUP); - attachInterrupt( - digitalPinToInterrupt(STK8XXX_INT), [] { STK_IRQ = true; }, RISING); -#endif - } else if (acceleremoter_type == ScanI2C::DeviceType::LIS3DH && lis.begin(accelerometer_found.address)) { - LOG_DEBUG("LIS3DH initializing\n"); - lis.setRange(LIS3DH_RANGE_2_G); - // Adjust threshold, higher numbers are less sensitive - lis.setClick(config.device.double_tap_as_button_press ? 2 : 1, ACCELEROMETER_CLICK_THRESHOLD); - } else if (acceleremoter_type == ScanI2C::DeviceType::BMA423 && - bmaSensor.begin(accelerometer_found.address, &readRegister, &writeRegister)) { - LOG_DEBUG("BMA423 initializing\n"); - bmaSensor.configAccelerometer(bmaSensor.RANGE_2G, bmaSensor.ODR_100HZ, bmaSensor.BW_NORMAL_AVG4, - bmaSensor.PERF_CONTINUOUS_MODE); - bmaSensor.enableAccelerometer(); - bmaSensor.configInterrupt(BMA4_LEVEL_TRIGGER, BMA4_ACTIVE_HIGH, BMA4_PUSH_PULL, BMA4_OUTPUT_ENABLE, - BMA4_INPUT_DISABLE); - -#ifdef BMA423_INT - pinMode(BMA4XX_INT, INPUT); - attachInterrupt( - BMA4XX_INT, - [] { - // Set interrupt to set irq value to true - BMA_IRQ = true; - }, - RISING); // Select the interrupt mode according to the actual circuit -#endif - -#ifdef T_WATCH_S3 - // Need to raise the wrist function, need to set the correct axis - bmaSensor.setReampAxes(bmaSensor.REMAP_TOP_LAYER_RIGHT_CORNER); -#else - bmaSensor.setReampAxes(bmaSensor.REMAP_BOTTOM_LAYER_BOTTOM_LEFT_CORNER); -#endif - // bmaSensor.enableFeature(bmaSensor.FEATURE_STEP_CNTR, true); - bmaSensor.enableFeature(bmaSensor.FEATURE_TILT, true); - bmaSensor.enableFeature(bmaSensor.FEATURE_WAKEUP, true); - // bmaSensor.resetPedometer(); - - // Turn on feature interrupt - bmaSensor.enablePedometerIRQ(); - bmaSensor.enableTiltIRQ(); - // It corresponds to isDoubleClick interrupt - bmaSensor.enableWakeupIRQ(); -#ifdef RAK_4631 - } else if (acceleremoter_type == ScanI2C::DeviceType::BMX160 && bmx160.begin()) { - bmx160.ODR_Config(BMX160_ACCEL_ODR_100HZ, BMX160_GYRO_ODR_100HZ); // set output data rate - -#endif - } else if (acceleremoter_type == ScanI2C::DeviceType::LSM6DS3 && lsm.begin_I2C(accelerometer_found.address)) { - LOG_DEBUG("LSM6DS3 initializing\n"); - // Default threshold of 2G, less sensitive options are 4, 8 or 16G - lsm.setAccelRange(LSM6DS_ACCEL_RANGE_2_G); -#ifndef LSM6DS3_WAKE_THRESH -#define LSM6DS3_WAKE_THRESH 20 -#endif - lsm.enableWakeup(config.display.wake_on_tap_or_motion, 1, LSM6DS3_WAKE_THRESH); - // Duration is number of occurances needed to trigger, higher threshold is less sensitive - } - } - void wakeScreen() - { - if (powerFSM.getState() == &stateDARK) { - LOG_INFO("Tap or motion detected. Turning on screen\n"); - powerFSM.trigger(EVENT_INPUT); - } - } - - void buttonPress() - { - LOG_DEBUG("Double-tap detected. Firing button press\n"); - powerFSM.trigger(EVENT_PRESS); - } - - ScanI2C::DeviceType acceleremoter_type; - Adafruit_MPU6050 mpu; - Adafruit_LIS3DH lis; -#ifdef STK8XXX_INT - STK8xxx stk8baxx; -#endif - Adafruit_LSM6DS3TRC lsm; - SensorBMA423 bmaSensor; - bool BMA_IRQ = false; -#ifdef RAK_4631 - bool showingScreen = false; - RAK_BMX160 bmx160; - float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0; - - static void drawFrameCalibration(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\nCompass"); - int16_t compassX = 0, compassY = 0; - uint16_t compassDiam = graphics::Screen::getCompassDiam(display->getWidth(), display->getHeight()); - - // coordinates for the center of the compass/circle - if (config.display.displaymode == meshtastic_Config_DisplayConfig_DisplayMode_DEFAULT) { - compassX = x + display->getWidth() - compassDiam / 2 - 5; - compassY = y + display->getHeight() / 2; - } else { - compassX = x + display->getWidth() - compassDiam / 2 - 5; - compassY = y + FONT_HEIGHT_SMALL + (display->getHeight() - FONT_HEIGHT_SMALL) / 2; - } - display->drawCircle(compassX, compassY, compassDiam / 2); - screen->drawCompassNorth(display, compassX, compassY, screen->getHeading() * PI / 180); - } -#endif -}; - -#endif \ No newline at end of file diff --git a/src/configuration.h b/src/configuration.h index 0b470eef39..ee7be5ff98 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -150,6 +150,8 @@ along with this program. If not, see . #define BMA423_ADDR 0x19 #define LSM6DS3_ADDR 0x6A #define BMX160_ADDR 0x69 +#define ICM20948_ADDR 0x69 +#define ICM20948_ADDR_ALT 0x68 // ----------------------------------------------------------------------------- // LED diff --git a/src/detect/ScanI2C.cpp b/src/detect/ScanI2C.cpp index eaba62a78a..a9d70edaa9 100644 --- a/src/detect/ScanI2C.cpp +++ b/src/detect/ScanI2C.cpp @@ -37,8 +37,8 @@ ScanI2C::FoundDevice ScanI2C::firstKeyboard() const ScanI2C::FoundDevice ScanI2C::firstAccelerometer() const { - ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3, BMX160, STK8BAXX}; - return firstOfOrNONE(6, types); + ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3, BMX160, STK8BAXX, ICM20948}; + return firstOfOrNONE(7, types); } ScanI2C::FoundDevice ScanI2C::find(ScanI2C::DeviceType) const diff --git a/src/detect/ScanI2C.h b/src/detect/ScanI2C.h index 090b1a9682..5e8c9824da 100644 --- a/src/detect/ScanI2C.h +++ b/src/detect/ScanI2C.h @@ -56,7 +56,8 @@ class ScanI2C DFROBOT_LARK, NAU7802, FT6336U, - STK8BAXX + STK8BAXX, + ICM20948 } DeviceType; // typedef uint8_t DeviceAddress; @@ -67,8 +68,9 @@ class ScanI2C } I2CPort; typedef struct DeviceAddress { - I2CPort port; - uint8_t address; + // set default values for ADDRESS_NONE + I2CPort port = I2CPort::NO_I2C; + uint8_t address = 0; explicit DeviceAddress(I2CPort port, uint8_t address); DeviceAddress(); @@ -119,4 +121,4 @@ class ScanI2C private: bool shouldSuppressScreen = false; -}; +}; \ No newline at end of file diff --git a/src/detect/ScanI2CTwoWire.cpp b/src/detect/ScanI2CTwoWire.cpp index c0e70503bd..757205b34a 100644 --- a/src/detect/ScanI2CTwoWire.cpp +++ b/src/detect/ScanI2CTwoWire.cpp @@ -382,10 +382,7 @@ void ScanI2CTwoWire::scanPort(I2CPort port, uint8_t *address, uint8_t asize) SCAN_SIMPLE_CASE(QMC5883L_ADDR, QMC5883L, "QMC5883L Highrate 3-Axis magnetic sensor found\n") SCAN_SIMPLE_CASE(HMC5883L_ADDR, HMC5883L, "HMC5883L 3-Axis digital compass found\n") - SCAN_SIMPLE_CASE(PMSA0031_ADDR, PMSA0031, "PMSA0031 air quality sensor found\n") - SCAN_SIMPLE_CASE(MPU6050_ADDR, MPU6050, "MPU6050 accelerometer found\n"); - SCAN_SIMPLE_CASE(BMX160_ADDR, BMX160, "BMX160 accelerometer found\n"); SCAN_SIMPLE_CASE(BMA423_ADDR, BMA423, "BMA423 accelerometer found\n"); SCAN_SIMPLE_CASE(LSM6DS3_ADDR, LSM6DS3, "LSM6DS3 accelerometer found at address 0x%x\n", (uint8_t)addr.address); SCAN_SIMPLE_CASE(TCA9535_ADDR, TCA9535, "TCA9535 I2C expander found\n"); @@ -397,6 +394,24 @@ void ScanI2CTwoWire::scanPort(I2CPort port, uint8_t *address, uint8_t asize) SCAN_SIMPLE_CASE(NAU7802_ADDR, NAU7802, "NAU7802 based scale found\n"); SCAN_SIMPLE_CASE(FT6336U_ADDR, FT6336U, "FT6336U touchscreen found\n"); + case ICM20948_ADDR: // same as BMX160_ADDR + case ICM20948_ADDR_ALT: // same as MPU6050_ADDR + registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x00), 1); + if (registerValue == 0xEA) { + type = ICM20948; + LOG_INFO("ICM20948 9-dof motion processor found\n"); + break; + } else if (addr.address == BMX160_ADDR) { + type = BMX160; + LOG_INFO("BMX160 accelerometer found\n"); + break; + } else { + type = MPU6050; + LOG_INFO("MPU6050 accelerometer found\n"); + break; + } + break; + default: LOG_INFO("Device found at address 0x%x was not able to be enumerated\n", addr.address); } diff --git a/src/main.cpp b/src/main.cpp index 01fccf2b08..0edfdc3c8f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -103,8 +103,8 @@ NRF52Bluetooth *nrf52Bluetooth = nullptr; #include "AmbientLightingThread.h" #include "PowerFSMThread.h" -#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR -#include "AccelerometerThread.h" +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C +#include "motion/AccelerometerThread.h" AccelerometerThread *accelerometerThread = nullptr; #endif @@ -575,6 +575,7 @@ void setup() SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::SHT4X, meshtastic_TelemetrySensorType_SHT4X) SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::AHT10, meshtastic_TelemetrySensorType_AHT10) SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::DFROBOT_LARK, meshtastic_TelemetrySensorType_DFROBOT_LARK) + SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::ICM20948, meshtastic_TelemetrySensorType_ICM20948) i2cScanner.reset(); #endif @@ -642,7 +643,7 @@ void setup() #endif #if !MESHTASTIC_EXCLUDE_I2C -#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) if (acc_info.type != ScanI2C::DeviceType::NONE) { accelerometerThread = new AccelerometerThread(acc_info.type); } diff --git a/src/main.h b/src/main.h index 52a3fff1f8..5722f7cf0c 100644 --- a/src/main.h +++ b/src/main.h @@ -56,8 +56,8 @@ extern AudioThread *audioThread; // Global Screen singleton. extern graphics::Screen *screen; -#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR -#include "AccelerometerThread.h" +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C +#include "motion/AccelerometerThread.h" extern AccelerometerThread *accelerometerThread; #endif @@ -86,4 +86,4 @@ void nrf52Setup(), esp32Setup(), nrf52Loop(), esp32Loop(), rp2040Setup(), clearB meshtastic_DeviceMetadata getDeviceMetadata(); // We default to 4MHz SPI, SPI mode 0 -extern SPISettings spiSettings; +extern SPISettings spiSettings; \ No newline at end of file diff --git a/src/modules/AdminModule.cpp b/src/modules/AdminModule.cpp index 48048b0547..64cf12aa43 100644 --- a/src/modules/AdminModule.cpp +++ b/src/modules/AdminModule.cpp @@ -35,7 +35,7 @@ #endif #if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR -#include "AccelerometerThread.h" +#include "motion/AccelerometerThread.h" #endif AdminModule *adminModule; diff --git a/src/motion/AccelerometerThread.h b/src/motion/AccelerometerThread.h new file mode 100755 index 0000000000..a4daf345e4 --- /dev/null +++ b/src/motion/AccelerometerThread.h @@ -0,0 +1,136 @@ +#pragma once +#ifndef _ACCELEROMETER_H_ +#define _ACCELEROMETER_H_ + +#include "configuration.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#include "../concurrency/OSThread.h" +#include "BMA423Sensor.h" +#include "BMX160Sensor.h" +#include "ICM20948Sensor.h" +#include "LIS3DHSensor.h" +#include "LSM6DS3Sensor.h" +#include "MPU6050Sensor.h" +#include "MotionSensor.h" +#include "STK8XXXSensor.h" + +extern ScanI2C::DeviceAddress accelerometer_found; + +class AccelerometerThread : public concurrency::OSThread +{ + private: + MotionSensor *sensor = nullptr; + bool isInitialised = false; + + public: + explicit AccelerometerThread(ScanI2C::FoundDevice foundDevice) : OSThread("AccelerometerThread") + { + device = foundDevice; + init(); + } + + explicit AccelerometerThread(ScanI2C::DeviceType type) : AccelerometerThread(ScanI2C::FoundDevice{type, accelerometer_found}) + { + } + + void start() + { + init(); + setIntervalFromNow(0); + }; + + protected: + int32_t runOnce() override + { + // Assume we should not keep the board awake + canSleep = true; + + if (isInitialised) + return sensor->runOnce(); + + return MOTION_SENSOR_CHECK_INTERVAL_MS; + } + + private: + ScanI2C::FoundDevice device; + + void init() + { + if (isInitialised) + return; + + if (device.address.port == ScanI2C::I2CPort::NO_I2C || device.address.address == 0 || device.type == ScanI2C::NONE) { + LOG_DEBUG("AccelerometerThread disabling due to no sensors found\n"); + disable(); + return; + } + +#ifndef RAK_4631 + if (!config.display.wake_on_tap_or_motion && !config.device.double_tap_as_button_press) { + LOG_DEBUG("AccelerometerThread disabling due to no interested configurations\n"); + disable(); + return; + } +#endif + + switch (device.type) { + case ScanI2C::DeviceType::BMA423: + sensor = new BMA423Sensor(device); + break; + case ScanI2C::DeviceType::MPU6050: + sensor = new MPU6050Sensor(device); + break; + case ScanI2C::DeviceType::BMX160: + sensor = new BMX160Sensor(device); + break; + case ScanI2C::DeviceType::LIS3DH: + sensor = new LIS3DHSensor(device); + break; + case ScanI2C::DeviceType::LSM6DS3: + sensor = new LSM6DS3Sensor(device); + break; + case ScanI2C::DeviceType::STK8BAXX: + sensor = new STK8XXXSensor(device); + break; +#ifndef ARCH_NRF52 + case ScanI2C::DeviceType::ICM20948: + sensor = new ICM20948Sensor(device); + break; +#endif + default: + disable(); + return; + } + + isInitialised = sensor->init(); + if (!isInitialised) { + clean(); + } + LOG_DEBUG("AccelerometerThread::init %s\n", isInitialised ? "ok" : "failed"); + } + + // Copy constructor (not implemented / included to avoid cppcheck warnings) + AccelerometerThread(const AccelerometerThread &other) : OSThread::OSThread("AccelerometerThread") {} + + // Destructor (included to avoid cppcheck warnings) + virtual ~AccelerometerThread() { clean(); } + + // Copy assignment (not implemented / included to avoid cppcheck warnings) + AccelerometerThread &operator=(const AccelerometerThread &other) { return *this; } + + // Cleanup resources + void clean() + { + isInitialised = false; + if (sensor != nullptr) { + delete sensor; + sensor = nullptr; + } + } +}; + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/BMA423Sensor.cpp b/src/motion/BMA423Sensor.cpp new file mode 100755 index 0000000000..ec07b7c40c --- /dev/null +++ b/src/motion/BMA423Sensor.cpp @@ -0,0 +1,62 @@ +#include "BMA423Sensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +using namespace MotionSensorI2C; + +BMA423Sensor::BMA423Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {} + +bool BMA423Sensor::init() +{ + if (sensor.begin(deviceAddress(), &MotionSensorI2C::readRegister, &MotionSensorI2C::writeRegister)) { + sensor.configAccelerometer(sensor.RANGE_2G, sensor.ODR_100HZ, sensor.BW_NORMAL_AVG4, sensor.PERF_CONTINUOUS_MODE); + sensor.enableAccelerometer(); + sensor.configInterrupt(BMA4_LEVEL_TRIGGER, BMA4_ACTIVE_HIGH, BMA4_PUSH_PULL, BMA4_OUTPUT_ENABLE, BMA4_INPUT_DISABLE); + +#ifdef BMA423_INT + pinMode(BMA4XX_INT, INPUT); + attachInterrupt( + BMA4XX_INT, + [] { + // Set interrupt to set irq value to true + BMA_IRQ = true; + }, + RISING); // Select the interrupt mode according to the actual circuit +#endif + +#ifdef T_WATCH_S3 + // Need to raise the wrist function, need to set the correct axis + sensor.setReampAxes(sensor.REMAP_TOP_LAYER_RIGHT_CORNER); +#else + sensor.setReampAxes(sensor.REMAP_BOTTOM_LAYER_BOTTOM_LEFT_CORNER); +#endif + // sensor.enableFeature(sensor.FEATURE_STEP_CNTR, true); + sensor.enableFeature(sensor.FEATURE_TILT, true); + sensor.enableFeature(sensor.FEATURE_WAKEUP, true); + // sensor.resetPedometer(); + + // Turn on feature interrupt + sensor.enablePedometerIRQ(); + sensor.enableTiltIRQ(); + + // It corresponds to isDoubleClick interrupt + sensor.enableWakeupIRQ(); + LOG_DEBUG("BMA423Sensor::init ok\n"); + return true; + } + LOG_DEBUG("BMA423Sensor::init failed\n"); + return false; +} + +int32_t BMA423Sensor::runOnce() +{ + if (sensor.readIrqStatus() != DEV_WIRE_NONE) { + if (sensor.isTilt() || sensor.isDoubleTap()) { + wakeScreen(); + return 500; + } + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#endif \ No newline at end of file diff --git a/src/motion/BMA423Sensor.h b/src/motion/BMA423Sensor.h new file mode 100755 index 0000000000..0bc0ddf8cd --- /dev/null +++ b/src/motion/BMA423Sensor.h @@ -0,0 +1,26 @@ +#pragma once +#ifndef _BMA423_SENSOR_H_ +#define _BMA423_SENSOR_H_ + +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#include +#include + +class BMA423Sensor : public MotionSensor +{ + private: + SensorBMA423 sensor; + volatile bool BMA_IRQ = false; + + public: + explicit BMA423Sensor(ScanI2C::FoundDevice foundDevice); + virtual bool init() override; + virtual int32_t runOnce() override; +}; + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp new file mode 100755 index 0000000000..f18c4b586f --- /dev/null +++ b/src/motion/BMX160Sensor.cpp @@ -0,0 +1,100 @@ +#include "BMX160Sensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +BMX160Sensor::BMX160Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {} + +#ifdef RAK_4631 + +// screen is defined in main.cpp +extern graphics::Screen *screen; + +bool BMX160Sensor::init() +{ + if (sensor.begin()) { + // set output data rate + sensor.ODR_Config(BMX160_ACCEL_ODR_100HZ, BMX160_GYRO_ODR_100HZ); + LOG_DEBUG("BMX160Sensor::init ok\n"); + return true; + } + LOG_DEBUG("BMX160Sensor::init failed\n"); + return false; +} + +int32_t BMX160Sensor::runOnce() +{ + sBmx160SensorData_t magAccel; + sBmx160SensorData_t gAccel; + + /* Get a new sensor event */ + sensor.getAllData(&magAccel, NULL, &gAccel); + + // experimental calibrate routine. Limited to between 10 and 30 seconds after boot + if (millis() > 12 * 1000 && millis() < 30 * 1000) { + if (!showingScreen) { + showingScreen = true; + screen->startAlert((FrameCallback)drawFrameCalibration); + } + if (magAccel.x > highestX) + highestX = magAccel.x; + if (magAccel.x < lowestX) + lowestX = magAccel.x; + if (magAccel.y > highestY) + highestY = magAccel.y; + if (magAccel.y < lowestY) + lowestY = magAccel.y; + if (magAccel.z > highestZ) + highestZ = magAccel.z; + if (magAccel.z < lowestZ) + lowestZ = magAccel.z; + } else if (showingScreen && millis() >= 30 * 1000) { + showingScreen = false; + screen->endAlert(); + } + + int highestRealX = highestX - (highestX + lowestX) / 2; + + magAccel.x -= (highestX + lowestX) / 2; + magAccel.y -= (highestY + lowestY) / 2; + magAccel.z -= (highestZ + lowestZ) / 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; + + // If we're set to one of the inverted positions + if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) { + ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ); + ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ); + } + + float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma); + + switch (config.display.compass_orientation) { + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0_INVERTED: + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0: + break; + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90: + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90_INVERTED: + heading += 90; + break; + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180: + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180_INVERTED: + heading += 180; + break; + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270: + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED: + heading += 270; + break; + } + screen->setHeading(heading); + + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/BMX160Sensor.h b/src/motion/BMX160Sensor.h new file mode 100755 index 0000000000..26f4772719 --- /dev/null +++ b/src/motion/BMX160Sensor.h @@ -0,0 +1,41 @@ +#pragma once + +#ifndef _BMX160_SENSOR_H_ +#define _BMX160_SENSOR_H_ + +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#ifdef RAK_4631 + +#include "Fusion/Fusion.h" +#include + +class BMX160Sensor : public MotionSensor +{ + private: + RAK_BMX160 sensor; + bool showingScreen = false; + float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0; + + public: + explicit BMX160Sensor(ScanI2C::FoundDevice foundDevice); + virtual bool init() override; + virtual int32_t runOnce() override; +}; + +#else + +// Stub +class BMX160Sensor : public MotionSensor +{ + public: + explicit BMX160Sensor(ScanI2C::FoundDevice foundDevice); +}; + +#endif + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/ICM20948Sensor.cpp b/src/motion/ICM20948Sensor.cpp new file mode 100755 index 0000000000..bd524dbf5e --- /dev/null +++ b/src/motion/ICM20948Sensor.cpp @@ -0,0 +1,190 @@ +#include "ICM20948Sensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !defined(ARCH_NRF52) && !MESHTASTIC_EXCLUDE_I2C + +// Flag when an interrupt has been detected +volatile static bool ICM20948_IRQ = false; + +// Interrupt service routine +void ICM20948SetInterrupt() +{ + ICM20948_IRQ = true; +} + +ICM20948Sensor::ICM20948Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {} + +bool ICM20948Sensor::init() +{ + // Initialise the sensor + sensor = ICM20948Singleton::GetInstance(); + if (!sensor->init(device)) + return false; + + // Enable simple Wake on Motion + return sensor->setWakeOnMotion(); +} + +#ifdef ICM_20948_INT_PIN + +int32_t ICM20948Sensor::runOnce() +{ + // Wake on motion using hardware interrupts - this is the most efficient way to check for motion + if (ICM20948_IRQ) { + ICM20948_IRQ = false; + sensor->clearInterrupts(); + wakeScreen(); + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#else + +int32_t ICM20948Sensor::runOnce() +{ + // Wake on motion using polling - this is not as efficient as using hardware interrupt pin (see above) + auto status = sensor->setBank(0); + if (sensor->status != ICM_20948_Stat_Ok) { + LOG_DEBUG("ICM20948Sensor::isWakeOnMotion failed to set bank - %s\n", sensor->statusString()); + return MOTION_SENSOR_CHECK_INTERVAL_MS; + } + + ICM_20948_INT_STATUS_t int_stat; + status = sensor->read(AGB0_REG_INT_STATUS, (uint8_t *)&int_stat, sizeof(ICM_20948_INT_STATUS_t)); + if (status != ICM_20948_Stat_Ok) { + LOG_DEBUG("ICM20948Sensor::isWakeOnMotion failed to read interrupts - %s\n", sensor->statusString()); + return MOTION_SENSOR_CHECK_INTERVAL_MS; + } + + if (int_stat.WOM_INT != 0) { + // Wake up! + wakeScreen(); + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#endif + +#endif + +// ---------------------------------------------------------------------- +// ICM20948Singleton +// ---------------------------------------------------------------------- + +#ifndef ARCH_NRF52 + +// Get a singleton wrapper for an Sparkfun ICM_20948_I2C +ICM20948Singleton *ICM20948Singleton::GetInstance() +{ + if (pinstance == nullptr) { + pinstance = new ICM20948Singleton(); + } + return pinstance; +} + +ICM20948Singleton::ICM20948Singleton() {} + +ICM20948Singleton::~ICM20948Singleton() {} + +ICM20948Singleton *ICM20948Singleton::pinstance{nullptr}; + +// Initialise the ICM20948 Sensor +bool ICM20948Singleton::init(ScanI2C::FoundDevice device) +{ +#ifdef ICM_20948_DEBUG + // Set ICM_20948_DEBUG to enable helpful debug messages on Serial + enableDebugging(); +#endif + +// startup +#ifdef Wire1 + ICM_20948_Status_e status = + begin(device.address.port == ScanI2C::I2CPort::WIRE1 ? Wire1 : Wire, device.address.address == ICM20948_ADDR ? 1 : 0); +#else + ICM_20948_Status_e status = begin(Wire, device.address.address == ICM20948_ADDR ? 1 : 0); +#endif + if (status != ICM_20948_Stat_Ok) { + LOG_DEBUG("ICM20948Sensor::init begin - %s\n", statusString()); + return false; + } + + // SW reset to make sure the device starts in a known state + if (swReset() != ICM_20948_Stat_Ok) { + LOG_DEBUG("ICM20948Sensor::init reset - %s\n", statusString()); + return false; + } + delay(200); + + // Now wake the sensor up + if (sleep(false) != ICM_20948_Stat_Ok) { + LOG_DEBUG("ICM20948Sensor::init wake - %s\n", statusString()); + return false; + } + + if (lowPower(false) != ICM_20948_Stat_Ok) { + LOG_DEBUG("ICM20948Sensor::init high power - %s\n", statusString()); + return false; + } + +#ifdef ICM_20948_INT_PIN + + // Active low + cfgIntActiveLow(true); + LOG_DEBUG("ICM20948Sensor::init set cfgIntActiveLow - %s\n", statusString()); + + // Push-pull + cfgIntOpenDrain(false); + LOG_DEBUG("ICM20948Sensor::init set cfgIntOpenDrain - %s\n", statusString()); + + // If enabled, *ANY* read will clear the INT_STATUS register. + cfgIntAnyReadToClear(true); + LOG_DEBUG("ICM20948Sensor::init set cfgIntAnyReadToClear - %s\n", statusString()); + + // Latch the interrupt until cleared + cfgIntLatch(true); + LOG_DEBUG("ICM20948Sensor::init set cfgIntLatch - %s\n", statusString()); + + // Set up an interrupt pin with an internal pullup for active low + pinMode(ICM_20948_INT_PIN, INPUT_PULLUP); + + // Set up an interrupt service routine + attachInterrupt(ICM_20948_INT_PIN, ICM20948SetInterrupt, FALLING); + +#endif + return true; +} + +#ifdef ICM_20948_DMP_IS_ENABLED + +// Stub +bool ICM20948Sensor::initDMP() +{ + return false; +} + +#endif + +bool ICM20948Singleton::setWakeOnMotion() +{ + // Set WoM threshold in milli G's + auto status = WOMThreshold(ICM_20948_WOM_THRESHOLD); + if (status != ICM_20948_Stat_Ok) + return false; + + // Enable WoM Logic mode 1 = Compare the current sample with the previous sample + status = WOMLogic(true, 1); + LOG_DEBUG("ICM20948Sensor::init set WOMLogic - %s\n", statusString()); + if (status != ICM_20948_Stat_Ok) + return false; + + // Enable interrupts on WakeOnMotion + status = intEnableWOM(true); + LOG_DEBUG("ICM20948Sensor::init set intEnableWOM - %s\n", statusString()); + return status == ICM_20948_Stat_Ok; + + // Clear any current interrupts + ICM20948_IRQ = false; + clearInterrupts(); + return true; +} + +#endif \ No newline at end of file diff --git a/src/motion/ICM20948Sensor.h b/src/motion/ICM20948Sensor.h new file mode 100755 index 0000000000..373c1164f1 --- /dev/null +++ b/src/motion/ICM20948Sensor.h @@ -0,0 +1,96 @@ +#pragma once +#ifndef _ICM_20948_SENSOR_H_ +#define _ICM_20948_SENSOR_H_ + +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !defined(ARCH_NRF52) && !MESHTASTIC_EXCLUDE_I2C + +#include + +// Set the default gyro scale - dps250, dps500, dps1000, dps2000 +#ifndef ICM_20948_MPU_GYRO_SCALE +#define ICM_20948_MPU_GYRO_SCALE dps250 +#endif + +// Set the default accelerometer scale - gpm2, gpm4, gpm8, gpm16 +#ifndef ICM_20948_MPU_ACCEL_SCALE +#define ICM_20948_MPU_ACCEL_SCALE gpm2 +#endif + +// Define a threshold for Wake on Motion Sensing (0mg to 1020mg) +#ifndef ICM_20948_WOM_THRESHOLD +#define ICM_20948_WOM_THRESHOLD 16U +#endif + +// Define a pin in variant.h to use interrupts to read the ICM-20948 +#ifndef ICM_20948_WOM_THRESHOLD +#define ICM_20948_INT_PIN 255 +#endif + +// Uncomment this line to enable helpful debug messages on Serial +// #define ICM_20948_DEBUG 1 + +// Uncomment this line to enable the onboard digital motion processor (to be added in a future PR) +// #define ICM_20948_DMP_IS_ENABLED 1 + +// Check for a mandatory compiler flag to use the DMP (to be added in a future PR) +#ifdef ICM_20948_DMP_IS_ENABLED +#ifndef ICM_20948_USE_DMP +#error To use the digital motion processor, please either set the compiler flag ICM_20948_USE_DMP or uncomment line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h +#endif +#endif + +// The I2C address of the Accelerometer (if found) from main.cpp +extern ScanI2C::DeviceAddress accelerometer_found; + +// Singleton wrapper for the Sparkfun ICM_20948_I2C class +class ICM20948Singleton : public ICM_20948_I2C +{ + private: + static ICM20948Singleton *pinstance; + + protected: + ICM20948Singleton(); + ~ICM20948Singleton(); + + public: + // Create a singleton instance (not thread safe) + static ICM20948Singleton *GetInstance(); + + // Singletons should not be cloneable. + ICM20948Singleton(ICM20948Singleton &other) = delete; + + // Singletons should not be assignable. + void operator=(const ICM20948Singleton &) = delete; + + // Initialise the motion sensor singleton for normal operation + bool init(ScanI2C::FoundDevice device); + + // Enable Wake on Motion interrupts (sensor must be initialised first) + bool setWakeOnMotion(); + +#ifdef ICM_20948_DMP_IS_ENABLED + // Initialise the motion sensor singleton for digital motion processing + bool initDMP(); +#endif +}; + +class ICM20948Sensor : public MotionSensor +{ + private: + ICM20948Singleton *sensor = nullptr; + + public: + explicit ICM20948Sensor(ScanI2C::FoundDevice foundDevice); + + // Initialise the motion sensor + virtual bool init() override; + + // Called each time our sensor gets a chance to run + virtual int32_t runOnce() override; +}; + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/LIS3DHSensor.cpp b/src/motion/LIS3DHSensor.cpp new file mode 100755 index 0000000000..e2df60b1ce --- /dev/null +++ b/src/motion/LIS3DHSensor.cpp @@ -0,0 +1,36 @@ +#include "LIS3DHSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +LIS3DHSensor::LIS3DHSensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {} + +bool LIS3DHSensor::init() +{ + if (sensor.begin(deviceAddress())) { + sensor.setRange(LIS3DH_RANGE_2_G); + // Adjust threshold, higher numbers are less sensitive + sensor.setClick(config.device.double_tap_as_button_press ? 2 : 1, MOTION_SENSOR_CHECK_INTERVAL_MS); + LOG_DEBUG("LIS3DHSensor::init ok\n"); + return true; + } + LOG_DEBUG("LIS3DHSensor::init failed\n"); + return false; +} + +int32_t LIS3DHSensor::runOnce() +{ + if (sensor.getClick() > 0) { + uint8_t click = sensor.getClick(); + if (!config.device.double_tap_as_button_press) { + wakeScreen(); + } + + if (config.device.double_tap_as_button_press && (click & 0x20)) { + buttonPress(); + return 500; + } + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#endif \ No newline at end of file diff --git a/src/motion/LIS3DHSensor.h b/src/motion/LIS3DHSensor.h new file mode 100755 index 0000000000..603d195a80 --- /dev/null +++ b/src/motion/LIS3DHSensor.h @@ -0,0 +1,24 @@ +#pragma once +#ifndef _LIS3DH_SENSOR_H_ +#define _LIS3DH_SENSOR_H_ + +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#include + +class LIS3DHSensor : public MotionSensor +{ + private: + Adafruit_LIS3DH sensor; + + public: + explicit LIS3DHSensor(ScanI2C::FoundDevice foundDevice); + virtual bool init() override; + virtual int32_t runOnce() override; +}; + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/LSM6DS3Sensor.cpp b/src/motion/LSM6DS3Sensor.cpp new file mode 100755 index 0000000000..64ef9a23b1 --- /dev/null +++ b/src/motion/LSM6DS3Sensor.cpp @@ -0,0 +1,33 @@ +#include "LSM6DS3Sensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +LSM6DS3Sensor::LSM6DS3Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {} + +bool LSM6DS3Sensor::init() +{ + if (sensor.begin_I2C(deviceAddress())) { + + // Default threshold of 2G, less sensitive options are 4, 8 or 16G + sensor.setAccelRange(LSM6DS_ACCEL_RANGE_2_G); + + // Duration is number of occurances needed to trigger, higher threshold is less sensitive + sensor.enableWakeup(config.display.wake_on_tap_or_motion, 1, LSM6DS3_WAKE_THRESH); + + LOG_DEBUG("LSM6DS3Sensor::init ok\n"); + return true; + } + LOG_DEBUG("LSM6DS3Sensor::init failed\n"); + return false; +} + +int32_t LSM6DS3Sensor::runOnce() +{ + if (sensor.shake()) { + wakeScreen(); + return 500; + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#endif \ No newline at end of file diff --git a/src/motion/LSM6DS3Sensor.h b/src/motion/LSM6DS3Sensor.h new file mode 100755 index 0000000000..77069ef3c3 --- /dev/null +++ b/src/motion/LSM6DS3Sensor.h @@ -0,0 +1,28 @@ +#pragma once +#ifndef _LSM6DS3_SENSOR_H_ +#define _LSM6DS3_SENSOR_H_ + +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#ifndef LSM6DS3_WAKE_THRESH +#define LSM6DS3_WAKE_THRESH 20 +#endif + +#include + +class LSM6DS3Sensor : public MotionSensor +{ + private: + Adafruit_LSM6DS3TRC sensor; + + public: + explicit LSM6DS3Sensor(ScanI2C::FoundDevice foundDevice); + virtual bool init() override; + virtual int32_t runOnce() override; +}; + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/MPU6050Sensor.cpp b/src/motion/MPU6050Sensor.cpp new file mode 100755 index 0000000000..77aaca46d6 --- /dev/null +++ b/src/motion/MPU6050Sensor.cpp @@ -0,0 +1,31 @@ +#include "MPU6050Sensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +MPU6050Sensor::MPU6050Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {} + +bool MPU6050Sensor::init() +{ + if (sensor.begin(deviceAddress())) { + // setup motion detection + sensor.setHighPassFilter(MPU6050_HIGHPASS_0_63_HZ); + sensor.setMotionDetectionThreshold(1); + sensor.setMotionDetectionDuration(20); + sensor.setInterruptPinLatch(true); // Keep it latched. Will turn off when reinitialized. + sensor.setInterruptPinPolarity(true); + LOG_DEBUG("MPU6050Sensor::init ok\n"); + return true; + } + LOG_DEBUG("MPU6050Sensor::init failed\n"); + return false; +} + +int32_t MPU6050Sensor::runOnce() +{ + if (sensor.getMotionInterruptStatus()) { + wakeScreen(); + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#endif \ No newline at end of file diff --git a/src/motion/MPU6050Sensor.h b/src/motion/MPU6050Sensor.h new file mode 100755 index 0000000000..2e6eafecd4 --- /dev/null +++ b/src/motion/MPU6050Sensor.h @@ -0,0 +1,24 @@ +#pragma once +#ifndef _MPU6050_SENSOR_H_ +#define _MPU6050_SENSOR_H_ + +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#include + +class MPU6050Sensor : public MotionSensor +{ + private: + Adafruit_MPU6050 sensor; + + public: + explicit MPU6050Sensor(ScanI2C::FoundDevice foundDevice); + virtual bool init() override; + virtual int32_t runOnce() override; +}; + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/MotionSensor.cpp b/src/motion/MotionSensor.cpp new file mode 100755 index 0000000000..20e396bba5 --- /dev/null +++ b/src/motion/MotionSensor.cpp @@ -0,0 +1,79 @@ +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +// screen is defined in main.cpp +extern graphics::Screen *screen; + +MotionSensor::MotionSensor(ScanI2C::FoundDevice foundDevice) +{ + device.address.address = foundDevice.address.address; + device.address.port = foundDevice.address.port; + device.type = foundDevice.type; + LOG_DEBUG("MotionSensor::MotionSensor port: %s address: 0x%x type: %d\n", + devicePort() == ScanI2C::I2CPort::WIRE1 ? "Wire1" : "Wire", (uint8_t)deviceAddress(), deviceType()); +} + +ScanI2C::DeviceType MotionSensor::deviceType() +{ + return device.type; +} + +uint8_t MotionSensor::deviceAddress() +{ + return device.address.address; +} + +ScanI2C::I2CPort MotionSensor::devicePort() +{ + return device.address.port; +} + +#ifdef RAK_4631 +void MotionSensor::drawFrameCalibration(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\nCompass"); + int16_t compassX = 0, compassY = 0; + uint16_t compassDiam = graphics::Screen::getCompassDiam(display->getWidth(), display->getHeight()); + + // coordinates for the center of the compass/circle + if (config.display.displaymode == meshtastic_Config_DisplayConfig_DisplayMode_DEFAULT) { + compassX = x + display->getWidth() - compassDiam / 2 - 5; + compassY = y + display->getHeight() / 2; + } else { + compassX = x + display->getWidth() - compassDiam / 2 - 5; + compassY = y + FONT_HEIGHT_SMALL + (display->getHeight() - FONT_HEIGHT_SMALL) / 2; + } + display->drawCircle(compassX, compassY, compassDiam / 2); + screen->drawCompassNorth(display, compassX, compassY, screen->getHeading() * PI / 180); +} +#endif + +#if !MESHTASTIC_EXCLUDE_POWER_FSM +void MotionSensor::wakeScreen() +{ + if (powerFSM.getState() == &stateDARK) { + LOG_DEBUG("MotionSensor::wakeScreen detected\n"); + powerFSM.trigger(EVENT_INPUT); + } +} + +void MotionSensor::buttonPress() +{ + LOG_DEBUG("MotionSensor::buttonPress detected\n"); + powerFSM.trigger(EVENT_PRESS); +} + +#else + +void MotionSensor::wakeScreen() {} + +void MotionSensor::buttonPress() {} + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/MotionSensor.h b/src/motion/MotionSensor.h new file mode 100755 index 0000000000..0f7f3479b2 --- /dev/null +++ b/src/motion/MotionSensor.h @@ -0,0 +1,85 @@ +#pragma once +#ifndef _MOTION_SENSOR_H_ +#define _MOTION_SENSOR_H_ + +#define MOTION_SENSOR_CHECK_INTERVAL_MS 100 +#define MOTION_SENSOR_CLICK_THRESHOLD 40 + +#include "../configuration.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#include "../PowerFSM.h" +#include "../detect/ScanI2C.h" +#include "../graphics/Screen.h" +#include "../graphics/ScreenFonts.h" +#include "../power.h" + +// Base class for motion processing +class MotionSensor +{ + public: + explicit MotionSensor(ScanI2C::FoundDevice foundDevice); + virtual ~MotionSensor(){}; + + // Get the device type + ScanI2C::DeviceType deviceType(); + + // Get the device address + uint8_t deviceAddress(); + + // Get the device port + ScanI2C::I2CPort devicePort(); + + // Initialise the motion sensor + inline virtual bool init() { return false; }; + + // The method that will be called each time our sensor gets a chance to run + // Returns the desired period for next invocation (or RUN_SAME for no change) + // Refer to /src/concurrency/OSThread.h for more information + inline virtual int32_t runOnce() { return MOTION_SENSOR_CHECK_INTERVAL_MS; }; + + protected: + // Turn on the screen when a tap or motion is detected + virtual void wakeScreen(); + + // Register a button press when a double-tap is detected + virtual void buttonPress(); + +#ifdef RAK_4631 + // 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); +#endif + + ScanI2C::FoundDevice device; +}; + +namespace MotionSensorI2C +{ + +static inline int readRegister(uint8_t address, uint8_t reg, uint8_t *data, uint8_t len) +{ + Wire.beginTransmission(address); + Wire.write(reg); + Wire.endTransmission(); + Wire.requestFrom((uint8_t)address, (uint8_t)len); + uint8_t i = 0; + while (Wire.available()) { + data[i++] = Wire.read(); + } + return 0; // Pass +} + +static inline int writeRegister(uint8_t address, uint8_t reg, uint8_t *data, uint8_t len) +{ + Wire.beginTransmission(address); + Wire.write(reg); + Wire.write(data, len); + return (0 != Wire.endTransmission()); +} + +} // namespace MotionSensorI2C + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/STK8XXXSensor.cpp b/src/motion/STK8XXXSensor.cpp new file mode 100755 index 0000000000..d4d69ef992 --- /dev/null +++ b/src/motion/STK8XXXSensor.cpp @@ -0,0 +1,40 @@ +#include "STK8XXXSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +STK8XXXSensor::STK8XXXSensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {} + +#ifdef STK8XXX_INT + +volatile static bool STK_IRQ; + +bool STK8XXXSensor::init() +{ + if (sensor.STK8xxx_Initialization(STK8xxx_VAL_RANGE_2G)) { + STK_IRQ = false; + sensor.STK8xxx_Anymotion_init(); + pinMode(STK8XXX_INT, INPUT_PULLUP); + attachInterrupt( + digitalPinToInterrupt(STK8XXX_INT), [] { STK_IRQ = true; }, RISING); + + LOG_DEBUG("STK8XXXSensor::init ok\n"); + return true; + } + LOG_DEBUG("STK8XXXSensor::init failed\n"); + return false; +} + +int32_t STK8XXXSensor::runOnce() +{ + if (STK_IRQ) { + STK_IRQ = false; + if (config.display.wake_on_tap_or_motion) { + wakeScreen(); + } + } + return MOTION_SENSOR_CHECK_INTERVAL_MS; +} + +#endif + +#endif \ No newline at end of file diff --git a/src/motion/STK8XXXSensor.h b/src/motion/STK8XXXSensor.h new file mode 100755 index 0000000000..190b916b45 --- /dev/null +++ b/src/motion/STK8XXXSensor.h @@ -0,0 +1,37 @@ +#pragma once +#ifndef _STK8XXX_SENSOR_H_ +#define _STK8XXX_SENSOR_H_ + +#include "MotionSensor.h" + +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C + +#ifdef STK8XXX_INT + +#include + +class STK8XXXSensor : public MotionSensor +{ + private: + STK8xxx sensor; + + public: + explicit STK8XXXSensor(ScanI2C::FoundDevice foundDevice); + virtual bool init() override; + virtual int32_t runOnce() override; +}; + +#else + +// Stub +class STK8XXXSensor : public MotionSensor +{ + public: + explicit STK8XXXSensor(ScanI2C::FoundDevice foundDevice); +}; + +#endif + +#endif + +#endif \ No newline at end of file From c97794cfa91aed75f7d47244b4801ed4dda7be77 Mon Sep 17 00:00:00 2001 From: David <2941290+dhskinner@users.noreply.github.com> Date: Mon, 23 Sep 2024 23:28:30 +1000 Subject: [PATCH 6/8] Tidy up --- platformio.ini | 3 ++- src/modules/AdminModule.cpp | 2 +- src/motion/AccelerometerThread.h | 2 -- src/motion/ICM20948Sensor.cpp | 8 ++------ src/motion/ICM20948Sensor.h | 2 +- 5 files changed, 6 insertions(+), 11 deletions(-) diff --git a/platformio.ini b/platformio.ini index 9b869a0364..01d0416ca1 100644 --- a/platformio.ini +++ b/platformio.ini @@ -148,6 +148,7 @@ lib_deps = adafruit/Adafruit SHT4x Library@^1.0.4 adafruit/Adafruit TSL2591 Library@^1.4.5 sparkfun/SparkFun Qwiic Scale NAU7802 Arduino Library@^1.0.5 + sparkfun/SparkFun 9DoF IMU Breakout - ICM 20948 - Arduino Library@^1.2.13 ClosedCube OPT3001@^1.1.2 emotibit/EmotiBit MLX90632@^1.0.8 dfrobot/DFRobot_RTU@^1.0.3 @@ -161,4 +162,4 @@ lib_deps = https://github.com/meshtastic/DFRobot_LarkWeatherStation#dee914270dc7cb3e43fbf034edd85a63a16a12ee - https://github.com/gjelsoe/STK8xxx-Accelerometer.git#v0.1.1 + https://github.com/gjelsoe/STK8xxx-Accelerometer.git#v0.1.1 \ No newline at end of file diff --git a/src/modules/AdminModule.cpp b/src/modules/AdminModule.cpp index 64cf12aa43..389a0db8d8 100644 --- a/src/modules/AdminModule.cpp +++ b/src/modules/AdminModule.cpp @@ -34,7 +34,7 @@ #include "modules/PositionModule.h" #endif -#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C #include "motion/AccelerometerThread.h" #endif diff --git a/src/motion/AccelerometerThread.h b/src/motion/AccelerometerThread.h index a4daf345e4..7344bdfcbf 100755 --- a/src/motion/AccelerometerThread.h +++ b/src/motion/AccelerometerThread.h @@ -94,11 +94,9 @@ class AccelerometerThread : public concurrency::OSThread case ScanI2C::DeviceType::STK8BAXX: sensor = new STK8XXXSensor(device); break; -#ifndef ARCH_NRF52 case ScanI2C::DeviceType::ICM20948: sensor = new ICM20948Sensor(device); break; -#endif default: disable(); return; diff --git a/src/motion/ICM20948Sensor.cpp b/src/motion/ICM20948Sensor.cpp index bd524dbf5e..f4ac2bee49 100755 --- a/src/motion/ICM20948Sensor.cpp +++ b/src/motion/ICM20948Sensor.cpp @@ -1,6 +1,6 @@ #include "ICM20948Sensor.h" -#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !defined(ARCH_NRF52) && !MESHTASTIC_EXCLUDE_I2C +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C // Flag when an interrupt has been detected volatile static bool ICM20948_IRQ = false; @@ -70,8 +70,6 @@ int32_t ICM20948Sensor::runOnce() // ICM20948Singleton // ---------------------------------------------------------------------- -#ifndef ARCH_NRF52 - // Get a singleton wrapper for an Sparkfun ICM_20948_I2C ICM20948Singleton *ICM20948Singleton::GetInstance() { @@ -185,6 +183,4 @@ bool ICM20948Singleton::setWakeOnMotion() ICM20948_IRQ = false; clearInterrupts(); return true; -} - -#endif \ No newline at end of file +} \ No newline at end of file diff --git a/src/motion/ICM20948Sensor.h b/src/motion/ICM20948Sensor.h index 373c1164f1..d5e246c8de 100755 --- a/src/motion/ICM20948Sensor.h +++ b/src/motion/ICM20948Sensor.h @@ -4,7 +4,7 @@ #include "MotionSensor.h" -#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !defined(ARCH_NRF52) && !MESHTASTIC_EXCLUDE_I2C +#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C #include From 157263629c5416c6d0336fa92583ca3a162f3458 Mon Sep 17 00:00:00 2001 From: David <2941290+dhskinner@users.noreply.github.com> Date: Mon, 23 Sep 2024 23:48:52 +1000 Subject: [PATCH 7/8] Update ICM20948Sensor.cpp --- src/motion/ICM20948Sensor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/motion/ICM20948Sensor.cpp b/src/motion/ICM20948Sensor.cpp index f4ac2bee49..d16968e06d 100755 --- a/src/motion/ICM20948Sensor.cpp +++ b/src/motion/ICM20948Sensor.cpp @@ -64,8 +64,6 @@ int32_t ICM20948Sensor::runOnce() #endif -#endif - // ---------------------------------------------------------------------- // ICM20948Singleton // ---------------------------------------------------------------------- @@ -183,4 +181,6 @@ bool ICM20948Singleton::setWakeOnMotion() ICM20948_IRQ = false; clearInterrupts(); return true; -} \ No newline at end of file +} + +#endif \ No newline at end of file From f87998d18a40e03e890c17846afd07599fb73ee3 Mon Sep 17 00:00:00 2001 From: David <2941290+dhskinner@users.noreply.github.com> Date: Tue, 24 Sep 2024 08:14:46 +1000 Subject: [PATCH 8/8] Update AccelerometerThread.h --- src/motion/AccelerometerThread.h | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/src/motion/AccelerometerThread.h b/src/motion/AccelerometerThread.h index 7344bdfcbf..f9cbb49a19 100755 --- a/src/motion/AccelerometerThread.h +++ b/src/motion/AccelerometerThread.h @@ -110,13 +110,28 @@ class AccelerometerThread : public concurrency::OSThread } // Copy constructor (not implemented / included to avoid cppcheck warnings) - AccelerometerThread(const AccelerometerThread &other) : OSThread::OSThread("AccelerometerThread") {} + AccelerometerThread(const AccelerometerThread &other) : OSThread::OSThread("AccelerometerThread") { this->copy(other); } // Destructor (included to avoid cppcheck warnings) virtual ~AccelerometerThread() { clean(); } // Copy assignment (not implemented / included to avoid cppcheck warnings) - AccelerometerThread &operator=(const AccelerometerThread &other) { return *this; } + AccelerometerThread &operator=(const AccelerometerThread &other) + { + this->copy(other); + return *this; + } + + // Take a very shallow copy (does not copy OSThread state nor the sensor object) + // If for some reason this is ever used, make sure to call init() after any copy + void copy(const AccelerometerThread &other) + { + if (this != &other) { + clean(); + this->device = ScanI2C::FoundDevice(other.device.type, + ScanI2C::DeviceAddress(other.device.address.port, other.device.address.address)); + } + } // Cleanup resources void clean()