diff --git a/src/custom_types.cpp b/src/custom_types.cpp new file mode 100644 index 0000000..c289790 --- /dev/null +++ b/src/custom_types.cpp @@ -0,0 +1,35 @@ +// Copyright (C) 2026 Hector van der Aa +// Copyright (C) 2026 Association Exergie +// SPDX-License-Identifier: GPL-3.0-or-later +#include "custom_types.h" +#include "math.h" + +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); + + + Vec2 res; + + res.x_ = (lng1_r - lng0_r) * lat0_cos * R; + res.y_ = (lat1_r - lat0_r) * R; + 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) { + float deltaX = B.x_ - A.x_; + float deltaY = B.y_ - A.y_; + return deltaX * deltaX + deltaY * deltaY; +} \ No newline at end of file diff --git a/src/custom_types.h b/src/custom_types.h index d9d9460..f2ee1e3 100644 --- a/src/custom_types.h +++ b/src/custom_types.h @@ -23,6 +23,15 @@ struct LatLng { float lng_; }; +struct Vec2 { + float x_; + 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); + struct TrackData { uint16_t magic_ = CONFIG_MAGIC; unsigned short id_; @@ -31,6 +40,12 @@ struct TrackData { LatLng point_b_; }; +struct GlobalTrackData { + bool loaded = false; + Vec2 center_; + TrackData root_; +}; + struct GpsSubData { uint32_t age_; bool valid_; diff --git a/src/data/track_store.cpp b/src/data/track_store.cpp index f2b73f4..36682d4 100644 --- a/src/data/track_store.cpp +++ b/src/data/track_store.cpp @@ -4,14 +4,14 @@ #include "data/track_store.h" #include "data/eeprom_layout.h" - volatile TrackData track_data_global = {}; + volatile GlobalTrackData track_data_global = {}; volatile TrackData track_data_temp_global = {}; - void trackGlobalRead(TrackData& out) { + void trackGlobalRead(GlobalTrackData& out) { copyFromVolatile(out, track_data_global); } - int trackGlobalRead(unsigned short idx, TrackData& out) { + int trackGlobalRead(unsigned short idx, GlobalTrackData& out) { if (idx < 1 || idx > 8) { return 1; } @@ -20,11 +20,13 @@ volatile TrackData track_data_temp_global = {}; if (track_data.magic_ != CONFIG_MAGIC) { return 1; } - out = track_data; + + out.loaded = true; + out.root_ = track_data; return 0; } - void trackGlobalWrite(const TrackData& in) { + void trackGlobalWrite(const GlobalTrackData& in) { copyToVolatile(track_data_global, in); } diff --git a/src/data/track_store.h b/src/data/track_store.h index 6295eba..428e672 100644 --- a/src/data/track_store.h +++ b/src/data/track_store.h @@ -6,12 +6,12 @@ #include "custom_types.h" #include -extern volatile TrackData track_data_global; +extern volatile GlobalTrackData track_data_global; extern volatile TrackData track_data_temp_global; -void trackGlobalRead(TrackData& out); -int trackGlobalRead(unsigned short idx, TrackData& out); -void trackGlobalWrite(const TrackData& in); +void trackGlobalRead(GlobalTrackData& out); +int trackGlobalRead(unsigned short idx, GlobalTrackData& out); +void trackGlobalWrite(const GlobalTrackData& in); void trackTempGlobalRead(TrackData& out); void trackTempGlobalWrite(const TrackData& in); diff --git a/src/modules/cmd/cmd.cpp b/src/modules/cmd/cmd.cpp index 8fb8030..31cdaac 100644 --- a/src/modules/cmd/cmd.cpp +++ b/src/modules/cmd/cmd.cpp @@ -149,7 +149,7 @@ int Cmd::dumpTrackSlot(unsigned short id) { configGlobalRead(config); bool occupied = config.track_slot_occupied_[id - 1]; - TrackData track_data; + GlobalTrackData track_data; int result = trackGlobalRead(id, track_data); if (result != 0) { #ifdef ERROR @@ -164,12 +164,12 @@ int Cmd::dumpTrackSlot(unsigned short id) { if (logger_ != nullptr) { logger_->info("Track dump for slot " + String(id)); logger_->info(String("\tOccupied flag: ") + String(occupied)); - logger_->info(String("\tID: ") + String(track_data.id_)); - logger_->info(String("\tName: ") + String(track_data.name_)); - logger_->info(String("\tPoint A lat: ") + String(track_data.point_a_.lat_, 6)); - logger_->info(String("\tPoint A lng: ") + String(track_data.point_a_.lng_, 6)); - logger_->info(String("\tPoint B lat: ") + String(track_data.point_b_.lat_, 6)); - logger_->info(String("\tPoint B lng: ") + String(track_data.point_b_.lng_, 6)); + logger_->info(String("\tID: ") + String(track_data.root_.id_)); + logger_->info(String("\tName: ") + String(track_data.root_.name_)); + logger_->info(String("\tPoint A lat: ") + String(track_data.root_.point_a_.lat_, 6)); + logger_->info(String("\tPoint A lng: ") + String(track_data.root_.point_a_.lng_, 6)); + logger_->info(String("\tPoint B lat: ") + String(track_data.root_.point_b_.lat_, 6)); + logger_->info(String("\tPoint B lng: ") + String(track_data.root_.point_b_.lng_, 6)); } #endif diff --git a/src/modules/config/config.cpp b/src/modules/config/config.cpp index 4fc4d01..624aa44 100644 --- a/src/modules/config/config.cpp +++ b/src/modules/config/config.cpp @@ -68,7 +68,10 @@ int Config::deleteTrack(unsigned short idx) { if (is_track_loaded_ && loaded_track_.id_ == idx) { is_track_loaded_ = false; loaded_track_ = {}; - trackGlobalWrite(loaded_track_); + GlobalTrackData track; + track.loaded = false; + track.root_ = loaded_track_; + trackGlobalWrite(track); } int write_result = this->writeConfig(); @@ -88,7 +91,10 @@ int Config::resetConfig() { loaded_track_ = {}; task_memory_stale_ = true; no_tracks_notice_shown_ = false; - trackGlobalWrite(loaded_track_); + GlobalTrackData track; + track.loaded = false; + track.root_ = loaded_track_; + trackGlobalWrite(track); #ifdef INFO if (logger_ != nullptr) { @@ -373,7 +379,10 @@ int Config::loadTrack(unsigned int idx) { } loaded_track_ = track_data; - trackGlobalWrite(loaded_track_); + GlobalTrackData track; + track.loaded = true; + track.root_ = track_data; + trackGlobalWrite(track); is_track_loaded_ = true; router::send(module::All, task::AllTrackLoaded); return 0; diff --git a/src/modules/gps/gps.cpp b/src/modules/gps/gps.cpp index ec2ea30..5a4aeb0 100644 --- a/src/modules/gps/gps.cpp +++ b/src/modules/gps/gps.cpp @@ -50,6 +50,13 @@ int Gps::loop(unsigned long timeout_ms) { } } + if (lap_active_) { + if (start_line_trigger_ == trigger_status::Idle) { + float current_lat = gps_->location.lat(); + float current_lng = gps_->location.lat(); + } + } + Task active; int res = queue_.pop(active); if (res == 0) { diff --git a/src/modules/lcd/lcd.cpp b/src/modules/lcd/lcd.cpp index 9c9a8a6..ef2a283 100644 --- a/src/modules/lcd/lcd.cpp +++ b/src/modules/lcd/lcd.cpp @@ -195,11 +195,11 @@ int Lcd::renderMsgTrackDetectOk() { display_->setCursor(3, 1); this->print("TRACK DETECTED"); - TrackData track_data; + GlobalTrackData track_data; trackGlobalRead(track_data); - display_->setCursor((20 - strlen(track_data.name_)) / 2, 2); - this->print(track_data.name_); + display_->setCursor((20 - strlen(track_data.root_.name_)) / 2, 2); + this->print(track_data.root_.name_); return 0; }