From fc7e401ddc86b744e6e2bdb42291f67ca8302073 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 13 Apr 2021 01:24:26 +0200
Subject: [PATCH] pus c working

---
 defaultcfg/fsfwconfig/FSFWConfig.h    | 2 +-
 tmtcpacket/pus/TmPacketPusC.cpp       | 6 ++++--
 tmtcpacket/pus/TmPacketPusC.h         | 6 ++++--
 tmtcpacket/pus/TmPacketStoredPusC.cpp | 2 +-
 4 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/defaultcfg/fsfwconfig/FSFWConfig.h b/defaultcfg/fsfwconfig/FSFWConfig.h
index 9a07b4eaf..231294020 100644
--- a/defaultcfg/fsfwconfig/FSFWConfig.h
+++ b/defaultcfg/fsfwconfig/FSFWConfig.h
@@ -45,7 +45,7 @@
 namespace fsfwconfig {
 //! Default timestamp size. The default timestamp will be an eight byte CDC
 //! short timestamp.
-static constexpr uint8_t FSFW_MISSION_TIMESTAMP_SIZE = 8;
+static constexpr uint8_t FSFW_MISSION_TIMESTAMP_SIZE = 7;
 
 //! Configure the allocated pool sizes for the event manager.
 static constexpr size_t FSFW_EVENTMGMR_MATCHTREE_NODES = 240;
diff --git a/tmtcpacket/pus/TmPacketPusC.cpp b/tmtcpacket/pus/TmPacketPusC.cpp
index a6b4c4289..12f0d3f84 100644
--- a/tmtcpacket/pus/TmPacketPusC.cpp
+++ b/tmtcpacket/pus/TmPacketPusC.cpp
@@ -72,8 +72,10 @@ void TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
     tmData->dataField.versionTimeReferenceField = VERSION_NUMBER_BYTE | timeRefField;
     tmData->dataField.serviceType = service;
     tmData->dataField.serviceSubtype = subservice;
-    tmData->dataField.subcounter = packetSubcounter;
-    tmData->dataField.destinationId = destinationId;
+    tmData->dataField.subcounterMsb = packetSubcounter << 8 & 0xff;
+    tmData->dataField.subcounterLsb = packetSubcounter & 0xff;
+    tmData->dataField.destinationIdMsb = destinationId << 8 & 0xff;
+    tmData->dataField.destinationIdLsb = destinationId & 0xff;
     //Timestamp packet
     if (TmPacketBase::checkAndSetStamper()) {
         timeStamper->addTimeStamp(tmData->dataField.time,
diff --git a/tmtcpacket/pus/TmPacketPusC.h b/tmtcpacket/pus/TmPacketPusC.h
index d072d5b9c..fdc275486 100644
--- a/tmtcpacket/pus/TmPacketPusC.h
+++ b/tmtcpacket/pus/TmPacketPusC.h
@@ -22,8 +22,10 @@ struct PUSTmDataFieldHeaderPusC {
     uint8_t versionTimeReferenceField;
     uint8_t serviceType;
     uint8_t serviceSubtype;
-    uint16_t subcounter;
-    uint16_t destinationId;
+    uint8_t subcounterMsb;
+    uint8_t subcounterLsb;
+    uint8_t destinationIdMsb;
+    uint8_t destinationIdLsb;
     uint8_t time[TimeStamperIF::MISSION_TIMESTAMP_SIZE];
 };
 
diff --git a/tmtcpacket/pus/TmPacketStoredPusC.cpp b/tmtcpacket/pus/TmPacketStoredPusC.cpp
index f006412e2..7f7744114 100644
--- a/tmtcpacket/pus/TmPacketStoredPusC.cpp
+++ b/tmtcpacket/pus/TmPacketStoredPusC.cpp
@@ -56,7 +56,7 @@ TmPacketStoredPusC::TmPacketStoredPusC(uint16_t apid, uint8_t service,
         TmPacketStoredBase::checkAndReportLostTm();
     }
     setData(p_data);
-    initializeTmPacket(apid, service, subservice, packetSubcounter);
+    initializeTmPacket(apid, service, subservice, packetSubcounter, destinationId, timeRefField);
     uint8_t *putDataHere = getSourceData();
     size_t size = 0;
     if (header != NULL) {