diff --git a/src/filewriter.cpp b/src/filewriter.cpp
index d5fa7ccd27d04cfca610210686d0ce4a65839b5a..c97ba448489e0264ca49009b64b10d0213f313f7 100644
--- a/src/filewriter.cpp
+++ b/src/filewriter.cpp
@@ -6,7 +6,8 @@
*/
#include "filewriter.h"
-#include "Macros.h"
+#include "macros.h"
+#include "decoder.h"
#include <stdexcept>
#include <vector>
#include <iostream>
@@ -124,11 +125,24 @@ void WavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t *imu_dat
IMUFileWriter::IMUFileWriter(std::string &filename_template, size_t num_channels, size_t sample_rate,size_t depth, size_t timestamp) :
FileWriter(filename_template, num_channels, sample_rate, depth),
- outfile(generate_filename(), std::ios::out | std::ios::trunc),
- last_timestamp(0) {
+ outfile(generate_filename(),
+ std::ios::out | std::ios::trunc),
+ last_timestamp(0),
+ rcvState(StateReception::Waiting),
+ msgDecodedFunction(0),
+ msgDecodedPayloadLength(0),
+ msgDecodedPayload(nullptr),
+ msgDecodedPayloadIndex(0),
+ msgDecoded(0) {
outfile << header;
}
+IMUFileWriter::~IMUFileWriter() {
+ if (msgDecodedPayload) {
+ free(msgDecodedPayload);
+ }
+}
+
inline float le16tof(uint8_t *array){
return static_cast<float>(static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t*>(array))));
}
@@ -143,115 +157,375 @@ float IMUFileWriter::GetFloatSafe(const unsigned char *p, int index) {
return result;
}
-void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) {
- uint8_t *imu_data_cur(imu_data);
+unsigned char IMUFileWriter::CalculateChecksum(int msgFunction,
+ int msgPayloadLength, const unsigned char msgPayload[])
+{
+ unsigned char checksum = 0;
+ checksum ^= static_cast<unsigned char>(0xFE);
+ checksum ^= static_cast<unsigned char>(msgFunction >> 8);
+ checksum ^= static_cast<unsigned char>(msgFunction >> 0);
+ checksum ^= static_cast<unsigned char>(msgPayloadLength >> 8);
+ checksum ^= static_cast<unsigned char>(msgPayloadLength >> 0);
+ for (int i = 0; i < msgPayloadLength; i++) {
+ checksum ^= msgPayload[i];
+ }
+ return checksum;
+}
- while(imu_data_cur + frame_size + 5 < imu_data + additional_data_size){
- IMUFileWriter::SensorType sensorType = static_cast<SensorType>(imu_data_cur[0]);
- unsigned char id = imu_data_cur[1];
- unsigned char nbChannels = imu_data_cur[2];
- float rangeScale = GetFloatSafe(imu_data_cur, 3);
- unsigned char resolutionBits = imu_data_cur[7];
- float samplingFrequency = GetFloatSafe(imu_data_cur, 8);
- unsigned short nbSamples = imu_data_cur[12];
-
- int lengthPerSample = nbChannels * resolutionBits / 8 + 4;
- double dataMaxValue = std::pow(2, resolutionBits) / 2.0;
-
- for(int i = 0; i < nbSamples && size >= static_cast<size_t>(lengthPerSample * i + 13); i++) {
- uint32_t timeStamp = BUILD_UINT32(imu_data_cur[13 + i * lengthPerSample+3],imu_data_cur[13 + i * lengthPerSample+2],imu_data_cur[13 + i * lengthPerSample+1],imu_data_cur[13 + i * lengthPerSample]);
- outfile << timeStamp;
- switch(sensorType) {
- case IMUFileWriter::SensorType::Accel:
- if (lastAccelTimeStamp >= 500000000)
- lastAccelTimeStamp = 0;
- if (timeStamp > lastAccelTimeStamp) {
- lastAccelTimeStamp = timeStamp;
- outfile << "ACCEL, " << timeStamp / 1000.0;
- outfile << ", " << BUILD_INT16(imu_data_cur[17 + i * lengthPerSample],imu_data_cur[17 + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
- outfile << ", " << BUILD_INT16(imu_data_cur[17 + size+ i * lengthPerSample],imu_data_cur[17 +size+ i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
- outfile << ", " << BUILD_INT16(imu_data_cur[17 + 2*size+ i * lengthPerSample],imu_data_cur[17 +2*size+ i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
+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
+ msgDecodedPayloadIndex = 0;
+ rcvState = StateReception::Waiting;
+ }
+ msgDecodedPayload[msgDecodedPayloadIndex++] = c;
+ if (msgDecodedPayloadIndex >= msgDecodedPayloadLength)
+ {
+ rcvState = StateReception::CheckSum;
+ msgDecodedPayloadIndex = 0;
+ }
+ 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;
+ }
+}
+
+void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, const unsigned char* msgPayload) {
+ unsigned int timeStamp = 0;
+ switch(static_cast<short>(command)) {
+ 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];
+ unsigned char range = msgPayload[3];
+ unsigned char resolutionBits = msgPayload[4];
+ unsigned short samplingFrequency = BUILD_UINT16(msgPayload[6], msgPayload[5]);
+ unsigned short nbSamples = BUILD_UINT16(msgPayload[8], msgPayload[7]);
+
+ int lengthPerSample = nbChannels * resolutionBits / 8 + 4;
+ double accelMaxValue = pow(2, resolutionBits)/2;
+ double gyroMaxValue = pow(2, resolutionBits) / 2;
+ double gyroRange = 250.0; //Hardcode pour le moment
+ 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);
+
+ 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 << std::endl;
+ }
+ break;
+ case IMUFileWriter::SensorType::Accel:
+ break;
+ case IMUFileWriter::SensorType::Gyro:
+ break;
+ case IMUFileWriter::SensorType::Mag:
+ break;
+ case IMUFileWriter::SensorType::Temperature:
+ break;
+ case IMUFileWriter::SensorType::Pressure:
+ break;
+ case IMUFileWriter::SensorType::Light:
+ break;
+ default:
+ break;
+ }
+ } 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): {
+ IMUFileWriter::SensorType sensorType = static_cast<SensorType>(msgPayload[0]);
+ unsigned char id = msgPayload[1];
+ unsigned char nbChannels = msgPayload[2];
+ float rangeScale = GetFloatSafe(msgPayload, 3);
+ unsigned char resolutionBits = msgPayload[7];
+ float samplingFrequency = GetFloatSafe(msgPayload, 8);
+ unsigned short nbSamples = msgPayload[12];
+
+ int lengthPerSample = nbChannels * resolutionBits / 8 + 4;
+ double dataMaxValue = std::pow(2, resolutionBits) / 2.0;
+
+ for(int i = 0; i < nbSamples && size >= static_cast<size_t>(lengthPerSample * i + 13); i++) {
+ uint32_t timeStamp = BUILD_UINT32(msgPayload[13 + i * lengthPerSample+3],msgPayload[13 + i * lengthPerSample+2],msgPayload[13 + i * lengthPerSample+1],msgPayload[13 + i * lengthPerSample]);
+ switch(sensorType) {
+ case IMUFileWriter::SensorType::Accel:
+ if (lastAccelTimeStamp >= 500000000)
+ lastAccelTimeStamp = 0;
+ if (timeStamp > lastAccelTimeStamp) {
+ lastAccelTimeStamp = timeStamp;
+ outfile << "ACCEL, " << timeStamp / 1000.0;
+ outfile << ", " << BUILD_INT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
+ outfile << ", " << BUILD_INT16(msgPayload[17 + size+ i * lengthPerSample],msgPayload[17 +size+ i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
+ outfile << ", " << BUILD_INT16(msgPayload[17 + 2*size+ i * lengthPerSample],msgPayload[17 +2*size+ i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
+ outfile << std::endl;
+ }
+ else {
+ //printf("TS Accel Error\n");
+ }
+ break;
+ case IMUFileWriter::SensorType::Gyro:
+ if (lastGyroTimeStamp >= 500000000)
+ lastGyroTimeStamp = 0;
+ if (timeStamp > lastGyroTimeStamp) {
+ lastGyroTimeStamp = timeStamp;
+ outfile << "GYRO, " << timeStamp;
+ outfile << ", " << BUILD_INT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
+ outfile << ", " << BUILD_INT16(msgPayload[17 + size+ i * lengthPerSample],msgPayload[17 +size+ i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
+ outfile << ", " << BUILD_INT16(msgPayload[17 + 2*size+ i * lengthPerSample],msgPayload[17 +2*size+ i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
+ outfile << std::endl;
+ }
+ else {
+ //printf("TS Gyro Error\n");
+ }
+ break;
+ case IMUFileWriter::SensorType::Mag:
+ if (lastMagTimeStamp >= 500000000)
+ lastMagTimeStamp = 0;
+ if (timeStamp > lastMagTimeStamp) {
+ lastMagTimeStamp = timeStamp;
+ outfile << "MAG, " << timeStamp;
+ outfile << ", " << BUILD_INT16(msgPayload[17 + i * lengthPerSample+1],msgPayload[17 + i * lengthPerSample]) * ( rangeScale / dataMaxValue);
+ outfile << ", " << BUILD_INT16(msgPayload[17 + size+ i * lengthPerSample+1],msgPayload[17 +size+ i * lengthPerSample]) * ( rangeScale / dataMaxValue);
+ outfile << ", " << BUILD_INT16(msgPayload[17 + 2*size+ i * lengthPerSample+1],msgPayload[17 +2*size+ i * lengthPerSample]) * ( rangeScale / dataMaxValue);
+ outfile << std::endl;
+ }
+ else {
+ //printf("TS Mag Error\n");
+ }
+ break;
+ case IMUFileWriter::SensorType::Temperature:
+ if (lastTemperatureTimeStamp >= 500000000)
+ lastTemperatureTimeStamp = 0;
+ if (timeStamp > lastTemperatureTimeStamp) {
+ lastTemperatureTimeStamp = timeStamp;
+ outfile << "TEMP, " << timeStamp;
+ outfile << ", " << GetFloatSafe(msgPayload,17 + i * lengthPerSample);
outfile << std::endl;
}
else {
- //printf("TS Accel Error\n");
+ //printf("TS Temperature Error\n");
}
- break;
- case IMUFileWriter::SensorType::Gyro:
- if (lastGyroTimeStamp >= 500000000)
- lastGyroTimeStamp = 0;
- if (timeStamp > lastGyroTimeStamp) {
- lastGyroTimeStamp = timeStamp;
- outfile << "GYRO, " << timeStamp;
- outfile << ", " << BUILD_INT16(imu_data_cur[17 + i * lengthPerSample],imu_data_cur[17 + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
- outfile << ", " << BUILD_INT16(imu_data_cur[17 + size+ i * lengthPerSample],imu_data_cur[17 +size+ i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
- outfile << ", " << BUILD_INT16(imu_data_cur[17 + 2*size+ i * lengthPerSample],imu_data_cur[17 +2*size+ i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
+ break;
+ case IMUFileWriter::SensorType::Pressure:
+ if (lastPressureTimeStamp >= 500000000)
+ lastPressureTimeStamp = 0;
+ if (timeStamp > lastPressureTimeStamp) {
+ lastPressureTimeStamp = timeStamp;
+ outfile << "PRESSURE, " << timeStamp;
+ outfile << ", " << IMUFileWriter::GetFloatSafe(msgPayload,17 + i * lengthPerSample);
outfile << std::endl;
}
else {
- //printf("TS Gyro Error\n");
+
}
- break;
- case IMUFileWriter::SensorType::Mag:
- if (lastMagTimeStamp >= 500000000)
- lastMagTimeStamp = 0;
- if (timeStamp > lastMagTimeStamp) {
- lastMagTimeStamp = timeStamp;
- outfile << "MAG, " << timeStamp;
- outfile << ", " << BUILD_INT16(imu_data_cur[17 + i * lengthPerSample+1],imu_data_cur[17 + i * lengthPerSample]) * ( rangeScale / dataMaxValue);
- outfile << ", " << BUILD_INT16(imu_data_cur[17 + size+ i * lengthPerSample+1],imu_data_cur[17 +size+ i * lengthPerSample]) * ( rangeScale / dataMaxValue);
- outfile << ", " << BUILD_INT16(imu_data_cur[17 + 2*size+ i * lengthPerSample+1],imu_data_cur[17 +2*size+ i * lengthPerSample]) * ( rangeScale / dataMaxValue);
+ break;
+ case IMUFileWriter::SensorType::Light:
+ if (lastLightTimeStamp >= 500000000)
+ lastLightTimeStamp = 0;
+ if (timeStamp > lastLightTimeStamp) {
+ lastLightTimeStamp = timeStamp;
+ outfile << "LIGHT, " << timeStamp;
+ outfile << ", " << BUILD_UINT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]);
+ outfile << ", " << BUILD_UINT16(msgPayload[17 + size+i * lengthPerSample],msgPayload[17 +size+ i * lengthPerSample+1]);
outfile << std::endl;
}
else {
- //printf("TS Mag Error\n");
+ //printf("TS Light Error\n");
}
- break;
- case IMUFileWriter::SensorType::Temperature:
- if (lastTemperatureTimeStamp >= 500000000)
- lastTemperatureTimeStamp = 0;
- if (timeStamp > lastTemperatureTimeStamp) {
- lastTemperatureTimeStamp = timeStamp;
- outfile << "TEMP, " << timeStamp;
- outfile << ", " << GetFloatSafe(imu_data_cur,17 + i * lengthPerSample);
+ break;
+ case IMUFileWriter::SensorType::Unknown:
+ outfile << "UNKNOWN, " << timeStamp;
outfile << std::endl;
- }
- else {
- //printf("TS Temperature Error\n");
- }
- break;
- case IMUFileWriter::SensorType::Pressure:
- if (lastPressureTimeStamp >= 500000000)
- lastPressureTimeStamp = 0;
- if (timeStamp > lastPressureTimeStamp) {
- lastPressureTimeStamp = timeStamp;
- outfile << "PRESSURE, " << timeStamp;
- outfile << ", " << IMUFileWriter::GetFloatSafe(imu_data_cur,17 + i * lengthPerSample);
- outfile << std::endl;
- }
- else {
+ default:
+ break;
+ }
+ }
+ }
+ break;
+ case static_cast<short>(GPS_DATA_PACKET): {
+ IMUFileWriter::GPSDatas gpsDatas;
+ unsigned short ms = BUILD_UINT16(payload[3], payload[4]);
+ if(ms > 999) {
+ ms = 0;
+ }
- }
- break;
- case IMUFileWriter::SensorType::Light:
- if (lastLightTimeStamp >= 500000000)
- lastLightTimeStamp = 0;
- if (timeStamp > lastLightTimeStamp) {
- lastLightTimeStamp = timeStamp;
- outfile << "LIGHT, " << timeStamp;
- outfile << ", " << BUILD_UINT16(imu_data_cur[17 + i * lengthPerSample],imu_data_cur[17 + i * lengthPerSample+1]);
- outfile << ", " << BUILD_UINT16(imu_data_cur[17 + size+i * lengthPerSample],imu_data_cur[17 +size+ i * lengthPerSample+1]);
- outfile << std::endl;
- }
- else {
- //printf("TS Light Error\n");
- }
- break;
- case IMUFileWriter::SensorType::Unknown:
- outfile << "UNKNOWN, " << timeStamp;
+ gps.Datas.dateOfFix.year = payload[7];
+ gpsDatas.dateOfFix.month = payload[6];
+ gpsDatas.dateOfFix.day = payload[5];
+ gpsDatas.dateOfFix.hour = payload[0];
+ gpsDatas.dateOfFix.minute = payload[1];
+ gpsDatas.dateOfFix.second = payload[2];
+
+ gpsDatas.fix = payload[8] != 0;
+ gpsDatas.fixQuality = payload[9];
+ gpsDatas.latitude = GetFloatSafe(payload.data(), 10);
+ gpsDatas.latitudeDirection = static_cast<char>(payload[14]);
+ gpsDatas.longitude = GetFloatSafe(payload.data(), 15);
+ gpsDatas.longitudeDirection = static_cast<char>(payload[19]);
+ gpsDatas.speed = GetFloatSafe(payload.data(), 20);
+ gpsDatas.angle = GetFloatSafe(payload.data(), 24);
+ gpsDatas.altitude = GetFloatSafe(payload.data(), 28);
+ gpsDatas.satellites = payload[32];
+ gpsDatas.antenna = payload[33];
+
+ if (lastGPSDate.year != gpsDatas.dateOfFix.year ||lastGPSDate.month != gpsDatas.dateOfFix.month ||lastGPSDate.day != gpsDatas.dateOfFix.day ||
+ lastGPSDate.hour!=gpsDatas.dateOfFix.hour || lastGPSDate.minute!=gpsDatas.dateOfFix.minute || lastGPSDate.second!=gpsDatas.dateOfFix.second): {
+ lastGPSDate = gpsDatas.dateOfFix;
+ outfile << "GPS, " << gpsDatas.dateOfFix.year;
+ outfile << "/" << gpsDatas.dateOfFix.month;
+ outfile << "/" << gpsDatas.dateOfFix.day;
+ outfile << " " << gpsDatas.dateOfFix.hour;
+ outfile << ":" << gpsDatas.dateOfFix.minute;
+ outfile << ":" << gpsDatas.dateOfFix.second;
+ outfile << " fix:" << gpsDatas.fix;
+ outfile << ", fixQual:" << gpsDatas.fixQuality;
+ outfile << ", Lat:" << gpsDatas.latitude;
+ outfile << " " << gpsDatas.latitudeDirection;
+ outfile << ", Lon:" << gpsDatas.longitude;
+ outfile << " " << gpsDatas.longitudeDirection;
+ outfile << ", speed:" << gpsDatas.speed;
+ outfile << ", ang:" << gpsDatas.angle;
+ outfile << ", alt:" << gpsDatas.altitude;
+ outfile << ", sat:" << gpsDatas.satellites;
+ outfile << std::endl;
+ }
+ }
+ break;
+ case static_cast<short>(GPS_PPS_PACKET): {
+ uint64_t PPSTimeStamp = BUILD_UINT64(payload[7], payload[6], payload[5], payload[4],
+ payload[3], payload[2], payload[1], payload[0]);
+
+ PPSTimeStamp *= 10; // Convert to nanoseconds (assuming internal clock frequency is 100MHz)
+
+ if(PPSTimeStamp>lastPPSTimeStampNS): {
+ lastPPSTimeStampNS = PPSTimeStamp;
+ outfile << "PPS: " << PPSTimeStamp;
+ outfile << std::endl;
+ }
+ }
+ break;
+ default: break;
+ }
+}
+
+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){
+ softwareMajorRev=sample[5];
+ softwareMinorRev=sample[6];
+
+ if(softwareMajorRev>=2) {
+ //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
+ enteteSize=16;
+
+ for(int i=enteteSize;i<hdr.sizeOfAdditionnalDataBuffer-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)){
+ // 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;
- default:
- break;
+ }
+ imu_data_cur += frame_size;
}
}
imu_data_cur += frame_size;
diff --git a/src/filewriter.h b/src/filewriter.h
index 317b54591fa0f5fa2b6e2503ddc3bb1b4db40b55..7d03242ab768c09548c6bacbfa4f4f3a3875b25e 100644
--- a/src/filewriter.h
+++ b/src/filewriter.h
@@ -12,6 +12,12 @@
#include <string>
#include <vector>
#include <fstream>
+#include <iostream>
+#include <cstdio>
+#define HS_DATA_PACKET_FULL_TIMESTAMP 0x0A0A
+#define HS_DATA_PACKET_FULL_TIMESTAMP_V2 0x0A0C
+#define GPS_DATA_PACKET 0x0A0D
+#define GPS_PPS_PACKET 0x0A0E
/** Abstract base class for writing sample data to files.
@@ -151,7 +157,8 @@ private:
Temperature = 4,
Pressure = 5,
Light = 6,
- Piezo = 7
+ Piezo = 7,
+ IMU = 8
};
struct DateTime {
unsigned short year;
@@ -162,6 +169,21 @@ private:
unsigned char minute;
unsigned char second;
};
+ struct GPSDatas {
+ DateTime dateOfFix;
+ bool fix;
+ unsigned char fixQuality;
+ double latitude;
+ char latitudeDirection;
+ double longitude;
+ char longitudeDirection;
+ double speed;
+ double angle;
+ double altitude;
+ unsigned char satellites;
+ unsigned char antenna;
+ };
+
std::ofstream outfile;
size_t last_timestamp;
unsigned int lastAccelTimeStamp;
@@ -173,6 +195,27 @@ private:
unsigned int lastTimeStamp;
DateTime lastGPSDate;
double lastPPSTimeStampNS;
+
+ enum class StateReception {
+ Waiting;
+ FunctionMSB;
+ FunctionLSB;
+ PayloadLengthMSB;
+ PayloadLengthLSB;
+ Payload;
+ CheckSum;
+ };
+
+ StateReception rcvState;
+ int msgDecodedFunction;
+ int msgDecodedPayloadLength;
+ unsigned char *msgDecodedPayload;
+ int msgDecodedPayloadIndex;
+ unsigned int msgDecoded;
+
+ void ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
+ const unsigned char* msgPayload);
+ float GetFloatSafe(const unsigned char *p, int index);
public:
/** Instantiates a splitted wave file writer.
* \param[in] filename_template The name of the file to write to. Will be
@@ -189,7 +232,12 @@ public:
* \param[in] samples_per_file The target number of samples (per channel)
* that will be written to a file before starting the next one.
*/
+ unsigned char CalculateChecksum(int msgFunction,
+ int msgPayloadLength, unsigned char msgPayload[]);
+ void DecodeMessage(unsigned char c);
+
IMUFileWriter(std::string &filename_template, size_t num_channels, size_t sample_rate, size_t depth, size_t timestamp);
+ ~IMUFileWriter();
void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override;
size_t get_last_timestamp();
float GetFloatSafe(const unsigned char *p, int index);