mirror of
https://github.com/alexhopeoconnor/firmware.git
synced 2026-10-13 15:11:20 +10:00
745b53698a
* Fixes * Remove BATTERY_LPCOMP_THRESHOLD BATTERY_LPCOMP_THRESHOLD is dead code — in main-nrf52.cpp it's inside #ifdef BATTERY_LPCOMP_INPUT, which this board intentionally doesn't define. The threshold value is never reached. * Trunk fix * Update MotionSensor.cpp * fix --------- Co-authored-by: Ben Meadors <benmmeadors@gmail.com> Co-authored-by: Jonathan Bennett <jbennett@incomsystems.biz>
111 lines
3.4 KiB
C++
111 lines
3.4 KiB
C++
#include "ICM42607PSensor.h"
|
|
|
|
#if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C && __has_include(<ICM42670P.h>)
|
|
|
|
#include "detect/ScanI2CTwoWire.h"
|
|
#include <ICM42670P.h>
|
|
#include <math.h>
|
|
|
|
static constexpr uint16_t ICM42607P_ACCEL_ODR_HZ = 50;
|
|
static constexpr uint16_t ICM42607P_ACCEL_FSR_G = 2;
|
|
static constexpr float ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG_VALUE =
|
|
#ifdef ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG
|
|
ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG;
|
|
#else
|
|
0.0f;
|
|
#endif
|
|
|
|
#ifdef ICM_42607P_INT_PIN
|
|
volatile static bool ICM42607P_IRQ = false;
|
|
|
|
void ICM42607PSetInterrupt()
|
|
{
|
|
ICM42607P_IRQ = true;
|
|
}
|
|
#endif
|
|
|
|
ICM42607PSensor::ICM42607PSensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {}
|
|
|
|
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());
|
|
TwoWire *wire = ScanI2CTwoWire::fetchI2CBus(device.address);
|
|
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
|
|
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;
|
|
}
|
|
|
|
float ax = static_cast<float>(event.accel[0]);
|
|
float ay = static_cast<float>(event.accel[1]);
|
|
const float az = static_cast<float>(event.accel[2]);
|
|
|
|
if (ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG_VALUE != 0.0f) {
|
|
static const float rotRad = ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG_VALUE * DEG_TO_RAD;
|
|
static const float cosTheta = cosf(rotRad);
|
|
static const float sinTheta = sinf(rotRad);
|
|
const float rotatedX = (ax * cosTheta) - (ay * sinTheta);
|
|
const float rotatedY = (ax * sinTheta) + (ay * cosTheta);
|
|
ax = rotatedX;
|
|
ay = rotatedY;
|
|
}
|
|
|
|
// Match the accel sign convention used by other FusionCompass sensor paths.
|
|
publishCompassAccelSample(ax, -ay, -az);
|
|
|
|
return MOTION_SENSOR_CHECK_INTERVAL_MS;
|
|
#endif
|
|
}
|
|
|
|
#endif
|