diff --git a/src/filewriter.cpp b/src/filewriter.cpp
index c97ba448489e0264ca49009b64b10d0213f313f7..3fbbec53c41df7f02310a76cd47f39d349697ca6 100644
--- a/src/filewriter.cpp
+++ b/src/filewriter.cpp
@@ -7,7 +7,6 @@
#include "filewriter.h"
#include "macros.h"
-#include "decoder.h"
#include <stdexcept>
#include <vector>
#include <iostream>
@@ -137,12 +136,6 @@ IMUFileWriter::IMUFileWriter(std::string &filename_template, size_t num_channels
outfile << header;
}
-IMUFileWriter::~IMUFileWriter() {
- if (msgDecodedPayload) {
- free(msgDecodedPayload);
- }
-}
-
inline float le16tof(uint8_t *array){
return static_cast<float>(static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t*>(array))));
}
@@ -242,7 +235,7 @@ void IMUFileWriter::DecodeMessage(unsigned char c) {
void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, const unsigned char* msgPayload) {
unsigned int timeStamp = 0;
- switch(static_cast<short>(command)) {
+ switch(static_cast<short>(msgFunction)) {
case static_cast<short>(HS_DATA_PACKET_FULL_TIMESTAMP): {
/*
IMUFileWriter::SensorType sensorType = static_cast<SensorType>(msgPayload[0]);
@@ -309,11 +302,12 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
unsigned char resolutionBits = msgPayload[7];
float samplingFrequency = GetFloatSafe(msgPayload, 8);
unsigned short nbSamples = msgPayload[12];
+ unsigned char dataSize = (resolutionBits / 8);
int lengthPerSample = nbChannels * resolutionBits / 8 + 4;
double dataMaxValue = std::pow(2, resolutionBits) / 2.0;
- for(int i = 0; i < nbSamples && size >= static_cast<size_t>(lengthPerSample * i + 13); i++) {
+ for(int i = 0; i < nbSamples && msgPayloadLength >= static_cast<size_t>(lengthPerSample * i + 13); i++) {
uint32_t timeStamp = BUILD_UINT32(msgPayload[13 + i * lengthPerSample+3],msgPayload[13 + i * lengthPerSample+2],msgPayload[13 + i * lengthPerSample+1],msgPayload[13 + i * lengthPerSample]);
switch(sensorType) {
case IMUFileWriter::SensorType::Accel:
@@ -323,8 +317,8 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
lastAccelTimeStamp = timeStamp;
outfile << "ACCEL, " << timeStamp / 1000.0;
outfile << ", " << BUILD_INT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
- outfile << ", " << BUILD_INT16(msgPayload[17 + size+ i * lengthPerSample],msgPayload[17 +size+ i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
- outfile << ", " << BUILD_INT16(msgPayload[17 + 2*size+ i * lengthPerSample],msgPayload[17 +2*size+ i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
+ outfile << ", " << BUILD_INT16(msgPayload[17 + dataSize + i * lengthPerSample],msgPayload[17 +dataSize + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
+ outfile << ", " << BUILD_INT16(msgPayload[17 + 2*dataSize + i * lengthPerSample],msgPayload[17 +2*dataSize + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
outfile << std::endl;
}
else {
@@ -338,8 +332,8 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
lastGyroTimeStamp = timeStamp;
outfile << "GYRO, " << timeStamp;
outfile << ", " << BUILD_INT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
- outfile << ", " << BUILD_INT16(msgPayload[17 + size+ i * lengthPerSample],msgPayload[17 +size+ i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
- outfile << ", " << BUILD_INT16(msgPayload[17 + 2*size+ i * lengthPerSample],msgPayload[17 +2*size+ i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
+ outfile << ", " << BUILD_INT16(msgPayload[17 + dataSize + i * lengthPerSample],msgPayload[17 +dataSize + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
+ outfile << ", " << BUILD_INT16(msgPayload[17 + 2*dataSize + i * lengthPerSample],msgPayload[17 +2*dataSize + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
outfile << std::endl;
}
else {
@@ -353,8 +347,8 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
lastMagTimeStamp = timeStamp;
outfile << "MAG, " << timeStamp;
outfile << ", " << BUILD_INT16(msgPayload[17 + i * lengthPerSample+1],msgPayload[17 + i * lengthPerSample]) * ( rangeScale / dataMaxValue);
- outfile << ", " << BUILD_INT16(msgPayload[17 + size+ i * lengthPerSample+1],msgPayload[17 +size+ i * lengthPerSample]) * ( rangeScale / dataMaxValue);
- outfile << ", " << BUILD_INT16(msgPayload[17 + 2*size+ i * lengthPerSample+1],msgPayload[17 +2*size+ i * lengthPerSample]) * ( rangeScale / dataMaxValue);
+ outfile << ", " << BUILD_INT16(msgPayload[17 + dataSize + i * lengthPerSample+1],msgPayload[17 +dataSize + i * lengthPerSample]) * ( rangeScale / dataMaxValue);
+ outfile << ", " << BUILD_INT16(msgPayload[17 + 2*dataSize + i * lengthPerSample+1],msgPayload[17 +2*dataSize + i * lengthPerSample]) * ( rangeScale / dataMaxValue);
outfile << std::endl;
}
else {
@@ -394,7 +388,7 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
lastLightTimeStamp = timeStamp;
outfile << "LIGHT, " << timeStamp;
outfile << ", " << BUILD_UINT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]);
- outfile << ", " << BUILD_UINT16(msgPayload[17 + size+i * lengthPerSample],msgPayload[17 +size+ i * lengthPerSample+1]);
+ outfile << ", " << BUILD_UINT16(msgPayload[17 + dataSize+i * lengthPerSample],msgPayload[17 +dataSize+ i * lengthPerSample+1]);
outfile << std::endl;
}
else {
@@ -412,32 +406,32 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
break;
case static_cast<short>(GPS_DATA_PACKET): {
IMUFileWriter::GPSDatas gpsDatas;
- unsigned short ms = BUILD_UINT16(payload[3], payload[4]);
+ unsigned short ms = BUILD_UINT16(msgPayload[3], msgPayload[4]);
if(ms > 999) {
ms = 0;
}
- gps.Datas.dateOfFix.year = payload[7];
- gpsDatas.dateOfFix.month = payload[6];
- gpsDatas.dateOfFix.day = payload[5];
- gpsDatas.dateOfFix.hour = payload[0];
- gpsDatas.dateOfFix.minute = payload[1];
- gpsDatas.dateOfFix.second = payload[2];
-
- gpsDatas.fix = payload[8] != 0;
- gpsDatas.fixQuality = payload[9];
- gpsDatas.latitude = GetFloatSafe(payload.data(), 10);
- gpsDatas.latitudeDirection = static_cast<char>(payload[14]);
- gpsDatas.longitude = GetFloatSafe(payload.data(), 15);
- gpsDatas.longitudeDirection = static_cast<char>(payload[19]);
- gpsDatas.speed = GetFloatSafe(payload.data(), 20);
- gpsDatas.angle = GetFloatSafe(payload.data(), 24);
- gpsDatas.altitude = GetFloatSafe(payload.data(), 28);
- gpsDatas.satellites = payload[32];
- gpsDatas.antenna = payload[33];
-
- if (lastGPSDate.year != gpsDatas.dateOfFix.year ||lastGPSDate.month != gpsDatas.dateOfFix.month ||lastGPSDate.day != gpsDatas.dateOfFix.day ||
- lastGPSDate.hour!=gpsDatas.dateOfFix.hour || lastGPSDate.minute!=gpsDatas.dateOfFix.minute || lastGPSDate.second!=gpsDatas.dateOfFix.second): {
+ gpsDatas.dateOfFix.year = msgPayload[7];
+ gpsDatas.dateOfFix.month = msgPayload[6];
+ gpsDatas.dateOfFix.day = msgPayload[5];
+ gpsDatas.dateOfFix.hour = msgPayload[0];
+ gpsDatas.dateOfFix.minute = msgPayload[1];
+ gpsDatas.dateOfFix.second = msgPayload[2];
+
+ gpsDatas.fix = msgPayload[8] != 0;
+ gpsDatas.fixQuality = msgPayload[9];
+ gpsDatas.latitude = GetFloatSafe(msgPayload, 10);
+ gpsDatas.latitudeDirection = static_cast<char>(msgPayload[14]);
+ gpsDatas.longitude = GetFloatSafe(msgPayload, 15);
+ gpsDatas.longitudeDirection = static_cast<char>(msgPayload[19]);
+ gpsDatas.speed = GetFloatSafe(msgPayload, 20);
+ gpsDatas.angle = GetFloatSafe(msgPayload, 24);
+ gpsDatas.altitude = GetFloatSafe(msgPayload, 28);
+ gpsDatas.satellites = msgPayload[32];
+ gpsDatas.antenna = msgPayload[33];
+
+ if (lastGPSDate.year != gpsDatas.dateOfFix.year || lastGPSDate.month != gpsDatas.dateOfFix.month ||lastGPSDate.day != gpsDatas.dateOfFix.day ||
+ lastGPSDate.hour!=gpsDatas.dateOfFix.hour || lastGPSDate.minute!=gpsDatas.dateOfFix.minute || lastGPSDate.second!=gpsDatas.dateOfFix.second) {
lastGPSDate = gpsDatas.dateOfFix;
outfile << "GPS, " << gpsDatas.dateOfFix.year;
outfile << "/" << gpsDatas.dateOfFix.month;
@@ -460,12 +454,12 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
}
break;
case static_cast<short>(GPS_PPS_PACKET): {
- uint64_t PPSTimeStamp = BUILD_UINT64(payload[7], payload[6], payload[5], payload[4],
- payload[3], payload[2], payload[1], payload[0]);
+ uint64_t PPSTimeStamp = BUILD_UINT64(msgPayload[7], msgPayload[6], msgPayload[5], msgPayload[4],
+ msgPayload[3], msgPayload[2], msgPayload[1], msgPayload[0]);
PPSTimeStamp *= 10; // Convert to nanoseconds (assuming internal clock frequency is 100MHz)
- if(PPSTimeStamp>lastPPSTimeStampNS): {
+ if(PPSTimeStamp>lastPPSTimeStampNS) {
lastPPSTimeStampNS = PPSTimeStamp;
outfile << "PPS: " << PPSTimeStamp;
outfile << std::endl;
@@ -480,12 +474,12 @@ 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){
- softwareMajorRev=sample[5];
- softwareMinorRev=sample[6];
+ uint8_t softwareMajorRev=sample[5];
+ uint8_t softwareMinorRev=sample[6];
if(softwareMajorRev>=2) {
//On recupere l'instant de fin du paquet courant
- timeStamp100MHzCurrentPacket=BUILD_UINT64(sample[14],
+ uint64_t timeStamp100MHzCurrentPacket=BUILD_UINT64(sample[14],
sample[13],
sample[12],
sample[11],
@@ -494,9 +488,9 @@ void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) {
sample[8],
sample[7]);
timeStamp100MHzCurrentPacket*=10; //To put the ts in ns
- enteteSize=16;
+ uint8_t enteteSize=16;
- for(int i=enteteSize;i<hdr.sizeOfAdditionnalDataBuffer-enteteSize; i++)
+ for(int i=enteteSize;i<size-enteteSize; i++)
{
DecodeMessage(sample[i]);
}
diff --git a/src/filewriter.h b/src/filewriter.h
index 7d03242ab768c09548c6bacbfa4f4f3a3875b25e..6beb3675cd55674cbe712e697cafd276bfe3687f 100644
--- a/src/filewriter.h
+++ b/src/filewriter.h
@@ -197,13 +197,13 @@ private:
double lastPPSTimeStampNS;
enum class StateReception {
- Waiting;
- FunctionMSB;
- FunctionLSB;
- PayloadLengthMSB;
- PayloadLengthLSB;
- Payload;
- CheckSum;
+ Waiting,
+ FunctionMSB,
+ FunctionLSB,
+ PayloadLengthMSB,
+ PayloadLengthLSB,
+ Payload,
+ CheckSum
};
StateReception rcvState;
@@ -233,14 +233,13 @@ public:
* that will be written to a file before starting the next one.
*/
unsigned char CalculateChecksum(int msgFunction,
- int msgPayloadLength, unsigned char msgPayload[]);
+ int msgPayloadLength, const unsigned char msgPayload[]);
void DecodeMessage(unsigned char c);
IMUFileWriter(std::string &filename_template, size_t num_channels, size_t sample_rate, size_t depth, size_t timestamp);
~IMUFileWriter();
void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override;
size_t get_last_timestamp();
- float GetFloatSafe(const unsigned char *p, int index);
};
/** Class for writing sample data to a sequence of PCM WAVE files, split up to