From 6e10ccd2d66777587dbe3926ef5d1d900a4b6e5b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 6 Mar 2023 09:17:03 +0100 Subject: [PATCH 1/5] More buffer time --- common/config/eive/definitions.h | 2 +- linux/devices/ImtqPollingTask.cpp | 5 +- linux/devices/ImtqPollingTask.h | 3 +- mission/devices/ImtqHandler.cpp | 32 +++++++++---- mission/devices/ImtqHandler.h | 6 ++- .../devices/devicedefinitions/imtqHelpers.h | 47 ++++++++++--------- 6 files changed, 59 insertions(+), 36 deletions(-) diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h index e3a05a56..2b1292b3 100644 --- a/common/config/eive/definitions.h +++ b/common/config/eive/definitions.h @@ -59,7 +59,7 @@ namespace spiSched { static constexpr uint32_t SCHED_BLOCK_1_SUS_READ_MS = 15; static constexpr uint32_t SCHED_BLOCK_2_SENSOR_READ_MS = 30; -static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42; +static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 43; static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45; static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 55; static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 105; diff --git a/linux/devices/ImtqPollingTask.cpp b/linux/devices/ImtqPollingTask.cpp index 51618981..82093aca 100644 --- a/linux/devices/ImtqPollingTask.cpp +++ b/linux/devices/ImtqPollingTask.cpp @@ -47,6 +47,9 @@ void ImtqPollingTask::handleMeasureStep() { size_t replyLen = 0; uint8_t* replyPtr; ImtqRepliesDefault replies(replyBuf.data()); + // If some startup handling is added later, set configured after it was done once. + replies.setConfigured(); + // Can be used later to verify correct timing (e.g. all data has been read) clearReadFlagsDefault(replies); auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) { @@ -223,7 +226,7 @@ ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) { ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { - const auto* imtqReq = reinterpret_cast(sendData); + const auto* imtqReq = reinterpret_cast(sendData); { MutexGuard mg(ipcLock); if (imtqReq->request == imtq::RequestType::ACTUATE) { diff --git a/linux/devices/ImtqPollingTask.h b/linux/devices/ImtqPollingTask.h index 5ae8f1f9..3b219c57 100644 --- a/linux/devices/ImtqPollingTask.h +++ b/linux/devices/ImtqPollingTask.h @@ -33,13 +33,12 @@ class ImtqPollingTask : public SystemObject, address_t i2cAddr = 0; uint32_t currentIntegrationTimeMs = 10; // Required in addition to integration time, otherwise old data might be read. - static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 3; + static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 5; bool ignoreNextActuateRequest = false; imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE; int16_t dipoles[3] = {}; uint16_t torqueDuration = 0; - // uint8_t startActuateRawBuf[3] = {}; std::array cmdBuf; std::array replyBuf; diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index b76f0575..a673a32e 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -131,12 +131,20 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) { ImtqHandler::~ImtqHandler() = default; void ImtqHandler::doStartUp() { - updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE); - updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE); - if (goToNormalMode) { - setMode(MODE_NORMAL); - } else { - setMode(_MODE_TO_ON); + if (internalState != InternalState::STARTUP) { + commandExecuted = false; + updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE); + updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE); + internalState = InternalState::STARTUP; + } + if (internalState == InternalState::STARTUP) { + if (commandExecuted) { + if (goToNormalMode) { + setMode(MODE_NORMAL); + } else { + setMode(_MODE_TO_ON); + } + } } } @@ -145,6 +153,7 @@ void ImtqHandler::doShutDown() { updatePeriodicReply(false, imtq::cmdIds::REPLY_WITH_TORQUE); specialRequestActive = false; firstReplyCycle = true; + internalState = InternalState::NONE; setMode(_MODE_POWER_DOWN); } @@ -163,7 +172,7 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { request.request = imtq::RequestType::DO_NOTHING; request.specialRequest = imtq::SpecialRequest::NONE; rawPacket = reinterpret_cast(&request); - rawPacketLen = sizeof(ImtqRequest); + rawPacketLen = sizeof(imtq::Request); return returnvalue::OK; } } @@ -183,7 +192,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma expectedReply = imtq::cmdIds::REPLY_NO_TORQUE; specialRequestActive = true; rawPacket = reinterpret_cast(&request); - rawPacketLen = sizeof(ImtqRequest); + rawPacketLen = sizeof(imtq::Request); }; switch (deviceCommand) { case (imtq::cmdIds::POS_X_SELF_TEST): { @@ -219,7 +228,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma request.specialRequest = imtq::SpecialRequest::NONE; expectedReply = imtq::cmdIds::REPLY_NO_TORQUE; rawPacket = reinterpret_cast(&request); - rawPacketLen = sizeof(ImtqRequest); + rawPacketLen = sizeof(imtq::Request); return returnvalue::OK; } case (imtq::cmdIds::START_ACTUATION_DIPOLE): { @@ -258,7 +267,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma torquer::TORQUEING = true; torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value); rawPacket = reinterpret_cast(&request); - rawPacketLen = sizeof(ImtqRequest); + rawPacketLen = sizeof(imtq::Request); return returnvalue::OK; } default: @@ -305,6 +314,9 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) { // sif::debug << "handle measure" << std::endl; ImtqRepliesDefault replies(packet); + if (replies.devWasConfigured() and internalState == InternalState::STARTUP) { + commandExecuted = true; + } if (specialRequestActive) { if (replies.wasSpecialRequestRead()) { uint8_t* specialRequest = replies.getSpecialRequest(); diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h index d579f7f3..dac5d2b0 100644 --- a/mission/devices/ImtqHandler.h +++ b/mission/devices/ImtqHandler.h @@ -83,6 +83,11 @@ class ImtqHandler : public DeviceHandlerBase { //! link between IMTQ and OBC. static const Event INVALID_ERROR_BYTE = MAKE_EVENT(8, severity::LOW); + enum class InternalState { NONE, STARTUP, SHUTDOWN } internalState = InternalState::NONE; + bool commandExecuted = false; + + imtq::Request request{}; + imtq::StatusDataset statusSet; imtq::DipoleActuationSet dipoleSet; imtq::RawMtmMeasurementNoTorque rawMtmNoTorque; @@ -123,7 +128,6 @@ class ImtqHandler : public DeviceHandlerBase { power::Switch_t switcher = power::NO_SWITCH; - ImtqRequest request{}; DeviceCommandId_t expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE; bool goToNormalMode = false; bool debugMode = false; diff --git a/mission/devices/devicedefinitions/imtqHelpers.h b/mission/devices/devicedefinitions/imtqHelpers.h index a066216d..87b69f8b 100644 --- a/mission/devices/devicedefinitions/imtqHelpers.h +++ b/mission/devices/devicedefinitions/imtqHelpers.h @@ -10,18 +10,6 @@ class ImtqHandler; namespace imtq { -enum ComStep : uint8_t { - DHB_OP = 0, - START_MEASURE_SEND = 1, - START_MEASURE_GET = 2, - READ_MEASURE_SEND = 3, - READ_MEASURE_GET = 4, - START_ACTUATE_SEND = 5, - START_ACTUATE_GET = 6, - READ_ACTUATE_SEND = 7, - READ_ACTUATE_GET = 8, -}; - enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE, DO_NOTHING }; enum class SpecialRequest : uint8_t { @@ -35,6 +23,26 @@ enum class SpecialRequest : uint8_t { GET_SELF_TEST_RESULT = 7 }; +struct Request { + imtq::RequestType request = imtq::RequestType::MEASURE_NO_ACTUATION; + imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE; + uint8_t integrationTimeSel = 3; + int16_t dipoles[3]{}; + uint16_t torqueDuration = 0; +}; + +enum ComStep : uint8_t { + DHB_OP = 0, + START_MEASURE_SEND = 1, + START_MEASURE_GET = 2, + READ_MEASURE_SEND = 3, + READ_MEASURE_GET = 4, + START_ACTUATE_SEND = 5, + START_ACTUATE_GET = 6, + READ_ACTUATE_SEND = 7, + READ_ACTUATE_GET = 8, +}; + static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER; static constexpr ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0); @@ -1124,25 +1132,21 @@ class NegZSelfTestSet : public StaticLocalDataSet { } // namespace imtq -struct ImtqRequest { - imtq::RequestType request = imtq::RequestType::MEASURE_NO_ACTUATION; - imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE; - int16_t dipoles[3]{}; - uint16_t torqueDuration = 0; -}; - struct ImtqRepliesDefault { friend class ImtqPollingTask; public: static constexpr size_t BASE_LEN = - imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::SYSTEM_STATE + 1 + + 1 + imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::SYSTEM_STATE + 1 + +imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::RAW_MTM_MEASUREMENT + 1 + imtq::replySize::ENG_HK_DATA_REPLY + 1 + imtq::replySize::CAL_MTM_MEASUREMENT + 1; ImtqRepliesDefault(const uint8_t* rawData) : rawData(const_cast(rawData)) { initPointers(); } + void setConfigured() { rawData[0] = true; } + bool devWasConfigured() const { return rawData[0]; } + uint8_t* getCalibMgmMeasurement() const { return calibMgmMeasurement + 1; } bool wasCalibMgmMeasurementRead() const { return calibMgmMeasurement[0]; }; @@ -1164,7 +1168,7 @@ struct ImtqRepliesDefault { private: void initPointers() { - swReset = rawData; + swReset = rawData + 1; systemState = swReset + imtq::replySize::DEFAULT_MIN_LEN + 1; startMtmMeasurement = systemState + imtq::replySize::SYSTEM_STATE + 1; rawMgmMeasurement = startMtmMeasurement + imtq::replySize::DEFAULT_MIN_LEN + 1; @@ -1172,6 +1176,7 @@ struct ImtqRepliesDefault { calibMgmMeasurement = engHk + imtq::replySize::ENG_HK_DATA_REPLY + 1; specialRequestReply = calibMgmMeasurement + imtq::replySize::CAL_MTM_MEASUREMENT + 1; } + uint8_t* rawData; uint8_t* swReset; uint8_t* systemState; From d18a0e98a557159536a9886e345b129a8baeda6d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 6 Mar 2023 11:02:56 +0100 Subject: [PATCH 2/5] why do we need so much buffer time? --- linux/devices/ImtqPollingTask.cpp | 6 ++++-- linux/devices/ImtqPollingTask.h | 2 +- mission/devices/ImtqHandler.cpp | 15 +++++++++++++-- mission/devices/ImtqHandler.h | 2 +- 4 files changed, 19 insertions(+), 6 deletions(-) diff --git a/linux/devices/ImtqPollingTask.cpp b/linux/devices/ImtqPollingTask.cpp index 82093aca..0148bfe8 100644 --- a/linux/devices/ImtqPollingTask.cpp +++ b/linux/devices/ImtqPollingTask.cpp @@ -138,7 +138,8 @@ void ImtqPollingTask::handleMeasureStep() { if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { return; } - if ((replyPtr[2] >> 7) == 0b1) { + if ((replyPtr[2] >> 7) == 0) { + sif::error << "IMTQ: MGM measurement still too old" << std::endl; replyPtr[0] = false; } } @@ -198,7 +199,8 @@ void ImtqPollingTask::handleActuateStep() { if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { return; } - if ((replyPtr[2] >> 7) == 0b1) { + if ((replyPtr[2] >> 7) == 0) { + sif::error << "IMTQ: MGM measurement still too old" << std::endl; replyPtr[0] = false; } } diff --git a/linux/devices/ImtqPollingTask.h b/linux/devices/ImtqPollingTask.h index 3b219c57..cb2d3882 100644 --- a/linux/devices/ImtqPollingTask.h +++ b/linux/devices/ImtqPollingTask.h @@ -33,7 +33,7 @@ class ImtqPollingTask : public SystemObject, address_t i2cAddr = 0; uint32_t currentIntegrationTimeMs = 10; // Required in addition to integration time, otherwise old data might be read. - static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 5; + static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 6; bool ignoreNextActuateRequest = false; imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE; diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index a673a32e..38b38e71 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -144,6 +144,7 @@ void ImtqHandler::doStartUp() { } else { setMode(_MODE_TO_ON); } + commandExecuted = false; } } } @@ -154,6 +155,7 @@ void ImtqHandler::doShutDown() { specialRequestActive = false; firstReplyCycle = true; internalState = InternalState::NONE; + commandExecuted = false; setMode(_MODE_POWER_DOWN); } @@ -171,6 +173,7 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { *id = imtq::cmdIds::REQUEST; request.request = imtq::RequestType::DO_NOTHING; request.specialRequest = imtq::SpecialRequest::NONE; + expectedReply = DeviceHandlerIF::NO_COMMAND_ID; rawPacket = reinterpret_cast(&request); rawPacketLen = sizeof(imtq::Request); return returnvalue::OK; @@ -180,6 +183,10 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { } ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { + if (internalState == InternalState::STARTUP) { + *id = imtq::cmdIds::REQUEST; + return buildCommandFromCommand(*id, nullptr, 0); + } return NOTHING_TO_SEND; } @@ -307,10 +314,14 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint ReturnValue_t result; ReturnValue_t status = returnvalue::OK; if (getMode() != MODE_NORMAL) { - // Ignore replies during transitions. + if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) { + ImtqRepliesDefault replies(packet); + if (replies.devWasConfigured() and internalState == InternalState::STARTUP) { + commandExecuted = true; + } + } return returnvalue::OK; } - // arrayprinter::print(packet, ImtqReplies::BASE_LEN); if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) { // sif::debug << "handle measure" << std::endl; ImtqRepliesDefault replies(packet); diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h index dac5d2b0..e337f40c 100644 --- a/mission/devices/ImtqHandler.h +++ b/mission/devices/ImtqHandler.h @@ -128,7 +128,7 @@ class ImtqHandler : public DeviceHandlerBase { power::Switch_t switcher = power::NO_SWITCH; - DeviceCommandId_t expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE; + DeviceCommandId_t expectedReply = DeviceHandlerIF::NO_COMMAND_ID; bool goToNormalMode = false; bool debugMode = false; bool specialRequestActive = false; From 193ed01bce3ac8fa1a8337d3daf486e38ebd4ced Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 6 Mar 2023 11:06:38 +0100 Subject: [PATCH 3/5] fix printouts --- mission/devices/ImtqHandler.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index 38b38e71..30fd02e9 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -370,7 +370,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint } if (not replies.wasGetRawMgmMeasurementRead() and not firstReplyCycle) { - sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl; + sif::warning << "IMTQ: Possible timing issue, raw MGM measurement was not read" << std::endl; } uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement(); result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement); @@ -381,7 +381,8 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint } if (not replies.wasCalibMgmMeasurementRead() and not firstReplyCycle) { - sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl; + sif::warning << "IMTQ: Possible timing issue, calib MGM measurement was not read" + << std::endl; } uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement(); result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement); From b48a6e23181ad52592aba03cb80b0948e8bf3bdc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 6 Mar 2023 11:11:35 +0100 Subject: [PATCH 4/5] 24 ms it takes.. --- linux/devices/ImtqPollingTask.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/linux/devices/ImtqPollingTask.cpp b/linux/devices/ImtqPollingTask.cpp index 0148bfe8..ea54b9c2 100644 --- a/linux/devices/ImtqPollingTask.cpp +++ b/linux/devices/ImtqPollingTask.cpp @@ -28,6 +28,7 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) { // Stopwatch watch; switch (currentRequest) { case imtq::RequestType::MEASURE_NO_ACTUATION: { + //Stopwatch watch; handleMeasureStep(); break; } From 4fa5aa6b845db06609e3f6e44feab45390b37d6d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 6 Mar 2023 11:28:25 +0100 Subject: [PATCH 5/5] remove printout and re-polling --- linux/devices/ImtqPollingTask.cpp | 37 +++++++++++++------------------ 1 file changed, 15 insertions(+), 22 deletions(-) diff --git a/linux/devices/ImtqPollingTask.cpp b/linux/devices/ImtqPollingTask.cpp index ea54b9c2..8a2f6b76 100644 --- a/linux/devices/ImtqPollingTask.cpp +++ b/linux/devices/ImtqPollingTask.cpp @@ -28,7 +28,8 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) { // Stopwatch watch; switch (currentRequest) { case imtq::RequestType::MEASURE_NO_ACTUATION: { - //Stopwatch watch; + // Measured to take 24 ms for debug and release builds. + // Stopwatch watch; handleMeasureStep(); break; } @@ -131,18 +132,12 @@ void ImtqPollingTask::handleMeasureStep() { if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { return; } + bool mgmMeasurementTooOld = false; // See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably // have old data. Which can be really bad for ACS. And everything. if ((replyPtr[2] >> 7) == 0) { - sif::error << "IMTQ: MGM measurement too old" << std::endl; - TaskFactory::delayTask(2); - if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { - return; - } - if ((replyPtr[2] >> 7) == 0) { - sif::error << "IMTQ: MGM measurement still too old" << std::endl; - replyPtr[0] = false; - } + replyPtr[0] = false; + mgmMeasurementTooOld = true; } cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA; @@ -154,7 +149,9 @@ void ImtqPollingTask::handleMeasureStep() { if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) { return; } - // sif::debug << "measure done" << std::endl; + if(mgmMeasurementTooOld) { + sif::error << "IMTQ: MGM measurement too old" << std::endl; + } return; } @@ -191,26 +188,22 @@ void ImtqPollingTask::handleActuateStep() { if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { return; } + bool measurementWasTooOld = false; // See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably // have old data. Which can be really bad for ACS. And everything. if ((replyPtr[2] >> 7) == 0) { - sif::error << "IMTQ: MGM measurement too old" << std::endl; - TaskFactory::delayTask(2); - cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT; - if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { - return; - } - if ((replyPtr[2] >> 7) == 0) { - sif::error << "IMTQ: MGM measurement still too old" << std::endl; - replyPtr[0] = false; - } + measurementWasTooOld = true; + replyPtr[0] = false; } cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA; if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) { return; } - // sif::debug << "measure with torque done" << std::endl; + + if(measurementWasTooOld) { + sif::error << "IMTQ: MGM measurement too old" << std::endl; + } return; }