mirror of
https://github.com/MarekZegare4/MeshCore-Solo.git
synced 2026-07-26 14:58:12 +00:00
c4cea53a40
- geo::parseLatLon now rounds lat/lon in double (matching the Add-by-coords form) instead of through a float cast that lost ~1 m near 1e8. - NearbyScreen: preserve the ping menu's selected row across a rebuild instead of snapping back to "Send" when a result line arrives. - NearbyScreen: guard the ping-menu input path so a vanished contact (selectedStoredPubKey == false) closes the popup rather than acting on an uninitialised pub-key buffer. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
113 lines
4.5 KiB
C++
113 lines
4.5 KiB
C++
#pragma once
|
|
// Shared geographic helpers used by the navigation features (Nearby Nodes,
|
|
// Waypoints, trail course-over-ground). Coordinates are fixed-point degrees
|
|
// scaled by 1e6 (the same representation used by the GPS provider and the
|
|
// contact/trail records). All functions are pure and header-inline.
|
|
|
|
#include <Arduino.h>
|
|
#include <math.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <ctype.h>
|
|
|
|
#ifndef M_PI
|
|
#define M_PI 3.14159265358979323846
|
|
#endif
|
|
|
|
namespace geo {
|
|
|
|
// Great-circle distance between two 1e6-scaled lat/lon points, in kilometres.
|
|
static inline float haversineKm(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2) {
|
|
static const float DEG2RAD = (float)M_PI / 180.0f;
|
|
float la1 = lat1 * (1e-6f * DEG2RAD);
|
|
float la2 = lat2 * (1e-6f * DEG2RAD);
|
|
float dla = (lat2 - lat1) * (1e-6f * DEG2RAD);
|
|
float dlo = (lon2 - lon1) * (1e-6f * DEG2RAD);
|
|
float a = sinf(dla/2)*sinf(dla/2) + cosf(la1)*cosf(la2)*sinf(dlo/2)*sinf(dlo/2);
|
|
return 6371.0f * 2.0f * asinf(sqrtf(a));
|
|
}
|
|
|
|
// Initial bearing from point 1 to point 2, degrees 0..359 (0 = north).
|
|
static inline int bearingDeg(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2) {
|
|
static const float DEG2RAD = (float)M_PI / 180.0f;
|
|
float la1 = lat1 * (1e-6f * DEG2RAD);
|
|
float la2 = lat2 * (1e-6f * DEG2RAD);
|
|
float dlo = (lon2 - lon1) * (1e-6f * DEG2RAD);
|
|
float b = atan2f(sinf(dlo)*cosf(la2),
|
|
cosf(la1)*sinf(la2) - sinf(la1)*cosf(la2)*cosf(dlo)) * (180.0f / (float)M_PI);
|
|
if (b < 0.0f) b += 360.0f;
|
|
return (int)(b + 0.5f) % 360;
|
|
}
|
|
|
|
// 8-point cardinal label for a bearing in degrees.
|
|
static inline const char* bearingCardinal(int deg) {
|
|
static const char* dirs[] = { "N","NE","E","SE","S","SW","W","NW" };
|
|
return dirs[((deg + 22) % 360) / 45];
|
|
}
|
|
|
|
// Human distance string. Metric: "850m" / "2.3km" / "140km". Imperial:
|
|
// "850ft" / "2.3mi" / "140mi" (feet below ~1000 ft, then miles).
|
|
static inline void fmtDist(char* buf, int n, float km, bool imperial) {
|
|
if (imperial) {
|
|
float ft = km * 3280.84f;
|
|
if (ft < 1000.0f) { snprintf(buf, n, "%dft", (int)(ft + 0.5f)); return; }
|
|
float mi = km * 0.621371f;
|
|
if (mi < 100.0f) snprintf(buf, n, "%.1fmi", mi);
|
|
else snprintf(buf, n, "%dmi", (int)(mi + 0.5f));
|
|
} else {
|
|
if (km < 1.0f) snprintf(buf, n, "%dm", (int)(km * 1000 + 0.5f));
|
|
else if (km < 100.0f) snprintf(buf, n, "%.1fkm", km);
|
|
else snprintf(buf, n, "%dkm", (int)(km + 0.5f));
|
|
}
|
|
}
|
|
|
|
// Tag marking a shared waypoint inside a message: "[WAY]<lat>,<lon> <label>".
|
|
// A plain {loc} expansion ("<lat>,<lon>") parses too — the tag just adds intent
|
|
// and a label, and keeps the text readable on apps/firmware that don't know it.
|
|
#define WAYPOINT_MSG_TAG "[WAY]"
|
|
|
|
// Scan a message for an embedded "lat,lon" location (decimal degrees, the same
|
|
// text {loc} emits). If a WAYPOINT_MSG_TAG precedes it, the trailing text is
|
|
// taken as the label. Returns true and fills lat/lon (1e6-scaled) on a valid,
|
|
// in-range coordinate; label (optional) gets the trimmed tag suffix or "".
|
|
static inline bool parseLatLon(const char* text, int32_t& lat_1e6, int32_t& lon_1e6,
|
|
char* label = nullptr, int label_n = 0) {
|
|
if (label && label_n > 0) label[0] = '\0';
|
|
if (!text) return false;
|
|
|
|
const char* tag = strstr(text, WAYPOINT_MSG_TAG);
|
|
const char* scan = tag ? tag + strlen(WAYPOINT_MSG_TAG) : text;
|
|
|
|
for (const char* p = scan; *p; p++) {
|
|
if (*p != '-' && *p != '+' && *p != '.' && !isdigit((unsigned char)*p)) continue;
|
|
char* end = nullptr;
|
|
double la = strtod(p, &end);
|
|
if (end == p) continue;
|
|
if (!memchr(p, '.', end - p)) continue; // require a decimal point (skip "5,6")
|
|
const char* q = end;
|
|
while (*q == ' ') q++;
|
|
if (*q != ',') { p = end - 1; continue; }
|
|
q++;
|
|
while (*q == ' ') q++;
|
|
char* end2 = nullptr;
|
|
double lo = strtod(q, &end2);
|
|
if (end2 == q) continue;
|
|
if (la < -90.0 || la > 90.0 || lo < -180.0 || lo > 180.0) continue;
|
|
|
|
lat_1e6 = (int32_t)(la * 1e6 + (la < 0 ? -0.5 : 0.5)); // round in double — float loses ~1m at 1e8
|
|
lon_1e6 = (int32_t)(lo * 1e6 + (lo < 0 ? -0.5 : 0.5));
|
|
if (label && label_n > 0 && tag) { // label only from a tagged share
|
|
const char* s = end2;
|
|
while (*s == ' ') s++;
|
|
int n = 0;
|
|
while (s[n] && n < label_n - 1) { label[n] = s[n]; n++; }
|
|
while (n > 0 && label[n - 1] == ' ') n--; // trim trailing spaces
|
|
label[n] = '\0';
|
|
}
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
} // namespace geo
|