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 "data_handling/SensorDataHandler.h"

 
namespace TelemetryFmt {

constexpr std::size_t kPacketCapacity = 120;

constexpr std::size_t kSyncZeros = 3;

constexpr std::size_t kU32Bytes = 4;

constexpr std::size_t kStartByteIndex = kSyncZeros;          // 3
constexpr std::size_t kTimestampIndex = kStartByteIndex + 1; // 4
constexpr std::size_t kPacketCounterIndex = kTimestampIndex + kU32Bytes; // 8
constexpr std::size_t kHeaderBytes = kSyncZeros + 1 + kU32Bytes + kU32Bytes; // 12 (CHANGED from 8 to 12)

constexpr std::size_t kEndMarkerBytes = kSyncZeros + 1;

constexpr std::uint8_t kStartByteValue = 51;

constexpr std::uint8_t kEndByteValue = 52;

constexpr std::size_t kBytesIn32Bit = 4;
constexpr unsigned kBitsPerByte = 8;
constexpr std::uint8_t kAllOnesByte = 0xFF;

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

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

inline std::uint16_t hz_to_period_ms(std::uint16_t hz) {
    return (hz == 0) ? 1000u
                     : static_cast<std::uint16_t>((1000u + hz - 1u) / hz);
}
 
} // namespace TelemetryFmt
 
// Backwards-compatible names if existing code uses START_BYTE / END_BYTE.
static const std::uint8_t START_BYTE = TelemetryFmt::kStartByteValue;
static const std::uint8_t END_BYTE   = TelemetryFmt::kEndByteValue;

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 periodMs;

    std::uint32_t lastSentTimestamp;

    SendableSensorData(SensorDataHandler* sdh, std::uint16_t sendFrequencyHz)
        : singleSDH(sdh),
          multiSDH(0),
          multiSDHLength(0),
          multiSDHDataLabel(0),
          periodMs(TelemetryFmt::hz_to_period_ms(sendFrequencyHz)),
          lastSentTimestamp(0) {}

    template <std::size_t M>
    SendableSensorData(const std::array<SensorDataHandler*, M>& sdhList,
                       std::uint8_t label,
                       std::uint16_t sendFrequencyHz)
        : singleSDH(0),
          multiSDH(sdhList.data()),
          multiSDHLength(M),
          multiSDHDataLabel(label),
          periodMs(TelemetryFmt::hz_to_period_ms(sendFrequencyHz)),
          lastSentTimestamp(0) {}

    bool shouldBeSent(std::uint32_t now) const {
        return (now - lastSentTimestamp) >= periodMs;
    }

    void markWasSent(std::uint32_t now) {
        lastSentTimestamp = now;
    }

    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)
        : streams(streams.data()),
          streamCount(N),
          rfdSerialConnection(rfdSerialConnection),
          nextEmptyPacketIndex(0),
          packet{} {}

    bool tick(std::uint32_t currentTimeMs);
 
private:
    // Packet building helpers
    void preparePacket(std::uint32_t timestamp);
    void addSingleSDHToPacket(SensorDataHandler* sdh);
    void addSSDToPacket(SendableSensorData* ssd);
    void setPacketToZero();
    void addEndMarker();
 
    // 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;
 
    // Packet state
    std::uint32_t packetCounter = 0;
    std::size_t nextEmptyPacketIndex;
    std::array<std::uint8_t, TelemetryFmt::kPacketCapacity> packet;
};
 
#endif