Telemetry.h

Go to the documentation of this file.

#ifndef TELEMETRY_H
#define TELEMETRY_H
 
#include <array>
#include <cstddef>
#include <cstdint>
#include <cstring>
 
#include "ArduinoHAL.h"
#include "UARTCommandHandler.h"
#include "data_handling/SensorDataHandler.h"

 
namespace TelemetryFmt {

constexpr std::size_t kPacketCapacity_bytes = 120;

constexpr std::size_t kSyncZeroCount_bytes = 3;

constexpr std::size_t kBytesInU32_bytes = 4;

constexpr std::size_t kStartByteIndex = kSyncZeroCount_bytes;          // 3
constexpr std::size_t kTimestampIndex = kStartByteIndex + 1; // 4
constexpr std::size_t kPacketCounterIndex = kTimestampIndex + kBytesInU32_bytes; // 8
constexpr std::size_t kHeaderSize_bytes = kSyncZeroCount_bytes + 1 + kBytesInU32_bytes + kBytesInU32_bytes; // 12 (CHANGED from 8 to 12)

constexpr std::size_t kEndMarkerSize_bytes = kSyncZeroCount_bytes + 1;

constexpr std::uint8_t kStartByteValue = 51;

constexpr std::uint8_t kEndByteValue = 52;

constexpr unsigned kBitsPerByte_bits = 8;
constexpr std::uint32_t kCommandModeInactivityTimeout_ms = 10000;
constexpr std::size_t kCommandEntrySequenceLength = 3;
constexpr char kCommandEntryChar = 'c';

static_assert(sizeof(std::uint32_t) == 4, "Expected 32-bit uint32_t");
static_assert(sizeof(float) == 4, "Expected 32-bit float");

inline void writeU32Be(std::uint8_t* dst, std::uint32_t v) {
 
    for (std::size_t i = 0; i < kBytesInU32_bytes; ++i) {
        const unsigned shift = static_cast<unsigned>((kBytesInU32_bytes - 1 - i) * kBitsPerByte_bits);
        dst[i] = static_cast<std::uint8_t>(v >> shift);
    }
}
 

inline std::uint16_t hzToPeriod_ms(std::uint16_t frequency_hz) {
    return (frequency_hz == 0) ? static_cast<std::uint16_t>(1000u)
                     : static_cast<std::uint16_t>((1000u + frequency_hz - 1u) / frequency_hz);
}
 
} // namespace TelemetryFmt

struct SendableSensorData {
    // --- Payload configuration ---

    SensorDataHandler* singleSDH;

    SensorDataHandler* const* multiSDH;

    // Shall not be changed after construction
    // Used to iterate over multiSDH
    const std::size_t multiSDHLength;

    std::uint8_t multiSDHDataLabel;
 
    // --- Scheduling state ---

    std::uint16_t period_ms;

    std::uint32_t lastSentTimestamp_ms;

    SendableSensorData(SensorDataHandler* sdh, std::uint16_t sendFrequency_hz)
        : singleSDH(sdh),
          multiSDH(0),
          multiSDHLength(0),
          multiSDHDataLabel(0),
          period_ms(TelemetryFmt::hzToPeriod_ms(sendFrequency_hz)),
          lastSentTimestamp_ms(0) {}

    template <std::size_t M>
    SendableSensorData(const std::array<SensorDataHandler*, M>& sdhList,
                       std::uint8_t label,
                       std::uint16_t sendFrequency_hz)
        : singleSDH(0),
          multiSDH(sdhList.data()),
          multiSDHLength(M),
          multiSDHDataLabel(label),
          period_ms(TelemetryFmt::hzToPeriod_ms(sendFrequency_hz)),
          lastSentTimestamp_ms(0) {}

    bool shouldBeSent(std::uint32_t now_ms) const {
        return (now_ms - lastSentTimestamp_ms) >= period_ms;
    }

    void markWasSent(std::uint32_t now_ms) {
        lastSentTimestamp_ms = now_ms;
    }

    bool isSingle() const { return singleSDH != 0; }

    bool isMulti() const { return (multiSDH != 0) && (multiSDHLength != 0); }
};

class Telemetry {
public:

    template <std::size_t N>
    Telemetry(const std::array<SendableSensorData*, N>& streams,
              Stream& rfdSerialConnection,
              CommandLine* commandLine = nullptr)
        : streams_(streams.data()),
          streamCount_(N),
          rfdSerialConnection_(rfdSerialConnection),
          commandLine_(commandLine),
          nextEmptyPacketIndex_(0),
          packet_{} {}

    bool tick(std::uint32_t currentTime_ms);

    bool isInCommandMode() const { return inCommandMode_; }

    void setCommandLine(CommandLine* newCommandLine) { commandLine_ = newCommandLine; }

    void lockCommandModeTimeout(std::uint32_t lockDuration_ms);

    void unlockCommandModeTimeout();

    void forceExitCommandMode();
 
private:
    // Packet building helpers
    void preparePacket(std::uint32_t timestamp_ms);
    void addSingleSDHToPacket(SensorDataHandler* sdh);
    void addSSDToPacket(SendableSensorData* ssd);
    void setPacketToZero();
    void addEndMarker();
    void checkForRadioCommandSequence(std::uint32_t currentTime_ms);
    void enterCommandMode(std::uint32_t currentTime_ms);
    void exitCommandMode();
    bool shouldPauseTelemetryForCommandMode(std::uint32_t currentTime_ms);
    bool canFitStreamWithEndMarker(const SendableSensorData* ssd) const;
    void tryAppendStream(SendableSensorData* stream, std::uint32_t currentTime_ms, bool& payloadAdded);
    bool finalizeAndSendPacket();
 
    // Non-owning view of the stream list
    SendableSensorData* const* streams_;
 
    // Number of streams in the list
    // Shall not be changed after construction
    // Used to iterate over streams
    const std::size_t streamCount_;
 
    // Output
    Stream& rfdSerialConnection_;
    CommandLine* commandLine_;
 
    // Packet state
    std::uint32_t packetCounter_ = 0;
    std::size_t nextEmptyPacketIndex_;
    std::array<std::uint8_t, TelemetryFmt::kPacketCapacity_bytes> packet_;
 
    // Command mode handling
    bool inCommandMode_ = false; 
    std::uint32_t commandModeEnteredTimestamp_ms_ = 0;
    std::uint32_t commandModeLastInputTimestamp_ms_ = 0;
    std::size_t commandEntryProgress_ = 0;
    bool commandModeTimeoutLocked_ = false;
    std::uint32_t commandModeTimeoutLockDeadline_ms_ = 0;
};
 
#endif