Init commit

mission/assemblies/TestAssembly.cpp

@@ -0,0 +1,192 @@
+/*
+ * TestAssembly.cpp
+ *
+ *  Created on: Apr 20, 2021
+ *      Author: steffen
+ */
+
+#include <mission/assemblies/TestAssembly.h>
+#include <common/config/commonSystemObjects.h>
+#include <fsfw/objectmanager/ObjectManagerIF.h>
+
+
+TestAssembly::TestAssembly(object_id_t objectId, object_id_t parentId):AssemblyBase(objectId, parentId) {
+	ModeListEntry newModeListEntry;
+	newModeListEntry.setObject(objects::TEST_DEVICE_HANDLER_0);
+	newModeListEntry.setMode(MODE_OFF);
+	newModeListEntry.setSubmode(SUBMODE_NONE);
+
+	commandTable.insert(newModeListEntry);
+
+	newModeListEntry.setObject(objects::TEST_DEVICE_HANDLER_1);
+	newModeListEntry.setMode(MODE_OFF);
+	newModeListEntry.setSubmode(SUBMODE_NONE);
+
+	commandTable.insert(newModeListEntry);
+
+}
+
+TestAssembly::~TestAssembly() {
+}
+
+ReturnValue_t TestAssembly::commandChildren(Mode_t mode,
+		Submode_t submode) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::info << "TestAssembly: Received command to go to mode " << mode << " submode " << (int) submode << std::endl;
+#else
+    sif::printInfo("TestAssembly: Received command to go to other mode \n");
+#endif
+	ReturnValue_t result = RETURN_OK;
+	if(mode == MODE_OFF){
+		commandTable[0].setMode(MODE_OFF);
+		commandTable[0].setSubmode(SUBMODE_NONE);
+		commandTable[1].setMode(MODE_OFF);
+		commandTable[1].setSubmode(SUBMODE_NONE);
+	}else if(mode == DeviceHandlerIF::MODE_NORMAL){
+		if(submode == submodes::SINGLE){
+			commandTable[0].setMode(MODE_OFF);
+			commandTable[0].setSubmode(SUBMODE_NONE);
+			commandTable[1].setMode(MODE_OFF);
+			commandTable[1].setSubmode(SUBMODE_NONE);
+			// We try to prefer 0 here but we try to switch to 1 even if it might fail
+			if(isDeviceAvailable(objects::TEST_DEVICE_HANDLER_0)){
+				if (childrenMap[objects::TEST_DEVICE_HANDLER_0].mode == MODE_ON){
+					commandTable[0].setMode(mode);
+					commandTable[0].setSubmode(SUBMODE_NONE);
+				}else{
+					commandTable[0].setMode(MODE_ON);
+					commandTable[0].setSubmode(SUBMODE_NONE);
+				    result = NEED_SECOND_STEP;
+				}
+			}else{
+				if (childrenMap[objects::TEST_DEVICE_HANDLER_1].mode == MODE_ON){
+					commandTable[1].setMode(mode);
+					commandTable[1].setSubmode(SUBMODE_NONE);
+				}else{
+					commandTable[1].setMode(MODE_ON);
+					commandTable[1].setSubmode(SUBMODE_NONE);
+				    result = NEED_SECOND_STEP;
+				}
+			}
+		}else{
+			// Dual Mode Normal
+			if (childrenMap[objects::TEST_DEVICE_HANDLER_0].mode == MODE_ON){
+				commandTable[0].setMode(mode);
+				commandTable[0].setSubmode(SUBMODE_NONE);
+			}else{
+				commandTable[0].setMode(MODE_ON);
+				commandTable[0].setSubmode(SUBMODE_NONE);
+			    result = NEED_SECOND_STEP;
+			}
+			if (childrenMap[objects::TEST_DEVICE_HANDLER_1].mode == MODE_ON){
+				commandTable[1].setMode(mode);
+				commandTable[1].setSubmode(SUBMODE_NONE);
+			}else{
+				commandTable[1].setMode(MODE_ON);
+				commandTable[1].setSubmode(SUBMODE_NONE);
+			    result = NEED_SECOND_STEP;
+			}
+		}
+	}else{
+		//Mode ON
+		if(submode == submodes::SINGLE){
+			commandTable[0].setMode(MODE_OFF);
+			commandTable[0].setSubmode(SUBMODE_NONE);
+			commandTable[1].setMode(MODE_OFF);
+			commandTable[1].setSubmode(SUBMODE_NONE);
+			// We try to prefer 0 here but we try to switch to 1 even if it might fail
+			if(isDeviceAvailable(objects::TEST_DEVICE_HANDLER_0)){
+				commandTable[0].setMode(MODE_ON);
+				commandTable[0].setSubmode(SUBMODE_NONE);
+			}else{
+				commandTable[1].setMode(MODE_ON);
+				commandTable[1].setSubmode(SUBMODE_NONE);
+			}
+		}else{
+			commandTable[0].setMode(MODE_ON);
+			commandTable[0].setSubmode(SUBMODE_NONE);
+			commandTable[1].setMode(MODE_ON);
+			commandTable[1].setSubmode(SUBMODE_NONE);
+		}
+	}
+
+
+
+	HybridIterator<ModeListEntry> iter(commandTable.begin(),
+			commandTable.end());
+	executeTable(iter);
+	return result;
+}
+
+ReturnValue_t TestAssembly::isModeCombinationValid(Mode_t mode,
+		Submode_t submode) {
+	switch (mode) {
+	case MODE_OFF:
+		if (submode == SUBMODE_NONE) {
+			return RETURN_OK;
+		} else {
+			return INVALID_SUBMODE;
+		}
+	case DeviceHandlerIF::MODE_NORMAL:
+	case MODE_ON:
+		if (submode < 3) {
+			return RETURN_OK;
+		} else {
+			return INVALID_SUBMODE;
+		}
+	}
+	return INVALID_MODE;
+}
+
+ReturnValue_t TestAssembly::initialize() {
+	ReturnValue_t result = AssemblyBase::initialize();
+	if(result != RETURN_OK){
+		return result;
+	}
+	handler0 = objectManager->get<TestDevice>(objects::TEST_DEVICE_HANDLER_0);
+	handler1 = objectManager->get<TestDevice>(objects::TEST_DEVICE_HANDLER_1);
+	if((handler0 == nullptr) or (handler1 == nullptr)){
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	handler0->setParentQueue(this->getCommandQueue());
+	handler1->setParentQueue(this->getCommandQueue());
+
+
+	result = registerChild(objects::TEST_DEVICE_HANDLER_0);
+		if (result != HasReturnvaluesIF::RETURN_OK) {
+			return result;
+		}
+	result = registerChild(objects::TEST_DEVICE_HANDLER_1);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	return result;
+}
+
+ReturnValue_t TestAssembly::checkChildrenStateOn(
+		Mode_t wantedMode, Submode_t wantedSubmode) {
+	if(submode == submodes::DUAL){
+		for(const auto& info:childrenMap){
+			if(info.second.mode != wantedMode or info.second.mode != wantedSubmode){
+				return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
+			}
+		}
+		return RETURN_OK;
+	}else if(submode == submodes::SINGLE){
+		for(const auto& info:childrenMap){
+			if(info.second.mode == wantedMode and info.second.mode != wantedSubmode){
+				return RETURN_OK;
+			}
+		}
+	}
+	return INVALID_SUBMODE;
+}
+
+bool TestAssembly::isDeviceAvailable(object_id_t object) {
+	if(healthHelper.healthTable->getHealth(object) == HasHealthIF::HEALTHY){
+		return true;
+	}else{
+		return false;
+	}
+}