diff --git a/src/filewriter.cpp b/src/filewriter.cpp
index 3fbbec53c41df7f02310a76cd47f39d349697ca6..f69c5b830f87f9e870914e25adc85c2dd463b679 100644
--- a/src/filewriter.cpp
+++ b/src/filewriter.cpp
@@ -166,71 +166,75 @@ unsigned char IMUFileWriter::CalculateChecksum(int msgFunction,
}
void IMUFileWriter::DecodeMessage(unsigned char c) {
- switch (rcvState) {
- case StateReception::Waiting:
- if (c == 0xFE)
- rcvState = StateReception::FunctionMSB;
- break;
- case StateReception::FunctionMSB:
- msgDecodedFunction = static_cast<int>(c << 8);
- rcvState = StateReception::FunctionLSB;
- break;
- case StateReception::FunctionLSB:
- msgDecodedFunction += static_cast<int>(c << 0);
- rcvState = StateReception::PayloadLengthMSB;
- break;
- case StateReception::PayloadLengthMSB:
- msgDecodedPayloadLength = static_cast<int>(c << 8);
- rcvState = StateReception::PayloadLengthLSB;
- break;
- case StateReception::PayloadLengthLSB:
- msgDecodedPayloadLength += static_cast<int>(c << 0);
- if (msgDecodedPayloadLength > 0) {
- if (msgDecodedPayloadLength < 1024) {
- msgDecodedPayloadIndex = 0;
- msgDecodedPayload = static_cast<unsigned char*>(malloc(msgDecodedPayloadLength));
- if (msgDecodedPayload == nullptr) {
- throw std::bad_alloc(); // Handle memory allocation failure
- }
- rcvState = StateReception::Payload;
- } else {
- rcvState = StateReception::Waiting;
- }
- } else
- rcvState = StateReception::CheckSum;
- break;
- case StateReception::Payload:
- if (msgDecodedPayloadIndex > msgDecodedPayloadLength)
- {
- //Erreur
+ switch (rcvState) {
+ case StateReception::Waiting:
+ if (c == 0xFE)
+ rcvState = StateReception::FunctionMSB;
+ break;
+ case StateReception::FunctionMSB:
+ msgDecodedFunction = static_cast<int16_t>(c << 8);
+ rcvState = StateReception::FunctionLSB;
+ break;
+ case StateReception::FunctionLSB:
+ msgDecodedFunction += static_cast<int16_t>(c << 0);
+ rcvState = StateReception::PayloadLengthMSB;
+ break;
+ case StateReception::PayloadLengthMSB:
+ 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<uint16_t>(c << 0);
+ if (msgDecodedPayloadLength > 0) {
+ if (msgDecodedPayloadLength < 1024) {
msgDecodedPayloadIndex = 0;
+ msgDecodedPayload = static_cast<unsigned char*>(malloc(msgDecodedPayloadLength));
+ if (msgDecodedPayload == nullptr) {
+ throw std::bad_alloc(); // Handle memory allocation failure
+ }
+ rcvState = StateReception::Payload;
+ } else {
rcvState = StateReception::Waiting;
}
- msgDecodedPayload[msgDecodedPayloadIndex++] = c;
- if (msgDecodedPayloadIndex >= msgDecodedPayloadLength)
- {
- rcvState = StateReception::CheckSum;
- msgDecodedPayloadIndex = 0;
- }
- break;
- case StateReception::CheckSum:
+ } else
+ rcvState = StateReception::CheckSum;
+ break;
+ case StateReception::Payload:
+ if (msgDecodedPayloadIndex > msgDecodedPayloadLength)
{
- unsigned char calculatedChecksum = CalculateChecksum(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
- unsigned char receivedChecksum = c;
- if (calculatedChecksum == receivedChecksum) {
- //Lance l'event de fin de decodage
- ProcessDecodedMessage(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
- msgDecoded++;
- } else {
- std::cerr << "Checksum error" << std::endl;
- }
+ //Erreur
+ msgDecodedPayloadIndex = 0;
rcvState = StateReception::Waiting;
}
+ msgDecodedPayload[msgDecodedPayloadIndex++] = c;
+ if (msgDecodedPayloadIndex >= msgDecodedPayloadLength)
+ {
+ rcvState = StateReception::CheckSum;
+ msgDecodedPayloadIndex = 0;
+ }
break;
- default:
- rcvState = StateReception::Waiting;
- break;
+ case StateReception::CheckSum:
+ {
+ unsigned char calculatedChecksum = CalculateChecksum(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
+ unsigned char receivedChecksum = c;
+ if (calculatedChecksum == receivedChecksum) {
+ //Lance l'event de fin de decodage
+ ProcessDecodedMessage(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
+ msgDecoded++;
+ } else {
+ std::cerr << "Checksum error" << std::endl;
+ }
+ rcvState = StateReception::Waiting;
}
+ break;
+ default:
+ 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,56 +477,57 @@ 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];
-
- if(softwareMajorRev>=2) {
- //On recupere l'instant de fin du paquet courant
- uint64_t timeStamp100MHzCurrentPacket=BUILD_UINT64(sample[14],
- sample[13],
- sample[12],
- sample[11],
- sample[10],
- sample[9],
- sample[8],
- sample[7]);
- timeStamp100MHzCurrentPacket*=10; //To put the ts in ns
- uint8_t enteteSize=16;
-
- for(int i=enteteSize;i<size-enteteSize; i++)
- {
- DecodeMessage(sample[i]);
- }
+ 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;
+
+ uint64_t timeStamp100MHzCurrentPacket=0;
+ if(true) {
+ //On recupere l'instant de fin du paquet courant
+ timeStamp100MHzCurrentPacket=BUILD_UINT64(sample[14],
+ sample[13],
+ sample[12],
+ sample[11],
+ sample[10],
+ sample[9],
+ sample[8],
+ sample[7]);
+ 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]);
}
- else {
- uint8_t *imu_data_cur(imu_data);
- while(imu_data_cur + frame_size + 5 < imu_data + additional_data_size){
- if(!(imu_data_cur[0]==0xFE && imu_data_cur[1]==0x0A && imu_data_cur[2]==0x0A && imu_data_cur[5]==0x08)){
+ }
+ else {
+ uint8_t *imu_data_cur(imu_data);
+ while(imu_data_cur + frame_size + 5 < imu_data + additional_data_size){
+ if(!(imu_data_cur[0]==0xFE && imu_data_cur[1]==0x0A && imu_data_cur[2]==0x0A && imu_data_cur[5]==0x08)) {
// skip trame if header is incorrect
imu_data_cur += frame_size + 5;
continue;
- }
- imu_data_cur += 5; // skip frame header
- auto timestamp = static_cast<int32_t>(__builtin_bswap32(*reinterpret_cast<uint32_t*>(imu_data_cur + 9)));
- if (timestamp > last_timestamp) {
- last_timestamp = timestamp;
- outfile << timestamp;
- outfile << "," << le16tof(imu_data_cur + 13) / 32756 * 19.62; // ax resolution +- 2g
- outfile << "," << le16tof(imu_data_cur + 15) / 32756 * 19.62; // ay resolution +- 2g
- outfile << "," << le16tof(imu_data_cur + 17) / 32756 * 19.62; // az resolution +- 2g
- outfile << "," << le16tof(imu_data_cur + 19) / 32756 * 250; // gx resolution +- 255deg/sec
- outfile << "," << le16tof(imu_data_cur + 21) / 32756 * 250; // gy resolution +- 255deg/sec
- outfile << "," << le16tof(imu_data_cur + 23) / 32756 * 250; // gz resolution +- 255deg/sec
- outfile << "," << le16tof(imu_data_cur + 25) / 32756 * 4900.; // mx +- 4900µTesla
- outfile << "," << le16tof(imu_data_cur + 27) / 32756 * (-4900.); // my +- 4900µTesla
- outfile << "," << le16tof(imu_data_cur + 29) / 32756 * (-4900.); // mz +- 4900µTesla
- outfile << std::endl;
- }
- imu_data_cur += frame_size;
}
+ imu_data_cur += 5; // skip frame header
+ auto timestamp = static_cast<int32_t>(__builtin_bswap32(*reinterpret_cast<uint32_t*>(imu_data_cur + 9)));
+ if (timestamp > last_timestamp) {
+ last_timestamp = timestamp;
+ outfile << timestamp;
+ outfile << "," << le16tof(imu_data_cur + 13) / 32756 * 19.62; // ax resolution +- 2g
+ outfile << "," << le16tof(imu_data_cur + 15) / 32756 * 19.62; // ay resolution +- 2g
+ outfile << "," << le16tof(imu_data_cur + 17) / 32756 * 19.62; // az resolution +- 2g
+ outfile << "," << le16tof(imu_data_cur + 19) / 32756 * 250; // gx resolution +- 255deg/sec
+ outfile << "," << le16tof(imu_data_cur + 21) / 32756 * 250; // gy resolution +- 255deg/sec
+ outfile << "," << le16tof(imu_data_cur + 23) / 32756 * 250; // gz resolution +- 255deg/sec
+ outfile << "," << le16tof(imu_data_cur + 25) / 32756 * 4900.; // mx +- 4900µTesla
+ outfile << "," << le16tof(imu_data_cur + 27) / 32756 * (-4900.); // my +- 4900µTesla
+ outfile << "," << le16tof(imu_data_cur + 29) / 32756 * (-4900.); // mz +- 4900µTesla
+ outfile << std::endl;
+ }
+ imu_data_cur += frame_size;
}
- imu_data_cur += frame_size;
}
}