Arming logic impl

src/custom_types.cpp

@@ -41,14 +41,14 @@ LatLng eqRectInverse(const Vec2& point, const LatLng& ref) {
     return res;
 }
 
-Vec2 abMidpoint(const Vec2& A, const Vec2& B) {
+Vec2 vec2Midpoint(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;
 }
 
-Vec2 abSum(const Vec2& A, const Vec2& B) {
+Vec2 vec2Sum(const Vec2& A, const Vec2& B) {
   Vec2 res;
   res.x_ = A.x_ + B.x_;
   res.y_ = A.y_ + B.y_;
@@ -59,8 +59,13 @@ float vecMod(const Vec2& in) {
   return sqrtf(in.x_*in.x_ + in.y_*in.y_);
 }
 
-float abSqDist(const Vec2& A, const Vec2& B) {
+float vec2SqDist(const Vec2& A, const Vec2& B) {
   float deltaX = B.x_ - A.x_;
   float deltaY = B.y_ - A.y_;
   return deltaX * deltaX + deltaY * deltaY;
+}
+
+
+float vec2Cross(const Vec2& B, const Vec2& M) {
+  return B.x_ * M.y_ - B.y_ * M.x_;
 }

src/custom_types.h

@@ -30,10 +30,11 @@ struct Vec2 {
 
 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 vec2Midpoint(const Vec2& A, const Vec2& B);
-Vec2 abSum(const Vec2& A, const Vec2& B);
+Vec2 vec2Sum(const Vec2& A, const Vec2& B);
 float vecMod(const Vec2& in);
-float abSqDist(const Vec2& A, const Vec2& B);
+float vec2SqDist(const Vec2& A, const Vec2& B);
+float vec2Cross(const Vec2& B, const Vec2& M);
 
 struct TrackData {
     uint16_t magic_ = CONFIG_MAGIC;
@@ -46,7 +47,7 @@ struct TrackData {
 struct GlobalTrackData {
     bool loaded_ = false;
     Vec2 center_;
-    float circle_radius_;
+    float circle_radius_sq_;
     TrackData root_;
 };
 

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.center_ = vec2Midpoint(point_b, (Vec2){0.0f,0.0f});
-  track.circle_radius_ = max(10.0f, vecMod(point_b) * 1.25f);
+  float radius = max(10.0f, vecMod(point_b) * 1.25f);
-  #ifdef DEBUG
+  track.circle_radius_sq_ = radius*radius;
-  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;

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;