mirror of
https://github.com/MarekZegare4/MeshCore-Solo.git
synced 2026-07-26 23:08:11 +00:00
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:
@@ -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) {
|
||||
|
||||
@@ -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();
|
||||
|
||||
Reference in New Issue
Block a user