From a49147215e31dea790baea509eeb91b679af3e8c Mon Sep 17 00:00:00 2001 From: Hector van der Aa Date: Tue, 31 Mar 2026 22:42:49 +0200 Subject: [PATCH] Updated GlobalTrackData struct to store precomputed circle radius --- src/custom_types.cpp | 41 ++++++++++++++++++++++++++++++----- src/custom_types.h | 12 ++++++---- src/data/track_store.cpp | 2 +- src/modules/config/config.cpp | 15 ++++++++++--- src/modules/gps/gps.cpp | 23 +++++++++----------- 5 files changed, 67 insertions(+), 26 deletions(-) diff --git a/src/custom_types.cpp b/src/custom_types.cpp index c289790..dede341 100644 --- a/src/custom_types.cpp +++ b/src/custom_types.cpp @@ -4,14 +4,13 @@ #include "custom_types.h" #include "math.h" -Vec2 eqRectProjection(const LatLng target, const LatLng ref) { +Vec2 eqRectProjection(const LatLng& target, const LatLng& ref) { const float R = 6371000.0; float lat0_r = ref.lat_ * M_PI / 180; float lng0_r = ref.lng_ * M_PI / 180; float lat1_r = target.lat_ * M_PI / 180; float lng1_r = target.lng_ * M_PI / 180; - float lat_avg = (lat0_r + lat1_r) * 0.5f; - float lat0_cos = cos(lat_avg); + float lat0_cos = cos((lat0_r + lat1_r) * 0.5f); Vec2 res; @@ -21,14 +20,46 @@ Vec2 eqRectProjection(const LatLng target, const LatLng ref) { return res; } -Vec2 abMidpoint(const Vec2 A, const Vec2 B) { +LatLng eqRectInverse(const Vec2& point, const LatLng& ref) { + const float R = 6371000.0f; + + float lat0 = ref.lat_ * M_PI / 180.0f; + float lng0 = ref.lng_ * M_PI / 180.0f; + + float cos_lat0 = cos(lat0); + + // Recover latitude + float lat = lat0 + (point.y_ / R); + + // Recover longitude + float lng = lng0 + (point.x_ / (R * cos_lat0)); + + LatLng res; + res.lat_ = lat * 180.0f / M_PI; + res.lng_ = lng * 180.0f / M_PI; + + return res; +} + +Vec2 abMidpoint(const Vec2& A, const Vec2& B) { Vec2 res; res.x_ = (A.x_ + B.x_) * 0.5f; res.y_ = (A.y_ + B.y_) * 0.5f; return res; } -float abSqDist(const Vec2 A, const Vec2 B) { +Vec2 abSum(const Vec2& A, const Vec2& B) { + Vec2 res; + res.x_ = A.x_ + B.x_; + res.y_ = A.y_ + B.y_; + return res; +} + +float vecMod(const Vec2& in) { + return sqrtf(in.x_*in.x_ + in.y_*in.y_); +} + +float abSqDist(const Vec2& A, const Vec2& B) { float deltaX = B.x_ - A.x_; float deltaY = B.y_ - A.y_; return deltaX * deltaX + deltaY * deltaY; diff --git a/src/custom_types.h b/src/custom_types.h index f2ee1e3..01585e0 100644 --- a/src/custom_types.h +++ b/src/custom_types.h @@ -28,9 +28,12 @@ struct Vec2 { float y_; }; -Vec2 eqRectProjection(const LatLng target, const LatLng ref); -Vec2 abMidpoint(const Vec2 A, const Vec2 B); -float abSqDist(const Vec2 A, const Vec2 B); +Vec2 eqRectProjection(const LatLng& target, const LatLng& ref); +LatLng eqRectInverse(const Vec2& point, const LatLng& ref); +Vec2 abMidpoint(const Vec2& A, const Vec2& B); +Vec2 abSum(const Vec2& A, const Vec2& B); +float vecMod(const Vec2& in); +float abSqDist(const Vec2& A, const Vec2& B); struct TrackData { uint16_t magic_ = CONFIG_MAGIC; @@ -41,8 +44,9 @@ struct TrackData { }; struct GlobalTrackData { - bool loaded = false; + bool loaded_ = false; Vec2 center_; + float circle_radius_; TrackData root_; }; diff --git a/src/data/track_store.cpp b/src/data/track_store.cpp index 36682d4..73a59c5 100644 --- a/src/data/track_store.cpp +++ b/src/data/track_store.cpp @@ -21,7 +21,7 @@ volatile TrackData track_data_temp_global = {}; return 1; } - out.loaded = true; + out.loaded_ = true; out.root_ = track_data; return 0; } diff --git a/src/modules/config/config.cpp b/src/modules/config/config.cpp index 624aa44..79b5ae0 100644 --- a/src/modules/config/config.cpp +++ b/src/modules/config/config.cpp @@ -69,7 +69,7 @@ int Config::deleteTrack(unsigned short idx) { is_track_loaded_ = false; loaded_track_ = {}; GlobalTrackData track; - track.loaded = false; + track.loaded_ = false; track.root_ = loaded_track_; trackGlobalWrite(track); } @@ -92,7 +92,7 @@ int Config::resetConfig() { task_memory_stale_ = true; no_tracks_notice_shown_ = false; GlobalTrackData track; - track.loaded = false; + track.loaded_ = false; track.root_ = loaded_track_; trackGlobalWrite(track); @@ -380,7 +380,16 @@ int Config::loadTrack(unsigned int idx) { loaded_track_ = track_data; GlobalTrackData track; - track.loaded = true; + track.loaded_ = true; + Vec2 point_b =eqRectProjection(track_data.point_b_, track_data.point_a_); + track.center_ = abMidpoint(point_b, (Vec2){0.0f,0.0f}); + track.circle_radius_ = max(10.0f, vecMod(point_b) * 1.25f); + #ifdef DEBUG + if (logger_ != nullptr) { + logger_->debug("Radius: " + String(track.circle_radius_)); + logger_->debug("Line Length: " + String(vecMod(point_b))); + } + #endif track.root_ = track_data; trackGlobalWrite(track); is_track_loaded_ = true; diff --git a/src/modules/gps/gps.cpp b/src/modules/gps/gps.cpp index 5a4aeb0..0ef784b 100644 --- a/src/modules/gps/gps.cpp +++ b/src/modules/gps/gps.cpp @@ -4,11 +4,11 @@ #include "gps.h" +#include "data/track_store.h" + #define MOD "modules/gps/gps.h" -int Gps::push(const Task &task) { - return queue_.push(task); -} +int Gps::push(const Task &task) { return queue_.push(task); } Gps::Gps(HardwareSerial *data_stream) : gps_(nullptr), data_stream_(data_stream), logger_(nullptr) { @@ -49,31 +49,28 @@ int Gps::loop(unsigned long timeout_ms) { return 1; } } - + if (lap_active_) { if (start_line_trigger_ == trigger_status::Idle) { - float current_lat = gps_->location.lat(); - float current_lng = gps_->location.lat(); } } - - Task active; + + Task active; int res = queue_.pop(active); if (res == 0) { if (active.target_ == module::Gps) { } else if (active.target_ == module::All) { - switch (active.type_) - { + switch (active.type_) { case task::AllTrackLoaded: - #ifdef DEBUG +#ifdef DEBUG if (logger_ != nullptr) { logger_->debug("GPS received track loaded sig"); } - #endif +#endif lap_active_ = true; break; - + default: break; }