diff --git a/src/filewriter.cpp b/src/filewriter.cpp
index 8fba290bbe09b41dc95c2c6c7012d432ac47c81a..68d8c22fa9f3b140192c440b0a0d554f3986fd10 100644
--- a/src/filewriter.cpp
+++ b/src/filewriter.cpp
@@ -127,11 +127,14 @@ IMUFileWriter::IMUFileWriter(std::string &filename_template, size_t qhb_version,
         outfile(generate_filename(),
         std::ios::out | std::ios::trunc),
         last_timestamp(0),
+        timestampByteReceived(0),
         rcvState(StateReception::Waiting),
         msgDecodedFunction(0),
         msgDecodedPayloadLength(0),
         msgDecodedPayload(nullptr),
         msgDecodedPayloadIndex(0),
+        softwareMajorRev(0),
+        softwareMinorRev(0),
         msgDecoded(0) {
         outfile << header;
 }
@@ -153,28 +156,24 @@ float IMUFileWriter::GetFloatSafe(const unsigned char *p, int index) {
 void IMUFileWriter::DecodeMessage(unsigned char c) {
     switch (rcvState) {
         case StateReception::Waiting:
-            std::cout << "Waiting : " << c; 
             if (c == 0xFE)
                 rcvState = StateReception::FunctionMSB;
             break;
         case StateReception::FunctionMSB:
-            std::cout << "CMD_MSB : " << c;
             msgDecodedFunction = static_cast<int16_t>(c << 8);
             rcvState = StateReception::FunctionLSB;
             break;
         case StateReception::FunctionLSB:
-            std::cout << "CMD_LSB : " << c;
             msgDecodedFunction += static_cast<int16_t>(c << 0);
             rcvState = StateReception::PayloadLengthMSB;
             break;
         case StateReception::PayloadLengthMSB:
-            std::cout << "PLD_MSB : " << c;
             msgDecodedPayloadLength = static_cast<uint16_t>(c << 8);
             rcvState = StateReception::PayloadLengthLSB;
             break;
         case StateReception::PayloadLengthLSB:
-            std::cout << "PLD_LSB : " << c;
             msgDecodedPayloadLength += static_cast<uint16_t>(c << 0);
+            std::cout << "PayloadLength : " << msgDecodedPayloadLength << "\n";
             if (msgDecodedPayloadLength > 0) {
                 if (msgDecodedPayloadLength < 1024) {
                     msgDecodedPayloadIndex = 0;
@@ -182,15 +181,34 @@ void IMUFileWriter::DecodeMessage(unsigned char c) {
                     if (msgDecodedPayload == nullptr) {
                         throw std::bad_alloc(); // Handle memory allocation failure
                     }
-                    rcvState = StateReception::Payload;
+                    rcvState = StateReception::SoftwareMajorRev;
+                    std::cout << "StateReception : SoftwareMajorRev\n";
                 } else {
                     rcvState = StateReception::Waiting;
+                    std::cout << "StateReception : Waiting\n";
                 }
             } else
                 rcvState = StateReception::Decode;
+                std::cout << "StateReception : Decode\n";
+            break;
+        case StateReception::SoftwareMajorRev:
+            softwareMajorRev = static_cast<u_int8_t>(c);
+            rcvState = StateReception::SoftwareMinorRev;
+            break;
+        case StateReception::SoftwareMinorRev:
+            softwareMinorRev = static_cast<u_int8_t>(c);
+            rcvState = StateReception::TimestampB;
+            break;
+        case StateReception::TimestampB:
+            last_timestamp += static_cast<uint64_t>(c << 8 * (8 - timestampByteReceived));
+            timestampByteReceived += 1;
+            if (timestampByteReceived == 8)
+            {
+                timestampByteReceived = 0;
+                rcvState = StateReception::Payload;
+            }
             break;
         case StateReception::Payload:
-            std::cout << "PLD : " << c;
             if (msgDecodedPayloadIndex > msgDecodedPayloadLength)
             {
                 //Erreur
@@ -206,10 +224,10 @@ void IMUFileWriter::DecodeMessage(unsigned char c) {
             break;
         case StateReception::Decode:
         {
-            std::cout << "DCD : " << c;
             //Lance l'event de fin de decodage
             ProcessDecodedMessage(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
             msgDecoded++;
+            last_timestamp = 0;
             rcvState = StateReception::Waiting;
         }
         break;
@@ -279,6 +297,7 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
         }
         break;
         case static_cast<short>(HS_DATA_PACKET_FULL_TIMESTAMP_V2): {
+            outfile << "PACKET TIMESTAMP: " << last_timestamp;
             IMUFileWriter::SensorType sensorType = static_cast<SensorType>(msgPayload[0]);
             unsigned char id = msgPayload[1];
             unsigned char nbChannels = msgPayload[2];
diff --git a/src/filewriter.h b/src/filewriter.h
index 8cbf897a80334dd503d4be767a8081ecab3c1950..e584a30c13c4ad6fb0926ee1fbc168b139c347bf 100644
--- a/src/filewriter.h
+++ b/src/filewriter.h
@@ -203,6 +203,9 @@ private:
         FunctionLSB,
         PayloadLengthMSB,
         PayloadLengthLSB,
+        SoftwareMajorRev,
+        SoftwareMinorRev,
+        TimestampB,
         Payload,
         Decode
     };
@@ -212,6 +215,9 @@ private:
     int msgDecodedPayloadLength;
     unsigned char *msgDecodedPayload;
     int msgDecodedPayloadIndex;
+    int softwareMajorRev;
+    int softwareMinorRev;
+    int timestampByteReceived;
     unsigned int msgDecoded;
 
     void ProcessDecodedMessage(int msgFunction, int msgPayloadLength,