diff --git a/src/base/task.h b/src/base/task.h index 8f9d910..0b31fd3 100644 --- a/src/base/task.h +++ b/src/base/task.h @@ -47,6 +47,7 @@ enum Type : uint8_t { ConfigTengSetLow, ConfigTengSetHigh, BatteryCal, + TelemetrySendLapPacket, AllConfigUpdated, AllTrackLoaded, AllStartLineTriggered, diff --git a/src/custom_types.h b/src/custom_types.h index 458a4e3..e4c3f40 100644 --- a/src/custom_types.h +++ b/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; } diff --git a/src/data/general_store.cpp b/src/data/general_store.cpp index 62e8379..3d0a89e 100644 --- a/src/data/general_store.cpp +++ b/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; } diff --git a/src/data/general_store.h b/src/data/general_store.h index a0fb17c..6262df7 100644 --- a/src/data/general_store.h +++ b/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); diff --git a/src/modules/cmd/cmd.cpp b/src/modules/cmd/cmd.cpp index f30d14b..85c1fa3 100644 --- a/src/modules/cmd/cmd.cpp +++ b/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) { @@ -608,6 +702,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); diff --git a/src/modules/cmd/cmd.h b/src/modules/cmd/cmd.h index 378cb2d..3e6f2c3 100644 --- a/src/modules/cmd/cmd.h +++ b/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; diff --git a/src/modules/cmd/help.h b/src/modules/cmd/help.h index 686be3f..f427da3 100644 --- a/src/modules/cmd/help.h +++ b/src/modules/cmd/help.h @@ -20,5 +20,8 @@ static constexpr char kGlobalHelpText[] = " BATTERY_PRINT_VBAT\n" " BATTERY_SET_LOW,\n" " THERMO_SET_LOW,\n" - " THERMO_SET_HIGH,\n"; + " THERMO_SET_HIGH,\n" + " DEBUG_UNLOCK\n" + " DEBUG_LOCK\n" + " DBG_SEND_BLANK_LAP\n"; } // namespace cmd_help diff --git a/src/modules/gps/gps.cpp b/src/modules/gps/gps.cpp index ce793f8..1eccbac 100644 --- a/src/modules/gps/gps.cpp +++ b/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(); diff --git a/src/modules/lap_counter/lap_counter.cpp b/src/modules/lap_counter/lap_counter.cpp index cfd9911..ca6d4f8 100644 --- a/src/modules/lap_counter/lap_counter.cpp +++ b/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; diff --git a/src/modules/telemetry/telemetry.cpp b/src/modules/telemetry/telemetry.cpp index 09c7057..d571de7 100644 --- a/src/modules/telemetry/telemetry.cpp +++ b/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; } diff --git a/src/modules/telemetry/telemetry.h b/src/modules/telemetry/telemetry.h index 8d181d9..4012029 100644 --- a/src/modules/telemetry/telemetry.h +++ b/src/modules/telemetry/telemetry.h @@ -17,6 +17,7 @@ private: unsigned long update_interval_ = 1000; TelemetryLoRaHeader lora_header_; TelemetryUARTHeader uart_header_; + RingBuffer queue_; public: int push(const Task &task) override;