Initial telemetry implementation

src/base/task.h

@@ -14,6 +14,7 @@ enum Id : uint8_t {
     Lcd,
     Battery,
     Thermocouple,
+    Telemetry,
     Count,
     Null,
     All,

src/custom_types.h

@@ -12,15 +12,15 @@ struct VehicleConfig {
     bool auto_detect_track_ = true;
     uint8_t track_fallback_ = 0;
     bool track_slot_occupied_[8] = {false};
-    double vbat_calibration_ = 0;
+    float vbat_calibration_ = 0;
-    double vbat_low_ = 0;
+    float vbat_low_ = 0;
-    double teng_low_ = 0;
+    float teng_low_ = 0;
-    double teng_high_ = 0;
+    float teng_high_ = 0;
 };
 
 struct LatLng {
-    double lat_;
+    float lat_;
-    double lng_;
+    float lng_;
 };
 
 struct TrackData {
@@ -34,7 +34,7 @@ struct TrackData {
 struct GpsSubData {
     uint32_t age_;
     bool valid_;
-    double value_;
+    float value_;
 };
 
 struct GpsData {

src/data/general_store.cpp

@@ -3,21 +3,21 @@
 // SPDX-License-Identifier: GPL-3.0-or-later
 #include "general_store.h"
 
- volatile double vbat_global = 0;
+ volatile float vbat_global = 0;
- volatile double teng_global = 0;
+ volatile float teng_global = 0;
 
- void vbatGlobalRead(double& out) {
+ void vbatGlobalRead(float& out) {
      out = vbat_global;
  }
 
- void vbatGlobalWrite(const double& in) {
+ void vbatGlobalWrite(const float& in) {
      vbat_global = in;
  }
 
- void tengGlobalRead(double& out) {
+ void tengGlobalRead(float& out) {
      out = teng_global;
  }
 
- void tengGlobalWrite(const double& in) {
+ void tengGlobalWrite(const float& in) {
      teng_global = in;
  }

src/data/general_store.h

@@ -3,11 +3,11 @@
 // SPDX-License-Identifier: GPL-3.0-or-later
 #pragma once
 
-extern volatile double vbat_global;
+extern volatile float vbat_global;
-extern volatile double teng_global;
+extern volatile float teng_global;
 
-void vbatGlobalRead(double& out);
+void vbatGlobalRead(float& out);
-void vbatGlobalWrite(const double& in);
+void vbatGlobalWrite(const float& in);
 
-void tengGlobalRead(double& out);
+void tengGlobalRead(float& out);
-void tengGlobalWrite(const double& in);
+void tengGlobalWrite(const float& in);

src/main.cpp

@@ -15,6 +15,7 @@
 #include "base/router.h"
 #include "modules/battery/battery.h"
 #include "modules/thermocouple/thermocouple.h"
+#include "modules/telemetry/telemetry.h"
 
 
 SystemLogger *logger = new SystemLogger(&Serial);
@@ -25,6 +26,7 @@ Config *system_config = new Config(logger);
 Cmd *command_handler = new Cmd(&Serial, logger);
 Battery *battery_module = new Battery(logger);
 Thermocouple *thermocouple_module = new Thermocouple(logger);
+Telemetry *telemetry_module = new Telemetry(&Serial1, logger);
 
 
 
@@ -37,39 +39,46 @@ void setup() {
   module_registry[module::Cmd] = command_handler;
   module_registry[module::Battery] = battery_module;
   module_registry[module::Thermocouple] = thermocouple_module;
+  module_registry[module::Telemetry] = telemetry_module;
 
   display->init();
   display->printMessage("Starting Initialization");
-  delay(1000);
+  delay(750);
 
   display->printMessage("Cmd Init...");
   command_handler->init();
-  delay(1000);
+  delay(750);
   display->printMessage("Cmd Init Complete");
-  delay(1000);
+  delay(750);
 
   display->printMessage("Config Init...");
   int result = system_config->autoInit();
-  delay(1000);
+  delay(750);
   if (result != 0) {
     display->printMessage("Configuration Read Failed");
   } else {
     display->printMessage("Config Init Complete");
   }
-  delay(1000);
+  delay(750);
 
   display->printMessage("GPS Init...");
   gps_module->init();
-  delay(1000);
+  delay(750);
   display->printMessage("GPS Init Complete");
-  delay(1000);
+  delay(750);
 
   display->printMessage("Sensors Init...");
   battery_module->init();
   thermocouple_module->init();
-  delay(1000);
+  delay(750);
   display->printMessage("Sensors Init Complete");
-  delay(1000);
+  delay(750);
+
+  display->printMessage("Telemetry Init...");
+  telemetry_module->init();
+  delay(750);
+  display->printMessage("Telemetry Init Complete");
+  delay(750);
   router::send(module::Lcd, task::DisplayDriverPrimary);
 }
 
@@ -80,4 +89,5 @@ void loop() {
   system_config->loop();
   battery_module->loop();
   thermocouple_module->loop();
+  telemetry_module->loop();
 }

src/modules/battery/battery.cpp

@@ -8,10 +8,10 @@
 #include "data/general_store.h"
 
 int Battery::calibrate(const Task &task) {
-  double actual_voltage;
+  float actual_voltage;
-  memcpy(&actual_voltage, &task.data_, sizeof(double));
+  memcpy(&actual_voltage, &task.data_, sizeof(float));
   int adc_read = analogRead(VBAT_PIN);
-  double cal_factor = actual_voltage / adc_read;
+  float cal_factor = actual_voltage / adc_read;
   uint32_t output_val;
   memcpy(&output_val, &cal_factor, sizeof(uint32_t));
   router::send(module::Config, task::ConfigVbatCalSet, output_val);

src/modules/battery/battery.h

@@ -14,9 +14,9 @@ class Battery : public ModuleBase {
 private:
   SystemLogger *logger_;
   RingBuffer<Task, 16> queue_;
-  double vbat_ = 0;
+  float vbat_ = 0;
-  double calibration_ = 0;
+  float calibration_ = 0;
-  double low_threshold_ = 0;
+  float low_threshold_ = 0;
   unsigned long warning_sent_at_ = 0;
   unsigned long warning_timeout_ = 10000;
   unsigned long update_interval_ = 1000;

src/modules/cmd/cmd.cpp

@@ -392,7 +392,7 @@ int Cmd::handleBatteryCal(unsigned short argc, char *argv[]) {
 #endif
     return 1;
   }
-  double vbat = strtod(argv[1], nullptr);
+  float vbat = strtod(argv[1], nullptr);
   uint32_t task_data;
   memcpy(&task_data, &vbat, sizeof(uint32_t));
 #ifdef INFO
@@ -415,7 +415,7 @@ int Cmd::handleBatteryPrintVbat(unsigned short argc) {
   }
 
 #ifdef INFO
-  double vbat;
+  float vbat;
   vbatGlobalRead(vbat);
   if (logger_ != nullptr) {
     logger_->info("VBAT: " + String(vbat));
@@ -433,7 +433,7 @@ int Cmd::handleBatterySetLow(unsigned short argc, char* argv[]) {
 #endif
     return 1;
   }
-  double low = strtod(argv[1], nullptr);
+  float low = strtod(argv[1], nullptr);
   uint32_t task_data;
   memcpy(&task_data, &low, sizeof(uint32_t));
 #ifdef INFO
@@ -454,7 +454,7 @@ int Cmd::handleThermoSetLow(unsigned short argc, char* argv[]) {
 #endif
     return 1;
   }
-  double low = strtod(argv[1], nullptr);
+  float low = strtod(argv[1], nullptr);
   uint32_t task_data;
   memcpy(&task_data, &low, sizeof(uint32_t));
 #ifdef INFO
@@ -475,7 +475,7 @@ int Cmd::handleThermoSetHigh(unsigned short argc, char* argv[]) {
 #endif
     return 1;
   }
-  double low = strtod(argv[1], nullptr);
+  float low = strtod(argv[1], nullptr);
   uint32_t task_data;
   memcpy(&task_data, &low, sizeof(uint32_t));
 #ifdef INFO

src/modules/config/config.cpp

@@ -99,22 +99,22 @@ int Config::resetConfig() {
   return this->writeConfig();
 }
 
-int Config::writeVbatCal(double value) {
+int Config::writeVbatCal(float value) {
   config_.vbat_calibration_ = value;
   return this->writeConfig();
 }
 
-int Config::writeVbatLow(double value) {
+int Config::writeVbatLow(float value) {
   config_.vbat_low_ = value;
   return this->writeConfig();
 }
 
-int Config::writeTengLow(double value) {
+int Config::writeTengLow(float value) {
   config_.teng_low_ = value;
   return this->writeConfig();
 }
 
-int Config::writeTengHigh(double value) {
+int Config::writeTengHigh(float value) {
   config_.teng_high_ = value;
   return this->writeConfig();
 }
@@ -188,9 +188,9 @@ int Config::taskConfigDetectTrack(unsigned long timeout_ms) {
     TrackData track_data;
     int result = this->getTrack(task_data.last_checked_, track_data);
     if (result == 0) {
-      double delta_lat = track_data.point_a_.lat_ - task_data.gps_lat_;
+      float delta_lat = track_data.point_a_.lat_ - task_data.gps_lat_;
-      double delta_lng = (track_data.point_a_.lng_ - task_data.gps_lng_) * task_data.cos_;
+      float delta_lng = (track_data.point_a_.lng_ - task_data.gps_lng_) * task_data.cos_;
-      double dist2 = delta_lat * delta_lat + delta_lng * delta_lng;
+      float dist2 = delta_lat * delta_lat + delta_lng * delta_lng;
 
       if (dist2 < task_data.sqdiff_ || task_data.smallest_idx_ == 0) {
         task_data.smallest_idx_ = task_data.last_checked_;
@@ -255,32 +255,32 @@ int Config::handleActiveTask(unsigned long timeout_ms) {
   }
 
   case task::ConfigVbatCalSet: {
-    double cal_value;
+    float cal_value;
-    memcpy(&cal_value, &active_task_.data_, sizeof(double));
+    memcpy(&cal_value, &active_task_.data_, sizeof(float));
     int result = this->writeVbatCal(cal_value);
     this->taskComplete();
     return result;
   }
 
   case task::ConfigVbatSetLow: {
-    double low_value;
+    float low_value;
-    memcpy(&low_value, &active_task_.data_, sizeof(double));
+    memcpy(&low_value, &active_task_.data_, sizeof(float));
     int result = this->writeVbatLow(low_value);
     this->taskComplete();
     return result;
   }
 
   case task::ConfigTengSetLow: {
-    double low_value;
+    float low_value;
-    memcpy(&low_value, &active_task_.data_, sizeof(double));
+    memcpy(&low_value, &active_task_.data_, sizeof(float));
     int result = this->writeTengLow(low_value);
     this->taskComplete();
     return result;
   }
 
   case task::ConfigTengSetHigh: {
-    double high_value;
+    float high_value;
-    memcpy(&high_value, &active_task_.data_, sizeof(double));
+    memcpy(&high_value, &active_task_.data_, sizeof(float));
     int result = this->writeTengHigh(high_value);
     this->taskComplete();
     return result;

src/modules/config/config.h

@@ -18,10 +18,10 @@
 struct TaskConfigTrackDetectData {
   unsigned short last_checked_ = 0;
   unsigned short smallest_idx_ = 0;
-  double cos_ = 0;
+  float cos_ = 0;
-  double sqdiff_ = 0;
+  float sqdiff_ = 0;
-  double gps_lat_ = 0;
+  float gps_lat_ = 0;
-  double gps_lng_ = 0;
+  float gps_lng_ = 0;
 };
 
 class Config : public ModuleBase {
@@ -47,10 +47,10 @@ private:
   int writeTrackFromTemp();
   int deleteTrack(unsigned short idx);
   int resetConfig();
-  int writeVbatCal(double value);
+  int writeVbatCal(float value);
-  int writeVbatLow(double value);
+  int writeVbatLow(float value);
-  int writeTengLow(double value);
+  int writeTengLow(float value);
-  int writeTengHigh(double value);
+  int writeTengHigh(float value);
 
 public:
   int push(const Task &task) override;

src/modules/lcd/lcd.cpp

@@ -31,7 +31,7 @@ void Lcd::print(const char c[]) {
   display_cleared_ = false;
 }
 
-void Lcd::print(double d, int digits) {
+void Lcd::print(float d, int digits) {
   display_->print(d, digits);
   display_cleared_ = false;
 }
@@ -151,9 +151,9 @@ int Lcd::renderDriverPrimary() {
   GpsData gps_data;
   gpsGlobalRead(gps_data);
   
-  double vbat;
+  float vbat;
   vbatGlobalRead(vbat);
-  double teng;
+  float teng;
   tengGlobalRead(teng);
 
   display_->setCursor(0,0);

src/modules/lcd/lcd.h

@@ -53,7 +53,7 @@ private:
   void print(const String &msg);
   void print(char c);
   void print(const char c[]);
-  void print(double d, int digits = 2);
+  void print(float d, int digits = 2);
   void print(unsigned long l, int base = 10);
   void print(long l, int base = 10);
   void print(unsigned int i, int base = 10);

src/modules/telemetry/telemetry.cpp

@@ -0,0 +1,47 @@
+// Copyright (C) 2026 Hector van der Aa <hector@h3cx.dev>
+// Copyright (C) 2026 Association Exergie <association.exergie@gmail.com>
+// SPDX-License-Identifier: GPL-3.0-or-later
+#include "telemetry.h"
+#include "base/router.h"
+#include "data/general_store.h"
+
+int Telemetry::push(const Task &task) { return queue_.push(task); }
+
+Telemetry::Telemetry(HardwareSerial* data_stream) : logger_(nullptr), data_stream_(data_stream) {}
+
+Telemetry::Telemetry(HardwareSerial* data_stream,SystemLogger *logger) : logger_(logger), data_stream_(data_stream) {}
+
+Telemetry::~Telemetry() {}
+
+int Telemetry::init() {
+    data_stream_->begin(115200);
+    lora_header_.source_ = 0x01;
+    lora_header_.dest_ = 0x02;
+    return 0;
+}
+
+int Telemetry::loop() {
+    unsigned long now = millis();
+    if (now > last_sent_ + update_interval_) {
+        TelemetryPacket2 packet;
+        tengGlobalRead(packet.teng);
+        vbatGlobalRead(packet.vbat);
+        GpsData gps;
+        gpsGlobalRead(gps);
+        packet.lat = gps.lat_.value_;
+        packet.lng = gps.lng_.value_;
+        packet.speed = gps.speed_.value_;
+        lora_header_.size_ = sizeof(packet);
+        lora_header_.crc16_ = crc16_ccitt((uint8_t*)&packet, sizeof(packet));
+        lora_header_.version_ = 2;
+        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));
+        }
+        last_sent_ = millis();
+    }
+    return 0;
+}

src/modules/telemetry/telemetry.h

@@ -0,0 +1,30 @@
+// Copyright (C) 2026 Hector van der Aa <hector@h3cx.dev>
+// Copyright (C) 2026 Association Exergie <association.exergie@gmail.com>
+// SPDX-License-Identifier: GPL-3.0-or-later
+#pragma once
+#include "base/module_base.h"
+#include "base/ring_buffer.h"
+#include "modules/logger/system_logger.h"
+#include "telemetry_common/telemetry_common.h"
+#include <Arduino.h>
+#include "custom_types.h"
+#include "data/gps_store.h"
+
+class Telemetry : public ModuleBase {
+private:
+  SystemLogger *logger_;
+  RingBuffer<Task, 16> queue_;
+  HardwareSerial *data_stream_;
+  unsigned long last_sent_ = 0;
+  unsigned long update_interval_ = 1000;
+  TelemetryLoRaHeader lora_header_;
+  TelemetryUARTHeader uart_header_;
+
+public:
+  int push(const Task &task) override;
+  Telemetry(HardwareSerial *data_stream);
+  Telemetry(HardwareSerial *data_stream, SystemLogger *logger);
+  ~Telemetry();
+  int init();
+  int loop();
+};

src/modules/thermocouple/thermocouple.h

@@ -17,9 +17,9 @@ private:
   SystemLogger *logger_;
   RingBuffer<Task, 16> queue_;
   MAX6675 *thermocouple_;
-  double temperature_;
+  float temperature_;
-  double low_;
+  float low_;
-  double high_;
+  float high_;
   unsigned long update_interval_ = 1000;
   unsigned long last_read_at_ = 0;
   unsigned long warning_sent_at_ = 0;

src/telemetry_common

@@ -0,0 +1 @@
+/home/hector/projects/Exergie/TelemetryCommon/