feat(nav): course-over-ground ring in UITask (compass without magnetometer)

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 <noreply@anthropic.com>
This commit is contained in:
Jakub
2026-06-03 08:20:59 +02:00
parent acec8e94dd
commit 11c0b7c4a5
2 changed files with 65 additions and 0 deletions

View File

@@ -3,6 +3,7 @@
#include "../MyMesh.h"
#include "../MsgExpand.h"
#include "../Features.h"
#include "../GeoUtils.h"
#include "target.h"
#ifdef WIFI_SSID
#include <WiFi.h>
@@ -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<UITask*>(this)->_cog_deg =
geo::bearingDeg(oldest.lat, oldest.lon, newest.lat, newest.lon);
deg_out = _cog_deg;
return true;
}
char UITask::checkDisplayOn(char c) {

View File

@@ -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();