364 lines
11 KiB
C++
Executable File
364 lines
11 KiB
C++
Executable File
#include "MotionSensor.h"
|
|
#include "FSCommon.h"
|
|
#include "SPILock.h"
|
|
#include "SafeFile.h"
|
|
#include "concurrency/LockGuard.h"
|
|
#include "graphics/draw/CompassRenderer.h"
|
|
|
|
#if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C
|
|
|
|
char timeRemainingBuffer[12];
|
|
|
|
namespace
|
|
{
|
|
constexpr uint32_t COMPASS_CALIBRATION_MAGIC = 0x4D43414CL; // "MCAL"
|
|
constexpr uint16_t COMPASS_CALIBRATION_VERSION = 1;
|
|
|
|
struct CompassCalibrationRecord {
|
|
uint32_t magic;
|
|
uint16_t version;
|
|
uint16_t reserved;
|
|
float highestX;
|
|
float lowestX;
|
|
float highestY;
|
|
float lowestY;
|
|
float highestZ;
|
|
float lowestZ;
|
|
};
|
|
|
|
bool isRangeValid(float highest, float lowest)
|
|
{
|
|
// NaN/Inf guard without pulling in extra math helpers.
|
|
return (highest == highest) && (lowest == lowest) && (highest > lowest);
|
|
}
|
|
|
|
struct CompassAccelSample {
|
|
float x = 0.0f;
|
|
float y = 0.0f;
|
|
float z = 0.0f;
|
|
uint32_t sampledAtMs = 0;
|
|
bool valid = false;
|
|
};
|
|
|
|
concurrency::Lock latestCompassAccelLock;
|
|
CompassAccelSample latestCompassAccelSample;
|
|
} // namespace
|
|
|
|
// screen is defined in main.cpp
|
|
extern std::unique_ptr<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("Motion MotionSensor port: %s address: 0x%x type: %d", 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;
|
|
}
|
|
|
|
bool MotionSensor::saveMagnetometerCalibration(const char *filePath, float highestX, float lowestX, float highestY, float lowestY,
|
|
float highestZ, float lowestZ)
|
|
{
|
|
#ifdef FSCom
|
|
if (!isRangeValid(highestX, lowestX) || !isRangeValid(highestY, lowestY) || !isRangeValid(highestZ, lowestZ)) {
|
|
return false;
|
|
}
|
|
|
|
FSCom.mkdir("/prefs");
|
|
CompassCalibrationRecord record = {
|
|
COMPASS_CALIBRATION_MAGIC, COMPASS_CALIBRATION_VERSION, 0, highestX, lowestX, highestY, lowestY, highestZ, lowestZ};
|
|
|
|
auto file = SafeFile(filePath, true);
|
|
const size_t written = file.write(reinterpret_cast<const uint8_t *>(&record), sizeof(record));
|
|
return (written == sizeof(record)) && file.close();
|
|
#else
|
|
return false;
|
|
#endif
|
|
}
|
|
|
|
bool MotionSensor::loadMagnetometerCalibration(const char *filePath, float &highestX, float &lowestX, float &highestY,
|
|
float &lowestY, float &highestZ, float &lowestZ)
|
|
{
|
|
#ifdef FSCom
|
|
CompassCalibrationRecord record = {};
|
|
size_t bytesRead = 0;
|
|
|
|
spiLock->lock();
|
|
auto file = FSCom.open(filePath, FILE_O_READ);
|
|
if (!file) {
|
|
spiLock->unlock();
|
|
return false;
|
|
}
|
|
bytesRead = file.read(reinterpret_cast<uint8_t *>(&record), sizeof(record));
|
|
file.close();
|
|
spiLock->unlock();
|
|
|
|
const bool headerValid = (bytesRead == sizeof(record)) && (record.magic == COMPASS_CALIBRATION_MAGIC) &&
|
|
(record.version == COMPASS_CALIBRATION_VERSION) && (record.reserved == 0U);
|
|
const bool rangeValid = isRangeValid(record.highestX, record.lowestX) && isRangeValid(record.highestY, record.lowestY) &&
|
|
isRangeValid(record.highestZ, record.lowestZ);
|
|
if (!headerValid || !rangeValid) {
|
|
return false;
|
|
}
|
|
|
|
highestX = record.highestX;
|
|
lowestX = record.lowestX;
|
|
highestY = record.highestY;
|
|
lowestY = record.lowestY;
|
|
highestZ = record.highestZ;
|
|
lowestZ = record.lowestZ;
|
|
|
|
return true;
|
|
#else
|
|
return false;
|
|
#endif
|
|
}
|
|
|
|
void MotionSensor::beginCalibrationDisplay(bool &showingScreen)
|
|
{
|
|
#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
|
|
if (!showingScreen) {
|
|
powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration
|
|
showingScreen = true;
|
|
if (screen)
|
|
screen->startAlert((FrameCallback)drawFrameCalibration);
|
|
}
|
|
#else
|
|
(void)showingScreen;
|
|
#endif
|
|
}
|
|
|
|
void MotionSensor::finishCalibrationIfExpired(bool &showingScreen, const char *filePath, float highestX, float lowestX,
|
|
float highestY, float lowestY, float highestZ, float lowestZ)
|
|
{
|
|
const uint32_t now = millis();
|
|
if ((int32_t)(now - endCalibrationAt) < 0)
|
|
return;
|
|
|
|
doCalibration = false;
|
|
endCalibrationAt = 0;
|
|
showingScreen = false;
|
|
saveMagnetometerCalibration(filePath, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
|
|
|
|
#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
|
|
if (screen) {
|
|
screen->setEndCalibration(0);
|
|
screen->endAlert();
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void MotionSensor::startCalibrationWindow(uint16_t forSeconds)
|
|
{
|
|
doCalibration = true;
|
|
const uint32_t calibrateFor = static_cast<uint32_t>(forSeconds) * 1000U;
|
|
endCalibrationAt = millis() + calibrateFor;
|
|
#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
|
|
if (screen)
|
|
screen->setEndCalibration(endCalibrationAt);
|
|
#endif
|
|
}
|
|
|
|
void MotionSensor::seedCalibrationExtrema(float x, float y, float z, float &highestX, float &lowestX, float &highestY,
|
|
float &lowestY, float &highestZ, float &lowestZ)
|
|
{
|
|
highestX = lowestX = x;
|
|
highestY = lowestY = y;
|
|
highestZ = lowestZ = z;
|
|
}
|
|
|
|
void MotionSensor::updateCalibrationExtrema(float x, float y, float z, float &highestX, float &lowestX, float &highestY,
|
|
float &lowestY, float &highestZ, float &lowestZ)
|
|
{
|
|
if (x > highestX)
|
|
highestX = x;
|
|
if (x < lowestX)
|
|
lowestX = x;
|
|
if (y > highestY)
|
|
highestY = y;
|
|
if (y < lowestY)
|
|
lowestY = y;
|
|
if (z > highestZ)
|
|
highestZ = z;
|
|
if (z < lowestZ)
|
|
lowestZ = z;
|
|
}
|
|
|
|
float MotionSensor::applyCompassOrientation(float heading)
|
|
{
|
|
switch (config.display.compass_orientation) {
|
|
case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90:
|
|
case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90_INVERTED:
|
|
return heading + 90;
|
|
case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180:
|
|
case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180_INVERTED:
|
|
return heading + 180;
|
|
case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270:
|
|
case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED:
|
|
return heading + 270;
|
|
default:
|
|
return heading;
|
|
}
|
|
}
|
|
|
|
void MotionSensor::publishCompassAccelSample(float x, float y, float z)
|
|
{
|
|
concurrency::LockGuard guard(&latestCompassAccelLock);
|
|
latestCompassAccelSample.x = x;
|
|
latestCompassAccelSample.y = y;
|
|
latestCompassAccelSample.z = z;
|
|
latestCompassAccelSample.sampledAtMs = millis();
|
|
latestCompassAccelSample.valid = true;
|
|
}
|
|
|
|
bool MotionSensor::getLatestCompassAccelSample(float &x, float &y, float &z, uint32_t &ageMs)
|
|
{
|
|
uint32_t sampledAtMs = 0;
|
|
{
|
|
concurrency::LockGuard guard(&latestCompassAccelLock);
|
|
if (!latestCompassAccelSample.valid) {
|
|
return false;
|
|
}
|
|
|
|
x = latestCompassAccelSample.x;
|
|
y = latestCompassAccelSample.y;
|
|
z = latestCompassAccelSample.z;
|
|
sampledAtMs = latestCompassAccelSample.sampledAtMs;
|
|
}
|
|
|
|
ageMs = millis() - sampledAtMs;
|
|
return true;
|
|
}
|
|
|
|
#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
|
|
void MotionSensor::drawFrameCalibration(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
|
|
{
|
|
if (screen == nullptr)
|
|
return;
|
|
|
|
const int16_t width = display->getWidth();
|
|
const int16_t height = display->getHeight();
|
|
const bool compactLayout = (height <= 80);
|
|
const int16_t margin = 4;
|
|
|
|
const uint32_t now = millis();
|
|
const uint32_t endCalibrationAt = screen->getEndCalibration();
|
|
uint32_t timeRemaining = 0;
|
|
if (endCalibrationAt > now) {
|
|
timeRemaining = (endCalibrationAt - now + 999) / 1000;
|
|
}
|
|
|
|
int16_t compassX = 0, compassY = 0;
|
|
uint16_t compassDiam = graphics::CompassRenderer::getCompassDiam(width, height);
|
|
const int16_t compassRadius = compassDiam / 2;
|
|
|
|
// coordinates for the center of the compass/circle
|
|
if (config.display.displaymode == meshtastic_Config_DisplayConfig_DisplayMode_DEFAULT) {
|
|
compassX = x + width - compassRadius - margin;
|
|
compassY = y + height / 2;
|
|
} else {
|
|
compassX = x + width - compassRadius - margin;
|
|
compassY = y + FONT_HEIGHT_SMALL + (height - FONT_HEIGHT_SMALL) / 2;
|
|
}
|
|
|
|
const int16_t textLeft = x + 1;
|
|
const int16_t textRight = compassX - compassRadius - margin;
|
|
const int16_t textWidth = textRight - textLeft;
|
|
int16_t lineY = y;
|
|
|
|
display->setTextAlignment(TEXT_ALIGN_LEFT);
|
|
if (textWidth > 12) {
|
|
const char *title = "Cal";
|
|
const char *line1 = "Figure-8";
|
|
const char *line2 = "Rotate axes";
|
|
const char *line3 = "Away from metal";
|
|
|
|
display->setFont(FONT_SMALL);
|
|
if (!compactLayout && display->getStringWidth("Compass Calibration") <= textWidth) {
|
|
display->setFont(FONT_MEDIUM);
|
|
title = "Compass Calibration";
|
|
line1 = "Move in figure-8";
|
|
line2 = "Rotate all axes";
|
|
line3 = "Keep from metal";
|
|
display->drawString(textLeft, lineY, title);
|
|
lineY += FONT_HEIGHT_MEDIUM;
|
|
display->setFont(FONT_SMALL);
|
|
} else if (display->getStringWidth("Compass Cal") <= textWidth) {
|
|
title = "Compass Cal";
|
|
if (textWidth >= display->getStringWidth("Move in figure-8")) {
|
|
line1 = "Move in figure-8";
|
|
line2 = "Rotate all axes";
|
|
line3 = "Keep from metal";
|
|
}
|
|
display->drawString(textLeft, lineY, title);
|
|
lineY += FONT_HEIGHT_SMALL;
|
|
} else {
|
|
display->drawString(textLeft, lineY, title);
|
|
lineY += FONT_HEIGHT_SMALL;
|
|
}
|
|
|
|
display->drawString(textLeft, lineY, line1);
|
|
lineY += FONT_HEIGHT_SMALL;
|
|
display->drawString(textLeft, lineY, line2);
|
|
lineY += FONT_HEIGHT_SMALL;
|
|
if (!compactLayout || textWidth >= display->getStringWidth(line3)) {
|
|
display->drawString(textLeft, lineY, line3);
|
|
}
|
|
}
|
|
|
|
if (textWidth >= display->getStringWidth("000s left")) {
|
|
snprintf(timeRemainingBuffer, sizeof(timeRemainingBuffer), "%lus left", (unsigned long)timeRemaining);
|
|
} else {
|
|
snprintf(timeRemainingBuffer, sizeof(timeRemainingBuffer), "%lus", (unsigned long)timeRemaining);
|
|
}
|
|
display->setFont(FONT_SMALL);
|
|
if (textWidth > 12) {
|
|
display->drawString(textLeft, y + height - FONT_HEIGHT_SMALL - 1, timeRemainingBuffer);
|
|
}
|
|
|
|
display->drawCircle(compassX, compassY, compassDiam / 2);
|
|
graphics::CompassRenderer::drawCompassNorth(display, compassX, compassY, screen->getHeading() * PI / 180, (compassDiam / 2));
|
|
}
|
|
#endif
|
|
|
|
#if !MESHTASTIC_EXCLUDE_POWER_FSM
|
|
void MotionSensor::wakeScreen()
|
|
{
|
|
if (powerFSM.getState() == &stateDARK) {
|
|
LOG_DEBUG("Motion wakeScreen detected");
|
|
if (config.display.wake_on_tap_or_motion)
|
|
powerFSM.trigger(EVENT_INPUT);
|
|
}
|
|
}
|
|
|
|
void MotionSensor::buttonPress()
|
|
{
|
|
LOG_DEBUG("Motion buttonPress detected");
|
|
powerFSM.trigger(EVENT_PRESS);
|
|
}
|
|
|
|
#else
|
|
|
|
void MotionSensor::wakeScreen() {}
|
|
|
|
void MotionSensor::buttonPress() {}
|
|
|
|
#endif
|
|
|
|
#endif
|