From efd7119b32c07df8b4ab5159884cd65a262f1b85 Mon Sep 17 00:00:00 2001 From: Hector van der Aa Date: Mon, 30 Mar 2026 22:44:14 +0200 Subject: [PATCH] Initial telemetry implementation --- src/base/task.h | 1 + src/custom_types.h | 14 ++++---- src/data/general_store.cpp | 12 +++---- src/data/general_store.h | 12 +++---- src/main.cpp | 28 ++++++++++----- src/modules/battery/battery.cpp | 6 ++-- src/modules/battery/battery.h | 6 ++-- src/modules/cmd/cmd.cpp | 10 +++--- src/modules/config/config.cpp | 30 ++++++++-------- src/modules/config/config.h | 16 ++++----- src/modules/lcd/lcd.cpp | 6 ++-- src/modules/lcd/lcd.h | 2 +- src/modules/telemetry/telemetry.cpp | 47 +++++++++++++++++++++++++ src/modules/telemetry/telemetry.h | 30 ++++++++++++++++ src/modules/thermocouple/thermocouple.h | 6 ++-- src/telemetry_common | 1 + 16 files changed, 158 insertions(+), 69 deletions(-) create mode 100644 src/modules/telemetry/telemetry.cpp create mode 100644 src/modules/telemetry/telemetry.h create mode 120000 src/telemetry_common diff --git a/src/base/task.h b/src/base/task.h index 4b430d9..942ea35 100644 --- a/src/base/task.h +++ b/src/base/task.h @@ -14,6 +14,7 @@ enum Id : uint8_t { Lcd, Battery, Thermocouple, + Telemetry, Count, Null, All, diff --git a/src/custom_types.h b/src/custom_types.h index 9ba2640..d9d9460 100644 --- a/src/custom_types.h +++ b/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; - double vbat_low_ = 0; - double teng_low_ = 0; - double teng_high_ = 0; + float vbat_calibration_ = 0; + float vbat_low_ = 0; + float teng_low_ = 0; + float teng_high_ = 0; }; struct LatLng { - double lat_; - double lng_; + float lat_; + float lng_; }; struct TrackData { @@ -34,7 +34,7 @@ struct TrackData { struct GpsSubData { uint32_t age_; bool valid_; - double value_; + float value_; }; struct GpsData { diff --git a/src/data/general_store.cpp b/src/data/general_store.cpp index 75bd20c..584fb4b 100644 --- a/src/data/general_store.cpp +++ b/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 double teng_global = 0; + volatile float vbat_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; } diff --git a/src/data/general_store.h b/src/data/general_store.h index a6babf8..4f3abf3 100644 --- a/src/data/general_store.h +++ b/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 double teng_global; +extern volatile float vbat_global; +extern volatile float teng_global; -void vbatGlobalRead(double& out); -void vbatGlobalWrite(const double& in); +void vbatGlobalRead(float& out); +void vbatGlobalWrite(const float& in); -void tengGlobalRead(double& out); -void tengGlobalWrite(const double& in); +void tengGlobalRead(float& out); +void tengGlobalWrite(const float& in); diff --git a/src/main.cpp b/src/main.cpp index d7db0d1..38efadd 100644 --- a/src/main.cpp +++ b/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(); } diff --git a/src/modules/battery/battery.cpp b/src/modules/battery/battery.cpp index 23ec473..194feff 100644 --- a/src/modules/battery/battery.cpp +++ b/src/modules/battery/battery.cpp @@ -8,10 +8,10 @@ #include "data/general_store.h" int Battery::calibrate(const Task &task) { - double actual_voltage; - memcpy(&actual_voltage, &task.data_, sizeof(double)); + float actual_voltage; + 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); diff --git a/src/modules/battery/battery.h b/src/modules/battery/battery.h index 3933278..83fcd39 100644 --- a/src/modules/battery/battery.h +++ b/src/modules/battery/battery.h @@ -14,9 +14,9 @@ class Battery : public ModuleBase { private: SystemLogger *logger_; RingBuffer queue_; - double vbat_ = 0; - double calibration_ = 0; - double low_threshold_ = 0; + float vbat_ = 0; + float calibration_ = 0; + float low_threshold_ = 0; unsigned long warning_sent_at_ = 0; unsigned long warning_timeout_ = 10000; unsigned long update_interval_ = 1000; diff --git a/src/modules/cmd/cmd.cpp b/src/modules/cmd/cmd.cpp index bd06f84..8fb8030 100644 --- a/src/modules/cmd/cmd.cpp +++ b/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 diff --git a/src/modules/config/config.cpp b/src/modules/config/config.cpp index 5bbfd65..aba8733 100644 --- a/src/modules/config/config.cpp +++ b/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_; - double 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 delta_lat = track_data.point_a_.lat_ - task_data.gps_lat_; + float delta_lng = (track_data.point_a_.lng_ - task_data.gps_lng_) * task_data.cos_; + 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; - memcpy(&cal_value, &active_task_.data_, sizeof(double)); + float cal_value; + memcpy(&cal_value, &active_task_.data_, sizeof(float)); int result = this->writeVbatCal(cal_value); this->taskComplete(); return result; } case task::ConfigVbatSetLow: { - double low_value; - memcpy(&low_value, &active_task_.data_, sizeof(double)); + float low_value; + memcpy(&low_value, &active_task_.data_, sizeof(float)); int result = this->writeVbatLow(low_value); this->taskComplete(); return result; } case task::ConfigTengSetLow: { - double low_value; - memcpy(&low_value, &active_task_.data_, sizeof(double)); + float low_value; + memcpy(&low_value, &active_task_.data_, sizeof(float)); int result = this->writeTengLow(low_value); this->taskComplete(); return result; } case task::ConfigTengSetHigh: { - double high_value; - memcpy(&high_value, &active_task_.data_, sizeof(double)); + float high_value; + memcpy(&high_value, &active_task_.data_, sizeof(float)); int result = this->writeTengHigh(high_value); this->taskComplete(); return result; diff --git a/src/modules/config/config.h b/src/modules/config/config.h index e7c8948..2d94b30 100644 --- a/src/modules/config/config.h +++ b/src/modules/config/config.h @@ -18,10 +18,10 @@ struct TaskConfigTrackDetectData { unsigned short last_checked_ = 0; unsigned short smallest_idx_ = 0; - double cos_ = 0; - double sqdiff_ = 0; - double gps_lat_ = 0; - double gps_lng_ = 0; + float cos_ = 0; + float sqdiff_ = 0; + float gps_lat_ = 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 writeVbatLow(double value); - int writeTengLow(double value); - int writeTengHigh(double value); + int writeVbatCal(float value); + int writeVbatLow(float value); + int writeTengLow(float value); + int writeTengHigh(float value); public: int push(const Task &task) override; diff --git a/src/modules/lcd/lcd.cpp b/src/modules/lcd/lcd.cpp index 2c7f29d..9c9a8a6 100644 --- a/src/modules/lcd/lcd.cpp +++ b/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); diff --git a/src/modules/lcd/lcd.h b/src/modules/lcd/lcd.h index 596af3b..b4c5414 100644 --- a/src/modules/lcd/lcd.h +++ b/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); diff --git a/src/modules/telemetry/telemetry.cpp b/src/modules/telemetry/telemetry.cpp new file mode 100644 index 0000000..835f092 --- /dev/null +++ b/src/modules/telemetry/telemetry.cpp @@ -0,0 +1,47 @@ +// Copyright (C) 2026 Hector van der Aa +// Copyright (C) 2026 Association Exergie +// 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; +} \ No newline at end of file diff --git a/src/modules/telemetry/telemetry.h b/src/modules/telemetry/telemetry.h new file mode 100644 index 0000000..0daf574 --- /dev/null +++ b/src/modules/telemetry/telemetry.h @@ -0,0 +1,30 @@ +// Copyright (C) 2026 Hector van der Aa +// Copyright (C) 2026 Association Exergie +// 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 +#include "custom_types.h" +#include "data/gps_store.h" + +class Telemetry : public ModuleBase { +private: + SystemLogger *logger_; + RingBuffer 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(); +}; \ No newline at end of file diff --git a/src/modules/thermocouple/thermocouple.h b/src/modules/thermocouple/thermocouple.h index 226e07a..5d85aa1 100644 --- a/src/modules/thermocouple/thermocouple.h +++ b/src/modules/thermocouple/thermocouple.h @@ -17,9 +17,9 @@ private: SystemLogger *logger_; RingBuffer queue_; MAX6675 *thermocouple_; - double temperature_; - double low_; - double high_; + float temperature_; + float low_; + float high_; unsigned long update_interval_ = 1000; unsigned long last_read_at_ = 0; unsigned long warning_sent_at_ = 0; diff --git a/src/telemetry_common b/src/telemetry_common new file mode 120000 index 0000000..66ae1b3 --- /dev/null +++ b/src/telemetry_common @@ -0,0 +1 @@ +/home/hector/projects/Exergie/TelemetryCommon/ \ No newline at end of file