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;