Skip to content
Snippets Groups Projects
Commit fc8b92c3 authored by Philemon Prevot's avatar Philemon Prevot
Browse files

Variable casting correction

parent cb139bd9
Branches
No related tags found
1 merge request!2HighBlueParser dev branch merged to empty main branch
Hide whitespace changes
Inline Side-by-side
Showing
with 106 additions and 101 deletions
src/filewriter.cpp
+ 106
− 101
View file @ fc8b92c3
......@@ -166,71 +166,75 @@ unsigned char IMUFileWriter::CalculateChecksum(int msgFunction,
}
void IMUFileWriter::DecodeMessage(unsigned char c) {
switch (rcvState) {
case StateReception::Waiting:
if (c == 0xFE)
rcvState = StateReception::FunctionMSB;
break;
case StateReception::FunctionMSB:
msgDecodedFunction = static_cast<int>(c << 8);
rcvState = StateReception::FunctionLSB;
break;
case StateReception::FunctionLSB:
msgDecodedFunction += static_cast<int>(c << 0);
rcvState = StateReception::PayloadLengthMSB;
break;
case StateReception::PayloadLengthMSB:
msgDecodedPayloadLength = static_cast<int>(c << 8);
rcvState = StateReception::PayloadLengthLSB;
break;
case StateReception::PayloadLengthLSB:
msgDecodedPayloadLength += static_cast<int>(c << 0);
if (msgDecodedPayloadLength > 0) {
if (msgDecodedPayloadLength < 1024) {
msgDecodedPayloadIndex = 0;
msgDecodedPayload = static_cast<unsigned char*>(malloc(msgDecodedPayloadLength));
if (msgDecodedPayload == nullptr) {
throw std::bad_alloc(); // Handle memory allocation failure
}
rcvState = StateReception::Payload;
} else {
rcvState = StateReception::Waiting;
}
} else
rcvState = StateReception::CheckSum;
break;
case StateReception::Payload:
if (msgDecodedPayloadIndex > msgDecodedPayloadLength)
{
//Erreur
switch (rcvState) {
case StateReception::Waiting:
if (c == 0xFE)
rcvState = StateReception::FunctionMSB;
break;
case StateReception::FunctionMSB:
msgDecodedFunction = static_cast<int16_t>(c << 8);
rcvState = StateReception::FunctionLSB;
break;
case StateReception::FunctionLSB:
msgDecodedFunction += static_cast<int16_t>(c << 0);
rcvState = StateReception::PayloadLengthMSB;
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:
msgDecodedPayloadLength += static_cast<uint16_t>(c << 0);
if (msgDecodedPayloadLength > 0) {
if (msgDecodedPayloadLength < 1024) {
msgDecodedPayloadIndex = 0;
msgDecodedPayload = static_cast<unsigned char*>(malloc(msgDecodedPayloadLength));
if (msgDecodedPayload == nullptr) {
throw std::bad_alloc(); // Handle memory allocation failure
}
rcvState = StateReception::Payload;
} else {
rcvState = StateReception::Waiting;
}
msgDecodedPayload[msgDecodedPayloadIndex++] = c;
if (msgDecodedPayloadIndex >= msgDecodedPayloadLength)
{
rcvState = StateReception::CheckSum;
msgDecodedPayloadIndex = 0;
}
break;
case StateReception::CheckSum:
} else
rcvState = StateReception::CheckSum;
break;
case StateReception::Payload:
if (msgDecodedPayloadIndex > msgDecodedPayloadLength)
{
unsigned char calculatedChecksum = CalculateChecksum(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
unsigned char receivedChecksum = c;
if (calculatedChecksum == receivedChecksum) {
//Lance l'event de fin de decodage
ProcessDecodedMessage(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
msgDecoded++;
} else {
std::cerr << "Checksum error" << std::endl;
}
//Erreur
msgDecodedPayloadIndex = 0;
rcvState = StateReception::Waiting;
}
msgDecodedPayload[msgDecodedPayloadIndex++] = c;
if (msgDecodedPayloadIndex >= msgDecodedPayloadLength)
{
rcvState = StateReception::CheckSum;
msgDecodedPayloadIndex = 0;
}
break;
default:
rcvState = StateReception::Waiting;
break;
case StateReception::CheckSum:
{
unsigned char calculatedChecksum = CalculateChecksum(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
unsigned char receivedChecksum = c;
if (calculatedChecksum == receivedChecksum) {
//Lance l'event de fin de decodage
ProcessDecodedMessage(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
msgDecoded++;
} else {
std::cerr << "Checksum error" << std::endl;
}
rcvState = StateReception::Waiting;
}
break;
default:
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) {
......@@ -473,56 +477,57 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
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){
uint8_t softwareMajorRev=sample[5];
uint8_t softwareMinorRev=sample[6];
if(softwareMajorRev>=2) {
//On recupere l'instant de fin du paquet courant
uint64_t timeStamp100MHzCurrentPacket=BUILD_UINT64(sample[14],
sample[13],
sample[12],
sample[11],
sample[10],
sample[9],
sample[8],
sample[7]);
timeStamp100MHzCurrentPacket*=10; //To put the ts in ns
uint8_t enteteSize=16;
for(int i=enteteSize;i<size-enteteSize; i++)
{
DecodeMessage(sample[i]);
}
uint8_t softwareMajorRev=sample[5];
uint8_t softwareMinorRev=sample[6];
std::cerr << "sMR" << static_cast<int>(sample[10]) << std::endl;
std::cerr << "Size : " << size << std::endl;
uint64_t timeStamp100MHzCurrentPacket=0;
if(true) {
//On recupere l'instant de fin du paquet courant
timeStamp100MHzCurrentPacket=BUILD_UINT64(sample[14],
sample[13],
sample[12],
sample[11],
sample[10],
sample[9],
sample[8],
sample[7]);
timeStamp100MHzCurrentPacket*=10; //To put the ts in ns
uint8_t enteteSize=16;
outfile << "PACKET TIMESTAMP: " << static_cast<int>(timeStamp100MHzCurrentPacket) << std::endl;
for(int i=enteteSize;i<size-enteteSize; i++)
{
DecodeMessage(sample[i]);
}
else {
uint8_t *imu_data_cur(imu_data);
while(imu_data_cur + frame_size + 5 < imu_data + additional_data_size){
if(!(imu_data_cur[0]==0xFE && imu_data_cur[1]==0x0A && imu_data_cur[2]==0x0A && imu_data_cur[5]==0x08)){
}
else {
uint8_t *imu_data_cur(imu_data);
while(imu_data_cur + frame_size + 5 < imu_data + additional_data_size){
if(!(imu_data_cur[0]==0xFE && imu_data_cur[1]==0x0A && imu_data_cur[2]==0x0A && imu_data_cur[5]==0x08)) {
// skip trame if header is incorrect
imu_data_cur += frame_size + 5;
continue;
}
imu_data_cur += 5; // skip frame header
auto timestamp = static_cast<int32_t>(__builtin_bswap32(*reinterpret_cast<uint32_t*>(imu_data_cur + 9)));
if (timestamp > last_timestamp) {
last_timestamp = timestamp;
outfile << timestamp;
outfile << "," << le16tof(imu_data_cur + 13) / 32756 * 19.62; // ax resolution +- 2g
outfile << "," << le16tof(imu_data_cur + 15) / 32756 * 19.62; // ay resolution +- 2g
outfile << "," << le16tof(imu_data_cur + 17) / 32756 * 19.62; // az resolution +- 2g
outfile << "," << le16tof(imu_data_cur + 19) / 32756 * 250; // gx resolution +- 255deg/sec
outfile << "," << le16tof(imu_data_cur + 21) / 32756 * 250; // gy resolution +- 255deg/sec
outfile << "," << le16tof(imu_data_cur + 23) / 32756 * 250; // gz resolution +- 255deg/sec
outfile << "," << le16tof(imu_data_cur + 25) / 32756 * 4900.; // mx +- 4900µTesla
outfile << "," << le16tof(imu_data_cur + 27) / 32756 * (-4900.); // my +- 4900µTesla
outfile << "," << le16tof(imu_data_cur + 29) / 32756 * (-4900.); // mz +- 4900µTesla
outfile << std::endl;
}
imu_data_cur += frame_size;
}
imu_data_cur += 5; // skip frame header
auto timestamp = static_cast<int32_t>(__builtin_bswap32(*reinterpret_cast<uint32_t*>(imu_data_cur + 9)));
if (timestamp > last_timestamp) {
last_timestamp = timestamp;
outfile << timestamp;
outfile << "," << le16tof(imu_data_cur + 13) / 32756 * 19.62; // ax resolution +- 2g
outfile << "," << le16tof(imu_data_cur + 15) / 32756 * 19.62; // ay resolution +- 2g
outfile << "," << le16tof(imu_data_cur + 17) / 32756 * 19.62; // az resolution +- 2g
outfile << "," << le16tof(imu_data_cur + 19) / 32756 * 250; // gx resolution +- 255deg/sec
outfile << "," << le16tof(imu_data_cur + 21) / 32756 * 250; // gy resolution +- 255deg/sec
outfile << "," << le16tof(imu_data_cur + 23) / 32756 * 250; // gz resolution +- 255deg/sec
outfile << "," << le16tof(imu_data_cur + 25) / 32756 * 4900.; // mx +- 4900µTesla
outfile << "," << le16tof(imu_data_cur + 27) / 32756 * (-4900.); // my +- 4900µTesla
outfile << "," << le16tof(imu_data_cur + 29) / 32756 * (-4900.); // mz +- 4900µTesla
outfile << std::endl;
}
imu_data_cur += frame_size;
}
imu_data_cur += frame_size;
}
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment