Remote shell (WIP)

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
+#endif

include/data_handling/Telemetry.h

@@ -7,6 +7,7 @@
 #include <cstring>
 
 #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 <std::size_t N>
     Telemetry(const std::array<SendableSensorData*, N>& streams,
-              Stream& rfdSerialConnection)
+              Stream& rfdSerialConnection,
+              CommandLine* commandLine = nullptr)
         : streams(streams.data()),
           streamCount(N),
           rfdSerialConnection(rfdSerialConnection),
+          commandLine(commandLine),
           nextEmptyPacketIndex(0),
           packet{} {}
     /**
@@ -211,13 +217,30 @@ 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);
     void addSingleSDHToPacket(SensorDataHandler* sdh);
     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<std::uint8_t, TelemetryFmt::kPacketCapacity> packet;
+
+    // Command mode handling
+    bool inCommandMode = false; 
+    std::uint32_t commandModeEnteredTimestamp = 0;
+    std::uint32_t commandModeLastInputTimestamp = 0;
+    std::size_t commandEntryProgress = 0;
 };
 
 #endif

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

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

include/simulation/Serial_Sim_LIS2MDL.h

@@ -3,23 +3,30 @@
 
 #include "Serial_Sim.h"
 
+#ifndef LIS2MDL_RATE_100_HZ
+#define LIS2MDL_RATE_100_HZ 0x06
+#endif
+
 class Adafruit_LIS2MDL {
 public:
     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

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<uint16_t>(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

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,22 +28,27 @@ 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);
         SerialSim::getInstance().updateGyro(gyro);
 
     }
 
+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;
 
 };