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
Branches
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 @@
#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];
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.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]);
}
......
src/filewriter.h
+ 8
− 9
View file @ c54e21e6
......@@ -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
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment