Skip to content
Snippets Groups Projects
Commit 10621fc3 authored by Philémon Prévot's avatar Philémon Prévot
Browse files

Modify IMUFileWriter::write to match log2wav decoding

parent fea29986
No related branches found
No related tags found
1 merge request!2HighBlueParser dev branch merged to empty main branch
Show whitespace changes
Inline Side-by-side
src/filewriter.cpp
+ 118
− 21
View file @ 10621fc3
...@@ -12,6 +12,8 @@ ...@@ -12,6 +12,8 @@
#include <chrono> #include <chrono>
#include <sstream> #include <sstream>
#include <iomanip> #include <iomanip>
#include <cstring>
#include <cmath>
FileWriter::FileWriter(std::string &filename_template, size_t num_channels, size_t sample_rate, size_t depth) : 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) { filename_template(filename_template), num_channels(num_channels), sample_rate(sample_rate), depth(depth) {
...@@ -130,31 +132,126 @@ inline float le16tof(uint8_t *array){ ...@@ -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)))); 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) { void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) {
uint8_t *imu_data_cur(imu_data); uint8_t *imu_data_cur(imu_data);
while(imu_data_cur + frame_size + 5 < imu_data + additional_data_size){ 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)){ SensorType sensorType = static_cast<SensorType>(imu_data_cur[0]);
// skip trame if header is incorrect unsigned char id = imu_data_cur[1];
imu_data_cur += frame_size + 5; unsigned char nbChannels = imu_data_cur[2];
continue; float rangeScale = GetFloatSafe(imu_data_cur, 3);
} unsigned char resolutionBits = imu_data_cur[7];
imu_data_cur += 5; // skip frame header float samplingFrequency = GetFloatSafe(imu_data_cur, 8);
auto timestamp = static_cast<int32_t>(__builtin_bswap32(*reinterpret_cast<uint32_t*>(imu_data_cur + 9))); unsigned short nbSamples = imu_data_cur[12];
if (timestamp > last_timestamp) {
last_timestamp = timestamp; int lengthPerSample = nbChannels * resolutionBits / 8 + 4;
outfile << timestamp; double dataMaxValue = std::pow(2, resolutionBits) / 2.0;
outfile << "," << le16tof(imu_data_cur + 13) / 32756 * 19.62; // ax resolution +- 2g
outfile << "," << le16tof(imu_data_cur + 15) / 32756 * 19.62; // ay resolution +- 2g // A CONVERTIR EN C++
outfile << "," << le16tof(imu_data_cur + 17) / 32756 * 19.62; // az resolution +- 2g for(int i = 0; i < nbSamples && size >= static_cast<size_t>(lengthPerSample * i + 13); i++) {
outfile << "," << le16tof(imu_data_cur + 19) / 32756 * 250; // gx resolution +- 255deg/sec 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 << "," << le16tof(imu_data_cur + 21) / 32756 * 250; // gy resolution +- 255deg/sec outfile << timeStamp
outfile << "," << le16tof(imu_data_cur + 23) / 32756 * 250; // gz resolution +- 255deg/sec switch (sensorType) {
outfile << "," << le16tof(imu_data_cur + 25) / 32756 * 4900.; // mx +- 4900µTesla case Accel:
outfile << "," << le16tof(imu_data_cur + 27) / 32756 * (-4900.); // my +- 4900µTesla if (lastAccelTimeStamp >= 500000000)
outfile << "," << le16tof(imu_data_cur + 29) / 32756 * (-4900.); // mz +- 4900µTesla 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; outfile << std::endl;
} }
imu_data_cur += frame_size; 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;
}
}
//
} }
} }
......
src/filewriter.h
+ 27
− 2
View file @ 10621fc3
...@@ -13,6 +13,7 @@ ...@@ -13,6 +13,7 @@
#include <vector> #include <vector>
#include <fstream> #include <fstream>
/** Abstract base class for writing sample data to files. /** Abstract base class for writing sample data to files.
*/ */
class FileWriter { class FileWriter {
...@@ -137,12 +138,34 @@ public: ...@@ -137,12 +138,34 @@ public:
class IMUFileWriter: public FileWriter { 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 frame_size = 32;
const size_t additional_data_size = 730; const size_t additional_data_size = 736;
private: private:
std::ofstream outfile; std::ofstream outfile;
size_t last_timestamp; 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: public:
/** Instantiates a splitted wave file writer. /** Instantiates a splitted wave file writer.
* \param[in] filename_template The name of the file to write to. Will be * \param[in] filename_template The name of the file to write to. Will be
...@@ -162,6 +185,7 @@ public: ...@@ -162,6 +185,7 @@ public:
IMUFileWriter(std::string &filename_template, size_t num_channels, size_t sample_rate, size_t depth, size_t timestamp); 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; void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override;
size_t get_last_timestamp(); 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 /** Class for writing sample data to a sequence of PCM WAVE files, split up to
...@@ -198,4 +222,5 @@ public: ...@@ -198,4 +222,5 @@ public:
void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override; void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override;
}; };
#endif // FILEWRITER_H #endif // FILEWRITER_H
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment