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;