get commanded dipole wip

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);
 

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): {
+	case(IMTQ::GET_COMMANDED_DIPOLE): {
-	    /* IMTQ expects low byte first */
+	    commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE;
-	    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;
+		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;
+    }
+}

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:
 

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 {

tmtc

@@ -1 +1 @@
-Subproject commit 986f88e2908c6bc555a279b53f11e82d9a8a851c
+Subproject commit 3e466f06ef7737f2f1bab8c3d68feb633da76dbc