diff --git a/include/UARTCommandHandler.h b/include/UARTCommandHandler.h index 02610fb..2ee0a12 100644 --- a/include/UARTCommandHandler.h +++ b/include/UARTCommandHandler.h @@ -36,6 +36,11 @@ class CommandLine { void readInput(); void processCommand(const std::string& command); void begin(); + 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){ @@ -47,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; @@ -66,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 \ No newline at end of file +#endif diff --git a/include/data_handling/Telemetry.h b/include/data_handling/Telemetry.h index d6307f9..f2cf994 100644 --- a/include/data_handling/Telemetry.h +++ b/include/data_handling/Telemetry.h @@ -7,6 +7,7 @@ #include #include "ArduinoHAL.h" +#include "UARTCommandHandler.h" #include "data_handling/SensorDataHandler.h" /** @@ -55,6 +56,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"); @@ -198,10 +202,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{} {} /** @@ -211,6 +217,16 @@ 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 Optional command line interface to drive while telemetry manages command mode. + */ + void setCommandLine(CommandLine* newCommandLine) { commandLine = newCommandLine; } + private: // Packet building helpers void preparePacket(std::uint32_t timestamp); @@ -218,6 +234,13 @@ class Telemetry { void addSSDToPacket(SendableSensorData* ssd); void setPacketToZero(); void addEndMarker(); + 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& payloadAdded); + bool finalizeAndSendPacket(); // Non-owning view of the stream list SendableSensorData* const* streams; @@ -229,11 +252,18 @@ class Telemetry { // Output Stream& rfdSerialConnection; + CommandLine* commandLine; // Packet state 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/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; }; diff --git a/src/UARTCommandHandler.cpp b/src/UARTCommandHandler.cpp index 9ad7f8f..ac24e9c 100644 --- a/src/UARTCommandHandler.cpp +++ b/src/UARTCommandHandler.cpp @@ -5,7 +5,18 @@ 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::switchUART(Stream* newUART) { + if (newUART == nullptr) { + return; + } + UART = newUART; +} + +void CommandLine::useDefaultUART() { + UART = defaultUART; } void CommandLine::begin() { @@ -51,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)) { @@ -69,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 47505c6..ebaab70 100644 --- a/src/data_handling/Telemetry.cpp +++ b/src/data_handling/Telemetry.cpp @@ -1,6 +1,7 @@ #include "data_handling/Telemetry.h" #include "ArduinoHAL.h" #include +#include // Helpers for checking if the packet has room for more data std::size_t bytesNeededForSSD(const SendableSensorData* ssd) { @@ -19,6 +20,69 @@ 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; + } + + while (rfdSerialConnection.available() > 0) { + const char receivedChar = static_cast(rfdSerialConnection.read()); + + if (receivedChar == TelemetryFmt::kCommandEntryChar) { + ++commandEntryProgress; + if (commandEntryProgress >= TelemetryFmt::kCommandEntrySequenceLength) { + enterCommandMode(currentTimeMs); + } + } else { + commandEntryProgress = 0; + } + } +} + +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(); + } +} + +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()). @@ -77,54 +141,64 @@ void Telemetry::addEndMarker() { nextEmptyPacketIndex += TelemetryFmt::kEndMarkerBytes; } -bool Telemetry::tick(uint32_t currentTime) { - bool sendingPacketThisTick = false; +bool Telemetry::canFitStreamWithEndMarker(const SendableSensorData* ssd) const { + const std::size_t payloadBytes = bytesNeededForSSD(ssd); + return hasRoom(nextEmptyPacketIndex, payloadBytes + TelemetryFmt::kEndMarkerBytes); +} - 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) +void Telemetry::tryAppendStream(SendableSensorData* stream, std::uint32_t currentTimeMs, bool& payloadAdded) { + if (!stream->shouldBeSent(currentTimeMs)) { + return; + } - if (!sendingPacketThisTick) { - setPacketToZero(); - preparePacket(currentTime); - sendingPacketThisTick = true; - } + if (!canFitStreamWithEndMarker(stream)) { + return; + } - // 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. - } - } + addSSDToPacket(stream); + stream->markWasSent(currentTimeMs); + payloadAdded = true; +} + +bool Telemetry::finalizeAndSendPacket() { + if (nextEmptyPacketIndex <= TelemetryFmt::kHeaderBytes) { + return false; } - // 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(); + if (!hasRoom(nextEmptyPacketIndex, TelemetryFmt::kEndMarkerBytes)) { + return false; + } - // Send used portion - for (std::size_t i = 0; i < nextEmptyPacketIndex; i++) { - rfdSerialConnection.write(packet[i]); - } + addEndMarker(); + for (std::size_t i = 0; i < nextEmptyPacketIndex; i++) { + rfdSerialConnection.write(packet[i]); + } - packetCounter++; //Increment after each successful send - - return true; - } + packetCounter++; + return true; +} + +bool Telemetry::tick(uint32_t currentTime) { + // Checks if we should put the telemetry into command mode + checkForRadioCommandSequence(currentTime); + + if (shouldPauseTelemetryForCommandMode(currentTime)) { + return 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, 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(); }