Added lap completion packet and patched incorrect time bug

src/base/task.h

@@ -47,6 +47,7 @@ enum Type : uint8_t {
     ConfigTengSetLow,
     ConfigTengSetHigh,
     BatteryCal,
+    TelemetrySendLapPacket,
     AllConfigUpdated,
     AllTrackLoaded,
     AllStartLineTriggered,

src/custom_types.h

@@ -79,13 +79,14 @@ inline void copyToVolatile(volatile T& dst, const T& src) {
 }
 
 static inline uint32_t hhmmsscc_to_cs(uint32_t t) {
-  uint32_t hours   =  t / 1000000;
-  uint32_t minutes = (t / 10000) % 100;
-  uint32_t seconds = (t / 100)   % 100;
-  uint32_t cs      =  t % 100;
+    uint32_t hours   =  t / 1000000;
+    uint32_t minutes = (t / 10000) % 100;
+    uint32_t seconds = (t / 100)   % 100;
+    uint32_t cs      =  t % 100;
 
-  return hours * 360000 +
-         minutes * 6000 +
-         seconds * 100 +
-         cs;
+    if (hours >= 24 || minutes >= 60 || seconds >= 60 || cs >= 100) {
+        return 0;
+    }
+
+    return hours * 360000 + minutes * 6000 + seconds * 100 + cs;
 }

src/data/general_store.cpp

@@ -7,6 +7,7 @@ volatile float vbat_global = 0;
 volatile float teng_global = 0;
 volatile int gps_trigger_global = 0;
 volatile uint32_t last_lap_time_global = 0;
+volatile uint32_t last_lap_start_global = 0;
 volatile uint16_t lap_count_global = 0;
 volatile float speed_avg_global = 0;
 volatile uint16_t injection_ctr_global = 0;
@@ -27,6 +28,10 @@ void lastLapTimeGlobalRead(uint32_t &out) { out = last_lap_time_global; }
 
 void lastLapTimeGlobalWrite(const uint32_t &in) { last_lap_time_global = in; }
 
+void lastLapStartGlobalRead(uint32_t &out) { out = last_lap_start_global; }
+
+void lastLapStartGlobalWrite(const uint32_t &in) { last_lap_start_global = in; }
+
 void lapCountGlobalRead(uint16_t &out) { out = lap_count_global; }
 
 void lapCountGlobalWrite(const uint16_t &in) { lap_count_global = in; }

src/data/general_store.h

@@ -8,6 +8,7 @@ extern volatile float vbat_global;
 extern volatile float teng_global;
 extern volatile int gps_trigger_global;
 extern volatile uint32_t last_lap_time_global;
+extern volatile uint32_t last_lap_start_global;
 extern volatile uint16_t lap_count_global;
 extern volatile float speed_avg_global;
 extern volatile uint16_t injection_ctr_global;
@@ -24,6 +25,9 @@ void gpsTriggerGlobalWrite(const int& in);
 void lastLapTimeGlobalRead(uint32_t& out);
 void lastLapTimeGlobalWrite(const uint32_t& in);
 
+void lastLapStartGlobalRead(uint32_t& out);
+void lastLapStartGlobalWrite(const uint32_t& in);
+
 void lapCountGlobalRead(uint16_t& out);
 void lapCountGlobalWrite(const uint16_t& in);
 

src/modules/cmd/cmd.cpp

@@ -133,6 +133,18 @@ Cmd::CommandId Cmd::parseCommandName(const char *input) {
     return ThermoSetHigh;
   }
   
+  if (strcmp(input, "DEBUG_UNLOCK") == 0) {
+    return DebugUnlock;
+  }
+  
+  if (strcmp(input, "DEBUG_LOCK") == 0) {
+    return DebugLock;
+  }
+  
+  if (strcmp(input, "DBG_SEND_BLANK_LAP") == 0) {
+    return DbgSendBlankLap;
+  }
+  
   if (strcmp(input, "HELP") == 0) {
     return HelpGlobal;
   }
@@ -552,6 +564,88 @@ int Cmd::handleHelpGlobal(unsigned short argc) {
   return 0;
 }
 
+int Cmd::handleDebugUnlock(unsigned short argc) {
+  if (argc != 1) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("DEBUG_UNLOCK expects no arguments");
+    }
+#endif
+    return 1;
+  }
+  if (debug_locked_) {
+    #ifdef INFO
+      if (logger_ != nullptr) {
+        logger_->info("God Mode enabled, system can be put into unknown states, use your powers wisely!");
+      }
+    #endif
+  } else {
+    #ifdef INFO
+    if (logger_ != nullptr) {
+      logger_->info("You are already in God Mode, unfortunately there is nothing higher");
+    }
+    #endif
+  }
+  debug_locked_ = false; 
+  return 0;
+}
+
+int Cmd::handleDebugLock(unsigned short argc) {
+  if (argc != 1) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("DEBUG_LOCK expects no arguments");
+    }
+#endif
+    return 1;
+  }
+  if (!debug_locked_) {
+    #ifdef INFO
+      if (logger_ != nullptr) {
+        logger_->info("Disabling God Mode, system will restart to be in a known state");
+      }
+    #endif
+    debug_locked_ = true;
+    delay(200);
+    wdt_enable(WDTO_15MS);
+    while (true) {
+    }
+    return 0;
+  } else {
+    #ifdef INFO
+    if (logger_ != nullptr) {
+      logger_->info("God Mode not enabled, nothing to lock");
+    }
+    #endif
+  }
+  return 0;
+}
+
+int Cmd::handleDbgSendBlankLap(unsigned short argc) {
+  if (argc != 1) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("DBG_SEND_BLANK_LAP expects no arguments");
+    }
+#endif
+    return 1;
+  }
+  if (!debug_locked_) {
+    #ifdef INFO
+    if (logger_ != nullptr) {
+      logger_->info("Sending blank lap packet");
+    }
+    #endif
+    router::sendAll(module::Cmd, task::AllStartLineTriggered);
+  } else {
+    #ifdef INFO
+    if (logger_ != nullptr) {
+      logger_->info("Unable to run debug commands when not in God Mode");
+    }
+    #endif
+  }
+}  
+  
 int Cmd::handleUnknownCommand(unsigned short argc, char *argv[]) {
 #ifdef ERROR
   if (logger_ != nullptr) {
@@ -609,6 +703,15 @@ int Cmd::dispatchCommand(CommandId command, unsigned short argc, char *argv[]) {
     case ThermoSetHigh:
       return this->handleThermoSetHigh(argc, argv);
       
+    case DebugUnlock:
+      return this->handleDebugUnlock(argc);
+      
+    case DebugLock:
+      return this->handleDebugLock(argc);
+      
+    case DbgSendBlankLap:
+      return this->handleDbgSendBlankLap(argc);
+
     case HelpGlobal:
       return this->handleHelpGlobal(argc);
 

src/modules/cmd/cmd.h

@@ -30,6 +30,9 @@ private:
     BatterySetLow,
     ThermoSetLow,
     ThermoSetHigh,
+    DebugUnlock,
+    DebugLock,
+    DbgSendBlankLap,
     HelpGlobal,
   };
 
@@ -39,6 +42,7 @@ private:
   char buffer_[256];
   unsigned int index_ = 0;
   bool buffer_open_ = false;
+  bool debug_locked_ = true;
 
   static const unsigned short MAX_ARGS = 10;
 
@@ -65,6 +69,9 @@ private:
   int handleThermoSetHigh(unsigned short argc, char *argv[]);
   int handleHelpGlobal(unsigned short argc);
   int handleUnknownCommand(unsigned short argc, char *argv[]);
+  int handleDebugUnlock(unsigned short argc);
+  int handleDebugLock(unsigned short argc);
+  int handleDbgSendBlankLap(unsigned short argc);
 
 public:
   int push(const Task &task) override;

src/modules/cmd/help.h

@@ -20,5 +20,8 @@ static constexpr char kGlobalHelpText[] =
     "  BATTERY_PRINT_VBAT\n"
     "  BATTERY_SET_LOW,<voltage>\n"
     "  THERMO_SET_LOW,<temperature>\n"
-    "  THERMO_SET_HIGH,<temperature>\n";
+    "  THERMO_SET_HIGH,<temperature>\n"
+    "  DEBUG_UNLOCK\n"
+    "  DEBUG_LOCK\n"
+    "  DBG_SEND_BLANK_LAP\n";
 } // namespace cmd_help

src/modules/gps/gps.cpp

@@ -175,7 +175,11 @@ GpsData Gps::getData() {
   output.course_.valid_ = gps_->course.isValid();
   output.course_.value_ = gps_->course.deg();
 
-  output.time_ = gps_->time.value();
+  if (gps_->time.isValid()) {
+    output.time_ = gps_->time.value();
+  } else {
+    output.time_ = 0;
+  }
   output.time_write_time_ = millis() - gps_->time.age();
 
   output.num_fix_ = gps_->sentencesWithFix();

src/modules/lap_counter/lap_counter.cpp

@@ -55,13 +55,15 @@ int LapCounter::loop() {
 
           lap_times_[lap_times_idx_] = lap_time;
           count_++;
+
           lapCountGlobalWrite(count_);
+          lastLapStartGlobalWrite(last_trigger_time_);
+          lastLapTimeGlobalWrite(lap_time);
 
           last_trigger_time_ = time_cs;
 
-          lastLapTimeGlobalWrite(lap_time);
-
           router::send(module::Lcd, task::DisplayMsgLapCounterLapTime, 1000);
+          router::send(module::Telemetry, task::TelemetrySendLapPacket);
         }
 
         break;

src/modules/telemetry/telemetry.cpp

@@ -5,10 +5,7 @@
 #include "base/router.h"
 #include "data/general_store.h"
 
-int Telemetry::push(const Task &task) {
-    (void)task;
-    return 0;
-}
+int Telemetry::push(const Task &task) { return queue_.push(task); }
 
 Telemetry::Telemetry(HardwareSerial* data_stream) : logger_(nullptr), data_stream_(data_stream) {}
 
@@ -46,6 +43,39 @@ int Telemetry::loop() {
             data_stream_->write((uint8_t*)&packet, sizeof(packet));
         }
         last_sent_ = millis();
+    } else {
+        
+        Task active_task;
+        int res = queue_.pop(active_task);
+
+        if (res == 0) {
+            if (active_task.target_ == module::Telemetry) {
+                switch (active_task.type_) {
+                    case task::TelemetrySendLapPacket: {
+                        TelemetryPacket3 packet;
+                        lastLapStartGlobalRead(packet.start_time_);
+                        lastLapTimeGlobalRead(packet.duration_);
+                        lapCountGlobalRead(packet.count_);
+                        lora_header_.size_ = sizeof(packet);
+                        lora_header_.crc16_ = crc16_ccitt((uint8_t*)&packet, sizeof(packet));
+                        lora_header_.version_ = 3;
+                        uart_header_.size_ = sizeof(packet) + sizeof(lora_header_);
+                        
+                        if (data_stream_->availableForWrite()) {
+                            data_stream_->write((uint8_t*)&uart_header_, sizeof(uart_header_));
+                            data_stream_->write((uint8_t*)&lora_header_, sizeof(lora_header_));
+                            data_stream_->write((uint8_t*)&packet, sizeof(packet));
+                        }
+                        #ifdef DEBUG
+                        if (logger_ != nullptr) {
+                            logger_->debug("Send lap complete telemetry packet");
+                        }
+                        #endif
+                    } 
+                }
+            } else if (active_task.target_ == module::All) {
+            }
+        }
     }
     return 0;
 }

src/modules/telemetry/telemetry.h

@@ -17,6 +17,7 @@ private:
   unsigned long update_interval_ = 1000;
   TelemetryLoRaHeader lora_header_;
   TelemetryUARTHeader uart_header_;
+  RingBuffer<Task, 16> queue_;
 
 public:
   int push(const Task &task) override;