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

Debug for compilation check part 1

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