diff --git a/src/filewriter.cpp b/src/filewriter.cpp
index 26748bbc519c24ccc9c46b90e8f86a82937a6837..bce7c0763835ca3c9220605393de221f3fc59968 100644
--- a/src/filewriter.cpp
+++ b/src/filewriter.cpp
@@ -6,6 +6,7 @@
*/
#include "filewriter.h"
+#include "Macros.h"
#include <stdexcept>
#include <vector>
#include <iostream>
@@ -146,7 +147,7 @@ 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){
- SensorType sensorType = static_cast<SensorType>(imu_data_cur[0]);
+ 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);
@@ -157,57 +158,56 @@ void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) {
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:
+ 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]) * ( 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 << ", " << 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 );
outfile << std::endl;
}
else {
//printf("TS Accel Error\n");
}
break;
- case Gyro:
+ 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]) * ( 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 << ", " << 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);
outfile << std::endl;
}
else {
//printf("TS Gyro Error\n");
}
break;
- case Mag:
+ 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]) * ( 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 << ", " << 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);
outfile << std::endl;
}
else {
//printf("TS Mag Error\n");
}
break;
- case Temperature:
+ case IMUFileWriter::SensorType::Temperature:
if (lastTemperatureTimeStamp >= 500000000)
lastTemperatureTimeStamp = 0;
if (timeStamp > lastTemperatureTimeStamp) {
@@ -220,33 +220,36 @@ void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) {
//printf("TS Temperature Error\n");
}
break;
- case Pressure:
+ case IMUFileWriter::SensorType::Pressure:
if (lastPressureTimeStamp >= 500000000)
lastPressureTimeStamp = 0;
if (timeStamp > lastPressureTimeStamp) {
lastPressureTimeStamp = timeStamp;
outfile << "PRESSURE, " << timeStamp;
- outfile << ", " << GetFloatSafe(imu_data_cur,17 + i * lengthPerSample);
+ outfile << ", " << IMUFileWriter::GetFloatSafe(imu_data_cur,17 + i * lengthPerSample);
outfile << std::endl;
}
else {
}
break;
- case Light:
+ case IMUFileWriter::SensorType::Light:
if (lastLightTimeStamp >= 500000000)
lastLightTimeStamp = 0;
if (timeStamp > lastLightTimeStamp) {
lastLightTimeStamp = timeStamp;
- outfile << "LIGHT, " << dataLight.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 + datasize+i * lengthPerSample],imu_data_cur[17 +datasize+ 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;
+ outfile << std::endl;
default:
break;
}
diff --git a/src/filewriter.h b/src/filewriter.h
index f9ae59c3e297cee1c3211913c67cba5886af9827..76b22e089a0534a3957684ce5a8c749316196d21 100644
--- a/src/filewriter.h
+++ b/src/filewriter.h
@@ -139,10 +139,29 @@ public:
class IMUFileWriter: public FileWriter {
// 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 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 = 736;
private:
+ enum class SensorType {
+ Unknown = 0,
+ Accel = 1,
+ Gyro = 2,
+ Mag = 3,
+ Temperature = 4,
+ Pressure = 5,
+ Light = 6,
+ Piezo = 7
+ };
+ struct DateTime {
+ unsigned short year;
+ unsigned char month;
+ unsigned char day;
+ unsigned char weekDay;
+ unsigned char hour;
+ unsigned char minute;
+ unsigned char second;
+ };
std::ofstream outfile;
size_t last_timestamp;
unsigned int lastAccelTimeStamp;
@@ -154,18 +173,6 @@ private:
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
@@ -185,7 +192,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);
+ float GetFloatSafe(const unsigned char *p, int index);
};
/** Class for writing sample data to a sequence of PCM WAVE files, split up to