mirror of
https://github.com/meshtastic/firmware.git
synced 2025-04-27 10:21:40 +00:00
b5d7718319
* Move waypoint screen draw into the waypoint module * Get the observer set up for the waypoint screen draw * Static squashing: screen dimensions Macros moved back to Screen.cpp, as a band-aid until we eventually move all those static functions into the Screen class. * Move getCompassDiam into Screen class (supress compiler warnings) At this stage, the method is still static, because it's used by drawNodeInfo, which has no tidy reference to our screen instance. This is probably just another band-aid until these static functions all move. * Use new getCompassDiam function in AccelerometerThread * Properly gate display code in WaypointModule --------- Co-authored-by: Todd Herbert <herbert.todd@gmail.com>
297 lines
11 KiB
C++
297 lines
11 KiB
C++
#pragma once
|
|
#include "configuration.h"
|
|
|
|
#if !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR
|
|
|
|
#include "PowerFSM.h"
|
|
#include "concurrency/OSThread.h"
|
|
#include "main.h"
|
|
#include "power.h"
|
|
|
|
#include <Adafruit_LIS3DH.h>
|
|
#include <Adafruit_LSM6DS3TRC.h>
|
|
#include <Adafruit_MPU6050.h>
|
|
#include <Arduino.h>
|
|
#include <SensorBMA423.hpp>
|
|
#include <Wire.h>
|
|
#ifdef RAK_4631
|
|
#include "Fusion/Fusion.h"
|
|
#include "graphics/Screen.h"
|
|
#include "graphics/ScreenFonts.h"
|
|
#include <Rak_BMX160.h>
|
|
#endif
|
|
|
|
#define ACCELEROMETER_CHECK_INTERVAL_MS 100
|
|
#define ACCELEROMETER_CLICK_THRESHOLD 40
|
|
|
|
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::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);
|
|
} 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;
|
|
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 |