diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index ebfb5aa2..34ce9d46 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -184,7 +184,8 @@ ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSi } ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { - ReturnValue_t result = returnvalue::OK; + ReturnValue_t result; + ReturnValue_t status = returnvalue::OK; // arrayprinter::print(packet, ImtqReplies::BASE_LEN); if (requestStep == imtq::RequestType::MEASURE) { requestStep = imtq::RequestType::ACTUATE; @@ -194,6 +195,8 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply); if (result == returnvalue::OK) { fillEngHkDataset(hkDatasetNoTorque, engHkReply); + } else { + status = result; } } @@ -202,6 +205,8 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint result = parseStatusByte(imtq::CC::GET_SYSTEM_STATE, sysStateReply); if (result == returnvalue::OK) { fillSystemStateIntoDataset(sysStateReply); + } else { + status = result; } } if (replies.wasGetRawMgmMeasurementRead()) { @@ -209,6 +214,8 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement); if (result == returnvalue::OK) { fillRawMtmDataset(rawMgmMeasurement); + } else { + status = result; } } @@ -217,6 +224,8 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement); if (result == returnvalue::OK) { fillRawMtmDataset(calibMgmMeasurement); + } else { + status = result; } } } else { @@ -230,6 +239,8 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement); if (result == returnvalue::OK) { fillRawMtmDataset(rawMgmMeasurement); + } else { + status = result; } } if (replies.wasEngHkRead()) { @@ -237,12 +248,13 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply); if (result != returnvalue::OK) { return result; + } else { + status = result; } fillEngHkDataset(hkDatasetNoTorque, engHkReply); } - return returnvalue::FAILED; } - return returnvalue::OK; + return status; } LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {