From 10621fc34efcef8f667ff4cc52183d1ce08ba7e1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phil=C3=A9mon=20Pr=C3=A9vot?= <philemon.prevot@gmail.com>
Date: Thu, 29 Aug 2024 06:41:46 +0200
Subject: [PATCH] Modify IMUFileWriter::write to match log2wav decoding
---
src/filewriter.cpp | 139 ++++++++++++++++++++++++++++++++++++++-------
src/filewriter.h | 29 +++++++++-
2 files changed, 145 insertions(+), 23 deletions(-)
diff --git a/src/filewriter.cpp b/src/filewriter.cpp
index d761514..26748bb 100644
--- a/src/filewriter.cpp
+++ b/src/filewriter.cpp
@@ -12,6 +12,8 @@
#include <chrono>
#include <sstream>
#include <iomanip>
+#include <cstring>
+#include <cmath>
FileWriter::FileWriter(std::string &filename_template, size_t num_channels, size_t sample_rate, size_t depth) :
filename_template(filename_template), num_channels(num_channels), sample_rate(sample_rate), depth(depth) {
@@ -130,31 +132,126 @@ inline float le16tof(uint8_t *array){
return static_cast<float>(static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t*>(array))));
}
+float IMUFileWriter::GetFloatSafe(const unsigned char *p, int index) {
+ unsigned char tmp[4];
+ std::memcpy(tmp, p + index, 4); // Copy 4 bytes from p + index into tmp
+
+ float result;
+ std::memcpy(&result, tmp, sizeof(result)); // Copy bytes from tmp into result
+
+ return result;
+}
+
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){
- 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;
+ 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;
+
+ // A CONVERTIR EN C++
+ 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 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]) * ( range / dataMaxValue );
+ outfile << ", " << BUILD_INT16(imu_data_cur[17 + datasize+ i * lengthPerSample],imu_data_cur[17 +datasize+ i * lengthPerSample+1]) * ( range / dataMaxValue );
+ outfile << ", " << BUILD_INT16(imu_data_cur[17 + 2*datasize+ i * lengthPerSample],imu_data_cur[17 +2*datasize+ i * lengthPerSample+1]) * ( range / dataMaxValue );
+ outfile << std::endl;
+ }
+ else {
+ //printf("TS Accel Error\n");
+ }
+ break;
+ case 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]) * ( range / dataMaxValue);
+ outfile << ", " << BUILD_INT16(imu_data_cur[17 + datasize+ i * lengthPerSample],imu_data_cur[17 +datasize+ i * lengthPerSample+1]) * ( range / dataMaxValue);
+ outfile << ", " << BUILD_INT16(imu_data_cur[17 + 2*datasize+ i * lengthPerSample],imu_data_cur[17 +2*datasize+ i * lengthPerSample+1]) * ( range / dataMaxValue);
+ outfile << std::endl;
+ }
+ else {
+ //printf("TS Gyro Error\n");
+ }
+ break;
+ case 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]) * ( range / dataMaxValue);
+ outfile << ", " << BUILD_INT16(imu_data_cur[17 + datasize+ i * lengthPerSample+1],imu_data_cur[17 +datasize+ i * lengthPerSample]) * ( range / dataMaxValue);
+ outfile << ", " << BUILD_INT16(imu_data_cur[17 + 2*datasize+ i * lengthPerSample+1],imu_data_cur[17 +2*datasize+ i * lengthPerSample]) * ( range / dataMaxValue);
+ outfile << std::endl;
+ }
+ else {
+ //printf("TS Mag Error\n");
+ }
+ break;
+ case Temperature:
+ if (lastTemperatureTimeStamp >= 500000000)
+ lastTemperatureTimeStamp = 0;
+ if (timeStamp > lastTemperatureTimeStamp) {
+ lastTemperatureTimeStamp = timeStamp;
+ outfile << "TEMP, " << timeStamp;
+ outfile << ", " << GetFloatSafe(imu_data_cur,17 + i * lengthPerSample);
+ outfile << std::endl;
+ }
+ else {
+ //printf("TS Temperature Error\n");
+ }
+ break;
+ case Pressure:
+ if (lastPressureTimeStamp >= 500000000)
+ lastPressureTimeStamp = 0;
+ if (timeStamp > lastPressureTimeStamp) {
+ lastPressureTimeStamp = timeStamp;
+ outfile << "PRESSURE, " << timeStamp;
+ outfile << ", " << GetFloatSafe(imu_data_cur,17 + i * lengthPerSample);
+ outfile << std::endl;
+ }
+ else {
+
+ }
+ break;
+ case Light:
+ if (lastLightTimeStamp >= 500000000)
+ lastLightTimeStamp = 0;
+ if (timeStamp > lastLightTimeStamp) {
+ lastLightTimeStamp = timeStamp;
+ outfile << "LIGHT, " << dataLight.timeStamp;
+ outfile << ", " << BUILD_UINT16(imu_data_cur[17 + i * lengthPerSample],imu_data_cur[17 + i * lengthPerSample+1]);
+ outfile << ", " << BUILD_UINT16(imu_data_cur[17 + datasize+i * lengthPerSample],imu_data_cur[17 +datasize+ i * lengthPerSample+1]);
+ outfile << std::endl;
+ }
+ else {
+ //printf("TS Light Error\n");
+ }
+ break;
+ default:
+ break;
+ }
}
- imu_data_cur += frame_size;
+ //
}
}
diff --git a/src/filewriter.h b/src/filewriter.h
index f96639e..f9ae59c 100644
--- a/src/filewriter.h
+++ b/src/filewriter.h
@@ -13,6 +13,7 @@
#include <vector>
#include <fstream>
+
/** Abstract base class for writing sample data to files.
*/
class FileWriter {
@@ -137,12 +138,34 @@ public:
class IMUFileWriter: public FileWriter {
- const std::string header = "Timestamp,ax,ay,az,gx,gy,gz,mx,my,mz\n";
+ // const std::string header = "Timestamp,ax,ay,az,gx,gy,gz,mx,my,mz\n";
+ const std::string header = "Sensor Type,TimeStamp(ms) or Time, val0,val1,val2,val3,val4,val5,val6,val7"
const size_t frame_size = 32;
- const size_t additional_data_size = 730;
+ const size_t additional_data_size = 736;
private:
std::ofstream outfile;
size_t last_timestamp;
+ unsigned int lastAccelTimeStamp;
+ unsigned int lastGyroTimeStamp;
+ unsigned int lastMagTimeStamp;
+ unsigned int lastLightTimeStamp;
+ unsigned int lastPressureTimeStamp;
+ unsigned int lastTemperatureTimeStamp;
+ unsigned int lastTimeStamp;
+ DateTime lastGPSDate;
+ double lastPPSTimeStampNS;
+ /** Struct to identify sensor type id in the additionnal data samples.
+ */
+ enum class SensorType {
+ Unknown = 0,
+ Accel = 1,
+ Gyro = 2,
+ Mag = 3,
+ Temperature = 4,
+ Pressure = 5,
+ Light = 6,
+ Piezo = 7
+ };
public:
/** Instantiates a splitted wave file writer.
* \param[in] filename_template The name of the file to write to. Will be
@@ -162,6 +185,7 @@ public:
IMUFileWriter(std::string &filename_template, size_t num_channels, size_t sample_rate, size_t depth, size_t timestamp);
void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override;
size_t get_last_timestamp();
+ float GetFloatSafe(unsigned char *p, int index);
};
/** Class for writing sample data to a sequence of PCM WAVE files, split up to
@@ -198,4 +222,5 @@ public:
void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override;
};
+
#endif // FILEWRITER_H
--
GitLab