eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

fixes for IMTQ mgm reading
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller
2022-08-18 18:18:44 +02:00
parent 3d0d10dc32
commit 726e448af9
3 changed files with 33 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
mission/devices/IMTQHandler.cpp
View File

@ -778,22 +778,37 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) {
PoolReadGuard rg(&rawMtmMeasurementSet);
int8_t offset = 2;
rawMtmMeasurementSet.mtmRawNt[0] = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
rawMtmMeasurementSet.mtmRawNt[1] = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
rawMtmMeasurementSet.mtmRawNt[2] = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
rawMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24) |
(*(packet + offset + 2) << 16) |
(*(packet + offset + 1) << 8) | (*(packet + offset));
unsigned int offset = 2;
size_t deSerLen = 16;
const uint8_t* dataStart = packet + offset;
int32_t xRaw = 0;
int32_t yRaw = 0;
int32_t zRaw = 0;
uint32_t coilActStatus = 0;
auto res = SerializeAdapter::deSerialize(&xRaw, &dataStart, &deSerLen,
SerializeIF::Endianness::LITTLE);
if(res != HasReturnvaluesIF::RETURN_OK) {
return;
}
res = SerializeAdapter::deSerialize(&yRaw, &dataStart, &deSerLen,
SerializeIF::Endianness::LITTLE);
if(res != HasReturnvaluesIF::RETURN_OK) {
return;
}
res = SerializeAdapter::deSerialize(&zRaw, &dataStart, &deSerLen,
SerializeIF::Endianness::LITTLE);
if(res != HasReturnvaluesIF::RETURN_OK) {
return;
}
res = SerializeAdapter::deSerialize(&coilActStatus, &dataStart, &deSerLen,
SerializeIF::Endianness::LITTLE);
if(res != HasReturnvaluesIF::RETURN_OK) {
return;
}
rawMtmMeasurementSet.mtmRawNt[0] = xRaw * 7.5;
rawMtmMeasurementSet.mtmRawNt[1] = yRaw * 7.5;
rawMtmMeasurementSet.mtmRawNt[2] = zRaw * 7.5;
rawMtmMeasurementSet.coilActuationStatus = static_cast<uint8_t>(coilActStatus);
rawMtmMeasurementSet.setValidity(true, true);
if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1