From a6ce8977d0f266c75ea6d89d01c7afe51c7b10e8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phil=C3=A9mon=20Pr=C3=A9vot?= <philemon.prevot@lis-lab.fr>
Date: Tue, 3 Sep 2024 14:47:18 +0200
Subject: [PATCH] Fix Accel timestamp and GPS print static_casting
---
src/filewriter.cpp | 32 +++++++++++++-------------------
src/filewriter.h | 2 +-
src/recorder.cpp | 1 -
3 files changed, 14 insertions(+), 21 deletions(-)
diff --git a/src/filewriter.cpp b/src/filewriter.cpp
index cf19123..561b777 100644
--- a/src/filewriter.cpp
+++ b/src/filewriter.cpp
@@ -166,7 +166,6 @@ void IMUFileWriter::DecodeMessage(unsigned char c) {
break;
case StateReception::PayloadLengthMSB:
msgDecodedPayloadLength = static_cast<uint16_t>(c << 8);
- std::cerr << "Message function : " << std::hex << static_cast<uint16_t>(msgDecodedFunction) << std::endl;
rcvState = StateReception::PayloadLengthLSB;
break;
case StateReception::PayloadLengthLSB:
@@ -211,9 +210,6 @@ void IMUFileWriter::DecodeMessage(unsigned char c) {
rcvState = StateReception::Waiting;
break;
}
- if(rcvState != StateReception::Waiting) {
- std::cerr << "State : " << static_cast<int>(rcvState) << std::endl;
- }
}
void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, const unsigned char* msgPayload) {
@@ -298,7 +294,7 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
lastAccelTimeStamp = 0;
if (timeStamp > lastAccelTimeStamp) {
lastAccelTimeStamp = timeStamp;
- outfile << "ACCEL, " << timeStamp / 1000.0;
+ outfile << "ACCEL, " << timeStamp;
outfile << ", " << BUILD_INT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + 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 );
@@ -361,7 +357,7 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
outfile << std::endl;
}
else {
-
+ //printf("TS Pressure Error\n");
}
break;
case IMUFileWriter::SensorType::Light:
@@ -416,14 +412,14 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
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;
- outfile << "/" << gpsDatas.dateOfFix.day;
- outfile << " " << gpsDatas.dateOfFix.hour;
- outfile << ":" << gpsDatas.dateOfFix.minute;
- outfile << ":" << gpsDatas.dateOfFix.second;
+ outfile << "GPS, " << static_cast<int>(gpsDatas.dateOfFix.year);
+ outfile << "/" << static_cast<int>(gpsDatas.dateOfFix.month);
+ outfile << "/" << static_cast<int>(gpsDatas.dateOfFix.day);
+ outfile << " " << static_cast<int>(gpsDatas.dateOfFix.hour);
+ outfile << ":" << static_cast<int>(gpsDatas.dateOfFix.minute);
+ outfile << ":" << static_cast<int>(gpsDatas.dateOfFix.second);
outfile << " fix:" << gpsDatas.fix;
- outfile << ", fixQual:" << gpsDatas.fixQuality;
+ outfile << ", fixQual:" << static_cast<int>(gpsDatas.fixQuality);
outfile << ", Lat:" << gpsDatas.latitude;
outfile << " " << gpsDatas.latitudeDirection;
outfile << ", Lon:" << gpsDatas.longitude;
@@ -431,7 +427,7 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
outfile << ", speed:" << gpsDatas.speed;
outfile << ", ang:" << gpsDatas.angle;
outfile << ", alt:" << gpsDatas.altitude;
- outfile << ", sat:" << gpsDatas.satellites;
+ outfile << ", sat:" << static_cast<int>(gpsDatas.satellites);
outfile << std::endl;
}
}
@@ -457,12 +453,10 @@ void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) {
uint8_t *imu_data_cur(imu_data);
uint8_t softwareMajorRev=imu_data[0];
- uint8_t softwareMinorRev=imu_data[6];
- std::cerr << "sMR" << static_cast<int>(imu_data[0]) << std::endl;
- std::cerr << "Size : " << size << std::endl;
+ uint8_t softwareMinorRev=imu_data[1];
- if(softwareMajorRev > 2) {
- for(int i=1; i<size-enteteSize; i++)
+ if(softwareMajorRev >= 2) {
+ for(int i=1; i<size-1; i++)
{
DecodeMessage(imu_data[i]);
}
diff --git a/src/filewriter.h b/src/filewriter.h
index a702af1..06554b9 100644
--- a/src/filewriter.h
+++ b/src/filewriter.h
@@ -190,7 +190,7 @@ private:
unsigned int lastGyroTimeStamp;
unsigned int lastMagTimeStamp;
unsigned int lastLightTimeStamp;
- unsigned int lastPressureTimeStamp;
+ unsigned int lastPressureTimeStamp = 0;
unsigned int lastTemperatureTimeStamp;
unsigned int lastTimeStamp;
DateTime lastGPSDate;
diff --git a/src/recorder.cpp b/src/recorder.cpp
index 9ee4080..5e149c1 100644
--- a/src/recorder.cpp
+++ b/src/recorder.cpp
@@ -178,7 +178,6 @@ void JasonRecorder::get_samples(std::vector<std::uint8_t> &samples, std::vector<
size_t start = this->additional_data_size + 6;
imu_data.resize(0);
imu_data.reserve(this->additional_data_size);
- std::cerr << "softMajorRev : " << std::dec << &buffer[4] << std::endl;
imu_data.insert(imu_data.begin(), &buffer[6], &buffer[start]);
size_t num_samples = (received - start);
num_samples = (num_samples / (num_channels * this->depth)) * num_channels * this->depth;
--
GitLab