Variable casting correction

src/filewriter.cpp

@@ -172,19 +172,20 @@ void IMUFileWriter::DecodeMessage(unsigned char c) {
                 rcvState = StateReception::FunctionMSB;
             break;
         case StateReception::FunctionMSB:
-                msgDecodedFunction = static_cast<int>(c << 8);
+            msgDecodedFunction = static_cast<int16_t>(c << 8);
             rcvState = StateReception::FunctionLSB;
             break;
         case StateReception::FunctionLSB:
-                msgDecodedFunction += static_cast<int>(c << 0);
+            msgDecodedFunction += static_cast<int16_t>(c << 0);
             rcvState = StateReception::PayloadLengthMSB;
             break;
         case StateReception::PayloadLengthMSB:
-                msgDecodedPayloadLength = static_cast<int>(c << 8);
+            msgDecodedPayloadLength = static_cast<uint16_t>(c << 8);
+            std::cerr << "Message function : " << std::hex << static_cast<uint16_t>(msgDecodedFunction) << std::endl;
             rcvState = StateReception::PayloadLengthLSB;
             break;
         case StateReception::PayloadLengthLSB:
-                msgDecodedPayloadLength += static_cast<int>(c << 0);
+            msgDecodedPayloadLength += static_cast<uint16_t>(c << 0);
             if (msgDecodedPayloadLength > 0) {
                 if (msgDecodedPayloadLength < 1024) {
                     msgDecodedPayloadIndex = 0;
@@ -231,6 +232,9 @@ void IMUFileWriter::DecodeMessage(unsigned char c) {
             rcvState = StateReception::Waiting;
             break;
     }
+    if(rcvState != StateReception::Waiting) {
+        std::cerr << "State : " << static_cast<int>(rcvState) << std::endl;
+    }
 }
 
 void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, const unsigned char* msgPayload) {
@@ -473,13 +477,15 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
 void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) {
     uint8_t *imu_data_cur(imu_data);
 
-    while(imu_data_cur + frame_size + 5 < imu_data + additional_data_size){
     uint8_t softwareMajorRev=sample[5];
     uint8_t softwareMinorRev=sample[6];
+    std::cerr << "sMR" << static_cast<int>(sample[10]) << std::endl;
+    std::cerr << "Size : " << size << std::endl;
     
-        if(softwareMajorRev>=2) {
+    uint64_t timeStamp100MHzCurrentPacket=0;
+    if(true) {
         //On recupere l'instant de fin du paquet courant
-            uint64_t timeStamp100MHzCurrentPacket=BUILD_UINT64(sample[14],
+        timeStamp100MHzCurrentPacket=BUILD_UINT64(sample[14],
                     sample[13],
                     sample[12],
                     sample[11],
@@ -490,6 +496,7 @@ void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) {
         timeStamp100MHzCurrentPacket*=10;     //To put the ts in ns
         uint8_t enteteSize=16;
 
+        outfile << "PACKET TIMESTAMP: " << static_cast<int>(timeStamp100MHzCurrentPacket) << std::endl;
         for(int i=enteteSize;i<size-enteteSize; i++)
         {
             DecodeMessage(sample[i]);
@@ -522,8 +529,6 @@ void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) {
             imu_data_cur += frame_size;
         }
     }
-        imu_data_cur += frame_size;
-    }
 }
 
 size_t IMUFileWriter::get_last_timestamp(){