mirror/firmware
1
0
Fork 0
mirror of https://github.com/meshtastic/firmware.git synced 2026-05-19 14:25:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Compass improvements/refactoring (#10166)

Browse Source 
* Infinite calibration loop fix

* Save calibration

* Screen refresh

* reduce repeated code

* reduce repeated code to reduce flash

* fix Waypoint compass size and no fix no heading labels

* Don't show compass unless we have a heading and location

* If no calculated heading from moving, we should have no heading

* Slow walking calculated heading and auto stale heading when not moving

* Triming flash space

* cleanup

* show "?" when no location or heading for distance and heading screen

* cleanup

* Stale heading logic

* final trim

* Compass Calibration screen redesign

* Trunk Fix

* Compile fix

* patch

* Update src/motion/MotionSensor.cpp

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* Update WaypointModule.cpp

---------

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
This commit is contained in:
HarukiToreda
2026-04-18 07:53:22 -04:00
committed by GitHub
parent 6208c243f9
commit aab4cd086f
15 changed files with 721 additions and 399 deletions
Download Patch File Download Diff File Expand all files Collapse all files

102
src/graphics/Screen.cpp
View File

@@ -60,6 +60,7 @@ along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "main.h"
#include "mesh-pb-constants.h"
#include "mesh/Channels.h"
#include "mesh/Default.h"
#include "mesh/generated/meshtastic/deviceonly.pb.h"
#include "modules/ExternalNotificationModule.h"
#include "modules/TextMessageModule.h"
@@ -98,6 +99,7 @@ namespace graphics
// This means the *visible* area (sh1106 can address 132, but shows 128 for example)
#define IDLE_FRAMERATE 1 // in fps
#define COMPASS_ACTIVE_FRAMERATE 20
// DEBUG
#define NUM_EXTRA_FRAMES 3 // text message and debug frame
@@ -135,6 +137,60 @@ static bool heartbeat = false;
extern bool hasUnreadMessage;
static inline float wrapHeading360(float heading)
{
if (heading < 0.0f) {
heading += 360.0f;
} else if (heading >= 360.0f) {
heading -= 360.0f;
}
return heading;
}
void Screen::setHeading(float heading)
{
const float wrappedHeading = wrapHeading360(heading);
if (!hasCompass) {
hasCompass = true;
compassHeading = wrappedHeading;
return;
}
// Interpolate using shortest-path angular delta to avoid jumps around 0/360.
float delta = wrappedHeading - compassHeading;
if (delta > 180.0f) {
delta -= 360.0f;
} else if (delta < -180.0f) {
delta += 360.0f;
}
// Adaptive filtering:
// - Strong damping for tiny deltas (jitter)
// - Faster response for larger turns
const float absDelta = (delta >= 0.0f) ? delta : -delta;
if (absDelta < 1.0f) {
return;
}
float alpha = 0.35f;
if (absDelta > 25.0f) {
alpha = 0.85f;
} else if (absDelta > 10.0f) {
alpha = 0.65f;
}
float step = delta * alpha;
const float maxStep = 12.0f;
if (step > maxStep) {
step = maxStep;
} else if (step < -maxStep) {
step = -maxStep;
}
compassHeading = wrapHeading360(compassHeading + step);
}
// ==============================
// Overlay Alert Banner Renderer
// ==============================
@@ -272,10 +328,25 @@ static void drawModuleFrame(OLEDDisplay *display, OLEDDisplayUiState *state, int
float Screen::estimatedHeading(double lat, double lon)
{
static double oldLat, oldLon;
static float b;
static float b = -1.0f;
static uint32_t lastHeadingAtMs = 0;
const uint32_t now = millis();
const uint32_t gpsUpdateIntervalSecs =
Default::getConfiguredOrDefault(config.position.gps_update_interval, default_gps_update_interval);
uint32_t effectiveUpdateIntervalSecs = gpsUpdateIntervalSecs;
if (config.position.position_broadcast_smart_enabled) {
const uint32_t smartMinIntervalSecs = Default::getConfiguredOrDefault(
config.position.broadcast_smart_minimum_interval_secs, default_broadcast_smart_minimum_interval_secs);
if (smartMinIntervalSecs > effectiveUpdateIntervalSecs) {
effectiveUpdateIntervalSecs = smartMinIntervalSecs;
}
}
// Two expected update windows; keep arithmetic 32-bit to avoid pulling in larger 64-bit helpers.
const uint32_t headingStaleMs =
(effectiveUpdateIntervalSecs > (UINT32_MAX / 2000U)) ? UINT32_MAX : (effectiveUpdateIntervalSecs * 2000U);
if (oldLat == 0) {
// just prepare for next time
// Need at least two position points before we can infer heading.
oldLat = lat;
oldLon = lon;
@@ -283,12 +354,20 @@ float Screen::estimatedHeading(double lat, double lon)
}
float d = GeoCoord::latLongToMeter(oldLat, oldLon, lat, lon);
if (d < 10) // haven't moved enough, just keep current bearing
if (d < 10) { // haven't moved enough, keep previous heading (invalid until first real movement)
if (lastHeadingAtMs != 0 && (now - lastHeadingAtMs) >= headingStaleMs) {
// Heading is stale after prolonged no-movement; force reacquire.
b = -1.0f;
oldLat = lat;
oldLon = lon;
}
return b;
}
b = GeoCoord::bearing(oldLat, oldLon, lat, lon) * RAD_TO_DEG;
oldLat = lat;
oldLon = lon;
lastHeadingAtMs = now;
return b;
}
@@ -923,9 +1002,22 @@ int32_t Screen::runOnce()
// but we should only call setTargetFPS when framestate changes, because
// otherwise that breaks animations.
if (targetFramerate != IDLE_FRAMERATE && ui->getUiState()->frameState == FIXED) {
uint32_t desiredFramerate = IDLE_FRAMERATE;
#if HAS_GPS && !defined(USE_EINK)
if (showingNormalScreen && hasCompass) {
const uint8_t currentFrame = ui->getUiState()->currentFrame;
if ((framesetInfo.positions.gps != 255 && currentFrame == framesetInfo.positions.gps) ||
(framesetInfo.positions.waypoint != 255 && currentFrame == framesetInfo.positions.waypoint) ||
(framesetInfo.positions.firstFavorite != 255 && currentFrame >= framesetInfo.positions.firstFavorite &&
currentFrame <= framesetInfo.positions.lastFavorite)) {
desiredFramerate = COMPASS_ACTIVE_FRAMERATE;
}
}
#endif
if (targetFramerate != desiredFramerate && ui->getUiState()->frameState == FIXED) {
// oldFrameState = ui->getUiState()->frameState;
targetFramerate = IDLE_FRAMERATE;
targetFramerate = desiredFramerate;
ui->setTargetFPS(targetFramerate);
forceDisplay();

10
src/graphics/Screen.h
View File

@@ -330,15 +330,11 @@ class Screen : public concurrency::OSThread
// Function to allow the AccelerometerThread to set the heading if a sensor provides it
// Mutex needed?
void setHeading(long _heading)
{
hasCompass = true;
compassHeading = fmod(_heading, 360);
}
void setHeading(float heading);
bool hasHeading() { return hasCompass; }
long getHeading() { return compassHeading; }
float getHeading() { return compassHeading; }
void setEndCalibration(uint32_t _endCalibrationAt) { endCalibrationAt = _endCalibrationAt; }
uint32_t getEndCalibration() { return endCalibrationAt; }
@@ -782,4 +778,4 @@ extern std::vector<std::string> functionSymbol;
extern std::string functionSymbolString;
extern graphics::Screen *screen;
#endif
#endif

76
src/graphics/draw/CompassRenderer.cpp
View File

@@ -1,10 +1,6 @@
#include "configuration.h"
#if HAS_SCREEN
#include "CompassRenderer.h"
#include "NodeDB.h"
#include "UIRenderer.h"
#include "configuration.h"
#include "gps/GeoCoord.h"
#include "graphics/ScreenFonts.h"
#include "graphics/SharedUIDisplay.h"
#include <cmath>
@@ -21,8 +17,8 @@ struct Point {
void rotate(float angle)
{
float cos_a = cos(angle);
float sin_a = sin(angle);
float cos_a = cosf(angle);
float sin_a = sinf(angle);
float new_x = x * cos_a - y * sin_a;
float new_y = x * sin_a + y * cos_a;
x = new_x;
@@ -51,21 +47,30 @@ void drawCompassNorth(OLEDDisplay *display, int16_t compassX, int16_t compassY,
if (currentResolution == ScreenResolution::High) {
radius += 4;
}
Point north(0, -radius);
if (uiconfig.compass_mode != meshtastic_CompassMode_FIXED_RING)
north.rotate(-myHeading);
north.translate(compassX, compassY);
float northX = 0.0f;
float northY = -radius;
if (uiconfig.compass_mode != meshtastic_CompassMode_FIXED_RING) {
const float c = cosf(-myHeading);
const float s = sinf(-myHeading);
const float rx = northX * c - northY * s;
const float ry = northX * s + northY * c;
northX = rx;
northY = ry;
}
northX += compassX;
northY += compassY;
display->setFont(FONT_SMALL);
display->setTextAlignment(TEXT_ALIGN_CENTER);
display->setColor(BLACK);
const int16_t nLabelWidth = display->getStringWidth("N");
if (currentResolution == ScreenResolution::High) {
display->fillRect(north.x - 8, north.y - 1, display->getStringWidth("N") + 3, FONT_HEIGHT_SMALL - 6);
display->fillRect(northX - 8, northY - 1, nLabelWidth + 3, FONT_HEIGHT_SMALL - 6);
} else {
display->fillRect(north.x - 4, north.y - 1, display->getStringWidth("N") + 2, FONT_HEIGHT_SMALL - 6);
display->fillRect(northX - 4, northY - 1, nLabelWidth + 2, FONT_HEIGHT_SMALL - 6);
}
display->setColor(WHITE);
display->drawString(north.x, north.y - 3, "N");
display->drawString(northX, northY - 3, "N");
}
void drawNodeHeading(OLEDDisplay *display, int16_t compassX, int16_t compassY, uint16_t compassDiam, float headingRadian)
@@ -113,11 +118,46 @@ void drawArrowToNode(OLEDDisplay *display, int16_t x, int16_t y, int16_t size, f
display->fillTriangle(tip.x, tip.y, right.x, right.y, tail.x, tail.y);
}
float estimatedHeading(double lat, double lon)
bool getHeadingRadians(double lat, double lon, float &headingRadian)
{
// Simple magnetic declination estimation
// This is a very basic implementation - the original might be more sophisticated
return 0.0f; // Return 0 for now, indicating no heading available
headingRadian = 0.0f;
if (uiconfig.compass_mode == meshtastic_CompassMode_FREEZE_HEADING)
return true;
if (!screen)
return false;
if (screen->hasHeading()) {
headingRadian = screen->getHeading() * DEG_TO_RAD;
return true;
}
const float estimatedHeadingDeg = screen->estimatedHeading(lat, lon);
if (!(estimatedHeadingDeg >= 0.0f))
return false;
headingRadian = estimatedHeadingDeg * DEG_TO_RAD;
return true;
}
float adjustBearingForCompassMode(float bearingRadian, float headingRadian)
{
if (uiconfig.compass_mode != meshtastic_CompassMode_FIXED_RING)
return bearingRadian - headingRadian;
return bearingRadian;
}
float radiansToDegrees360(float angleRadian)
{
constexpr float fullTurnDeg = 360.0f;
float degrees = angleRadian * RAD_TO_DEG;
if (degrees < 0.0f)
degrees += fullTurnDeg;
else if (degrees >= fullTurnDeg)
degrees -= fullTurnDeg;
return degrees;
}
uint16_t getCompassDiam(uint32_t displayWidth, uint32_t displayHeight)
@@ -137,4 +177,4 @@ uint16_t getCompassDiam(uint32_t displayWidth, uint32_t displayHeight)
} // namespace CompassRenderer
} // namespace graphics
#endif
#endif

5
src/graphics/draw/CompassRenderer.h
View File

@@ -1,7 +1,6 @@
#pragma once
#include "graphics/Screen.h"
#include "mesh/generated/meshtastic/mesh.pb.h"
#include <OLEDDisplay.h>
#include <OLEDDisplayUi.h>
@@ -25,7 +24,9 @@ void drawNodeHeading(OLEDDisplay *display, int16_t compassX, int16_t compassY, u
void drawArrowToNode(OLEDDisplay *display, int16_t x, int16_t y, int16_t size, float bearing);
// Navigation and location functions
float estimatedHeading(double lat, double lon);
bool getHeadingRadians(double lat, double lon, float &headingRadian);
float adjustBearingForCompassMode(float bearingRadian, float headingRadian);
float radiansToDegrees360(float angleRadian);
uint16_t getCompassDiam(uint32_t displayWidth, uint32_t displayHeight);
} // namespace CompassRenderer

71
src/graphics/draw/NodeListRenderer.cpp
View File

@@ -409,14 +409,13 @@ void drawNodeDistance(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16
}
}
if (strlen(distStr) > 0) {
int offset = (currentResolution == ScreenResolution::High)
? (isLeftCol ? 7 : 10) // Offset for Wide Screens (Left Column:Right Column)
: (isLeftCol ? 4 : 7); // Offset for Narrow Screens (Left Column:Right Column)
int rightEdge = x + columnWidth - offset;
int textWidth = display->getStringWidth(distStr);
display->drawString(rightEdge - textWidth, y, distStr);
}
const char *distanceLabel = (strlen(distStr) > 0) ? distStr : "?";
int offset = (currentResolution == ScreenResolution::High)
? (isLeftCol ? 7 : 10) // Offset for Wide Screens (Left Column:Right Column)
: (isLeftCol ? 4 : 7); // Offset for Narrow Screens (Left Column:Right Column)
int rightEdge = x + columnWidth - offset;
int textWidth = display->getStringWidth(distanceLabel);
display->drawString(rightEdge - textWidth, y, distanceLabel);
}
void drawEntryDynamic_Nodes(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16_t x, int16_t y, int columnWidth)
@@ -467,8 +466,8 @@ void drawEntryCompass(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16
}
}
void drawCompassArrow(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16_t x, int16_t y, int columnWidth, float myHeading,
double userLat, double userLon)
void drawCompassArrow(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16_t x, int16_t y, int columnWidth,
float myHeadingRadian, double userLat, double userLon)
{
if (!nodeDB->hasValidPosition(node))
return;
@@ -482,11 +481,11 @@ void drawCompassArrow(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16
double nodeLat = node->position.latitude_i * 1e-7;
double nodeLon = node->position.longitude_i * 1e-7;
float bearing = GeoCoord::bearing(userLat, userLon, nodeLat, nodeLon);
float bearingToNode = RAD_TO_DEG * bearing;
float relativeBearing = fmod((bearingToNode - myHeading + 360), 360);
float relativeBearing = CompassRenderer::adjustBearingForCompassMode(bearing, myHeadingRadian);
float relativeBearingDeg = CompassRenderer::radiansToDegrees360(relativeBearing);
// Shrink size by 2px
int size = FONT_HEIGHT_SMALL - 5;
CompassRenderer::drawArrowToNode(display, centerX, centerY, size, relativeBearing);
CompassRenderer::drawArrowToNode(display, centerX, centerY, size, relativeBearingDeg);
/*
float angle = relativeBearing * DEG_TO_RAD;
float halfSize = size / 2.0;
@@ -516,12 +515,27 @@ void drawCompassArrow(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16
*/
}
void drawCompassUnknown(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16_t x, int16_t y, int columnWidth, float, double,
double)
{
if (!nodeDB->hasValidPosition(node))
return;
bool isLeftCol = (x < SCREEN_WIDTH / 2);
int arrowXOffset = (currentResolution == ScreenResolution::High) ? (isLeftCol ? 22 : 24) : (isLeftCol ? 12 : 18);
int centerX = x + columnWidth - arrowXOffset;
display->setFont(FONT_SMALL);
display->setTextAlignment(TEXT_ALIGN_CENTER);
display->drawString(centerX, y, "?");
}
// =============================
// Main Screen Functions
// =============================
void drawNodeListScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y, const char *title,
EntryRenderer renderer, NodeExtrasRenderer extras, float heading, double lat, double lon)
EntryRenderer renderer, NodeExtrasRenderer extras, float headingRadian, double lat, double lon)
{
const int COMMON_HEADER_HEIGHT = FONT_HEIGHT_SMALL - 1;
const int rowYOffset = FONT_HEIGHT_SMALL - 3;
@@ -606,7 +620,7 @@ void drawNodeListScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t
renderer(display, node, xPos, yPos, columnWidth);
if (extras)
extras(display, node, xPos, yPos, columnWidth, heading, lat, lon);
extras(display, node, xPos, yPos, columnWidth, headingRadian, lat, lon);
lastNodeY = max(lastNodeY, yPos + FONT_HEIGHT_SMALL);
yOffset += rowYOffset;
@@ -801,9 +815,13 @@ void drawDistanceScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t
#endif
void drawNodeListWithCompasses(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
{
float heading = 0;
bool validHeading = false;
float headingRadian = 0.0f;
auto ourNode = nodeDB->getMeshNode(nodeDB->getNodeNum());
if (!ourNode || !nodeDB->hasValidPosition(ourNode)) {
drawNodeListScreen(display, state, x, y, "Bearings", drawEntryCompass, drawCompassUnknown, headingRadian, 0.0, 0.0);
return;
}
double lat = DegD(ourNode->position.latitude_i);
double lon = DegD(ourNode->position.longitude_i);
@@ -815,21 +833,12 @@ void drawNodeListWithCompasses(OLEDDisplay *display, OLEDDisplayUiState *state,
lastSwitchTime = now;
}
#endif
if (uiconfig.compass_mode != meshtastic_CompassMode_FREEZE_HEADING) {
#if HAS_GPS
if (screen->hasHeading()) {
heading = screen->getHeading(); // degrees
validHeading = true;
} else {
heading = screen->estimatedHeading(lat, lon);
validHeading = !isnan(heading);
}
#endif
if (!validHeading)
return;
if (!CompassRenderer::getHeadingRadians(lat, lon, headingRadian)) {
drawNodeListScreen(display, state, x, y, "Bearings", drawEntryCompass, drawCompassUnknown, headingRadian, lat, lon);
return;
}
drawNodeListScreen(display, state, x, y, "Bearings", drawEntryCompass, drawCompassArrow, heading, lat, lon);
drawNodeListScreen(display, state, x, y, "Bearings", drawEntryCompass, drawCompassArrow, headingRadian, lat, lon);
}
/// Draw a series of fields in a column, wrapping to multiple columns if needed

6
src/graphics/draw/NodeListRenderer.h
View File

@@ -32,7 +32,7 @@ enum ListMode_Location { MODE_DISTANCE = 0, MODE_BEARING = 1, MODE_COUNT_LOCATIO
// Main node list screen function
void drawNodeListScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y, const char *title,
EntryRenderer renderer, NodeExtrasRenderer extras = nullptr, float heading = 0, double lat = 0,
EntryRenderer renderer, NodeExtrasRenderer extras = nullptr, float headingRadian = 0, double lat = 0,
double lon = 0);
// Entry renderers
@@ -43,8 +43,8 @@ void drawEntryDynamic_Nodes(OLEDDisplay *display, meshtastic_NodeInfoLite *node,
void drawEntryCompass(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16_t x, int16_t y, int columnWidth);
// Extras renderers
void drawCompassArrow(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16_t x, int16_t y, int columnWidth, float myHeading,
double userLat, double userLon);
void drawCompassArrow(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16_t x, int16_t y, int columnWidth,
float myHeadingRadian, double userLat, double userLon);
// Screen frame functions
void drawLastHeardScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y);

231
src/graphics/draw/UIRenderer.cpp
View File

@@ -41,6 +41,15 @@ static inline void drawSatelliteIcon(OLEDDisplay *display, int16_t x, int16_t y)
}
}
static void drawCompassStatusText(OLEDDisplay *display, int16_t compassX, int16_t compassY, const char *statusLine1,
const char *statusLine2)
{
display->setTextAlignment(TEXT_ALIGN_CENTER);
display->drawString(compassX, compassY - FONT_HEIGHT_SMALL, statusLine1);
display->drawString(compassX, compassY, statusLine2);
display->setTextAlignment(TEXT_ALIGN_LEFT);
}
void graphics::UIRenderer::rebuildFavoritedNodes()
{
favoritedNodes.clear();
@@ -692,51 +701,54 @@ void UIRenderer::drawFavoriteNode(OLEDDisplay *display, OLEDDisplayUiState *stat
display->drawString(x, getTextPositions(display)[line++], batLine);
}
bool showCompass = false;
float myHeading = 0.0f;
float bearing = 0.0f;
const bool hasOwnPositionFix = (ourNode && nodeDB->hasValidPosition(ourNode));
const bool hasNodePositionFix = nodeDB->hasValidPosition(node);
const char *statusLine1 = nullptr;
const char *statusLine2 = nullptr;
if (hasOwnPositionFix && hasNodePositionFix) {
const auto &op = ourNode->position;
showCompass = CompassRenderer::getHeadingRadians(DegD(op.latitude_i), DegD(op.longitude_i), myHeading);
if (showCompass) {
const auto &p = node->position;
bearing = GeoCoord::bearing(DegD(op.latitude_i), DegD(op.longitude_i), DegD(p.latitude_i), DegD(p.longitude_i));
bearing = CompassRenderer::adjustBearingForCompassMode(bearing, myHeading);
} else {
statusLine1 = "No";
statusLine2 = "Heading";
}
} else if (!hasOwnPositionFix || !hasNodePositionFix) {
statusLine1 = "No";
statusLine2 = "Fix";
}
// --- Compass Rendering: landscape (wide) screens use the original side-aligned logic ---
if (SCREEN_WIDTH > SCREEN_HEIGHT) {
bool showCompass = false;
if (ourNode && (nodeDB->hasValidPosition(ourNode) || screen->hasHeading()) && nodeDB->hasValidPosition(node)) {
showCompass = true;
}
if (showCompass) {
if (showCompass || statusLine1) {
const int16_t topY = getTextPositions(display)[1];
const int16_t bottomY = SCREEN_HEIGHT - (FONT_HEIGHT_SMALL - 1);
const int16_t usableHeight = bottomY - topY - 5;
int16_t compassRadius = usableHeight / 2;
if (compassRadius < 8)
compassRadius = 8;
const int16_t compassDiam = compassRadius * 2;
const int16_t compassX = x + SCREEN_WIDTH - compassRadius - 8;
const int16_t compassY = topY + (usableHeight / 2) + ((FONT_HEIGHT_SMALL - 1) / 2) + 2;
const auto &op = ourNode->position;
float myHeading = screen->hasHeading() ? screen->getHeading() * PI / 180
: screen->estimatedHeading(DegD(op.latitude_i), DegD(op.longitude_i));
const auto &p = node->position;
/* unused
float d =
GeoCoord::latLongToMeter(DegD(p.latitude_i), DegD(p.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
*/
float bearing = GeoCoord::bearing(DegD(op.latitude_i), DegD(op.longitude_i), DegD(p.latitude_i), DegD(p.longitude_i));
if (uiconfig.compass_mode == meshtastic_CompassMode_FREEZE_HEADING) {
myHeading = 0;
} else {
bearing -= myHeading;
}
const int16_t compassDiam = compassRadius * 2;
display->drawCircle(compassX, compassY, compassRadius);
CompassRenderer::drawCompassNorth(display, compassX, compassY, myHeading, compassRadius);
CompassRenderer::drawNodeHeading(display, compassX, compassY, compassDiam, bearing);
if (showCompass) {
CompassRenderer::drawCompassNorth(display, compassX, compassY, myHeading, compassRadius);
CompassRenderer::drawNodeHeading(display, compassX, compassY, compassDiam, bearing);
} else {
drawCompassStatusText(display, compassX, compassY, statusLine1, statusLine2);
}
}
// else show nothing
} else {
// Portrait or square: put compass at the bottom and centered, scaled to fit available space
bool showCompass = false;
if (ourNode && (nodeDB->hasValidPosition(ourNode) || screen->hasHeading()) && nodeDB->hasValidPosition(node)) {
showCompass = true;
}
if (showCompass) {
if (showCompass || statusLine1) {
int yBelowContent = (line > 0 && line <= 5) ? (getTextPositions(display)[line - 1] + FONT_HEIGHT_SMALL + 2)
: getTextPositions(display)[1];
const int margin = 4;
@@ -747,8 +759,8 @@ void UIRenderer::drawFavoriteNode(OLEDDisplay *display, OLEDDisplayUiState *stat
#else
const int navBarHeight = 0;
#endif
int availableHeight = SCREEN_HEIGHT - yBelowContent - navBarHeight - margin;
// --------- END PATCH FOR EINK NAV BAR -----------
int availableHeight = SCREEN_HEIGHT - yBelowContent - navBarHeight - margin;
if (availableHeight < FONT_HEIGHT_SMALL * 2)
return;
@@ -762,25 +774,13 @@ void UIRenderer::drawFavoriteNode(OLEDDisplay *display, OLEDDisplayUiState *stat
int compassX = x + SCREEN_WIDTH / 2;
int compassY = yBelowContent + availableHeight / 2;
const auto &op = ourNode->position;
float myHeading = 0;
if (uiconfig.compass_mode != meshtastic_CompassMode_FREEZE_HEADING) {
myHeading = screen->hasHeading() ? screen->getHeading() * PI / 180
: screen->estimatedHeading(DegD(op.latitude_i), DegD(op.longitude_i));
}
graphics::CompassRenderer::drawCompassNorth(display, compassX, compassY, myHeading, compassRadius);
const auto &p = node->position;
/* unused
float d =
GeoCoord::latLongToMeter(DegD(p.latitude_i), DegD(p.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
*/
float bearing = GeoCoord::bearing(DegD(op.latitude_i), DegD(op.longitude_i), DegD(p.latitude_i), DegD(p.longitude_i));
if (uiconfig.compass_mode != meshtastic_CompassMode_FREEZE_HEADING)
bearing -= myHeading;
graphics::CompassRenderer::drawNodeHeading(display, compassX, compassY, compassRadius * 2, bearing);
display->drawCircle(compassX, compassY, compassRadius);
if (showCompass) {
graphics::CompassRenderer::drawCompassNorth(display, compassX, compassY, myHeading, compassRadius);
graphics::CompassRenderer::drawNodeHeading(display, compassX, compassY, compassRadius * 2, bearing);
} else {
drawCompassStatusText(display, compassX, compassY, statusLine1, statusLine2);
}
}
// else show nothing
}
@@ -1216,6 +1216,7 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
// === Header ===
graphics::drawCommonHeader(display, x, y, titleStr);
const int *textPos = getTextPositions(display);
// === First Row: My Location ===
#if HAS_GPS
@@ -1230,12 +1231,12 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
} else {
displayLine = config.position.gps_mode == meshtastic_Config_PositionConfig_GpsMode_NOT_PRESENT ? "No GPS" : "GPS off";
}
drawSatelliteIcon(display, x, getTextPositions(display)[line]);
drawSatelliteIcon(display, x, textPos[line]);
int xOffset = (currentResolution == ScreenResolution::High) ? 6 : 0;
display->drawString(x + 11 + xOffset, getTextPositions(display)[line++], displayLine);
display->drawString(x + 11 + xOffset, textPos[line++], displayLine);
} else {
// Onboard GPS
UIRenderer::drawGps(display, 0, getTextPositions(display)[line++], gpsStatus);
UIRenderer::drawGps(display, 0, textPos[line++], gpsStatus);
}
config.display.heading_bold = origBold;
@@ -1244,18 +1245,36 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
geoCoord.updateCoords(int32_t(gpsStatus->getLatitude()), int32_t(gpsStatus->getLongitude()),
int32_t(gpsStatus->getAltitude()));
// === Determine Compass Heading ===
float heading = 0;
meshtastic_NodeInfoLite *ourNode = nodeDB->getMeshNode(nodeDB->getNodeNum());
const bool hasOwnPositionFix = (ourNode && nodeDB->hasValidPosition(ourNode));
const bool hasLiveGpsFix =
(gpsStatus && gpsStatus->getHasLock() && (gpsStatus->getLatitude() != 0 || gpsStatus->getLongitude() != 0));
const bool hasSensorHeading = screen->hasHeading();
float heading = 0.0f;
bool validHeading = false;
if (uiconfig.compass_mode == meshtastic_CompassMode_FREEZE_HEADING) {
validHeading = true;
} else {
if (screen->hasHeading()) {
heading = radians(screen->getHeading());
validHeading = true;
const char *statusLine1 = nullptr;
const char *statusLine2 = nullptr;
if (hasSensorHeading || hasLiveGpsFix || hasOwnPositionFix) {
double headingLat = 0.0;
double headingLon = 0.0;
if (hasLiveGpsFix) {
headingLat = DegD(gpsStatus->getLatitude());
headingLon = DegD(gpsStatus->getLongitude());
} else if (hasOwnPositionFix) {
const auto &op = ourNode->position;
headingLat = DegD(op.latitude_i);
headingLon = DegD(op.longitude_i);
}
validHeading = CompassRenderer::getHeadingRadians(headingLat, headingLon, heading);
}
if (!validHeading) {
if (hasSensorHeading || hasLiveGpsFix || hasOwnPositionFix) {
statusLine1 = "No";
statusLine2 = "Heading";
} else {
heading = screen->estimatedHeading(geoCoord.getLatitude() * 1e-7, geoCoord.getLongitude() * 1e-7);
validHeading = !isnan(heading);
statusLine1 = "No";
statusLine2 = "Fix";
}
}
@@ -1273,18 +1292,18 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
getUptimeStr(delta, "Last: ", uptimeStr, sizeof(uptimeStr), true);
#endif
display->drawString(0, getTextPositions(display)[line++], uptimeStr);
display->drawString(0, textPos[line++], uptimeStr);
} else {
display->drawString(0, getTextPositions(display)[line++], "Last: ?");
display->drawString(0, textPos[line++], "Last: ?");
}
// === Third Row: Line 1 GPS Info ===
UIRenderer::drawGpsCoordinates(display, x, getTextPositions(display)[line++], gpsStatus, "line1");
UIRenderer::drawGpsCoordinates(display, x, textPos[line++], gpsStatus, "line1");
if (uiconfig.gps_format != meshtastic_DeviceUIConfig_GpsCoordinateFormat_OLC &&
uiconfig.gps_format != meshtastic_DeviceUIConfig_GpsCoordinateFormat_MLS) {
// === Fourth Row: Line 2 GPS Info ===
UIRenderer::drawGpsCoordinates(display, x, getTextPositions(display)[line++], gpsStatus, "line2");
UIRenderer::drawGpsCoordinates(display, x, textPos[line++], gpsStatus, "line2");
}
// === Final Row: Altitude ===
@@ -1295,14 +1314,14 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
} else {
snprintf(altitudeLine, sizeof(altitudeLine), "Alt: %.0im", alt);
}
display->drawString(x, getTextPositions(display)[line++], altitudeLine);
display->drawString(x, textPos[line++], altitudeLine);
}
#if !defined(M5STACK_UNITC6L)
// === Draw Compass if heading is valid ===
if (validHeading) {
// === Draw Compass ===
if (validHeading || statusLine1) {
// --- Compass Rendering: landscape (wide) screens use original side-aligned logic ---
if (SCREEN_WIDTH > SCREEN_HEIGHT) {
const int16_t topY = getTextPositions(display)[1];
const int16_t topY = textPos[1];
const int16_t bottomY = SCREEN_HEIGHT - (FONT_HEIGHT_SMALL - 1); // nav row height
const int16_t usableHeight = bottomY - topY - 5;
@@ -1315,29 +1334,33 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
// Center vertically and nudge down slightly to keep "N" clear of header
const int16_t compassY = topY + (usableHeight / 2) + ((FONT_HEIGHT_SMALL - 1) / 2) + 2;
CompassRenderer::drawNodeHeading(display, compassX, compassY, compassDiam, -heading);
display->drawCircle(compassX, compassY, compassRadius);
if (validHeading) {
CompassRenderer::drawNodeHeading(display, compassX, compassY, compassDiam, -heading);
// "N" label
float northAngle = 0;
if (uiconfig.compass_mode != meshtastic_CompassMode_FIXED_RING)
northAngle = -heading;
float radius = compassRadius;
int16_t nX = compassX + (radius - 1) * sin(northAngle);
int16_t nY = compassY - (radius - 1) * cos(northAngle);
int16_t nLabelWidth = display->getStringWidth("N") + 2;
int16_t nLabelHeightBox = FONT_HEIGHT_SMALL + 1;
// "N" label
float northAngle = 0;
if (uiconfig.compass_mode != meshtastic_CompassMode_FIXED_RING)
northAngle = -heading;
float radius = compassRadius;
int16_t nX = compassX + (radius - 1) * sin(northAngle);
int16_t nY = compassY - (radius - 1) * cos(northAngle);
int16_t nLabelWidth = display->getStringWidth("N") + 2;
int16_t nLabelHeightBox = FONT_HEIGHT_SMALL + 1;
display->setColor(BLACK);
display->fillRect(nX - nLabelWidth / 2, nY - nLabelHeightBox / 2, nLabelWidth, nLabelHeightBox);
display->setColor(WHITE);
display->setFont(FONT_SMALL);
display->setTextAlignment(TEXT_ALIGN_CENTER);
display->drawString(nX, nY - FONT_HEIGHT_SMALL / 2, "N");
display->setColor(BLACK);
display->fillRect(nX - nLabelWidth / 2, nY - nLabelHeightBox / 2, nLabelWidth, nLabelHeightBox);
display->setColor(WHITE);
display->setFont(FONT_SMALL);
display->setTextAlignment(TEXT_ALIGN_CENTER);
display->drawString(nX, nY - FONT_HEIGHT_SMALL / 2, "N");
} else {
drawCompassStatusText(display, compassX, compassY, statusLine1, statusLine2);
}
} else {
// Portrait or square: put compass at the bottom and centered, scaled to fit available space
// For E-Ink screens, account for navigation bar at the bottom!
int yBelowContent = getTextPositions(display)[5] + FONT_HEIGHT_SMALL + 2;
int yBelowContent = textPos[5] + FONT_HEIGHT_SMALL + 2;
const int margin = 4;
int availableHeight =
#if defined(USE_EINK)
@@ -1358,25 +1381,29 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
int compassX = x + SCREEN_WIDTH / 2;
int compassY = yBelowContent + availableHeight / 2;
CompassRenderer::drawNodeHeading(display, compassX, compassY, compassRadius * 2, -heading);
display->drawCircle(compassX, compassY, compassRadius);
if (validHeading) {
CompassRenderer::drawNodeHeading(display, compassX, compassY, compassRadius * 2, -heading);
// "N" label
float northAngle = 0;
if (uiconfig.compass_mode != meshtastic_CompassMode_FIXED_RING)
northAngle = -heading;
float radius = compassRadius;
int16_t nX = compassX + (radius - 1) * sin(northAngle);
int16_t nY = compassY - (radius - 1) * cos(northAngle);
int16_t nLabelWidth = display->getStringWidth("N") + 2;
int16_t nLabelHeightBox = FONT_HEIGHT_SMALL + 1;
// "N" label
float northAngle = 0;
if (uiconfig.compass_mode != meshtastic_CompassMode_FIXED_RING)
northAngle = -heading;
float radius = compassRadius;
int16_t nX = compassX + (radius - 1) * sin(northAngle);
int16_t nY = compassY - (radius - 1) * cos(northAngle);
int16_t nLabelWidth = display->getStringWidth("N") + 2;
int16_t nLabelHeightBox = FONT_HEIGHT_SMALL + 1;
display->setColor(BLACK);
display->fillRect(nX - nLabelWidth / 2, nY - nLabelHeightBox / 2, nLabelWidth, nLabelHeightBox);
display->setColor(WHITE);
display->setFont(FONT_SMALL);
display->setTextAlignment(TEXT_ALIGN_CENTER);
display->drawString(nX, nY - FONT_HEIGHT_SMALL / 2, "N");
display->setColor(BLACK);
display->fillRect(nX - nLabelWidth / 2, nY - nLabelHeightBox / 2, nLabelWidth, nLabelHeightBox);
display->setColor(WHITE);
display->setFont(FONT_SMALL);
display->setTextAlignment(TEXT_ALIGN_CENTER);
display->drawString(nX, nY - FONT_HEIGHT_SMALL / 2, "N");
} else {
drawCompassStatusText(display, compassX, compassY, statusLine1, statusLine2);
}
}
}
#endif

156
src/modules/WaypointModule.cpp
View File

@@ -15,15 +15,6 @@
WaypointModule *waypointModule;
static inline float degToRad(float deg)
{
return deg * PI / 180.0f;
}
static inline float radToDeg(float rad)
{
return rad * 180.0f / PI;
}
ProcessMessage WaypointModule::handleReceived(const meshtastic_MeshPacket &mp)
{
#if defined(DEBUG_PORT) && !defined(DEBUG_MUTE)
@@ -91,9 +82,7 @@ void WaypointModule::drawFrame(OLEDDisplay *display, OLEDDisplayUiState *state,
// === Header ===
graphics::drawCommonHeader(display, x, y, titleStr);
const int w = display->getWidth();
const int h = display->getHeight();
const int *textPos = graphics::getTextPositions(display);
// Decode the waypoint
const meshtastic_MeshPacket &mp = devicestate.rx_waypoint;
@@ -108,71 +97,118 @@ void WaypointModule::drawFrame(OLEDDisplay *display, OLEDDisplayUiState *state,
getTimeAgoStr(sinceReceived(&mp), lastStr, sizeof(lastStr));
// Will contain distance information, passed as a field to drawColumns
char distStr[20];
char distStr[20] = "";
// Get our node, to use our own position
meshtastic_NodeInfoLite *ourNode = nodeDB->getMeshNode(nodeDB->getNodeNum());
// Dimensions / co-ordinates for the compass/circle
const uint16_t compassDiam = graphics::CompassRenderer::getCompassDiam(w, h);
const int16_t compassX = x + w - (compassDiam / 2) - 5;
const int16_t compassY = (config.display.displaymode == meshtastic_Config_DisplayConfig_DisplayMode_DEFAULT)
? y + h / 2
: y + FONT_HEIGHT_SMALL + (h - FONT_HEIGHT_SMALL) / 2;
// Match compass sizing/placement to favorite node screen logic.
const int w = display->getWidth();
int16_t compassRadius = 8;
int16_t compassX = x + w - compassRadius - 8;
int16_t compassY = y + display->getHeight() / 2;
// If our node has a position:
if (ourNode && (nodeDB->hasValidPosition(ourNode) || screen->hasHeading())) {
const meshtastic_PositionLite &op = ourNode->position;
float myHeading;
if (uiconfig.compass_mode == meshtastic_CompassMode_FREEZE_HEADING) {
myHeading = 0;
} else {
if (screen->hasHeading())
myHeading = degToRad(screen->getHeading());
else
myHeading = screen->estimatedHeading(DegD(op.latitude_i), DegD(op.longitude_i));
if (SCREEN_WIDTH > SCREEN_HEIGHT) {
const int16_t topY = textPos[1];
const int16_t bottomY = SCREEN_HEIGHT - (FONT_HEIGHT_SMALL - 1);
const int16_t usableHeight = bottomY - topY - 5;
compassRadius = usableHeight / 2;
if (compassRadius < 8)
compassRadius = 8;
compassX = x + SCREEN_WIDTH - compassRadius - 8;
compassY = topY + (usableHeight / 2) + ((FONT_HEIGHT_SMALL - 1) / 2) + 2;
} else {
// Waypoint content uses rows 1..4, so place the compass below that block.
const int yBelowContent = textPos[4] + FONT_HEIGHT_SMALL + 2;
const int margin = 4;
#if defined(USE_EINK)
const int iconSize = (graphics::currentResolution == graphics::ScreenResolution::High) ? 16 : 8;
const int navBarHeight = iconSize + 6;
#else
const int navBarHeight = 0;
#endif
const int availableHeight = SCREEN_HEIGHT - yBelowContent - navBarHeight - margin;
if (availableHeight > 0) {
compassRadius = availableHeight / 2;
if (compassRadius < 8)
compassRadius = 8;
if (compassRadius * 2 > SCREEN_WIDTH - 16)
compassRadius = (SCREEN_WIDTH - 16) / 2;
if (compassRadius < 8)
compassRadius = 8;
compassX = x + SCREEN_WIDTH / 2;
compassY = yBelowContent + availableHeight / 2;
}
graphics::CompassRenderer::drawCompassNorth(display, compassX, compassY, myHeading, (compassDiam / 2));
}
const uint16_t compassDiam = compassRadius * 2;
// Compass bearing to waypoint
float bearingToOther =
GeoCoord::bearing(DegD(op.latitude_i), DegD(op.longitude_i), DegD(wp.latitude_i), DegD(wp.longitude_i));
// If the top of the compass is a static north then bearingToOther can be drawn on the compass directly
// If the top of the compass is not a static north we need adjust bearingToOther based on heading
if (uiconfig.compass_mode != meshtastic_CompassMode_FREEZE_HEADING)
bearingToOther -= myHeading;
graphics::CompassRenderer::drawNodeHeading(display, compassX, compassY, compassDiam, bearingToOther);
const bool hasOwnPositionFix = (ourNode && nodeDB->hasValidPosition(ourNode));
const char *statusLine1 = nullptr;
const char *statusLine2 = nullptr;
float bearingToOtherDegrees = (bearingToOther < 0) ? bearingToOther + 2 * PI : bearingToOther;
bearingToOtherDegrees = radToDeg(bearingToOtherDegrees);
// Distance only needs our own position fix; compass/bearing additionally needs heading.
if (hasOwnPositionFix) {
const meshtastic_PositionLite &op = ourNode->position;
const float d =
GeoCoord::latLongToMeter(DegD(wp.latitude_i), DegD(wp.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
// Distance to Waypoint
float d = GeoCoord::latLongToMeter(DegD(wp.latitude_i), DegD(wp.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
// Always show distance once we have an own-position fix, even without heading.
if (config.display.units == meshtastic_Config_DisplayConfig_DisplayUnits_IMPERIAL) {
float feet = d * METERS_TO_FEET;
snprintf(distStr, sizeof(distStr), feet < (2 * MILES_TO_FEET) ? "%.0fft %.0f°" : "%.1fmi %.0f°",
feet < (2 * MILES_TO_FEET) ? feet : feet / MILES_TO_FEET, bearingToOtherDegrees);
snprintf(distStr, sizeof(distStr), feet < (2 * MILES_TO_FEET) ? "%.0fft" : "%.1fmi",
feet < (2 * MILES_TO_FEET) ? feet : feet / MILES_TO_FEET);
} else {
snprintf(distStr, sizeof(distStr), d < 2000 ? "%.0fm %.0f°" : "%.1fkm %.0f°", d < 2000 ? d : d / 1000,
bearingToOtherDegrees);
snprintf(distStr, sizeof(distStr), d < 2000 ? "%.0fm" : "%.1fkm", d < 2000 ? d : d / 1000);
}
float myHeading = 0.0f;
const bool hasHeading =
graphics::CompassRenderer::getHeadingRadians(DegD(op.latitude_i), DegD(op.longitude_i), myHeading);
if (hasHeading) {
// Draw compass circle
display->drawCircle(compassX, compassY, compassRadius);
graphics::CompassRenderer::drawCompassNorth(display, compassX, compassY, myHeading, compassRadius);
// Compass bearing to waypoint
float bearingToOther =
GeoCoord::bearing(DegD(op.latitude_i), DegD(op.longitude_i), DegD(wp.latitude_i), DegD(wp.longitude_i));
bearingToOther = graphics::CompassRenderer::adjustBearingForCompassMode(bearingToOther, myHeading);
graphics::CompassRenderer::drawNodeHeading(display, compassX, compassY, compassDiam, bearingToOther);
const float bearingToOtherDegrees = graphics::CompassRenderer::radiansToDegrees360(bearingToOther);
// Distance to waypoint with relative bearing when heading is available.
if (config.display.units == meshtastic_Config_DisplayConfig_DisplayUnits_IMPERIAL) {
float feet = d * METERS_TO_FEET;
snprintf(distStr, sizeof(distStr), feet < (2 * MILES_TO_FEET) ? "%.0fft %.0f°" : "%.1fmi %.0f°",
feet < (2 * MILES_TO_FEET) ? feet : feet / MILES_TO_FEET, bearingToOtherDegrees);
} else {
snprintf(distStr, sizeof(distStr), d < 2000 ? "%.0fm %.0f°" : "%.1fkm %.0f°", d < 2000 ? d : d / 1000,
bearingToOtherDegrees);
}
} else {
statusLine1 = "No";
statusLine2 = "Heading";
}
} else {
// No own fix yet, so compass/bearing data would be misleading.
statusLine1 = "No";
statusLine2 = "Fix";
}
else {
display->drawString(compassX - FONT_HEIGHT_SMALL / 4, compassY - FONT_HEIGHT_SMALL / 2, "?");
// ? in the distance field
snprintf(distStr, sizeof(distStr), "? %s ?°",
(config.display.units == meshtastic_Config_DisplayConfig_DisplayUnits_IMPERIAL) ? "mi" : "km");
if (statusLine1) {
display->drawCircle(compassX, compassY, compassRadius);
display->setTextAlignment(TEXT_ALIGN_CENTER);
display->drawString(compassX, compassY - FONT_HEIGHT_SMALL, statusLine1);
display->drawString(compassX, compassY, statusLine2);
}
// Draw compass circle
display->drawCircle(compassX, compassY, compassDiam / 2);
display->setTextAlignment(TEXT_ALIGN_LEFT); // Something above me changes to a different alignment, forcing a fix here!
display->drawString(0, graphics::getTextPositions(display)[line++], lastStr);
display->drawString(0, graphics::getTextPositions(display)[line++], wp.name);
display->drawString(0, graphics::getTextPositions(display)[line++], wp.description);
display->drawString(0, graphics::getTextPositions(display)[line++], distStr);
display->drawString(0, textPos[line++], lastStr);
display->drawString(0, textPos[line++], wp.name);
display->drawString(0, textPos[line++], wp.description);
if (distStr[0])
display->drawString(0, textPos[line++], distStr);
}
#endif

24
src/motion/BMM150Sensor.cpp
View File

@@ -7,9 +7,6 @@
extern graphics::Screen *screen;
#endif
// Flag when an interrupt has been detected
volatile static bool BMM150_IRQ = false;
BMM150Sensor::BMM150Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {}
bool BMM150Sensor::init()
@@ -23,24 +20,7 @@ int32_t BMM150Sensor::runOnce()
{
#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
float heading = sensor->getCompassDegree();
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;
}
heading = applyCompassOrientation(heading);
if (screen)
screen->setHeading(heading);
#endif
@@ -90,4 +70,4 @@ bool BMM150Singleton::init(ScanI2C::FoundDevice device)
return true;
}
#endif
#endif

68
src/motion/BMX160Sensor.cpp
View File

@@ -16,6 +16,7 @@ bool BMX160Sensor::init()
if (sensor.begin()) {
// set output data rate
sensor.ODR_Config(BMX160_ACCEL_ODR_100HZ, BMX160_GYRO_ODR_100HZ);
loadMagnetometerCalibration(compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
LOG_DEBUG("BMX160 init ok");
return true;
}
@@ -33,42 +34,12 @@ int32_t BMX160Sensor::runOnce()
sensor.getAllData(&magAccel, NULL, &gAccel);
if (doCalibration) {
if (!showingScreen) {
powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration
showingScreen = true;
if (screen)
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;
uint32_t now = millis();
if (now > endCalibrationAt) {
doCalibration = false;
endCalibrationAt = 0;
showingScreen = false;
if (screen)
screen->endAlert();
}
// LOG_DEBUG("BMX160 min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", lowestX, highestX,
// lowestY, highestY, lowestZ, highestZ);
beginCalibrationDisplay(showingScreen);
updateCalibrationExtrema(magAccel.x, magAccel.y, magAccel.z, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
finishCalibrationIfExpired(showingScreen, compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ,
lowestZ);
}
int highestRealX = highestX - (highestX + lowestX) / 2;
magAccel.x -= (highestX + lowestX) / 2;
magAccel.y -= (highestY + lowestY) / 2;
magAccel.z -= (highestZ + lowestZ) / 2;
@@ -88,23 +59,7 @@ int32_t BMX160Sensor::runOnce()
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;
}
heading = applyCompassOrientation(heading);
if (screen)
screen->setHeading(heading);
#endif
@@ -119,15 +74,8 @@ void BMX160Sensor::calibrate(uint16_t forSeconds)
sBmx160SensorData_t gAccel;
LOG_DEBUG("BMX160 calibration started for %is", forSeconds);
sensor.getAllData(&magAccel, NULL, &gAccel);
highestX = magAccel.x, lowestX = magAccel.x;
highestY = magAccel.y, lowestY = magAccel.y;
highestZ = magAccel.z, lowestZ = magAccel.z;
doCalibration = true;
uint16_t calibrateFor = forSeconds * 1000; // calibrate for seconds provided
endCalibrationAt = millis() + calibrateFor;
if (screen)
screen->setEndCalibration(endCalibrationAt);
seedCalibrationExtrema(magAccel.x, magAccel.y, magAccel.z, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
startCalibrationWindow(forSeconds);
#endif
}

3
src/motion/BMX160Sensor.h
View File

@@ -17,6 +17,7 @@ class BMX160Sensor : public MotionSensor
private:
RAK_BMX160 sensor;
bool showingScreen = false;
static constexpr const char *compassCalibrationFileName = "/prefs/compass_bmx160.dat";
float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0;
public:
@@ -39,4 +40,4 @@ class BMX160Sensor : public MotionSensor
#endif
#endif
#endif

88
src/motion/ICM20948Sensor.cpp
View File

@@ -26,7 +26,11 @@ bool ICM20948Sensor::init()
return false;
// Enable simple Wake on Motion
return sensor->setWakeOnMotion();
bool wakeOnMotionOk = sensor->setWakeOnMotion();
if (wakeOnMotionOk) {
loadMagnetometerCalibration(compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
}
return wakeOnMotionOk;
}
#ifdef ICM_20948_INT_PIN
@@ -47,7 +51,8 @@ int32_t ICM20948Sensor::runOnce()
int32_t ICM20948Sensor::runOnce()
{
#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
if (screen && !screen->isScreenOn() && !config.display.wake_on_tap_or_motion && !config.device.double_tap_as_button_press) {
if (screen && !doCalibration && !screen->isScreenOn() && !config.display.wake_on_tap_or_motion &&
!config.device.double_tap_as_button_press) {
if (!isAsleep) {
LOG_DEBUG("sleeping IMU");
sensor->sleep(true);
@@ -69,38 +74,10 @@ int32_t ICM20948Sensor::runOnce()
}
if (doCalibration) {
if (!showingScreen) {
powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration
showingScreen = true;
if (screen)
screen->startAlert((FrameCallback)drawFrameCalibration);
}
if (magX > highestX)
highestX = magX;
if (magX < lowestX)
lowestX = magX;
if (magY > highestY)
highestY = magY;
if (magY < lowestY)
lowestY = magY;
if (magZ > highestZ)
highestZ = magZ;
if (magZ < lowestZ)
lowestZ = magZ;
uint32_t now = millis();
if (now > endCalibrationAt) {
doCalibration = false;
endCalibrationAt = 0;
showingScreen = false;
if (screen)
screen->endAlert();
}
// LOG_DEBUG("ICM20948 min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", lowestX, highestX,
// lowestY, highestY, lowestZ, highestZ);
beginCalibrationDisplay(showingScreen);
updateCalibrationExtrema(magX, magY, magZ, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
finishCalibrationIfExpired(showingScreen, compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ,
lowestZ);
}
magX -= (highestX + lowestX) / 2;
@@ -122,23 +99,7 @@ int32_t ICM20948Sensor::runOnce()
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;
}
heading = applyCompassOrientation(heading);
if (screen)
screen->setHeading(heading);
#endif
@@ -169,26 +130,16 @@ int32_t ICM20948Sensor::runOnce()
void ICM20948Sensor::calibrate(uint16_t forSeconds)
{
#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
LOG_DEBUG("Old calibration data: highestX = %f, lowestX = %f, highestY = %f, lowestY = %f, highestZ = %f, lowestZ = %f",
highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
LOG_DEBUG("BMX160 calibration started for %is", forSeconds);
LOG_DEBUG("ICM20948 cal start %is", forSeconds);
if (sensor->dataReady()) {
sensor->getAGMT();
highestX = sensor->agmt.mag.axes.x;
lowestX = sensor->agmt.mag.axes.x;
highestY = sensor->agmt.mag.axes.y;
lowestY = sensor->agmt.mag.axes.y;
highestZ = sensor->agmt.mag.axes.z;
lowestZ = sensor->agmt.mag.axes.z;
seedCalibrationExtrema(sensor->agmt.mag.axes.x, sensor->agmt.mag.axes.y, sensor->agmt.mag.axes.z, highestX, lowestX,
highestY, lowestY, highestZ, lowestZ);
} else {
highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0;
seedCalibrationExtrema(0.0f, 0.0f, 0.0f, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
}
doCalibration = true;
uint16_t calibrateFor = forSeconds * 1000; // calibrate for seconds provided
endCalibrationAt = millis() + calibrateFor;
if (screen)
screen->setEndCalibration(endCalibrationAt);
startCalibrationWindow(forSeconds);
#endif
}
// ----------------------------------------------------------------------
@@ -314,11 +265,6 @@ bool ICM20948Singleton::setWakeOnMotion()
status = intEnableWOM(true);
LOG_DEBUG("ICM20948 init set intEnableWOM - %s", statusString());
return status == ICM_20948_Stat_Ok;
// Clear any current interrupts
ICM20948_IRQ = false;
clearInterrupts();
return true;
}
#endif

3
src/motion/ICM20948Sensor.h
View File

@@ -83,6 +83,7 @@ class ICM20948Sensor : public MotionSensor
ICM20948Singleton *sensor = nullptr;
bool showingScreen = false;
bool isAsleep = false;
static constexpr const char *compassCalibrationFileName = "/prefs/compass_icm20948.dat";
#ifdef MUZI_BASE
float highestX = 449.000000, lowestX = -140.000000, highestY = 422.000000, lowestY = -232.000000, highestZ = 749.000000,
lowestZ = 98.000000;
@@ -103,4 +104,4 @@ class ICM20948Sensor : public MotionSensor
#endif
#endif
#endif

259
src/motion/MotionSensor.cpp
View File

@@ -1,10 +1,37 @@
#include "MotionSensor.h"
#include "FSCommon.h"
#include "SPILock.h"
#include "SafeFile.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);
}
} // namespace
// screen is defined in main.cpp
extern graphics::Screen *screen;
@@ -32,33 +59,237 @@ 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;
}
}
#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
void MotionSensor::drawFrameCalibration(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
{
if (screen == nullptr)
return;
// 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");
uint8_t timeRemaining = (screen->getEndCalibration() - millis()) / 1000;
sprintf(timeRemainingBuffer, "( %02d )", timeRemaining);
display->setFont(FONT_SMALL);
display->drawString(x, y + 40, timeRemainingBuffer);
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(display->getWidth(), display->getHeight());
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 + display->getWidth() - compassDiam / 2 - 5;
compassY = y + display->getHeight() / 2;
compassX = x + width - compassRadius - margin;
compassY = y + height / 2;
} else {
compassX = x + display->getWidth() - compassDiam / 2 - 5;
compassY = y + FONT_HEIGHT_SMALL + (display->getHeight() - FONT_HEIGHT_SMALL) / 2;
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));
}

18
src/motion/MotionSensor.h
View File

@@ -2,7 +2,7 @@
#ifndef _MOTION_SENSOR_H_
#define _MOTION_SENSOR_H_
#define MOTION_SENSOR_CHECK_INTERVAL_MS 100
#define MOTION_SENSOR_CHECK_INTERVAL_MS 50
#define MOTION_SENSOR_CLICK_THRESHOLD 40
#include "../configuration.h"
@@ -54,6 +54,20 @@ class MotionSensor
static void drawFrameCalibration(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y);
#endif
bool saveMagnetometerCalibration(const char *filePath, float highestX, float lowestX, float highestY, float lowestY,
float highestZ, float lowestZ);
bool loadMagnetometerCalibration(const char *filePath, float &highestX, float &lowestX, float &highestY, float &lowestY,
float &highestZ, float &lowestZ);
void beginCalibrationDisplay(bool &showingScreen);
void finishCalibrationIfExpired(bool &showingScreen, const char *filePath, float highestX, float lowestX, float highestY,
float lowestY, float highestZ, float lowestZ);
void startCalibrationWindow(uint16_t forSeconds);
static void seedCalibrationExtrema(float x, float y, float z, float &highestX, float &lowestX, float &highestY,
float &lowestY, float &highestZ, float &lowestZ);
static void updateCalibrationExtrema(float x, float y, float z, float &highestX, float &lowestX, float &highestY,
float &lowestY, float &highestZ, float &lowestZ);
static float applyCompassOrientation(float heading);
ScanI2C::FoundDevice device;
// Do calibration if true
@@ -63,4 +77,4 @@ class MotionSensor
#endif
#endif
#endif