From 191f4b6d0c296980dac641693bd8e1812172c904 Mon Sep 17 00:00:00 2001 From: "Jakob.Meier" Date: Sun, 25 Apr 2021 15:53:44 +0200 Subject: [PATCH] get commanded dipole wip --- .../pollingSequenceFactory.cpp | 3 +- mission/devices/IMTQHandler.cpp | 93 ++++++++++++++----- mission/devices/IMTQHandler.h | 1 - .../IMTQHandlerDefinitions.h | 4 + tmtc | 2 +- 5 files changed, 78 insertions(+), 25 deletions(-) diff --git a/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 02e6b625..f7422786 100644 --- a/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -85,9 +85,10 @@ ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence) thisSequence->addSlot(objects::RTD_IC16, length * 0.4, DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::RTD_IC17, length * 0.4, DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::RTD_IC18, length * 0.4, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE); #endif /* Q7S_ADD_RTD_DEVICES */ + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0.6, DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0.6, DeviceHandlerIF::SEND_READ); diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp index 59f6dd46..80cc1550 100644 --- a/mission/devices/IMTQHandler.cpp +++ b/mission/devices/IMTQHandler.cpp @@ -42,25 +42,31 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand( DeviceCommandId_t deviceCommand, const uint8_t * commandData, size_t commandDataLen) { switch(deviceCommand) { + case(IMTQ::START_ACTUATION_DIPOLE): { + /* IMTQ expects low byte first */ + commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; + commandBuffer[1] = *(commandData + 1); + commandBuffer[2] = *(commandData); + commandBuffer[3] = *(commandData + 3); + commandBuffer[4] = *(commandData + 2); + commandBuffer[5] = *(commandData + 5); + commandBuffer[6] = *(commandData + 4); + commandBuffer[7] = *(commandData + 7); + commandBuffer[8] = *(commandData + 6); + rawPacket = commandBuffer; + rawPacketLen = 9; + return RETURN_OK; + } case(IMTQ::GET_ENG_HK_DATA): { commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA; rawPacket = commandBuffer; rawPacketLen = 1; return RETURN_OK; } - case(IMTQ::START_ACTUATION_DIPOLE): { - /* IMTQ expects low byte first */ - commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; - commandBuffer[1] = *(commandData + 1); - commandBuffer[2] = *(commandData); - commandBuffer[3] = *(commandData + 3); - commandBuffer[4] = *(commandData + 2); - commandBuffer[5] = *(commandData + 5); - commandBuffer[6] = *(commandData + 4); - commandBuffer[7] = *(commandData + 7); - commandBuffer[8] = *(commandData + 6); + case(IMTQ::GET_COMMANDED_DIPOLE): { + commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE; rawPacket = commandBuffer; - rawPacketLen = 9; + rawPacketLen = 1; return RETURN_OK; } default: @@ -70,8 +76,12 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand( } void IMTQHandler::fillCommandAndReplyMap() { + this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr, + IMTQ::SIZE_STATUS_REPLY); this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset, - IMTQ::SIZE_ENG_HK_DATA_REPLY, false, true, IMTQ::SIZE_ENG_HK_DATA_REPLY); + IMTQ::SIZE_ENG_HK_DATA_REPLY); + this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr, + IMTQ::SIZE_ENG_HK_DATA_REPLY); } ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start, @@ -80,10 +90,18 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start, ReturnValue_t result = RETURN_OK; switch(*start) { + case(IMTQ::CC::START_ACTUATION_DIPOLE): + *foundLen = IMTQ::SIZE_STATUS_REPLY; + *foundId = IMTQ::START_ACTUATION_DIPOLE; + break; case(IMTQ::CC::GET_ENG_HK_DATA): *foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY; *foundId = IMTQ::GET_ENG_HK_DATA; break; + case(IMTQ::CC::GET_COMMANDED_DIPOLE): + *foundLen = IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY; + *foundId = IMTQ::GET_COMMANDED_DIPOLE; + break; default: sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl; result = IGNORE_REPLY_DATA; @@ -105,9 +123,15 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, } switch (id) { + case (IMTQ::START_ACTUATION_DIPOLE): + // Replies only the status byte which is already handled with parseStatusByte + break; case (IMTQ::GET_ENG_HK_DATA): fillEngHkDataset(packet); break; + case (IMTQ::GET_COMMANDED_DIPOLE): + handleGetCommandedDipoleReply(packet); + break; default: { sif::debug << "IMTQHandler::interpretDeviceReply: Unknown device reply id" << std::endl; return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; @@ -143,31 +167,29 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat return HasReturnvaluesIF::RETURN_OK; } -size_t DeviceHandlerBase::getNextReplyLength(DeviceCommandId_t commandId){ - DeviceReplyIter iter = deviceReplyMap.find(commandId); - if(iter != deviceReplyMap.end()) { - return iter->second.replyLen; - }else{ - return 0; - } -} - ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) { uint8_t cmdErrorField = *(packet + 1) & 0xF; switch (cmdErrorField) { case 0: return RETURN_OK; case 1: + sif::error << "IMTQHandler::parseStatusByte: Command rejected without reason" << std::endl; return REJECTED_WITHOUT_REASON; case 2: + sif::error << "IMTQHandler::parseStatusByte: Command has invalid command code" << std::endl; return INVALID_COMMAND_CODE; case 3: + sif::error << "IMTQHandler::parseStatusByte: Command has missing parameter" << std::endl; return PARAMETER_MISSING; case 4: + sif::error << "IMTQHandler::parseStatusByte: Command has invalid parameter" << std::endl; return PARAMETER_INVALID; case 5: + sif::error << "IMTQHandler::parseStatusByte: CC unavailable" << std::endl; return CC_UNAVAILABLE; case 7: + sif::error << "IMTQHandler::parseStatusByte: IMQT replied internal processing error" + << std::endl; return INTERNAL_PROCESSING_ERROR; default: sif::error << "IMTQHandler::parseStatusByte: CMD Error field contains unknown error code " @@ -223,3 +245,30 @@ void IMTQHandler::setModeNormal() { mode = MODE_NORMAL; } + + +void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) { + + ReturnValue_t result = RETURN_OK; + + if (wiretappingMode == RAW) { + /* Data already sent in doGetRead() */ + return result; + } + + DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId); + if (iter == deviceReplyMap.end()) { + sif::debug << "IMTQHandler::handleDeviceTM: Unknown reply id" << std::endl; + return; + } + MessageQueueId_t queueId = iter->second.command->second.sendReplyTo; + + if (queueId == NO_COMMANDER) { + return; + } + + result = actionHelper.reportData(queueId, replyId, data, dataSize); + if (result != RETURN_OK) { + sif::debug << "IMTQHandler::handleDeviceTM: Failed to report data" << std::endl; + } +} diff --git a/mission/devices/IMTQHandler.h b/mission/devices/IMTQHandler.h index 21ba770f..2e2a7612 100644 --- a/mission/devices/IMTQHandler.h +++ b/mission/devices/IMTQHandler.h @@ -37,7 +37,6 @@ protected: uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) override; - virtual size_t getNextReplyLength(DeviceCommandId_t commandId) override; private: diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h index 23ff8b9a..60bd511e 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -6,6 +6,7 @@ namespace IMTQ { static const DeviceCommandId_t NONE = 0x0; static const DeviceCommandId_t GET_ENG_HK_DATA = 0x1; static const DeviceCommandId_t START_ACTUATION_DIPOLE = 0x2; + static const DeviceCommandId_t GET_COMMANDED_DIPOLE = 0x3; static const uint8_t GET_TEMP_REPLY_SIZE = 2; static const uint8_t CFGR_CMD_SIZE = 3; @@ -13,7 +14,9 @@ namespace IMTQ { static const uint32_t ENG_HK_DATA_SET_ID = GET_ENG_HK_DATA; static const uint8_t SIZE_ENG_HK_COMMAND = 1; + static const uint8_t SIZE_STATUS_REPLY = 2; static const uint8_t SIZE_ENG_HK_DATA_REPLY = 24; + static const uint8_t SIZE_GET_COMMANDED_DIPOLE_REPLY = 8; static const uint8_t MAX_REPLY_SIZE = SIZE_ENG_HK_DATA_REPLY; static const uint8_t MAX_COMMAND_SIZE = 9; @@ -28,6 +31,7 @@ namespace IMTQ { static const uint8_t START_ACTUATION_DIPOLE = 0x6; static const uint8_t SOFTWARE_RESET = 0xAA; static const uint8_t GET_ENG_HK_DATA = 0x4A; + static const uint8_t GET_COMMANDED_DIPOLE = 0x46; }; enum IMTQPoolIds: lp_id_t { diff --git a/tmtc b/tmtc index 986f88e2..3e466f06 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 986f88e2908c6bc555a279b53f11e82d9a8a851c +Subproject commit 3e466f06ef7737f2f1bab8c3d68feb633da76dbc