From 11c0b7c4a50280ed3be59a2b67b99463378256d1 Mon Sep 17 00:00:00 2001 From: Jakub <106778416+MarekZegare4@users.noreply.github.com> Date: Wed, 3 Jun 2026 08:20:59 +0200 Subject: [PATCH] feat(nav): course-over-ground ring in UITask (compass without magnetometer) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Phase 2. A 5-fix GPS ring sampled every ~1 s in UITask::loop, filled whenever there's a valid fix — independent of trail recording, so the heading is available to navigation at all times, not only while logging a trail. - pushCogFix() rejects gross outliers (a fix implying > 50 m/s since the previous one) so a single bad GPS reading can't swing the heading. - currentCourse(deg) returns the bearing across the window (oldest→newest) once the window spans ≥ 6 m of real movement; below that it holds the last good heading, and returns false only until the first valid heading exists. This is the "two absolute bearings (To / Hdg)" heading source for the upcoming nav view. Co-Authored-By: Claude Opus 4.8 --- examples/companion_radio/ui-new/UITask.cpp | 50 ++++++++++++++++++++++ examples/companion_radio/ui-new/UITask.h | 15 +++++++ 2 files changed, 65 insertions(+) diff --git a/examples/companion_radio/ui-new/UITask.cpp b/examples/companion_radio/ui-new/UITask.cpp index fc23a730..199b7ac8 100644 --- a/examples/companion_radio/ui-new/UITask.cpp +++ b/examples/companion_radio/ui-new/UITask.cpp @@ -3,6 +3,7 @@ #include "../MyMesh.h" #include "../MsgExpand.h" #include "../Features.h" +#include "../GeoUtils.h" #include "target.h" #ifdef WIFI_SSID #include @@ -1910,6 +1911,55 @@ void UITask::loop() { (uint32_t)rtc_clock.getCurrentTime(), md); } } + + // Course-over-ground sampling — every ~1 s regardless of trail state, so the + // heading is available to navigation even when not recording a trail. + if ((int32_t)(millis() - _next_cog_sample_ms) >= 0) { + _next_cog_sample_ms = millis() + 1000UL; + LocationProvider* loc = _sensors ? _sensors->getLocationProvider() : nullptr; + if (loc && loc->isValid()) { + pushCogFix((int32_t)loc->getLatitude(), (int32_t)loc->getLongitude()); + } + } +} + +// Insert a GPS fix into the course-over-ground ring, rejecting gross outliers +// (a jump implying an impossible speed) so one bad fix can't swing the heading. +void UITask::pushCogFix(int32_t lat, int32_t lon) { + uint32_t now = millis(); + if (_cog_count > 0) { + const CogFix& prev = _cog[(_cog_head + _cog_count - 1) % COG_RING]; + uint32_t dt = now - prev.ms; + if (dt > 0) { + float dist_m = geo::haversineKm(prev.lat, prev.lon, lat, lon) * 1000.0f; + float speed = dist_m / (dt / 1000.0f); // m/s + if (speed > 50.0f) return; // > 180 km/h between fixes → reject + } + } + int pos; + if (_cog_count < COG_RING) { pos = (_cog_head + _cog_count) % COG_RING; _cog_count++; } + else { pos = _cog_head; _cog_head = (_cog_head + 1) % COG_RING; } + _cog[pos].lat = lat; _cog[pos].lon = lon; _cog[pos].ms = now; +} + +bool UITask::currentCourse(int& deg_out) const { + static const float COG_MIN_MOVE_M = 6.0f; // window must span ≥ this to be a real heading + if (_cog_count < 2) { + if (_cog_deg >= 0) { deg_out = _cog_deg; return true; } // hold last good + return false; + } + const CogFix& oldest = _cog[_cog_head]; + const CogFix& newest = _cog[(_cog_head + _cog_count - 1) % COG_RING]; + float span_m = geo::haversineKm(oldest.lat, oldest.lon, newest.lat, newest.lon) * 1000.0f; + if (span_m < COG_MIN_MOVE_M) { + if (_cog_deg >= 0) { deg_out = _cog_deg; return true; } // standing still → hold last + return false; + } + // Cache as last-good (mutable-free: recompute is cheap, but keep _cog_deg fresh). + const_cast(this)->_cog_deg = + geo::bearingDeg(oldest.lat, oldest.lon, newest.lat, newest.lon); + deg_out = _cog_deg; + return true; } char UITask::checkDisplayOn(char c) { diff --git a/examples/companion_radio/ui-new/UITask.h b/examples/companion_radio/ui-new/UITask.h index 7b9eac70..5cb8d955 100644 --- a/examples/companion_radio/ui-new/UITask.h +++ b/examples/companion_radio/ui-new/UITask.h @@ -80,6 +80,18 @@ class UITask : public AbstractUITask { TrailStore _trail; uint32_t _next_trail_sample_ms = 0; + // Course-over-ground ring — a heading source independent of trail recording. + // Filled from the same periodic GPS poll regardless of _trail.isActive(). + // Heading = bearing across the window (oldest→newest) once the cumulative + // movement clears COG_MIN_MOVE_M; gross GPS jumps are rejected on insert. + static const int COG_RING = 5; + struct CogFix { int32_t lat, lon; uint32_t ms; }; + CogFix _cog[COG_RING]; + uint8_t _cog_head = 0, _cog_count = 0; + int _cog_deg = -1; // last good heading, -1 = none yet + uint32_t _next_cog_sample_ms = 0; + void pushCogFix(int32_t lat, int32_t lon); + // Ping state bool _ping_active = false; uint32_t _ping_tag = 0; @@ -131,6 +143,9 @@ public: void gotoAutoAdvertScreen(); void gotoTrailScreen(); TrailStore& trail() { return _trail; } + // Current course over ground in degrees (0..359), or false if not enough + // recent movement to derive a stable heading. Independent of trail logging. + bool currentCourse(int& deg_out) const; void playMelody(const char* melody); void stopMelody(); bool isMelodyPlaying();