Add QHBV2 compatibility

src/filewriter.cpp

@@ -216,7 +216,6 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
     unsigned int timeStamp = 0;
     switch(static_cast<short>(msgFunction)) {
         case static_cast<short>(HS_DATA_PACKET_FULL_TIMESTAMP): {
-            /*
             IMUFileWriter::SensorType sensorType = static_cast<SensorType>(msgPayload[0]);
             unsigned char id = msgPayload[1];
             unsigned char nbChannels = msgPayload[2];
@@ -232,22 +231,22 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
             double magRange = 4900.0;       //Fixe
 
             for(int i=0; i < nbSamples && msgPayloadLength >= lengthPerSample * i + 9; i++) {
-                timeStamp = BUILD_UINT32(9 + i * lengthPerSample, 9 + i * 9 + i * lengthPerSample+1, 9 + i * lengthPerSample+2, 9 + i * lengthPerSample+3);
+                timeStamp = BUILD_UINT32(msgPayload[9 + i * lengthPerSample+3], msgPayload[9 + i * lengthPerSample+2], msgPayload[9 + i * lengthPerSample+1], msgPayload[9 + i * lengthPerSample]);
                 
-                if(timeStamp > lastTimeStamp): {
+                if(timeStamp > lastTimeStamp) {
                     lastTimeStamp = timeStamp;
                     switch(sensorType) {
                         case IMUFileWriter::SensorType::IMU: {
                             outfile << timeStamp;
-                            outfile << ", " << BUILD_UINT16(13 + i * lengthPerSample,13 + i * lengthPerSample+1); // AccelX
-                            outfile << ", " << BUILD_UINT16(15 + i * lengthPerSample,15 + i * lengthPerSample+1); // AccelY
-                            outfile << ", " << BUILD_UINT16(17 + i * lengthPerSample,17 + i * lengthPerSample+1); // AccelZ
-                            outfile << ", " << BUILD_UINT16(19 + i * lengthPerSample,19 + i * lengthPerSample+1); // GyroX
-                            outfile << ", " << BUILD_UINT16(21 + i * lengthPerSample,21 + i * lengthPerSample+1); // GyroY
-                            outfile << ", " << BUILD_UINT16(23 + i * lengthPerSample,23 + i * lengthPerSample+1); // GyroZ
-                            outfile << ", " << BUILD_UINT16(25 + i * lengthPerSample,25 + i * lengthPerSample+1); // MagX
-                            outfile << ", " << BUILD_UINT16(27 + i * lengthPerSample,27 + i * lengthPerSample+1); // MagY
-                            outfile << ", " << BUILD_UINT16(29 + i * lengthPerSample,29 + i * lengthPerSample+1); // MagZ
+                            outfile << ", " << BUILD_UINT16(msgPayload[13 + i * lengthPerSample],msgPayload[13 + i * lengthPerSample+1]); // AccelX
+                            outfile << ", " << BUILD_UINT16(msgPayload[15 + i * lengthPerSample],msgPayload[15 + i * lengthPerSample+1]); // AccelY
+                            outfile << ", " << BUILD_UINT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]); // AccelZ
+                            outfile << ", " << BUILD_UINT16(msgPayload[19 + i * lengthPerSample],msgPayload[19 + i * lengthPerSample+1]); // GyroX
+                            outfile << ", " << BUILD_UINT16(msgPayload[21 + i * lengthPerSample],msgPayload[21 + i * lengthPerSample+1]); // GyroY
+                            outfile << ", " << BUILD_UINT16(msgPayload[23 + i * lengthPerSample],msgPayload[23 + i * lengthPerSample+1]); // GyroZ
+                            outfile << ", " << BUILD_UINT16(msgPayload[25 + i * lengthPerSample],msgPayload[25 + i * lengthPerSample+1]); // MagX
+                            outfile << ", " << BUILD_UINT16(msgPayload[27 + i * lengthPerSample],msgPayload[27 + i * lengthPerSample+1]); // MagY
+                            outfile << ", " << BUILD_UINT16(msgPayload[29 + i * lengthPerSample],msgPayload[29 + i * lengthPerSample+1]); // MagZ
                             outfile << std::endl;
                         }
                         break;
@@ -269,8 +268,7 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
                 } else {
                     outfile << "TS IMU Error" << std::endl;
                 }
-            }*/
-           outfile << "Not implemented yet. Deprecated version?" << std::endl; // Commented block added by Philémon Prévot 29/08/2024
+            }
         }
         break;
         case static_cast<short>(HS_DATA_PACKET_FULL_TIMESTAMP_V2): {
@@ -452,10 +450,8 @@ 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);
 
-    uint8_t softwareMajorRev=imu_data[0];
-    uint8_t softwareMinorRev=imu_data[1];
-    
-    if(softwareMajorRev >= 2) {
+    if(!(imu_data[0]==0xFE) && (imu_data[0]>= 2)) {
+        std::cerr << "Pantoufle" << std::endl;
         for(int i=1; i<size-1; i++)
         {
             DecodeMessage(imu_data[i]);

src/filewriter.h

@@ -192,7 +192,7 @@ private:
     unsigned int lastLightTimeStamp;
     unsigned int lastPressureTimeStamp = 0;
     unsigned int lastTemperatureTimeStamp;
-    unsigned int lastTimeStamp;
+    unsigned int lastTimeStamp = 0;
     DateTime lastGPSDate;
     double lastPPSTimeStampNS;