Add Heltec mesh node t1 (#10416)
* add heltec-mesh-node-t1 * fixed low power * Update the sensor enumeration values. Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> * Fix memory leak in ICM42607PSensor * fix ST7735_MISO error --------- Co-authored-by: Ben Meadors <benmmeadors@gmail.com> Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
This commit is contained in:
Quency-D
co-authored by
GitHub
Ben Meadors
Copilot Autofix powered by AI
GitHub
Ben Meadors
Copilot Autofix powered by AI
parent
d3c7f05baa
commit
60303968bb
@@ -15,6 +15,7 @@
|
||||
#endif
|
||||
#include "BMM150Sensor.h"
|
||||
#include "BMX160Sensor.h"
|
||||
#include "ICM42607PSensor.h"
|
||||
#include "ICM20948Sensor.h"
|
||||
#include "LIS3DHSensor.h"
|
||||
#include "LSM6DS3Sensor.h"
|
||||
@@ -111,6 +112,9 @@ class AccelerometerThread : public concurrency::OSThread
|
||||
case ScanI2C::DeviceType::ICM20948:
|
||||
sensor = new ICM20948Sensor(device);
|
||||
break;
|
||||
case ScanI2C::DeviceType::ICM42607P:
|
||||
sensor = new ICM42607PSensor(device);
|
||||
break;
|
||||
case ScanI2C::DeviceType::BMM150:
|
||||
sensor = new BMM150Sensor(device);
|
||||
break;
|
||||
|
||||
@@ -0,0 +1,98 @@
|
||||
#include "ICM42607PSensor.h"
|
||||
|
||||
#if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C && __has_include(<ICM42670P.h>)
|
||||
|
||||
#include "detect/ScanI2CTwoWire.h"
|
||||
#include <ICM42670P.h>
|
||||
|
||||
static constexpr uint16_t ICM42607P_ACCEL_ODR_HZ = 50;
|
||||
static constexpr uint16_t ICM42607P_ACCEL_FSR_G = 2;
|
||||
static constexpr float ICM42607P_COUNTS_PER_G = 32768.0f / ICM42607P_ACCEL_FSR_G;
|
||||
|
||||
#ifdef ICM_42607P_INT_PIN
|
||||
volatile static bool ICM42607P_IRQ = false;
|
||||
|
||||
void ICM42607PSetInterrupt()
|
||||
{
|
||||
ICM42607P_IRQ = true;
|
||||
}
|
||||
#endif
|
||||
|
||||
ICM42607PSensor::ICM42607PSensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice)
|
||||
{
|
||||
wire = ScanI2CTwoWire::fetchI2CBus(foundDevice.address);
|
||||
}
|
||||
|
||||
ICM42607PSensor::~ICM42607PSensor() = default;
|
||||
|
||||
bool ICM42607PSensor::init()
|
||||
{
|
||||
bool addressLsb = deviceAddress() == ICM42607P_ADDR_ALT;
|
||||
|
||||
LOG_DEBUG("ICM-42607-P begin on addr 0x%02X (port=%d)", deviceAddress(), devicePort());
|
||||
sensor.reset();
|
||||
auto newSensor = std::make_unique<ICM42670>(*wire, addressLsb);
|
||||
|
||||
int status = newSensor->begin();
|
||||
// ICM42670P library returns -3 for ICM42607P because WHO_AM_I differs; the register map is compatible.
|
||||
if (status != 0 && status != -3) {
|
||||
LOG_DEBUG("ICM-42607-P init error %d", status);
|
||||
return false;
|
||||
}
|
||||
|
||||
status = newSensor->startAccel(ICM42607P_ACCEL_ODR_HZ, ICM42607P_ACCEL_FSR_G);
|
||||
if (status != 0) {
|
||||
LOG_DEBUG("ICM-42607-P accel start error %d", status);
|
||||
return false;
|
||||
}
|
||||
|
||||
#ifdef ICM_42607P_INT_PIN
|
||||
ICM42607P_IRQ = false;
|
||||
status = newSensor->startWakeOnMotion(ICM_42607P_INT_PIN, ICM42607PSetInterrupt);
|
||||
if (status != 0) {
|
||||
LOG_DEBUG("ICM-42607-P wake-on-motion start error %d", status);
|
||||
return false;
|
||||
}
|
||||
LOG_DEBUG("ICM-42607-P wake-on-motion interrupt ok pin=%d", ICM_42607P_INT_PIN);
|
||||
#endif
|
||||
|
||||
sensor = std::move(newSensor);
|
||||
LOG_DEBUG("ICM-42607-P init ok");
|
||||
return true;
|
||||
}
|
||||
|
||||
int32_t ICM42607PSensor::runOnce()
|
||||
{
|
||||
#ifdef ICM_42607P_INT_PIN
|
||||
if (ICM42607P_IRQ) {
|
||||
ICM42607P_IRQ = false;
|
||||
LOG_DEBUG("ICM-42607-P motion interrupt");
|
||||
wakeScreen();
|
||||
}
|
||||
return MOTION_SENSOR_CHECK_INTERVAL_MS;
|
||||
#else
|
||||
int16_t x = 0;
|
||||
int16_t y = 0;
|
||||
int16_t z = 0;
|
||||
inv_imu_sensor_event_t event = {};
|
||||
|
||||
if (sensor == nullptr || sensor->getDataFromRegisters(event) != 0) {
|
||||
return MOTION_SENSOR_CHECK_INTERVAL_MS;
|
||||
}
|
||||
|
||||
// getDataFromRegisters() fills accel[] but does not set sensor_mask in this library version.
|
||||
if (event.accel[0] == 0 && event.accel[1] == 0 && event.accel[2] == 0) {
|
||||
return MOTION_SENSOR_CHECK_INTERVAL_MS;
|
||||
}
|
||||
|
||||
x = event.accel[0];
|
||||
y = event.accel[1];
|
||||
z = event.accel[2];
|
||||
// LOG_DEBUG("ICM-42607-P accel read x=%.3fg y=%.3fg z=%.3fg", (float)x / ICM42607P_COUNTS_PER_G,
|
||||
// (float)y / ICM42607P_COUNTS_PER_G, (float)z / ICM42607P_COUNTS_PER_G);
|
||||
|
||||
return MOTION_SENSOR_CHECK_INTERVAL_MS;
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,28 @@
|
||||
#pragma once
|
||||
#ifndef _ICM42607P_SENSOR_H_
|
||||
#define _ICM42607P_SENSOR_H_
|
||||
|
||||
#include "MotionSensor.h"
|
||||
|
||||
#if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C && __has_include(<ICM42670P.h>)
|
||||
|
||||
#include <memory>
|
||||
|
||||
class ICM42670;
|
||||
|
||||
class ICM42607PSensor : public MotionSensor
|
||||
{
|
||||
private:
|
||||
std::unique_ptr<ICM42670> sensor;
|
||||
TwoWire *wire = nullptr;
|
||||
|
||||
public:
|
||||
explicit ICM42607PSensor(ScanI2C::FoundDevice foundDevice);
|
||||
~ICM42607PSensor() override;
|
||||
virtual bool init() override;
|
||||
virtual int32_t runOnce() override;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,115 @@
|
||||
#include "MMC5983MASensor.h"
|
||||
|
||||
#if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C && __has_include(<SparkFun_MMC5983MA_Arduino_Library.h>)
|
||||
|
||||
#include "detect/ScanI2CTwoWire.h"
|
||||
|
||||
#if !defined(MESHTASTIC_EXCLUDE_SCREEN)
|
||||
extern graphics::Screen *screen;
|
||||
#endif
|
||||
|
||||
static constexpr float MMC5983MA_ZERO_FIELD = 131072.0f;
|
||||
static constexpr float MMC5983MA_COUNTS_PER_GAUSS = 16384.0f;
|
||||
static constexpr uint16_t MMC5983MA_CONTINUOUS_FREQUENCY_HZ = 10;
|
||||
static constexpr float MMC5983MA_HEADING_OFFSET_DEG = 180.0f;
|
||||
|
||||
MMC5983MASensor::MMC5983MASensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {}
|
||||
|
||||
bool MMC5983MASensor::init()
|
||||
{
|
||||
LOG_DEBUG("MMC5983MA begin on addr 0x%02X (port=%d)", device.address.address, device.address.port);
|
||||
TwoWire *wire = ScanI2CTwoWire::fetchI2CBus(device.address);
|
||||
|
||||
if (!sensor.begin(*wire)) {
|
||||
LOG_DEBUG("MMC5983MA init error");
|
||||
return false;
|
||||
}
|
||||
|
||||
sensor.softReset();
|
||||
sensor.setFilterBandwidth(100);
|
||||
sensor.performSetOperation();
|
||||
sensor.enableAutomaticSetReset();
|
||||
continuousMode = sensor.setContinuousModeFrequency(MMC5983MA_CONTINUOUS_FREQUENCY_HZ);
|
||||
continuousMode &= sensor.enableContinuousMode();
|
||||
|
||||
if (!continuousMode) {
|
||||
LOG_DEBUG("MMC5983MA continuous mode failed, using single-shot reads");
|
||||
sensor.disableContinuousMode();
|
||||
}
|
||||
|
||||
loadMagnetometerCalibration(compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MMC5983MASensor::readMagnetometer(float &xGauss, float &yGauss, float &zGauss)
|
||||
{
|
||||
uint32_t rawX = 0;
|
||||
uint32_t rawY = 0;
|
||||
uint32_t rawZ = 0;
|
||||
|
||||
if (!(continuousMode ? sensor.readFieldsXYZ(&rawX, &rawY, &rawZ) : sensor.getMeasurementXYZ(&rawX, &rawY, &rawZ))) {
|
||||
LOG_DEBUG("MMC5983MA read failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
xGauss = ((float)rawX - MMC5983MA_ZERO_FIELD) / MMC5983MA_COUNTS_PER_GAUSS;
|
||||
yGauss = ((float)rawY - MMC5983MA_ZERO_FIELD) / MMC5983MA_COUNTS_PER_GAUSS;
|
||||
zGauss = ((float)rawZ - MMC5983MA_ZERO_FIELD) / MMC5983MA_COUNTS_PER_GAUSS;
|
||||
return true;
|
||||
}
|
||||
|
||||
int32_t MMC5983MASensor::runOnce()
|
||||
{
|
||||
float magX = 0, magY = 0, magZ = 0;
|
||||
if (!readMagnetometer(magX, magY, magZ)) {
|
||||
return MOTION_SENSOR_CHECK_INTERVAL_MS;
|
||||
}
|
||||
|
||||
#if !defined(MESHTASTIC_EXCLUDE_SCREEN)
|
||||
if (doCalibration) {
|
||||
beginCalibrationDisplay(showingScreen);
|
||||
updateCalibrationExtrema(magX, magY, magZ, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
|
||||
finishCalibrationIfExpired(showingScreen, compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ,
|
||||
lowestZ);
|
||||
}
|
||||
#endif
|
||||
|
||||
magX -= (highestX + lowestX) / 2;
|
||||
magY -= (highestY + lowestY) / 2;
|
||||
magZ -= (highestZ + lowestZ) / 2;
|
||||
|
||||
#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
|
||||
float heading = atan2f(magY, magX) * RAD_TO_DEG + MMC5983MA_HEADING_OFFSET_DEG;
|
||||
if (heading < 0.0f) {
|
||||
heading += 360.0f;
|
||||
} else if (heading >= 360.0f) {
|
||||
heading -= 360.0f;
|
||||
}
|
||||
|
||||
heading = applyCompassOrientation(heading);
|
||||
if (screen) {
|
||||
screen->setHeading(heading);
|
||||
}
|
||||
#endif
|
||||
|
||||
return MOTION_SENSOR_CHECK_INTERVAL_MS;
|
||||
}
|
||||
|
||||
void MMC5983MASensor::calibrate(uint16_t forSeconds)
|
||||
{
|
||||
#if !defined(MESHTASTIC_EXCLUDE_SCREEN)
|
||||
float xGauss = 0.0f;
|
||||
float yGauss = 0.0f;
|
||||
float zGauss = 0.0f;
|
||||
|
||||
LOG_DEBUG("MMC5983MA calibration started for %is", forSeconds);
|
||||
if (readMagnetometer(xGauss, yGauss, zGauss)) {
|
||||
seedCalibrationExtrema(xGauss, yGauss, zGauss, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
|
||||
} else {
|
||||
seedCalibrationExtrema(0.0f, 0.0f, 0.0f, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
|
||||
}
|
||||
startCalibrationWindow(forSeconds);
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,31 @@
|
||||
#pragma once
|
||||
#ifndef _MMC5983MA_SENSOR_H_
|
||||
#define _MMC5983MA_SENSOR_H_
|
||||
|
||||
#include "MotionSensor.h"
|
||||
|
||||
#if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C && __has_include(<SparkFun_MMC5983MA_Arduino_Library.h>)
|
||||
|
||||
#include <SparkFun_MMC5983MA_Arduino_Library.h>
|
||||
|
||||
class MMC5983MASensor : public MotionSensor
|
||||
{
|
||||
private:
|
||||
SFE_MMC5983MA sensor;
|
||||
bool continuousMode = false;
|
||||
bool showingScreen = false;
|
||||
static constexpr const char *compassCalibrationFileName = "/prefs/compass_mmc5983ma.dat";
|
||||
float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0;
|
||||
|
||||
bool readMagnetometer(float &xGauss, float &yGauss, float &zGauss);
|
||||
|
||||
public:
|
||||
explicit MMC5983MASensor(ScanI2C::FoundDevice foundDevice);
|
||||
virtual bool init() override;
|
||||
virtual int32_t runOnce() override;
|
||||
virtual void calibrate(uint16_t forSeconds) override;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,115 @@
|
||||
#pragma once
|
||||
#ifndef _MAGNETOMETER_THREAD_H_
|
||||
#define _MAGNETOMETER_THREAD_H_
|
||||
|
||||
#include "configuration.h"
|
||||
|
||||
#if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C && !MESHTASTIC_EXCLUDE_MAGNETOMETER
|
||||
|
||||
#include "../concurrency/OSThread.h"
|
||||
#include "MMC5983MASensor.h"
|
||||
#include "MotionSensor.h"
|
||||
|
||||
extern ScanI2C::DeviceAddress magnetometer_found;
|
||||
|
||||
class MagnetometerThread : public concurrency::OSThread
|
||||
{
|
||||
private:
|
||||
MotionSensor *sensor = nullptr;
|
||||
ScanI2C::FoundDevice device;
|
||||
bool isInitialised = false;
|
||||
|
||||
public:
|
||||
explicit MagnetometerThread(ScanI2C::FoundDevice foundDevice) : OSThread("Magnetometer")
|
||||
{
|
||||
device = foundDevice;
|
||||
init();
|
||||
}
|
||||
|
||||
explicit MagnetometerThread(ScanI2C::DeviceType type) : MagnetometerThread(ScanI2C::FoundDevice{type, magnetometer_found}) {}
|
||||
|
||||
void start()
|
||||
{
|
||||
init();
|
||||
setIntervalFromNow(0);
|
||||
};
|
||||
|
||||
void calibrate(uint16_t forSeconds)
|
||||
{
|
||||
if (sensor) {
|
||||
sensor->calibrate(forSeconds);
|
||||
setIntervalFromNow(0);
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
int32_t runOnce() override
|
||||
{
|
||||
canSleep = true;
|
||||
|
||||
if (isInitialised) {
|
||||
return sensor->runOnce();
|
||||
}
|
||||
|
||||
return MOTION_SENSOR_CHECK_INTERVAL_MS;
|
||||
}
|
||||
|
||||
private:
|
||||
void init()
|
||||
{
|
||||
if (isInitialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (device.address.port == ScanI2C::I2CPort::NO_I2C || device.address.address == 0 || device.type == ScanI2C::NONE) {
|
||||
LOG_DEBUG("MagnetometerThread Disable due to no sensors found");
|
||||
disable();
|
||||
return;
|
||||
}
|
||||
|
||||
switch (device.type) {
|
||||
case ScanI2C::DeviceType::MMC5983MA:
|
||||
sensor = new MMC5983MASensor(device);
|
||||
break;
|
||||
default:
|
||||
disable();
|
||||
return;
|
||||
}
|
||||
|
||||
isInitialised = sensor->init();
|
||||
if (!isInitialised) {
|
||||
clean();
|
||||
}
|
||||
LOG_DEBUG("MagnetometerThread::init %s", isInitialised ? "ok" : "failed");
|
||||
}
|
||||
|
||||
MagnetometerThread(const MagnetometerThread &other) : OSThread::OSThread("Magnetometer") { this->copy(other); }
|
||||
|
||||
virtual ~MagnetometerThread() { clean(); }
|
||||
|
||||
MagnetometerThread &operator=(const MagnetometerThread &other)
|
||||
{
|
||||
this->copy(other);
|
||||
return *this;
|
||||
}
|
||||
|
||||
void copy(const MagnetometerThread &other)
|
||||
{
|
||||
if (this != &other) {
|
||||
clean();
|
||||
this->device = ScanI2C::FoundDevice(other.device.type,
|
||||
ScanI2C::DeviceAddress(other.device.address.port, other.device.address.address));
|
||||
}
|
||||
}
|
||||
|
||||
void clean()
|
||||
{
|
||||
isInitialised = false;
|
||||
delete sensor;
|
||||
sensor = nullptr;
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user