From c7183b730eaec7371fc2c72ce8aa0341d79ac130 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Mar 2022 10:28:55 +0100 Subject: [PATCH] check children state on implementation --- mission/system/AcsBoardAssembly.cpp | 161 +++++++++++++++++----------- mission/system/AcsBoardAssembly.h | 1 + 2 files changed, 97 insertions(+), 65 deletions(-) diff --git a/mission/system/AcsBoardAssembly.cpp b/mission/system/AcsBoardAssembly.cpp index 49190470..15332901 100644 --- a/mission/system/AcsBoardAssembly.cpp +++ b/mission/system/AcsBoardAssembly.cpp @@ -29,15 +29,7 @@ ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) if (currentMode == mode and submode == currentSubmode) { return result; } - helper.gyro0SideAMode = childrenMap[helper.gyro0AdisIdSideA].mode; - helper.gyro1SideAMode = childrenMap[helper.gyro1L3gIdSideA].mode; - helper.gyro2SideBMode = childrenMap[helper.gyro2AdisIdSideB].mode; - helper.gyro3SideBMode = childrenMap[helper.gyro2AdisIdSideB].mode; - helper.mgm0SideAMode = childrenMap[helper.mgm0Lis3IdSideA].mode; - helper.mgm1SideAMode = childrenMap[helper.mgm1Rm3100IdSideA].mode; - helper.mgm2SideBMode = childrenMap[helper.mgm2Lis3IdSideB].mode; - helper.mgm3SideBMode = childrenMap[helper.mgm3Rm3100IdSideB].mode; - helper.gpsMode = childrenMap[helper.gpsId].mode; + refreshHelperModes(); if (state == States::MODE_COMMANDING) { if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) { powerStateMachine(submode); @@ -70,45 +62,34 @@ ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) } ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) { + refreshHelperModes(); + if(wantedSubmode == A_SIDE) { + if((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode) or + (helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode) or + helper.gpsMode != wantedMode) { + return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; + } + return RETURN_OK; + } else if (wantedSubmode == B_SIDE) { + if((helper.gyro2SideBMode != wantedMode and helper.gyro3SideBMode != wantedMode) or + (helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or + helper.gpsMode != wantedMode) { + return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; + } + return RETURN_OK; + } else if(wantedSubmode == DUAL_MODE) { + if((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode and + helper.gyro2AdisIdSideB != wantedMode and helper.gyro3SideBMode != wantedMode) or + (helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode and + helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or + helper.gpsMode != wantedMode) { + return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; + } + return RETURN_OK; + } return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t AcsBoardAssembly::initialize() { - ReturnValue_t result = registerChild(helper.gyro0AdisIdSideA); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - result = registerChild(helper.gyro1L3gIdSideA); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - result = registerChild(helper.gyro2AdisIdSideB); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - result = registerChild(helper.gyro3L3gIdSideB); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - result = registerChild(helper.mgm0Lis3IdSideA); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - result = registerChild(helper.mgm1Rm3100IdSideA); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - result = registerChild(helper.mgm2Lis3IdSideB); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - result = registerChild(helper.mgm3Rm3100IdSideB); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - return result; -} - void AcsBoardAssembly::initModeTableEntry(object_id_t id, ModeListEntry& entry) { modeTable.insert(entry); entry.setObject(id); @@ -117,26 +98,6 @@ void AcsBoardAssembly::initModeTableEntry(object_id_t id, ModeListEntry& entry) entry.setInheritSubmode(false); } -ReturnValue_t AcsBoardAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) { - return HasReturnvaluesIF::RETURN_OK; -} - -bool AcsBoardAssembly::isUseable(object_id_t object, Mode_t mode) { - if (healthHelper.healthTable->isFaulty(object)) { - return false; - } - - // Check if device is already in target mode - if (childrenMap[object].mode == mode) { - return true; - } - - if (healthHelper.healthTable->isCommandable(object)) { - return true; - } - return false; -} - ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) { ReturnValue_t result = RETURN_OK; Mode_t tgtMode = DeviceHandlerIF::MODE_NORMAL; @@ -275,3 +236,73 @@ void AcsBoardAssembly::powerStateMachine(Submode_t submode) { // TODO: Could check for a timeout (temporal or cycles) here and resend command } } + +ReturnValue_t AcsBoardAssembly::initialize() { + ReturnValue_t result = registerChild(helper.gyro0AdisIdSideA); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = registerChild(helper.gyro1L3gIdSideA); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = registerChild(helper.gyro2AdisIdSideB); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = registerChild(helper.gyro3L3gIdSideB); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = registerChild(helper.mgm0Lis3IdSideA); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = registerChild(helper.mgm1Rm3100IdSideA); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = registerChild(helper.mgm2Lis3IdSideB); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = registerChild(helper.mgm3Rm3100IdSideB); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return result; +} + +ReturnValue_t AcsBoardAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) { + if(submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE) { + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_OK; +} + +bool AcsBoardAssembly::isUseable(object_id_t object, Mode_t mode) { + if (healthHelper.healthTable->isFaulty(object)) { + return false; + } + + // Check if device is already in target mode + if (childrenMap[object].mode == mode) { + return true; + } + + if (healthHelper.healthTable->isCommandable(object)) { + return true; + } + return false; +} + +void AcsBoardAssembly::refreshHelperModes() { + helper.gyro0SideAMode = childrenMap[helper.gyro0AdisIdSideA].mode; + helper.gyro1SideAMode = childrenMap[helper.gyro1L3gIdSideA].mode; + helper.gyro2SideBMode = childrenMap[helper.gyro2AdisIdSideB].mode; + helper.gyro3SideBMode = childrenMap[helper.gyro2AdisIdSideB].mode; + helper.mgm0SideAMode = childrenMap[helper.mgm0Lis3IdSideA].mode; + helper.mgm1SideAMode = childrenMap[helper.mgm1Rm3100IdSideA].mode; + helper.mgm2SideBMode = childrenMap[helper.mgm2Lis3IdSideB].mode; + helper.mgm3SideBMode = childrenMap[helper.mgm3Rm3100IdSideB].mode; +} diff --git a/mission/system/AcsBoardAssembly.h b/mission/system/AcsBoardAssembly.h index 63136393..c1e2a29e 100644 --- a/mission/system/AcsBoardAssembly.h +++ b/mission/system/AcsBoardAssembly.h @@ -93,6 +93,7 @@ class AcsBoardAssembly : public AssemblyBase { bool isUseable(object_id_t object, Mode_t mode); ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode); void powerStateMachine(Submode_t submode); + void refreshHelperModes(); }; #endif /* MISSION_SYSTEM_ACSBOARDASSEMBLY_H_ */