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

Correcting scope resolution error

parent 094425ff
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
+ 25
− 22
View file @ 7cec728e
...@@ -6,6 +6,7 @@ ...@@ -6,6 +6,7 @@
*/ */
#include "filewriter.h" #include "filewriter.h"
#include "Macros.h"
#include <stdexcept> #include <stdexcept>
#include <vector> #include <vector>
#include <iostream> #include <iostream>
...@@ -146,7 +147,7 @@ void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) { ...@@ -146,7 +147,7 @@ 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){
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 id = imu_data_cur[1];
unsigned char nbChannels = imu_data_cur[2]; unsigned char nbChannels = imu_data_cur[2];
float rangeScale = GetFloatSafe(imu_data_cur, 3); float rangeScale = GetFloatSafe(imu_data_cur, 3);
...@@ -157,57 +158,56 @@ void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) { ...@@ -157,57 +158,56 @@ void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) {
int lengthPerSample = nbChannels * resolutionBits / 8 + 4; int lengthPerSample = nbChannels * resolutionBits / 8 + 4;
double dataMaxValue = std::pow(2, resolutionBits) / 2.0; 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++) { 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]); 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 outfile << timeStamp;
switch(sensorType) { switch(sensorType) {
case Accel: case IMUFileWriter::SensorType::Accel:
if (lastAccelTimeStamp >= 500000000) if (lastAccelTimeStamp >= 500000000)
lastAccelTimeStamp = 0; lastAccelTimeStamp = 0;
if (timeStamp > lastAccelTimeStamp) { if (timeStamp > lastAccelTimeStamp) {
lastAccelTimeStamp = timeStamp; lastAccelTimeStamp = timeStamp;
outfile << "ACCEL, " << timeStamp / 1000.0; 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 + i * lengthPerSample],imu_data_cur[17 + i * lengthPerSample+1]) * ( rangeScale / 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 + size+ i * lengthPerSample],imu_data_cur[17 +size+ i * lengthPerSample+1]) * ( rangeScale / 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 + 2*size+ i * lengthPerSample],imu_data_cur[17 +2*size+ i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
outfile << std::endl; outfile << std::endl;
} }
else { else {
//printf("TS Accel Error\n"); //printf("TS Accel Error\n");
} }
break; break;
case Gyro: case IMUFileWriter::SensorType::Gyro:
if (lastGyroTimeStamp >= 500000000) if (lastGyroTimeStamp >= 500000000)
lastGyroTimeStamp = 0; lastGyroTimeStamp = 0;
if (timeStamp > lastGyroTimeStamp) { if (timeStamp > lastGyroTimeStamp) {
lastGyroTimeStamp = timeStamp; lastGyroTimeStamp = timeStamp;
outfile << "GYRO, " << 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 + i * lengthPerSample],imu_data_cur[17 + i * lengthPerSample+1]) * ( rangeScale / 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 + size+ i * lengthPerSample],imu_data_cur[17 +size+ i * lengthPerSample+1]) * ( rangeScale / 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 + 2*size+ i * lengthPerSample],imu_data_cur[17 +2*size+ i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
outfile << std::endl; outfile << std::endl;
} }
else { else {
//printf("TS Gyro Error\n"); //printf("TS Gyro Error\n");
} }
break; break;
case Mag: case IMUFileWriter::SensorType::Mag:
if (lastMagTimeStamp >= 500000000) if (lastMagTimeStamp >= 500000000)
lastMagTimeStamp = 0; lastMagTimeStamp = 0;
if (timeStamp > lastMagTimeStamp) { if (timeStamp > lastMagTimeStamp) {
lastMagTimeStamp = timeStamp; lastMagTimeStamp = timeStamp;
outfile << "MAG, " << 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 + i * lengthPerSample+1],imu_data_cur[17 + i * lengthPerSample]) * ( rangeScale / 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 + size+ i * lengthPerSample+1],imu_data_cur[17 +size+ i * lengthPerSample]) * ( rangeScale / 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 + 2*size+ i * lengthPerSample+1],imu_data_cur[17 +2*size+ i * lengthPerSample]) * ( rangeScale / dataMaxValue);
outfile << std::endl; outfile << std::endl;
} }
else { else {
//printf("TS Mag Error\n"); //printf("TS Mag Error\n");
} }
break; break;
case Temperature: case IMUFileWriter::SensorType::Temperature:
if (lastTemperatureTimeStamp >= 500000000) if (lastTemperatureTimeStamp >= 500000000)
lastTemperatureTimeStamp = 0; lastTemperatureTimeStamp = 0;
if (timeStamp > lastTemperatureTimeStamp) { if (timeStamp > lastTemperatureTimeStamp) {
...@@ -220,33 +220,36 @@ void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) { ...@@ -220,33 +220,36 @@ void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) {
//printf("TS Temperature Error\n"); //printf("TS Temperature Error\n");
} }
break; break;
case Pressure: case IMUFileWriter::SensorType::Pressure:
if (lastPressureTimeStamp >= 500000000) if (lastPressureTimeStamp >= 500000000)
lastPressureTimeStamp = 0; lastPressureTimeStamp = 0;
if (timeStamp > lastPressureTimeStamp) { if (timeStamp > lastPressureTimeStamp) {
lastPressureTimeStamp = timeStamp; lastPressureTimeStamp = timeStamp;
outfile << "PRESSURE, " << timeStamp; outfile << "PRESSURE, " << timeStamp;
outfile << ", " << GetFloatSafe(imu_data_cur,17 + i * lengthPerSample); outfile << ", " << IMUFileWriter::GetFloatSafe(imu_data_cur,17 + i * lengthPerSample);
outfile << std::endl; outfile << std::endl;
} }
else { else {
} }
break; break;
case Light: case IMUFileWriter::SensorType::Light:
if (lastLightTimeStamp >= 500000000) if (lastLightTimeStamp >= 500000000)
lastLightTimeStamp = 0; lastLightTimeStamp = 0;
if (timeStamp > lastLightTimeStamp) { if (timeStamp > lastLightTimeStamp) {
lastLightTimeStamp = timeStamp; 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 + 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; outfile << std::endl;
} }
else { else {
//printf("TS Light Error\n"); //printf("TS Light Error\n");
} }
break; break;
case IMUFileWriter::SensorType::Unknown:
outfile << "UNKNOWN, " << timeStamp;
outfile << std::endl;
default: default:
break; break;
} }
......
src/filewriter.h
+ 21
− 14
View file @ 7cec728e
...@@ -139,10 +139,29 @@ public: ...@@ -139,10 +139,29 @@ 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 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 = 736; const size_t additional_data_size = 736;
private: 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; std::ofstream outfile;
size_t last_timestamp; size_t last_timestamp;
unsigned int lastAccelTimeStamp; unsigned int lastAccelTimeStamp;
...@@ -154,18 +173,6 @@ private: ...@@ -154,18 +173,6 @@ private:
unsigned int lastTimeStamp; unsigned int lastTimeStamp;
DateTime lastGPSDate; DateTime lastGPSDate;
double lastPPSTimeStampNS; 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
...@@ -185,7 +192,7 @@ public: ...@@ -185,7 +192,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); float GetFloatSafe(const 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
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment