started SUS assembly
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
fixes for PCDU code and switcher list handling
This commit is contained in:
@ -1,12 +1,11 @@
|
||||
#include "AcsBoardAssembly.h"
|
||||
|
||||
#include <devices/powerSwitcherList.h>
|
||||
#include <fsfw/power/PowerSwitchIF.h>
|
||||
#include <fsfw/serviceinterface.h>
|
||||
|
||||
AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId,
|
||||
PowerSwitchIF* switcher, AcsBoardHelper helper)
|
||||
: AssemblyBase(objectId, parentId), switcher(switcher), helper(helper) {
|
||||
: AssemblyBase(objectId, parentId), pwrSwitcher(switcher), helper(helper) {
|
||||
if (switcher == nullptr) {
|
||||
sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid Power Switcher "
|
||||
"IF passed"
|
||||
@ -26,34 +25,31 @@ AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId,
|
||||
|
||||
ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
if (currentMode == mode and submode == currentSubmode) {
|
||||
return result;
|
||||
}
|
||||
refreshHelperModes();
|
||||
if (state == States::MODE_COMMANDING) {
|
||||
if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
|
||||
powerStateMachine(submode);
|
||||
powerStateMachine(mode, submode);
|
||||
if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
|
||||
if (state == States::MODE_COMMANDING) {
|
||||
handleNormalOrOnModeCmd(mode, submode);
|
||||
} else {
|
||||
modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::GYRO_1_A].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::GYRO_3_B].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::MGM_0_A].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::MGM_0_A].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::MGM_1_A].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::MGM_1_A].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::MGM_2_B].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::MGM_2_B].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::MGM_3_B].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::GPS].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
|
||||
}
|
||||
} else {
|
||||
modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::GYRO_1_A].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::GYRO_3_B].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::MGM_0_A].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::MGM_0_A].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::MGM_1_A].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::MGM_1_A].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::MGM_2_B].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::MGM_2_B].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::MGM_3_B].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::GPS].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
|
||||
}
|
||||
|
||||
HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
|
||||
@ -63,25 +59,25 @@ 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
|
||||
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
|
||||
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
|
||||
} 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.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or
|
||||
helper.gpsMode != wantedMode) {
|
||||
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
|
||||
}
|
||||
@ -102,7 +98,7 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
Mode_t tgtMode = DeviceHandlerIF::MODE_NORMAL;
|
||||
auto cmdSeq = [&](object_id_t objectId, ModeTableIdx tableIdx) {
|
||||
if(tgtMode == DeviceHandlerIF::MODE_NORMAL) {
|
||||
if (tgtMode == DeviceHandlerIF::MODE_NORMAL) {
|
||||
if (isUseable(objectId, mode)) {
|
||||
if (helper.gyro0SideAMode != MODE_OFF) {
|
||||
modeTable[tableIdx].setMode(tgtMode);
|
||||
@ -113,7 +109,7 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
|
||||
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
||||
}
|
||||
}
|
||||
} else if(tgtMode == MODE_ON) {
|
||||
} else if (tgtMode == MODE_ON) {
|
||||
if (isUseable(objectId, mode)) {
|
||||
modeTable[tableIdx].setMode(MODE_ON);
|
||||
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
||||
@ -172,62 +168,82 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
|
||||
return result;
|
||||
}
|
||||
|
||||
void AcsBoardAssembly::powerStateMachine(Submode_t submode) {
|
||||
ReturnValue_t switchStateA = switcher->getSwitchState(pcduSwitches::ACS_BOARD_SIDE_A);
|
||||
ReturnValue_t switchStateB = switcher->getSwitchState(pcduSwitches::ACS_BOARD_SIDE_B);
|
||||
switch (submode) {
|
||||
case (A_SIDE): {
|
||||
if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_OFF) {
|
||||
void AcsBoardAssembly::powerStateMachine(Mode_t mode, Submode_t submode) {
|
||||
ReturnValue_t switchStateA = RETURN_OK;
|
||||
ReturnValue_t switchStateB = RETURN_OK;
|
||||
if (state == States::IDLE or state == States::SWITCHING_POWER) {
|
||||
switchStateA = pwrSwitcher->getSwitchState(SWITCH_A);
|
||||
switchStateB = pwrSwitcher->getSwitchState(SWITCH_B);
|
||||
if (mode == MODE_OFF) {
|
||||
if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) {
|
||||
state = States::MODE_COMMANDING;
|
||||
return;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case (B_SIDE): {
|
||||
if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_ON) {
|
||||
state = States::MODE_COMMANDING;
|
||||
return;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case (DUAL_MODE): {
|
||||
if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_ON) {
|
||||
state = States::MODE_COMMANDING;
|
||||
return;
|
||||
} else {
|
||||
switch (submode) {
|
||||
case (A_SIDE): {
|
||||
if (switchStateA == PowerSwitchIF::SWITCH_ON and
|
||||
switchStateB == PowerSwitchIF::SWITCH_OFF) {
|
||||
state = States::MODE_COMMANDING;
|
||||
return;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case (B_SIDE): {
|
||||
if (switchStateA == PowerSwitchIF::SWITCH_OFF and
|
||||
switchStateB == PowerSwitchIF::SWITCH_ON) {
|
||||
state = States::MODE_COMMANDING;
|
||||
return;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case (DUAL_MODE): {
|
||||
if (switchStateA == PowerSwitchIF::SWITCH_ON and
|
||||
switchStateB == PowerSwitchIF::SWITCH_ON) {
|
||||
state = States::MODE_COMMANDING;
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (state == States::IDLE) {
|
||||
switch (submode) {
|
||||
case (A_SIDE): {
|
||||
if (switchStateA != PowerSwitchIF::SWITCH_ON) {
|
||||
// Set A side on first in power switcher IF
|
||||
switcher->sendSwitchCommand(pcduSwitches::ACS_BOARD_SIDE_A, true);
|
||||
}
|
||||
if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
|
||||
switcher->sendSwitchCommand(pcduSwitches::ACS_BOARD_SIDE_B, false);
|
||||
}
|
||||
break;
|
||||
if (mode == MODE_OFF) {
|
||||
if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
|
||||
pwrSwitcher->sendSwitchCommand(SWITCH_A, false);
|
||||
}
|
||||
case (B_SIDE): {
|
||||
if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
|
||||
// Set A side on first in power switcher IF
|
||||
switcher->sendSwitchCommand(pcduSwitches::ACS_BOARD_SIDE_A, false);
|
||||
}
|
||||
if (switchStateB != PowerSwitchIF::SWITCH_ON) {
|
||||
switcher->sendSwitchCommand(pcduSwitches::ACS_BOARD_SIDE_B, true);
|
||||
}
|
||||
break;
|
||||
if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
|
||||
pwrSwitcher->sendSwitchCommand(SWITCH_B, false);
|
||||
}
|
||||
case (DUAL_MODE): {
|
||||
if (switchStateA != PowerSwitchIF::SWITCH_ON) {
|
||||
// Set A side on first in power switcher IF
|
||||
switcher->sendSwitchCommand(pcduSwitches::ACS_BOARD_SIDE_A, true);
|
||||
} else {
|
||||
switch (submode) {
|
||||
case (A_SIDE): {
|
||||
if (switchStateA != PowerSwitchIF::SWITCH_ON) {
|
||||
pwrSwitcher->sendSwitchCommand(SWITCH_A, true);
|
||||
}
|
||||
if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
|
||||
pwrSwitcher->sendSwitchCommand(SWITCH_B, false);
|
||||
}
|
||||
break;
|
||||
}
|
||||
if (switchStateB != PowerSwitchIF::SWITCH_ON) {
|
||||
switcher->sendSwitchCommand(pcduSwitches::ACS_BOARD_SIDE_B, true);
|
||||
case (B_SIDE): {
|
||||
if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
|
||||
pwrSwitcher->sendSwitchCommand(SWITCH_A, false);
|
||||
}
|
||||
if (switchStateB != PowerSwitchIF::SWITCH_ON) {
|
||||
pwrSwitcher->sendSwitchCommand(SWITCH_B, true);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case (DUAL_MODE): {
|
||||
if (switchStateA != PowerSwitchIF::SWITCH_ON) {
|
||||
pwrSwitcher->sendSwitchCommand(SWITCH_A, true);
|
||||
}
|
||||
if (switchStateB != PowerSwitchIF::SWITCH_ON) {
|
||||
pwrSwitcher->sendSwitchCommand(SWITCH_B, true);
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
state = States::SWITCHING_POWER;
|
||||
@ -274,7 +290,7 @@ ReturnValue_t AcsBoardAssembly::initialize() {
|
||||
}
|
||||
|
||||
ReturnValue_t AcsBoardAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
|
||||
if(submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE) {
|
||||
if (submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE) {
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
@ -296,6 +312,11 @@ bool AcsBoardAssembly::isUseable(object_id_t object, Mode_t mode) {
|
||||
return false;
|
||||
}
|
||||
|
||||
void AcsBoardAssembly::handleModeReached() {
|
||||
AssemblyBase::handleModeReached();
|
||||
state = States::IDLE;
|
||||
}
|
||||
|
||||
void AcsBoardAssembly::refreshHelperModes() {
|
||||
helper.gyro0SideAMode = childrenMap[helper.gyro0AdisIdSideA].mode;
|
||||
helper.gyro1SideAMode = childrenMap[helper.gyro1L3gIdSideA].mode;
|
||||
|
||||
Reference in New Issue
Block a user