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

bugfixes imtq
Some checks are pending
EIVE/eive-obsw/pipeline/pr-develop Build started...
Details

Browse Source
This commit is contained in:
Robin Müller
2023-03-02 14:42:42 +01:00
parent 6104bf7adb
commit 14188a4f3b
5 changed files with 24 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
mission/devices/ImtqHandler.cpp
View File

@ -312,7 +312,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(rawMgmMeasurement);
fillRawMtmDataset(rawMtmNoTorque, rawMgmMeasurement);
} else {
status = result;
}
@ -323,7 +323,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(calibMgmMeasurement);
fillCalibratedMtmDataset(calibMgmMeasurement);
} else {
status = result;
}
@ -345,7 +345,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(rawMgmMeasurement);
fillRawMtmDataset(rawMtmWithTorque, rawMgmMeasurement);
} else {
status = result;
}
@ -361,7 +361,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
} else {
status = result;
}
fillEngHkDataset(hkDatasetNoTorque, engHkReply);
fillEngHkDataset(hkDatasetWithTorque, engHkReply);
if (firstReplyCycle) {
firstReplyCycle = false;
}
@ -847,8 +847,11 @@ void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
}
}
void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) {
PoolReadGuard rg(&rawMtmNoTorque);
void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet) {
PoolReadGuard rg(&set);
if(rg.getReadResult() != returnvalue::OK) {
sif::error << "ImtqHandler::fillRawMtmDataset: Lock failure" << std::endl;
}
unsigned int offset = 2;
size_t deSerLen = 16;
const uint8_t* dataStart = packet + offset;
@ -876,18 +879,18 @@ void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) {
if (res != returnvalue::OK) {
return;
}
rawMtmNoTorque.mtmRawNt[0] = xRaw * 7.5;
rawMtmNoTorque.mtmRawNt[1] = yRaw * 7.5;
rawMtmNoTorque.mtmRawNt[2] = zRaw * 7.5;
rawMtmNoTorque.coilActuationStatus = static_cast<uint8_t>(coilActStatus);
rawMtmNoTorque.setValidity(true, true);
set.mtmRawNt[0] = static_cast<float>(xRaw) * 7.5;
set.mtmRawNt[1] = static_cast<float>(yRaw) * 7.5;
set.mtmRawNt[2] = static_cast<float>(zRaw) * 7.5;
set.coilActuationStatus = static_cast<uint8_t>(coilActStatus);
set.setValidity(true, true);
if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::info << "IMTQ raw MTM measurement X: " << rawMtmNoTorque.mtmRawNt[0] << " nT" << std::endl;
sif::info << "IMTQ raw MTM measurement Y: " << rawMtmNoTorque.mtmRawNt[1] << " nT" << std::endl;
sif::info << "IMTQ raw MTM measurement Z: " << rawMtmNoTorque.mtmRawNt[2] << " nT" << std::endl;
sif::info << "IMTQ raw MTM measurement X: " << set.mtmRawNt[0] << " nT" << std::endl;
sif::info << "IMTQ raw MTM measurement Y: " << set.mtmRawNt[1] << " nT" << std::endl;
sif::info << "IMTQ raw MTM measurement Z: " << set.mtmRawNt[2] << " nT" << std::endl;
sif::info << "IMTQ coil actuation status during MTM measurement: "
<< (unsigned int)rawMtmNoTorque.coilActuationStatus.value << std::endl;
<< (unsigned int)set.coilActuationStatus.value << std::endl;
#endif
}
}