From 2234ed7109bac340632269441e56d2262fb58f8c Mon Sep 17 00:00:00 2001 From: Hector van der Aa Date: Fri, 3 Apr 2026 14:34:21 +0200 Subject: [PATCH] GPS Line detect impl --- src/data/general_store.cpp | 9 +++ src/data/general_store.h | 4 ++ src/modules/cmd/cmd.cpp | 7 ++- src/modules/cmd/cmd.h | 1 + src/modules/gps/gps.cpp | 109 ++++++++++++++++++++++++------------- src/modules/gps/gps.h | 15 +++-- src/modules/lcd/lcd.cpp | 66 ++++++++++++++++++---- src/modules/lcd/lcd.h | 2 + 8 files changed, 158 insertions(+), 55 deletions(-) diff --git a/src/data/general_store.cpp b/src/data/general_store.cpp index 584fb4b..a60e360 100644 --- a/src/data/general_store.cpp +++ b/src/data/general_store.cpp @@ -5,6 +5,7 @@ volatile float vbat_global = 0; volatile float teng_global = 0; + volatile int gps_trigger_global = 0; void vbatGlobalRead(float& out) { out = vbat_global; @@ -21,3 +22,11 @@ void tengGlobalWrite(const float& in) { teng_global = in; } + +void gpsTriggerGlobalRead(int& out) { + out = gps_trigger_global; +} + +void gpsTriggerGlobalWrite(const int& in) { + gps_trigger_global = in; +} diff --git a/src/data/general_store.h b/src/data/general_store.h index 4f3abf3..e5e7648 100644 --- a/src/data/general_store.h +++ b/src/data/general_store.h @@ -5,9 +5,13 @@ extern volatile float vbat_global; extern volatile float teng_global; +extern volatile int gps_trigger_global; void vbatGlobalRead(float& out); void vbatGlobalWrite(const float& in); void tengGlobalRead(float& out); void tengGlobalWrite(const float& in); + +void gpsTriggerGlobalRead(int& out); +void gpsTriggerGlobalWrite(const int& in); diff --git a/src/modules/cmd/cmd.cpp b/src/modules/cmd/cmd.cpp index 31cdaac..24e583d 100644 --- a/src/modules/cmd/cmd.cpp +++ b/src/modules/cmd/cmd.cpp @@ -104,6 +104,10 @@ Cmd::CommandId Cmd::parseCommandName(const char *input) { return DisplayGpsDebug; } + if (strcmp(input, "DISPLAY_GPS_LINE_DEBUG") == 0) { + return DisplayGpsLineDebug; + } + if (strcmp(input, "DISPLAY_DRIVER_PRIMARY") == 0) { return DisplayDriverPrimary; } @@ -365,6 +369,7 @@ int Cmd::handleDisplayGpsDebug(unsigned short argc) { return router::send(module::Lcd, task::DisplayGpsDebug); } + int Cmd::handleDisplayDriverPrimary(unsigned short argc) { if (argc != 1) { #ifdef ERROR @@ -525,7 +530,7 @@ int Cmd::dispatchCommand(CommandId command, unsigned short argc, char *argv[]) { case DisplayGpsDebug: return this->handleDisplayGpsDebug(argc); - + case DisplayDriverPrimary: return this->handleDisplayDriverPrimary(argc); diff --git a/src/modules/cmd/cmd.h b/src/modules/cmd/cmd.h index 7c9096c..d57592e 100644 --- a/src/modules/cmd/cmd.h +++ b/src/modules/cmd/cmd.h @@ -24,6 +24,7 @@ private: ConfigDump, TrackAutodetect, DisplayGpsDebug, + DisplayGpsLineDebug, DisplayDriverPrimary, BatteryCal, BatteryPrintVbat, diff --git a/src/modules/gps/gps.cpp b/src/modules/gps/gps.cpp index 7ed99be..b7e584e 100644 --- a/src/modules/gps/gps.cpp +++ b/src/modules/gps/gps.cpp @@ -34,6 +34,7 @@ int Gps::init() { int Gps::loop(unsigned long timeout_ms) { unsigned long timeout = millis() + timeout_ms; + bool timed_out = false; while (data_stream_->available() > 0) { if (gps_->encode(data_stream_->read())) { @@ -47,39 +48,8 @@ int Gps::loop(unsigned long timeout_ms) { } if (millis() > timeout) { - return 1; - } - } - - if (lap_active_) { - if (millis() - last_check_ > check_interval_) { - if (start_line_trigger_ == trigger_status::Idle) { - GpsData gps; - gpsGlobalRead(gps); - LatLng vehicle_position = {gps.lat_.value_, gps.lng_.value_}; - - Vec2 vehicle_pos_vec = - eqRectProjection(vehicle_position, track_point_a_); - float sq_diff = vec2SqDist(track_vec_center_, vehicle_pos_vec); - if (sq_diff < track_sq_dist_) { - start_line_trigger_ = trigger_status::Armed; - last_arm_ = millis(); - float cross = vec2Cross(track_vec_b_, vehicle_pos_vec); - if (cross > 0) { - arm_sign_ = 1; - } else { - arm_sign_ = -1; - } - } else { - float cross = vec2Cross(track_vec_b_, vehicle_pos_vec); - #ifdef DEBUG - if (logger_ != nullptr) { - logger_->debug(String(cross, 6)); - } - #endif - } - } - last_check_ = millis(); + timed_out = true; + break; } } @@ -90,21 +60,29 @@ int Gps::loop(unsigned long timeout_ms) { } else if (active.target_ == module::All) { switch (active.type_) { - case task::AllTrackLoaded: + case task::AllTrackLoaded: { #ifdef DEBUG if (logger_ != nullptr) { logger_->debug("GPS received track loaded sig"); } #endif - lap_active_ = true; GlobalTrackData track; trackGlobalRead(track); + lap_active_ = track.loaded_; + start_line_trigger_ = trigger_status::Idle; + gpsTriggerGlobalWrite(start_line_trigger_); + arm_sign_ = 0; + if (!track.loaded_) { + break; + } track_point_a_ = track.root_.point_a_; track_point_b_ = track.root_.point_b_; - track_vec_center_ = track.center_; - track_sq_dist_ = track.circle_radius_sq_; track_vec_b_ = eqRectProjection(track_point_b_, track_point_a_); + track_vec_center_ = track.center_; + start_line_length_ = vecMod(track_vec_b_); + start_line_sq_dist_ = track.circle_radius_sq_; break; + } default: break; @@ -112,6 +90,63 @@ int Gps::loop(unsigned long timeout_ms) { } } + if (lap_active_ && millis() - last_check_ > check_interval_) { + unsigned long now = millis(); + GpsData gps; + gpsGlobalRead(gps); + LatLng vehicle_position = {gps.lat_.value_, gps.lng_.value_}; + + Vec2 vehicle_pos_vec = eqRectProjection(vehicle_position, track_point_a_); + float center_dist_sq = vec2SqDist(track_vec_center_, vehicle_pos_vec); + float cross = vec2Cross(track_vec_b_, vehicle_pos_vec); + int sign = 0; + if (cross < 0) { + sign = -1; + } else if (cross > 0) { + sign = 1; + } + + switch (start_line_trigger_) { + case trigger_status::Idle: { + if (center_dist_sq < start_line_sq_dist_) { + start_line_trigger_ = trigger_status::Armed; + gpsTriggerGlobalWrite(start_line_trigger_); + arm_sign_ = sign; + state_changed_at_ = now; + } + break; + }; + + case trigger_status::Armed: { + if (center_dist_sq > start_line_sq_dist_) { + start_line_trigger_ = trigger_status::Idle; + gpsTriggerGlobalWrite(start_line_trigger_); + arm_sign_ = 0; + state_changed_at_ = 0; + } else if ((sign == -1 && arm_sign_ == 1) || (sign == 1 && arm_sign_ == -1)) { + start_line_trigger_ = trigger_status::Trigd; + gpsTriggerGlobalWrite(start_line_trigger_); + arm_sign_ = 0; + state_changed_at_ = now; + } + break; + }; + + case trigger_status::Trigd: { + if (center_dist_sq > start_line_sq_dist_) { + start_line_trigger_ = trigger_status::Idle; + gpsTriggerGlobalWrite(start_line_trigger_); + arm_sign_ = 0; + state_changed_at_ = now; + } + break; + }; + + default: + break; + } + last_check_ = now; + } return 0; } diff --git a/src/modules/gps/gps.h b/src/modules/gps/gps.h index 2788cf3..8562b5b 100644 --- a/src/modules/gps/gps.h +++ b/src/modules/gps/gps.h @@ -11,14 +11,15 @@ #include "base/ring_buffer.h" #include "base/module_base.h" #include "data/gps_store.h" +#include "data/general_store.h" #include "base/router.h" namespace trigger_status { enum TriggerStatus { - Idle, - Armed, - Trigd, + Idle = 0, + Armed = 1, + Trigd = 2, }; } @@ -35,14 +36,16 @@ private: LatLng track_point_b_; Vec2 track_vec_b_; Vec2 track_vec_center_; - float track_sq_dist_; + float start_line_length_ = 0; + float start_line_sq_dist_ = 0; bool lap_active_ = false; unsigned long last_check_ = 0; - unsigned long check_interval_ = 250; - unsigned long last_arm_ = 0; + unsigned long check_interval_ = 100; + unsigned long state_changed_at_ = 0; int arm_sign_ = 0; + public: int push(const Task &task) override; Gps(HardwareSerial *data_stream); diff --git a/src/modules/lcd/lcd.cpp b/src/modules/lcd/lcd.cpp index ef2a283..b754c36 100644 --- a/src/modules/lcd/lcd.cpp +++ b/src/modules/lcd/lcd.cpp @@ -6,6 +6,8 @@ #include #include +#include "modules/gps/gps.h" +#include "data/general_store.h" #define MOD "modules/lcd/lcd.h" @@ -155,26 +157,54 @@ int Lcd::renderDriverPrimary() { vbatGlobalRead(vbat); float teng; tengGlobalRead(teng); + + int line_trigger; + gpsTriggerGlobalRead(line_trigger); display_->setCursor(0,0); this->print("GPS:"); if (gps_data.num_fix_ != 0) { - this->print("V"); + this->print("Y"); } else { - this->print("X"); + this->print("N"); + } + + display_->setCursor(7,0); + this->print("TRIG:"); + switch (line_trigger) + { + case 0: + this->print("I"); + break; + + case 1: + this->print("A"); + break; + + case 2: + this->print("T"); + break; + + default: + this->print("NULL"); + break; } - display_->setCursor(3,2); - this->print("SPEED: "); - this->print(gps_data.speed_.value_); + display_->setCursor(0,2); + this->print("SPD:"); + if (gps_data.speed_.valid_) { + this->print(gps_data.speed_.value_, 1); + } else { + this->print("NA"); + } display_->setCursor(0,3); - this->print("Vbat:"); - this->print(vbat); + this->print("V:"); + this->print(vbat, 1); display_->setCursor(10,3); - this->print("Teng:"); - this->print(teng); + this->print("T:"); + this->print(teng, 1); return 0; } @@ -188,6 +218,15 @@ int Lcd::renderMsgGpsFix() { return 0; } +int Lcd::renderMsgGpsTrigger() { + this->clear(); + display_->setCursor(6, 1); + this->print("GPS INFO"); + display_->setCursor(4, 2); + this->print("LINE TRIGGER"); + return 0; +} + int Lcd::renderMsgTrackDetectOk() { this->clear(); display_->setCursor(6, 0); @@ -445,7 +484,8 @@ int Lcd::loop(unsigned long timeout_ms) { break; case task::DisplayMsgEngineTempHigh: - activateMessage(screen::MsgEngineTempLow, next_task.data_); + activateMessage(screen::MsgEngineTempHigh, next_task.data_); + break; default: break; @@ -480,7 +520,7 @@ int Lcd::loop(unsigned long timeout_ms) { case screen::GpsDebug: this->renderGpsDebug(); break; - + case screen::DriverPrimary: this->renderDriverPrimary(); break; @@ -489,6 +529,10 @@ int Lcd::loop(unsigned long timeout_ms) { this->renderMsgGpsFix(); break; + case screen::MsgGpsTrigger: + this->renderMsgGpsTrigger(); + break; + case screen::MsgTrackDetectOk: this->renderMsgTrackDetectOk(); break; diff --git a/src/modules/lcd/lcd.h b/src/modules/lcd/lcd.h index b4c5414..769f662 100644 --- a/src/modules/lcd/lcd.h +++ b/src/modules/lcd/lcd.h @@ -23,6 +23,7 @@ enum LcdScreen : uint8_t { GpsDebug, DriverPrimary, MsgGpsFix, + MsgGpsTrigger, MsgTrackDetectOk, MsgConfigNoTracks, MsgBatteryLow, @@ -67,6 +68,7 @@ private: int renderGpsDebug(); int renderDriverPrimary(); int renderMsgGpsFix(); + int renderMsgGpsTrigger(); int renderMsgTrackDetectOk(); int renderMsgConfigNoTracks(); int renderMsgBatteryLow();