Arming logic impl

src/modules/config/config.cpp

@@ -380,16 +380,14 @@ int Config::loadTrack(unsigned int idx) {
 
   loaded_track_ = track_data;
   GlobalTrackData track;
+
   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.center_ = vec2Midpoint(point_b, (Vec2){0.0f,0.0f});
+  float radius = max(10.0f, vecMod(point_b) * 1.25f);
+  track.circle_radius_sq_ = radius*radius;
+
   track.root_ = track_data;
   trackGlobalWrite(track);
   is_track_loaded_ = true;

src/modules/gps/gps.cpp

@@ -3,6 +3,7 @@
 // SPDX-License-Identifier: GPL-3.0-or-later
 
 #include "gps.h"
+#include "math.h"
 
 #include "data/track_store.h"
 
@@ -51,7 +52,34 @@ int Gps::loop(unsigned long timeout_ms) {
   }
 
   if (lap_active_) {
-    if (start_line_trigger_ == trigger_status::Idle) {
+    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();
     }
   }
 
@@ -69,6 +97,13 @@ int Gps::loop(unsigned long timeout_ms) {
         }
 #endif
         lap_active_ = true;
+        GlobalTrackData track;
+        trackGlobalRead(track);
+        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_);
         break;
 
       default:

src/modules/gps/gps.h

@@ -30,6 +30,13 @@ private:
   RingBuffer<Task, 16> queue_;
   uint32_t last_fix_value_ = 0;
   trigger_status::TriggerStatus start_line_trigger_ = trigger_status::Idle;
+
+  LatLng track_point_a_;
+  LatLng track_point_b_;
+  Vec2 track_vec_b_;
+  Vec2 track_vec_center_;
+  float track_sq_dist_;
+
   bool lap_active_ = false;
   unsigned long last_check_ = 0;
   unsigned long check_interval_ = 250;