Telemetry

include/data_handling/Telemetry.h

@@ -39,7 +39,8 @@ constexpr std::size_t kU32Bytes = 4;
 /** Header layout: [0..2]=0, [3]=START, [4..7]=timestamp (big-endian). */
 constexpr std::size_t kStartByteIndex = kSyncZeros;          // 3
 constexpr std::size_t kTimestampIndex = kStartByteIndex + 1; // 4
-constexpr std::size_t kHeaderBytes    = kSyncZeros + 1 + kU32Bytes;
+constexpr std::size_t kPacketCounterIndex = kTimestampIndex + kU32Bytes; // 8
+constexpr std::size_t kHeaderBytes = kSyncZeros + 1 + kU32Bytes + kU32Bytes; // 12 (CHANGED from 8 to 12)
 
 /** End marker layout: 3 zeros followed by an end byte. */
 constexpr std::size_t kEndMarkerBytes = kSyncZeros + 1;
@@ -230,6 +231,7 @@ class Telemetry {
     Stream& rfdSerialConnection;
 
     // Packet state
+    std::uint32_t packetCounter = 0;
     std::size_t nextEmptyPacketIndex;
     std::array<std::uint8_t, TelemetryFmt::kPacketCapacity> packet;
 };

src/data_handling/Telemetry.cpp

@@ -32,6 +32,9 @@ void Telemetry::preparePacket(std::uint32_t timestamp) {
     // Write the timestamp in big-endian format
     TelemetryFmt::write_u32_be(&this->packet[TelemetryFmt::kTimestampIndex], timestamp);
 
+    // Packet counter (4 bytes, big-endian)
+    TelemetryFmt::write_u32_be(&this->packet[TelemetryFmt::kPacketCounterIndex], packetCounter);
+
     nextEmptyPacketIndex = TelemetryFmt::kHeaderBytes;
 }
 
@@ -113,6 +116,9 @@ bool Telemetry::tick(uint32_t currentTime) {
             for (std::size_t i = 0; i < nextEmptyPacketIndex; i++) {
                 rfdSerialConnection.write(packet[i]);
             }
+
+            packetCounter++; //Increment after each successful send
+
             return true;
         }