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 @@
#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
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;
}
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 @@
#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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment