GPS Line detect impl

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;
+}

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);

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);
       

src/modules/cmd/cmd.h

@@ -24,6 +24,7 @@ private:
     ConfigDump,
     TrackAutodetect,
     DisplayGpsDebug,
+    DisplayGpsLineDebug,
     DisplayDriverPrimary,
     BatteryCal,
     BatteryPrintVbat,

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;
 }
 

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);

src/modules/lcd/lcd.cpp

@@ -6,6 +6,8 @@
 
 #include <Wire.h>
 #include <string.h>
+#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;

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();