From 23d1e0728a9dc01e94d00b8cf366aa393b4743ff Mon Sep 17 00:00:00 2001 From: Elan456 Date: Wed, 11 Mar 2026 14:16:22 -0400 Subject: [PATCH 1/7] feat: add command move activitation for telemetry --- include/UARTCommandHandler.h | 3 ++- include/data_handling/Telemetry.h | 24 +++++++++++++++++++ src/UARTCommandHandler.cpp | 5 ++++ src/data_handling/Telemetry.cpp | 39 +++++++++++++++++++++++++++++++ 4 files changed, 70 insertions(+), 1 deletion(-) diff --git a/include/UARTCommandHandler.h b/include/UARTCommandHandler.h index 02610fb..e807371 100644 --- a/include/UARTCommandHandler.h +++ b/include/UARTCommandHandler.h @@ -36,6 +36,7 @@ class CommandLine { void readInput(); void processCommand(const std::string& command); void begin(); + void setUART(Stream* newUART); // Pass-through functions for the UART object void println(const std::string& message){ @@ -68,4 +69,4 @@ class CommandLine { bool lastWasCR_ = false; // Track if the last character was a carriage return for proper newline handling }; -#endif \ No newline at end of file +#endif diff --git a/include/data_handling/Telemetry.h b/include/data_handling/Telemetry.h index d6307f9..9ba212a 100644 --- a/include/data_handling/Telemetry.h +++ b/include/data_handling/Telemetry.h @@ -55,6 +55,9 @@ constexpr std::uint8_t kEndByteValue = 52; constexpr std::size_t kBytesIn32Bit = 4; constexpr unsigned kBitsPerByte = 8; constexpr std::uint8_t kAllOnesByte = 0xFF; +constexpr std::uint32_t kCommandModeInactivityTimeoutMs = 10000; +constexpr std::size_t kCommandEntrySequenceLength = 3; +constexpr char kCommandEntryChar = 'c'; /** Assumptions used by float packing. */ static_assert(sizeof(std::uint32_t) == 4, "Expected 32-bit uint32_t"); @@ -211,6 +214,20 @@ class Telemetry { */ bool tick(std::uint32_t currentTimeMs); + /** + * @brief True if telemetry is currently paused for radio command mode. + */ + bool isInCommandMode() const { return inCommandMode; } + + /** + * @brief Refresh command mode inactivity timer after external input handling. + */ + void noteCommandInput(std::uint32_t currentTimeMs) { + if (inCommandMode) { + commandModeLastInputTimestamp = currentTimeMs; + } + } + private: // Packet building helpers void preparePacket(std::uint32_t timestamp); @@ -218,6 +235,7 @@ class Telemetry { void addSSDToPacket(SendableSensorData* ssd); void setPacketToZero(); void addEndMarker(); + void checkForRadioCommandSequence(std::uint32_t currentTimeMs); // Non-owning view of the stream list SendableSensorData* const* streams; @@ -234,6 +252,12 @@ class Telemetry { std::uint32_t packetCounter = 0; std::size_t nextEmptyPacketIndex; std::array packet; + + // Command mode handling + bool inCommandMode = false; + std::uint32_t commandModeEnteredTimestamp = 0; + std::uint32_t commandModeLastInputTimestamp = 0; + std::size_t commandEntryProgress = 0; }; #endif diff --git a/src/UARTCommandHandler.cpp b/src/UARTCommandHandler.cpp index 9ad7f8f..d341f63 100644 --- a/src/UARTCommandHandler.cpp +++ b/src/UARTCommandHandler.cpp @@ -8,6 +8,11 @@ constexpr int COMMAND_CHARS_ASCII_END = 31; // ASCII control characters end at 3 CommandLine::CommandLine(Stream * UART) : UART(UART) { } +void CommandLine::setUART(Stream* newUART) { + UART = newUART; + UART->print(SHELL_PROMPT); +} + void CommandLine::begin() { help(); UART->print(SHELL_PROMPT); diff --git a/src/data_handling/Telemetry.cpp b/src/data_handling/Telemetry.cpp index 47505c6..d63750d 100644 --- a/src/data_handling/Telemetry.cpp +++ b/src/data_handling/Telemetry.cpp @@ -19,6 +19,28 @@ bool hasRoom(std::size_t nextIndex, std::size_t bytesToAdd) { return nextIndex + bytesToAdd <= TelemetryFmt::kPacketCapacity; } +void Telemetry::checkForRadioCommandSequence(std::uint32_t currentTimeMs) { + if (inCommandMode) { + return; + } + + while (rfdSerialConnection.available() > 0) { + const char receivedChar = static_cast(rfdSerialConnection.read()); + + if (receivedChar == TelemetryFmt::kCommandEntryChar) { + ++commandEntryProgress; + if (commandEntryProgress >= TelemetryFmt::kCommandEntrySequenceLength) { + inCommandMode = true; + commandModeEnteredTimestamp = currentTimeMs; + commandModeLastInputTimestamp = currentTimeMs; + commandEntryProgress = 0; + } + } else { + commandEntryProgress = 0; + } + } +} + void Telemetry::preparePacket(std::uint32_t timestamp) { // This write the header of the packet with sync bytes, start byte, and timestamp. // Only clear what we own in the header (whole-packet clearing happens in setPacketToZero()). @@ -78,6 +100,23 @@ void Telemetry::addEndMarker() { } bool Telemetry::tick(uint32_t currentTime) { + // Checks if we should put the telemetry into command mode + checkForRadioCommandSequence(currentTime); + + // Checks if we should exit command mode due to inactivity. + if (inCommandMode) { + if ((currentTime - commandModeLastInputTimestamp) >= TelemetryFmt::kCommandModeInactivityTimeoutMs) { + inCommandMode = false; + // Exited command mode, so continue with sending telemetry as normal below. + } else { + // staying in command mode, so don't send any telemetry data this tick + // to keep the radio free for command responses. + return false; + } + } + + // All the logic below is for building and sending telemetry packets + bool sendingPacketThisTick = false; for (std::size_t i = 0; i < streamCount; i++) { From 24b0051b6781da75135dda3805445f3648e32a68 Mon Sep 17 00:00:00 2001 From: Elan456 Date: Wed, 11 Mar 2026 17:07:36 -0400 Subject: [PATCH 2/7] chore: remove extra AS shell print --- src/UARTCommandHandler.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/UARTCommandHandler.cpp b/src/UARTCommandHandler.cpp index d341f63..838b360 100644 --- a/src/UARTCommandHandler.cpp +++ b/src/UARTCommandHandler.cpp @@ -10,7 +10,6 @@ CommandLine::CommandLine(Stream * UART) : UART(UART) { void CommandLine::setUART(Stream* newUART) { UART = newUART; - UART->print(SHELL_PROMPT); } void CommandLine::begin() { From 88c4b0ff0d2346bcf18cc8e104b5042b197e7fde Mon Sep 17 00:00:00 2001 From: Elan456 Date: Thu, 12 Mar 2026 15:56:59 -0400 Subject: [PATCH 3/7] ref: offload cmdLine UART switching to telemetry --- include/UARTCommandHandler.h | 8 ++++++- include/data_handling/Telemetry.h | 17 +++++++------ src/UARTCommandHandler.cpp | 18 ++++++++++++-- src/data_handling/Telemetry.cpp | 40 +++++++++++++++++++++++++++---- 4 files changed, 68 insertions(+), 15 deletions(-) diff --git a/include/UARTCommandHandler.h b/include/UARTCommandHandler.h index e807371..2ee0a12 100644 --- a/include/UARTCommandHandler.h +++ b/include/UARTCommandHandler.h @@ -36,7 +36,11 @@ class CommandLine { void readInput(); void processCommand(const std::string& command); void begin(); - void setUART(Stream* newUART); + void switchUART(Stream* newUART); + void useDefaultUART(); + Stream* getDefaultUART() const { return defaultUART; } + Stream* getActiveUART() const { return UART; } + uint32_t getLastInteractionTimestamp() const { return lastInteractionTimestamp_; } // Pass-through functions for the UART object void println(const std::string& message){ @@ -48,6 +52,7 @@ class CommandLine { private: Stream * UART; // Pointer to the UART object + Stream * defaultUART; // Stream provided at construction; used by useDefaultUART() struct Command { std::string longName; std::string shortName; @@ -67,6 +72,7 @@ class CommandLine { void handleChar_(char receivedChar); bool lastWasCR_ = false; // Track if the last character was a carriage return for proper newline handling + uint32_t lastInteractionTimestamp_ = 0; // millis() when input bytes were last consumed }; #endif diff --git a/include/data_handling/Telemetry.h b/include/data_handling/Telemetry.h index 9ba212a..5eddd65 100644 --- a/include/data_handling/Telemetry.h +++ b/include/data_handling/Telemetry.h @@ -9,6 +9,8 @@ #include "ArduinoHAL.h" #include "data_handling/SensorDataHandler.h" +class CommandLine; + /** * @file Telemetry.h * @brief Packs SensorDataHandler values into a fixed-size byte packet and streams over a Stream (UART). @@ -201,10 +203,12 @@ class Telemetry { */ template Telemetry(const std::array& streams, - Stream& rfdSerialConnection) + Stream& rfdSerialConnection, + CommandLine* commandLine = nullptr) : streams(streams.data()), streamCount(N), rfdSerialConnection(rfdSerialConnection), + commandLine(commandLine), nextEmptyPacketIndex(0), packet{} {} /** @@ -220,13 +224,9 @@ class Telemetry { bool isInCommandMode() const { return inCommandMode; } /** - * @brief Refresh command mode inactivity timer after external input handling. + * @brief Optional command line interface to drive while telemetry manages command mode. */ - void noteCommandInput(std::uint32_t currentTimeMs) { - if (inCommandMode) { - commandModeLastInputTimestamp = currentTimeMs; - } - } + void setCommandLine(CommandLine* newCommandLine) { commandLine = newCommandLine; } private: // Packet building helpers @@ -236,6 +236,8 @@ class Telemetry { void setPacketToZero(); void addEndMarker(); void checkForRadioCommandSequence(std::uint32_t currentTimeMs); + void enterCommandMode(std::uint32_t currentTimeMs); + void exitCommandMode(); // Non-owning view of the stream list SendableSensorData* const* streams; @@ -247,6 +249,7 @@ class Telemetry { // Output Stream& rfdSerialConnection; + CommandLine* commandLine; // Packet state std::uint32_t packetCounter = 0; diff --git a/src/UARTCommandHandler.cpp b/src/UARTCommandHandler.cpp index 838b360..ac24e9c 100644 --- a/src/UARTCommandHandler.cpp +++ b/src/UARTCommandHandler.cpp @@ -5,13 +5,20 @@ constexpr int COMMAND_CHARS_ASCII_END = 31; // ASCII control characters end at 31, so we can ignore those in input -CommandLine::CommandLine(Stream * UART) : UART(UART) { +CommandLine::CommandLine(Stream * UART) : UART(UART), defaultUART(UART) { } -void CommandLine::setUART(Stream* newUART) { +void CommandLine::switchUART(Stream* newUART) { + if (newUART == nullptr) { + return; + } UART = newUART; } +void CommandLine::useDefaultUART() { + UART = defaultUART; +} + void CommandLine::begin() { help(); UART->print(SHELL_PROMPT); @@ -55,7 +62,10 @@ void tokenizeWhitespace(const std::string& line, } // namespace void CommandLine::readInput() { // NOLINT(readability-function-cognitive-complexity) + bool consumedInputThisCall = false; + while (UART->available() > 0) { + consumedInputThisCall = true; const char receivedChar = static_cast(UART->read()); if (isBackspace_(receivedChar)) { @@ -73,6 +83,10 @@ void CommandLine::readInput() { // NOLINT(readability-function-cognitive-complex handleChar_(receivedChar); } } + + if (consumedInputThisCall) { + lastInteractionTimestamp_ = millis(); + } } void CommandLine::handleBackspace_() { diff --git a/src/data_handling/Telemetry.cpp b/src/data_handling/Telemetry.cpp index d63750d..799ea01 100644 --- a/src/data_handling/Telemetry.cpp +++ b/src/data_handling/Telemetry.cpp @@ -1,6 +1,8 @@ #include "data_handling/Telemetry.h" #include "ArduinoHAL.h" +#include "UARTCommandHandler.h" #include +#include // Helpers for checking if the packet has room for more data std::size_t bytesNeededForSSD(const SendableSensorData* ssd) { @@ -19,6 +21,10 @@ bool hasRoom(std::size_t nextIndex, std::size_t bytesToAdd) { return nextIndex + bytesToAdd <= TelemetryFmt::kPacketCapacity; } +bool isTimestampNewer(std::uint32_t lhs, std::uint32_t rhs) { + return static_cast(lhs - rhs) > 0; +} + void Telemetry::checkForRadioCommandSequence(std::uint32_t currentTimeMs) { if (inCommandMode) { return; @@ -30,10 +36,7 @@ void Telemetry::checkForRadioCommandSequence(std::uint32_t currentTimeMs) { if (receivedChar == TelemetryFmt::kCommandEntryChar) { ++commandEntryProgress; if (commandEntryProgress >= TelemetryFmt::kCommandEntrySequenceLength) { - inCommandMode = true; - commandModeEnteredTimestamp = currentTimeMs; - commandModeLastInputTimestamp = currentTimeMs; - commandEntryProgress = 0; + enterCommandMode(currentTimeMs); } } else { commandEntryProgress = 0; @@ -41,6 +44,26 @@ void Telemetry::checkForRadioCommandSequence(std::uint32_t currentTimeMs) { } } +void Telemetry::enterCommandMode(std::uint32_t currentTimeMs) { + inCommandMode = true; + commandModeEnteredTimestamp = currentTimeMs; + commandModeLastInputTimestamp = currentTimeMs; + commandEntryProgress = 0; + + if (commandLine != nullptr) { + commandLine->switchUART(&rfdSerialConnection); + commandLine->print(SHELL_PROMPT); + } +} + +void Telemetry::exitCommandMode() { + inCommandMode = false; + + if (commandLine != nullptr) { + commandLine->useDefaultUART(); + } +} + void Telemetry::preparePacket(std::uint32_t timestamp) { // This write the header of the packet with sync bytes, start byte, and timestamp. // Only clear what we own in the header (whole-packet clearing happens in setPacketToZero()). @@ -105,8 +128,15 @@ bool Telemetry::tick(uint32_t currentTime) { // Checks if we should exit command mode due to inactivity. if (inCommandMode) { + if (commandLine != nullptr) { + const std::uint32_t lastInteractionTimestamp = commandLine->getLastInteractionTimestamp(); + if (isTimestampNewer(lastInteractionTimestamp, commandModeLastInputTimestamp)) { + commandModeLastInputTimestamp = lastInteractionTimestamp; + } + } + if ((currentTime - commandModeLastInputTimestamp) >= TelemetryFmt::kCommandModeInactivityTimeoutMs) { - inCommandMode = false; + exitCommandMode(); // Exited command mode, so continue with sending telemetry as normal below. } else { // staying in command mode, so don't send any telemetry data this tick From 0af85e16ff9979fd07c534ac0ac2dc2d4c3a873c Mon Sep 17 00:00:00 2001 From: Elan456 Date: Thu, 12 Mar 2026 16:04:02 -0400 Subject: [PATCH 4/7] ref: redistrbute constant config defs amongst sim sensors --- include/simulation/Serial_Sim.h | 16 ------- include/simulation/Serial_Sim_BMP390.h | 56 +++++++++++++++++++++--- include/simulation/Serial_Sim_LIS2MDL.h | 11 ++++- include/simulation/Serial_Sim_LIS3MDL.h | 43 +++++++++++++----- include/simulation/Serial_Sim_LSM6DSOX.h | 33 ++++++++++---- 5 files changed, 116 insertions(+), 43 deletions(-) diff --git a/include/simulation/Serial_Sim.h b/include/simulation/Serial_Sim.h index efad5b5..83fe977 100644 --- a/include/simulation/Serial_Sim.h +++ b/include/simulation/Serial_Sim.h @@ -6,22 +6,6 @@ #include "state_estimation/BurnoutStateMachine.h" #include "state_estimation/StateMachine.h" -#define LSM6DS_ACCEL_RANGE_16_G 0x03 -#define LSM6DS_GYRO_RANGE_2000_DPS 0x03 -#define LSM6DS_RATE_104_HZ 0x04 -// LIS3MDL (Magnetometer 1.3) Define -#define LIS3MDL_DATARATE_155_HZ 0x06 -#define LIS3MDL_RANGE_4_GAUSS 0x01 -#define LIS3MDL_CONTINUOUSMODE 0x00 -#define LIS3MDL_MEDIUMMODE 0x01 -// LIS2MDL (Magnetometer 1.4) Define -#define LIS2MDL_RATE_100_HZ 0x06 -// BMP3 (Barometric Pressure Sensor) Define -#define BMP3_OVERSAMPLING_8X 0x03 -#define BMP3_OVERSAMPLING_4X 0x02 -#define BMP3_IIR_FILTER_COEFF_3 0x03 -#define BMP3_ODR_100_HZ 0x05 - /** * @brief Serial-based sensor/flight simulation singleton for hardware-in-the-loop. * @note When to use: feed prerecorded or live PC-side simulation data into diff --git a/include/simulation/Serial_Sim_BMP390.h b/include/simulation/Serial_Sim_BMP390.h index d24f7d8..968dd26 100644 --- a/include/simulation/Serial_Sim_BMP390.h +++ b/include/simulation/Serial_Sim_BMP390.h @@ -1,8 +1,44 @@ -#ifndef SERIAL_SIM_BMP3_H -#define SERIAL_SIM_BMP3_H +#ifndef SERIAL_SIM_BMP390_H +#define SERIAL_SIM_BMP390_H #include "Serial_Sim.h" +#ifndef BMP3_NO_OVERSAMPLING +#define BMP3_NO_OVERSAMPLING 0x00 +#endif + +#ifndef BMP3_OVERSAMPLING_2X +#define BMP3_OVERSAMPLING_2X 0x01 +#endif + +#ifndef BMP3_OVERSAMPLING_4X +#define BMP3_OVERSAMPLING_4X 0x02 +#endif + +#ifndef BMP3_OVERSAMPLING_8X +#define BMP3_OVERSAMPLING_8X 0x03 +#endif + +#ifndef BMP3_OVERSAMPLING_16X +#define BMP3_OVERSAMPLING_16X 0x04 +#endif + +#ifndef BMP3_OVERSAMPLING_32X +#define BMP3_OVERSAMPLING_32X 0x05 +#endif + +#ifndef BMP3_IIR_FILTER_COEFF_3 +#define BMP3_IIR_FILTER_COEFF_3 0x02 +#endif + +#ifndef BMP3_ODR_100_HZ +#define BMP3_ODR_100_HZ 0x01 +#endif + +#ifndef BMP3_ODR_50_HZ +#define BMP3_ODR_50_HZ 0x02 +#endif + /** * @brief Mock BMP3XX sensor backed by SerialSim data. * @note When to use: compile flight code on a host without real hardware while @@ -16,10 +52,10 @@ class Adafruit_BMP3XX { bool begin_I2C(int addr) { return true; } // Mock successful initialization bool begin_I2C() { return true; } // Mock successful initialization - void setTemperatureOversampling(int oversampling) {} - void setPressureOversampling(int oversampling) {} - void setIIRFilterCoeff(int coeff) {} - void setOutputDataRate(int rate) {} + void setTemperatureOversampling(int oversampling) { temperatureOversampling = oversampling; } + void setPressureOversampling(int oversampling) { pressureOversampling = oversampling; } + void setIIRFilterCoeff(int coeff) { iirFilterCoeff = coeff; } + void setOutputDataRate(int rate) { outputDataRate = rate; } void setConversionDelay(int delay) {} void startConversion() {} bool updateConversion(){return true;} @@ -70,6 +106,12 @@ class Adafruit_BMP3XX { } +private: + int temperatureOversampling = BMP3_NO_OVERSAMPLING; + int pressureOversampling = BMP3_OVERSAMPLING_2X; + int iirFilterCoeff = BMP3_IIR_FILTER_COEFF_3; + int outputDataRate = BMP3_ODR_100_HZ; + }; -#endif // SERIAL_SIM_BMP3_H +#endif // SERIAL_SIM_BMP390_H diff --git a/include/simulation/Serial_Sim_LIS2MDL.h b/include/simulation/Serial_Sim_LIS2MDL.h index 0779836..5dc87cc 100644 --- a/include/simulation/Serial_Sim_LIS2MDL.h +++ b/include/simulation/Serial_Sim_LIS2MDL.h @@ -3,6 +3,10 @@ #include "Serial_Sim.h" +#ifndef LIS2MDL_RATE_100_HZ +#define LIS2MDL_RATE_100_HZ 0x06 +#endif + class Adafruit_LIS2MDL { public: Adafruit_LIS2MDL (){} @@ -10,16 +14,19 @@ class Adafruit_LIS2MDL { bool begin_SPI(int cs) { return true; } // Mock successful initialization SPI bool begin(int addr) { return true; } // Mock successful initialization I2C - void setDataRate(int rate) {} + void setDataRate(int rate) { dataRate = rate; } void enableInterrupts(bool a) {} - int getDataRate() { return 100; } // Mock as 155 Hz + int getDataRate() { return dataRate; } void getEvent(sensors_event_t *mag) { SerialSim::getInstance().updateMag(mag); } +private: + int dataRate = LIS2MDL_RATE_100_HZ; + }; #endif // SERIAL_SIM_LIS2MDL_H diff --git a/include/simulation/Serial_Sim_LIS3MDL.h b/include/simulation/Serial_Sim_LIS3MDL.h index 663f5ab..6c1da13 100644 --- a/include/simulation/Serial_Sim_LIS3MDL.h +++ b/include/simulation/Serial_Sim_LIS3MDL.h @@ -3,6 +3,22 @@ #include "Serial_Sim.h" +#ifndef LIS3MDL_DATARATE_155_HZ +#define LIS3MDL_DATARATE_155_HZ 0x06 +#endif + +#ifndef LIS3MDL_RANGE_4_GAUSS +#define LIS3MDL_RANGE_4_GAUSS 0x01 +#endif + +#ifndef LIS3MDL_CONTINUOUSMODE +#define LIS3MDL_CONTINUOUSMODE 0x00 +#endif + +#ifndef LIS3MDL_MEDIUMMODE +#define LIS3MDL_MEDIUMMODE 0x01 +#endif + /** * @brief Mock LIS3MDL magnetometer sourcing readings from SerialSim. * @note When to use: magnetometer-dependent code paths during desktop or HIL @@ -16,27 +32,34 @@ class Adafruit_LIS3MDL { bool begin_I2C(int addr) { return true; } // Mock successful initialization bool begin_I2C() { return true; } // Mock successful initialization - void setDataRate(int rate) {} - void setRange(int range) {} - void setOperationMode(int mode) {} - void setPerformanceMode(int mode) {} - void setIntThreshold(int threshold) {} + void setDataRate(int rate) { dataRate = rate; } + void setRange(int range) { this->range = range; } + void setOperationMode(int mode) { operationMode = mode; } + void setPerformanceMode(int mode) { performanceMode = mode; } + void setIntThreshold(int threshold) { intThreshold = static_cast(threshold); } void configInterrupt(bool a, bool b, bool c) {} void configInterrupt(bool a, bool b, bool c, bool d, bool e, bool f) {} - int getPerformanceMode() { return 0; } // Mock as 0 - int getOperationMode() { return 0; } // Mock as 0 - int getRange() { return 4; } // Mock as 4 Gauss - uint16_t getIntThreshold() { return 0; } // Mock as 0 + int getPerformanceMode() { return performanceMode; } + int getOperationMode() { return operationMode; } + int getRange() { return range; } + uint16_t getIntThreshold() { return intThreshold; } - int getDataRate() { return 155; } // Mock as 155 Hz + int getDataRate() { return dataRate; } void getEvent(sensors_event_t *mag) { SerialSim::getInstance().updateMag(mag); } +private: + int dataRate = LIS3MDL_DATARATE_155_HZ; + int range = LIS3MDL_RANGE_4_GAUSS; + int operationMode = LIS3MDL_CONTINUOUSMODE; + int performanceMode = LIS3MDL_MEDIUMMODE; + uint16_t intThreshold = 0; + }; #endif // SERIAL_SIM_LIS3MDL_H diff --git a/include/simulation/Serial_Sim_LSM6DSOX.h b/include/simulation/Serial_Sim_LSM6DSOX.h index d55548e..2311609 100644 --- a/include/simulation/Serial_Sim_LSM6DSOX.h +++ b/include/simulation/Serial_Sim_LSM6DSOX.h @@ -3,6 +3,18 @@ #include "Serial_Sim.h" +#ifndef LSM6DS_ACCEL_RANGE_16_G +#define LSM6DS_ACCEL_RANGE_16_G 0x03 +#endif + +#ifndef LSM6DS_GYRO_RANGE_2000_DPS +#define LSM6DS_GYRO_RANGE_2000_DPS 0x03 +#endif + +#ifndef LSM6DS_RATE_104_HZ +#define LSM6DS_RATE_104_HZ 0x04 +#endif + /** * @brief Mock LSM6DSOX IMU backed by SerialSim accelerometer/gyro data. * @note When to use: firmware simulation or CI builds where the real IMU is @@ -16,15 +28,15 @@ class Adafruit_LSM6DSOX { bool begin_I2C(int addr) { return true; } // Mock successful initialization bool begin_I2C() { return true; } // Mock successful initialization - void setAccelRange(int range) {} - void setGyroRange(int range) {} - void setAccelDataRate(int rate) {} - void setGyroDataRate(int rate) {} + void setAccelRange(int range) { accelRange = range; } + void setGyroRange(int range) { gyroRange = range; } + void setAccelDataRate(int rate) { accelDataRate = rate; } + void setGyroDataRate(int rate) { gyroDataRate = rate; } - int getAccelRange() { return 16; } // Mock as 16G - int getGyroRange() { return 2000; } // Mock as 2000 DPS - int getAccelDataRate() { return 104; } // Mock as 104 Hz - int getGyroDataRate() { return 104; } // Mock as 104 Hz + int getAccelRange() { return accelRange; } + int getGyroRange() { return gyroRange; } + int getAccelDataRate() { return accelDataRate; } + int getGyroDataRate() { return gyroDataRate; } void getEvent(sensors_event_t *accel, sensors_event_t *gyro, sensors_event_t *temp) { SerialSim::getInstance().updateAcl(accel); @@ -32,6 +44,11 @@ class Adafruit_LSM6DSOX { } +private: + int accelRange = LSM6DS_ACCEL_RANGE_16_G; + int gyroRange = LSM6DS_GYRO_RANGE_2000_DPS; + int accelDataRate = LSM6DS_RATE_104_HZ; + int gyroDataRate = LSM6DS_RATE_104_HZ; }; From 7c545d28d36b2bc4eb6e7b1dd88a77384de33772 Mon Sep 17 00:00:00 2001 From: Elan456 Date: Thu, 12 Mar 2026 16:10:25 -0400 Subject: [PATCH 5/7] ref: include UARTCommandHandler in Telemetry.h --- include/data_handling/Telemetry.h | 3 +-- src/data_handling/Telemetry.cpp | 1 - 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/include/data_handling/Telemetry.h b/include/data_handling/Telemetry.h index 5eddd65..b1c5d65 100644 --- a/include/data_handling/Telemetry.h +++ b/include/data_handling/Telemetry.h @@ -7,10 +7,9 @@ #include #include "ArduinoHAL.h" +#include "UARTCommandHandler.h" #include "data_handling/SensorDataHandler.h" -class CommandLine; - /** * @file Telemetry.h * @brief Packs SensorDataHandler values into a fixed-size byte packet and streams over a Stream (UART). diff --git a/src/data_handling/Telemetry.cpp b/src/data_handling/Telemetry.cpp index 799ea01..b314491 100644 --- a/src/data_handling/Telemetry.cpp +++ b/src/data_handling/Telemetry.cpp @@ -1,6 +1,5 @@ #include "data_handling/Telemetry.h" #include "ArduinoHAL.h" -#include "UARTCommandHandler.h" #include #include From 6941032861f0d9a3e575991878c1bb766b72eec9 Mon Sep 17 00:00:00 2001 From: Elan456 Date: Thu, 12 Mar 2026 16:22:45 -0400 Subject: [PATCH 6/7] ref: breakup telemetry.tick with helper functions --- include/data_handling/Telemetry.h | 4 + src/data_handling/Telemetry.cpp | 128 ++++++++++++++++-------------- 2 files changed, 73 insertions(+), 59 deletions(-) diff --git a/include/data_handling/Telemetry.h b/include/data_handling/Telemetry.h index b1c5d65..a7a84b3 100644 --- a/include/data_handling/Telemetry.h +++ b/include/data_handling/Telemetry.h @@ -237,6 +237,10 @@ class Telemetry { void checkForRadioCommandSequence(std::uint32_t currentTimeMs); void enterCommandMode(std::uint32_t currentTimeMs); void exitCommandMode(); + bool shouldPauseTelemetryForCommandMode(std::uint32_t currentTimeMs); + bool canFitStreamWithEndMarker(const SendableSensorData* ssd) const; + void tryAppendStream(SendableSensorData* stream, std::uint32_t currentTimeMs, bool& packetStarted, bool& payloadAdded); + bool finalizeAndSendPacket(); // Non-owning view of the stream list SendableSensorData* const* streams; diff --git a/src/data_handling/Telemetry.cpp b/src/data_handling/Telemetry.cpp index b314491..fb6c47a 100644 --- a/src/data_handling/Telemetry.cpp +++ b/src/data_handling/Telemetry.cpp @@ -63,6 +63,26 @@ void Telemetry::exitCommandMode() { } } +bool Telemetry::shouldPauseTelemetryForCommandMode(std::uint32_t currentTimeMs) { + if (!inCommandMode) { + return false; + } + + if (commandLine != nullptr) { + const std::uint32_t lastInteractionTimestamp = commandLine->getLastInteractionTimestamp(); + if (isTimestampNewer(lastInteractionTimestamp, commandModeLastInputTimestamp)) { + commandModeLastInputTimestamp = lastInteractionTimestamp; + } + } + + if ((currentTimeMs - commandModeLastInputTimestamp) >= TelemetryFmt::kCommandModeInactivityTimeoutMs) { + exitCommandMode(); + return false; + } + + return true; +} + void Telemetry::preparePacket(std::uint32_t timestamp) { // This write the header of the packet with sync bytes, start byte, and timestamp. // Only clear what we own in the header (whole-packet clearing happens in setPacketToZero()). @@ -121,78 +141,68 @@ void Telemetry::addEndMarker() { nextEmptyPacketIndex += TelemetryFmt::kEndMarkerBytes; } -bool Telemetry::tick(uint32_t currentTime) { - // Checks if we should put the telemetry into command mode - checkForRadioCommandSequence(currentTime); +bool Telemetry::canFitStreamWithEndMarker(const SendableSensorData* ssd) const { + const std::size_t payloadBytes = bytesNeededForSSD(ssd); + return hasRoom(nextEmptyPacketIndex, payloadBytes + TelemetryFmt::kEndMarkerBytes); +} - // Checks if we should exit command mode due to inactivity. - if (inCommandMode) { - if (commandLine != nullptr) { - const std::uint32_t lastInteractionTimestamp = commandLine->getLastInteractionTimestamp(); - if (isTimestampNewer(lastInteractionTimestamp, commandModeLastInputTimestamp)) { - commandModeLastInputTimestamp = lastInteractionTimestamp; - } - } +void Telemetry::tryAppendStream(SendableSensorData* stream, std::uint32_t currentTimeMs, bool& packetStarted, bool& payloadAdded) { + if (!stream->shouldBeSent(currentTimeMs)) { + return; + } - if ((currentTime - commandModeLastInputTimestamp) >= TelemetryFmt::kCommandModeInactivityTimeoutMs) { - exitCommandMode(); - // Exited command mode, so continue with sending telemetry as normal below. - } else { - // staying in command mode, so don't send any telemetry data this tick - // to keep the radio free for command responses. - return false; - } + if (!packetStarted) { + setPacketToZero(); + preparePacket(currentTimeMs); + packetStarted = true; } - // All the logic below is for building and sending telemetry packets + if (!canFitStreamWithEndMarker(stream)) { + return; + } - bool sendingPacketThisTick = false; + addSSDToPacket(stream); + stream->markWasSent(currentTimeMs); + payloadAdded = true; +} - for (std::size_t i = 0; i < streamCount; i++) { - // i is safe because streamCount comes from the array passed in by the client - if (streams[i]->shouldBeSent(currentTime)) { // NOLINT(cppcoreguidelines-pro-bounds-pointer-arithmetic) +bool Telemetry::finalizeAndSendPacket() { + if (nextEmptyPacketIndex <= TelemetryFmt::kHeaderBytes) { + return false; + } - if (!sendingPacketThisTick) { - setPacketToZero(); - preparePacket(currentTime); - sendingPacketThisTick = true; - } + if (!hasRoom(nextEmptyPacketIndex, TelemetryFmt::kEndMarkerBytes)) { + return false; + } - // Compute how many bytes we need for this stream's payload. - const std::size_t payloadBytes = bytesNeededForSSD(streams[i]); // NOLINT(cppcoreguidelines-pro-bounds-pointer-arithmetic) - const std::size_t totalBytesIfAdded = payloadBytes + TelemetryFmt::kEndMarkerBytes; - - // Only add if it fits (payload + end marker). - if (hasRoom(nextEmptyPacketIndex, totalBytesIfAdded)) { - addSSDToPacket(streams[i]); // NOLINT(cppcoreguidelines-pro-bounds-pointer-arithmetic) - streams[i]->markWasSent(currentTime); // NOLINT(cppcoreguidelines-pro-bounds-pointer-arithmetic) - } else { - // Not enough room. Skip this stream for now. - // It will be sent on the next tick as long as the packet isn't filled before reaching it again. - // If we have too many high-frequency streams, the stream as the end of the list may be starved. - } - } + addEndMarker(); + for (std::size_t i = 0; i < nextEmptyPacketIndex; i++) { + rfdSerialConnection.write(packet[i]); } - // Only send if we actually added any payload beyond the header. - if (sendingPacketThisTick && nextEmptyPacketIndex > TelemetryFmt::kHeaderBytes) { - // Ensure end marker itself fits - if (hasRoom(nextEmptyPacketIndex, TelemetryFmt::kEndMarkerBytes)) { - addEndMarker(); + packetCounter++; + return true; +} - // Send used portion - for (std::size_t i = 0; i < nextEmptyPacketIndex; i++) { - rfdSerialConnection.write(packet[i]); - } +bool Telemetry::tick(uint32_t currentTime) { + // Checks if we should put the telemetry into command mode + checkForRadioCommandSequence(currentTime); - packetCounter++; //Increment after each successful send - - return true; - } + if (shouldPauseTelemetryForCommandMode(currentTime)) { + return false; + } + + bool packetStarted = false; + bool payloadAdded = false; + + for (std::size_t i = 0; i < streamCount; i++) { + // i is safe because streamCount comes from the array passed in by the client + tryAppendStream(streams[i], currentTime, packetStarted, payloadAdded); // NOLINT(cppcoreguidelines-pro-bounds-pointer-arithmetic) + } - // If somehow we can't fit the end marker, drop the packet. - // (This shouldn't happen with the checks above.) + if (!payloadAdded) { + return false; } - return false; + return finalizeAndSendPacket(); } From d691d7a8c6971a0d6af01c2b74f9f4754be187c4 Mon Sep 17 00:00:00 2001 From: Elan456 Date: Thu, 12 Mar 2026 16:25:32 -0400 Subject: [PATCH 7/7] ref: remove packet initializtion logic from tryAppendStream --- include/data_handling/Telemetry.h | 2 +- src/data_handling/Telemetry.cpp | 14 +++++--------- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/include/data_handling/Telemetry.h b/include/data_handling/Telemetry.h index a7a84b3..f2cf994 100644 --- a/include/data_handling/Telemetry.h +++ b/include/data_handling/Telemetry.h @@ -239,7 +239,7 @@ class Telemetry { void exitCommandMode(); bool shouldPauseTelemetryForCommandMode(std::uint32_t currentTimeMs); bool canFitStreamWithEndMarker(const SendableSensorData* ssd) const; - void tryAppendStream(SendableSensorData* stream, std::uint32_t currentTimeMs, bool& packetStarted, bool& payloadAdded); + void tryAppendStream(SendableSensorData* stream, std::uint32_t currentTimeMs, bool& payloadAdded); bool finalizeAndSendPacket(); // Non-owning view of the stream list diff --git a/src/data_handling/Telemetry.cpp b/src/data_handling/Telemetry.cpp index fb6c47a..ebaab70 100644 --- a/src/data_handling/Telemetry.cpp +++ b/src/data_handling/Telemetry.cpp @@ -146,17 +146,11 @@ bool Telemetry::canFitStreamWithEndMarker(const SendableSensorData* ssd) const { return hasRoom(nextEmptyPacketIndex, payloadBytes + TelemetryFmt::kEndMarkerBytes); } -void Telemetry::tryAppendStream(SendableSensorData* stream, std::uint32_t currentTimeMs, bool& packetStarted, bool& payloadAdded) { +void Telemetry::tryAppendStream(SendableSensorData* stream, std::uint32_t currentTimeMs, bool& payloadAdded) { if (!stream->shouldBeSent(currentTimeMs)) { return; } - if (!packetStarted) { - setPacketToZero(); - preparePacket(currentTimeMs); - packetStarted = true; - } - if (!canFitStreamWithEndMarker(stream)) { return; } @@ -192,12 +186,14 @@ bool Telemetry::tick(uint32_t currentTime) { return false; } - bool packetStarted = false; + setPacketToZero(); + preparePacket(currentTime); + bool payloadAdded = false; for (std::size_t i = 0; i < streamCount; i++) { // i is safe because streamCount comes from the array passed in by the client - tryAppendStream(streams[i], currentTime, packetStarted, payloadAdded); // NOLINT(cppcoreguidelines-pro-bounds-pointer-arithmetic) + tryAppendStream(streams[i], currentTime, payloadAdded); // NOLINT(cppcoreguidelines-pro-bounds-pointer-arithmetic) } if (!payloadAdded) {