finished most of sus handler
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
This commit is contained in:
parent
a7c1dafce5
commit
0595e29100
@ -86,22 +86,13 @@ ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
void AcsBoardAssembly::initModeTableEntry(object_id_t id, ModeListEntry& entry) {
|
||||
modeTable.insert(entry);
|
||||
entry.setObject(id);
|
||||
entry.setMode(MODE_OFF);
|
||||
entry.setSubmode(SUBMODE_NONE);
|
||||
entry.setInheritSubmode(false);
|
||||
}
|
||||
|
||||
ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
|
||||
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 (isUseable(objectId, mode)) {
|
||||
if (helper.gyro0SideAMode != MODE_OFF) {
|
||||
modeTable[tableIdx].setMode(tgtMode);
|
||||
auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, ModeTableIdx tableIdx) {
|
||||
if (mode == DeviceHandlerIF::MODE_NORMAL) {
|
||||
if (isUseable(objectId, devMode)) {
|
||||
if (mode != MODE_OFF) {
|
||||
modeTable[tableIdx].setMode(devMode);
|
||||
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
||||
} else {
|
||||
result = NEED_SECOND_STEP;
|
||||
@ -109,8 +100,8 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
|
||||
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
||||
}
|
||||
}
|
||||
} else if (tgtMode == MODE_ON) {
|
||||
if (isUseable(objectId, mode)) {
|
||||
} else if (devMode == MODE_ON) {
|
||||
if (isUseable(objectId, devMode)) {
|
||||
modeTable[tableIdx].setMode(MODE_ON);
|
||||
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
||||
}
|
||||
@ -118,11 +109,11 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
|
||||
};
|
||||
switch (submode) {
|
||||
case (A_SIDE): {
|
||||
cmdSeq(helper.gyro0AdisIdSideA, ModeTableIdx::GYRO_0_A);
|
||||
cmdSeq(helper.gyro1L3gIdSideA, ModeTableIdx::GYRO_1_A);
|
||||
cmdSeq(helper.mgm0Lis3IdSideA, ModeTableIdx::MGM_0_A);
|
||||
cmdSeq(helper.mgm1Rm3100IdSideA, ModeTableIdx::MGM_1_A);
|
||||
cmdSeq(helper.gpsId, ModeTableIdx::GPS);
|
||||
cmdSeq(helper.gyro0AdisIdSideA, helper.gyro0SideAMode, ModeTableIdx::GYRO_0_A);
|
||||
cmdSeq(helper.gyro1L3gIdSideA, helper.gyro1SideAMode, ModeTableIdx::GYRO_1_A);
|
||||
cmdSeq(helper.mgm0Lis3IdSideA, helper.mgm0SideAMode, ModeTableIdx::MGM_0_A);
|
||||
cmdSeq(helper.mgm1Rm3100IdSideA, helper.mgm1SideAMode, ModeTableIdx::MGM_1_A);
|
||||
cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
|
||||
modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF);
|
||||
@ -134,11 +125,11 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
|
||||
return result;
|
||||
}
|
||||
case (B_SIDE): {
|
||||
cmdSeq(helper.gyro2AdisIdSideB, ModeTableIdx::GYRO_2_B);
|
||||
cmdSeq(helper.gyro3L3gIdSideB, ModeTableIdx::GYRO_3_B);
|
||||
cmdSeq(helper.mgm2Lis3IdSideB, ModeTableIdx::MGM_2_B);
|
||||
cmdSeq(helper.mgm3Rm3100IdSideB, ModeTableIdx::MGM_3_B);
|
||||
cmdSeq(helper.gpsId, ModeTableIdx::GPS);
|
||||
cmdSeq(helper.gyro2AdisIdSideB, helper.gyro2SideBMode, ModeTableIdx::GYRO_2_B);
|
||||
cmdSeq(helper.gyro3L3gIdSideB, helper.gyro3SideBMode, ModeTableIdx::GYRO_3_B);
|
||||
cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
|
||||
cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
|
||||
cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
|
||||
modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
|
||||
modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE);
|
||||
modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF);
|
||||
@ -150,15 +141,15 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
|
||||
return result;
|
||||
}
|
||||
case (DUAL_MODE): {
|
||||
cmdSeq(helper.gpsId, ModeTableIdx::GPS);
|
||||
cmdSeq(helper.gyro0AdisIdSideA, ModeTableIdx::GYRO_0_A);
|
||||
cmdSeq(helper.gyro1L3gIdSideA, ModeTableIdx::GYRO_1_A);
|
||||
cmdSeq(helper.gyro2AdisIdSideB, ModeTableIdx::GYRO_2_B);
|
||||
cmdSeq(helper.gyro3L3gIdSideB, ModeTableIdx::GYRO_3_B);
|
||||
cmdSeq(helper.mgm0Lis3IdSideA, ModeTableIdx::MGM_0_A);
|
||||
cmdSeq(helper.mgm1Rm3100IdSideA, ModeTableIdx::MGM_1_A);
|
||||
cmdSeq(helper.mgm2Lis3IdSideB, ModeTableIdx::MGM_2_B);
|
||||
cmdSeq(helper.mgm3Rm3100IdSideB, ModeTableIdx::MGM_3_B);
|
||||
cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
|
||||
cmdSeq(helper.gyro0AdisIdSideA, helper.gyro0SideAMode, ModeTableIdx::GYRO_0_A);
|
||||
cmdSeq(helper.gyro1L3gIdSideA, helper.gyro1SideAMode, ModeTableIdx::GYRO_1_A);
|
||||
cmdSeq(helper.mgm0Lis3IdSideA, helper.mgm0SideAMode, ModeTableIdx::MGM_0_A);
|
||||
cmdSeq(helper.mgm1Rm3100IdSideA, helper.mgm1SideAMode, ModeTableIdx::MGM_1_A);
|
||||
cmdSeq(helper.gyro2AdisIdSideB, helper.gyro2SideBMode, ModeTableIdx::GYRO_2_B);
|
||||
cmdSeq(helper.gyro3L3gIdSideB, helper.gyro3SideBMode, ModeTableIdx::GYRO_3_B);
|
||||
cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
|
||||
cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
|
||||
return result;
|
||||
}
|
||||
default: {
|
||||
@ -318,6 +309,14 @@ void AcsBoardAssembly::handleModeReached() {
|
||||
state = States::IDLE;
|
||||
}
|
||||
|
||||
void AcsBoardAssembly::handleModeTransitionFailed(ReturnValue_t result) {
|
||||
// Some ACS board components are required for Safe-Mode. It would be good if the software
|
||||
// transitions from A side to B side and from B side to dual mode autonomously
|
||||
// to ensure that that enough sensors are available witout an operators intervention.
|
||||
// Therefore, the failure handler is overriden to perform these steps.
|
||||
// TODO: Implement transitions mentioned above
|
||||
}
|
||||
|
||||
void AcsBoardAssembly::refreshHelperModes() {
|
||||
helper.gyro0SideAMode = childrenMap[helper.gyro0AdisIdSideA].mode;
|
||||
helper.gyro1SideAMode = childrenMap[helper.gyro1L3gIdSideA].mode;
|
||||
@ -328,3 +327,11 @@ void AcsBoardAssembly::refreshHelperModes() {
|
||||
helper.mgm2SideBMode = childrenMap[helper.mgm2Lis3IdSideB].mode;
|
||||
helper.mgm3SideBMode = childrenMap[helper.mgm3Rm3100IdSideB].mode;
|
||||
}
|
||||
|
||||
void AcsBoardAssembly::initModeTableEntry(object_id_t id, ModeListEntry& entry) {
|
||||
entry.setObject(id);
|
||||
entry.setMode(MODE_OFF);
|
||||
entry.setSubmode(SUBMODE_NONE);
|
||||
entry.setInheritSubmode(false);
|
||||
modeTable.insert(entry);
|
||||
}
|
||||
|
||||
@ -78,7 +78,6 @@ class AcsBoardAssembly : public AssemblyBase {
|
||||
|
||||
AcsBoardHelper helper;
|
||||
FixedArrayList<ModeListEntry, NUMBER_DEVICES_MODE_TABLE> modeTable;
|
||||
void initModeTableEntry(object_id_t id, ModeListEntry& entry);
|
||||
|
||||
ReturnValue_t initialize() override;
|
||||
|
||||
@ -87,6 +86,7 @@ class AcsBoardAssembly : public AssemblyBase {
|
||||
ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
|
||||
ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
|
||||
void handleModeReached() override;
|
||||
void handleModeTransitionFailed(ReturnValue_t result) override;
|
||||
|
||||
/**
|
||||
* Check whether it makes sense to send mode commands to the device
|
||||
@ -97,6 +97,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(Mode_t mode, Submode_t submode);
|
||||
void initModeTableEntry(object_id_t id, ModeListEntry& entry);
|
||||
void refreshHelperModes();
|
||||
};
|
||||
|
||||
|
||||
@ -6,15 +6,105 @@
|
||||
|
||||
SusAssembly::SusAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
|
||||
SusAssHelper helper)
|
||||
: AssemblyBase(objectId, parentId), helper(helper), pwrSwitcher(pwrSwitcher) {}
|
||||
: AssemblyBase(objectId, parentId), helper(helper), pwrSwitcher(pwrSwitcher) {
|
||||
ModeListEntry entry;
|
||||
for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
|
||||
initModeTableEntry(helper.susIds[idx], entry);
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
refreshHelperModes();
|
||||
powerStateMachine(mode, submode);
|
||||
if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
|
||||
if (state == States::MODE_COMMANDING) {
|
||||
handleNormalOrOnModeCmd(mode, submode);
|
||||
}
|
||||
} else {
|
||||
for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
|
||||
modeTable[idx].setMode(MODE_OFF);
|
||||
modeTable[idx].setSubmode(SUBMODE_NONE);
|
||||
}
|
||||
}
|
||||
|
||||
HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
|
||||
executeTable(tableIter);
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
auto cmdSeq = [&](object_id_t objectId, uint8_t tableIdx) {
|
||||
if (mode == DeviceHandlerIF::MODE_NORMAL) {
|
||||
if (isUseable(objectId, mode)) {
|
||||
if (helper.susModes[tableIdx] != MODE_OFF) {
|
||||
modeTable[tableIdx].setMode(mode);
|
||||
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
||||
} else {
|
||||
result = NEED_SECOND_STEP;
|
||||
modeTable[tableIdx].setMode(MODE_ON);
|
||||
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
||||
}
|
||||
}
|
||||
} else if (mode == MODE_ON) {
|
||||
if (isUseable(objectId, mode)) {
|
||||
modeTable[tableIdx].setMode(MODE_ON);
|
||||
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
||||
}
|
||||
}
|
||||
};
|
||||
switch (submode) {
|
||||
case (NOMINAL): {
|
||||
for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
|
||||
cmdSeq(helper.susIds[idx], idx);
|
||||
// Switch off devices on redundant side
|
||||
modeTable[idx + NUMBER_SUN_SENSORS_ONE_SIDE].setMode(MODE_OFF);
|
||||
modeTable[idx + NUMBER_SUN_SENSORS_ONE_SIDE].setSubmode(SUBMODE_NONE);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
case (REDUNDANT): {
|
||||
for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) {
|
||||
cmdSeq(helper.susIds[idx], idx);
|
||||
// Switch devices on nominal side
|
||||
modeTable[idx - NUMBER_SUN_SENSORS_ONE_SIDE].setMode(MODE_OFF);
|
||||
modeTable[idx - NUMBER_SUN_SENSORS_ONE_SIDE].setSubmode(SUBMODE_NONE);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
case (DUAL_MODE): {
|
||||
for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
|
||||
cmdSeq(helper.susIds[idx], idx);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
|
||||
refreshHelperModes();
|
||||
if (wantedSubmode == NOMINAL) {
|
||||
for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
|
||||
if (helper.susModes[idx] != wantedMode) {
|
||||
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
|
||||
}
|
||||
}
|
||||
return RETURN_OK;
|
||||
} else if (wantedSubmode == REDUNDANT) {
|
||||
for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) {
|
||||
if (helper.susModes[idx] != wantedMode) {
|
||||
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
|
||||
}
|
||||
}
|
||||
return RETURN_OK;
|
||||
} else {
|
||||
// Trigger event if devices are faulty? This is the last fallback mode, returning
|
||||
// a failure here would trigger a transition to MODE_OFF unless handleModeTransitionFailed
|
||||
// is overriden
|
||||
return RETURN_OK;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
@ -115,8 +205,29 @@ void SusAssembly::powerStateMachine(Mode_t mode, Submode_t submode) {
|
||||
}
|
||||
}
|
||||
|
||||
void SusAssembly::handleModeReached() {
|
||||
AssemblyBase::handleModeReached();
|
||||
state = States::IDLE;
|
||||
}
|
||||
|
||||
void SusAssembly::handleModeTransitionFailed(ReturnValue_t result) {
|
||||
// The sun-sensors are required for the Safe-Mode. It would be good if the software
|
||||
// transitions from nominal side to redundant side and from redundant side to dual mode
|
||||
// autonomously to ensure that that enough sensors are available witout an operators intervention.
|
||||
// Therefore, the failure handler is overriden to perform these steps.
|
||||
// TODO: Implement transitions mentioned above
|
||||
}
|
||||
|
||||
void SusAssembly::refreshHelperModes() {
|
||||
for (uint8_t idx = 0; idx < helper.susModes.size(); idx++) {
|
||||
helper.susModes[idx] = childrenMap[helper.susIds[idx]].mode;
|
||||
}
|
||||
}
|
||||
|
||||
void SusAssembly::initModeTableEntry(object_id_t id, ModeListEntry& entry) {
|
||||
entry.setObject(id);
|
||||
entry.setMode(MODE_OFF);
|
||||
entry.setSubmode(SUBMODE_NONE);
|
||||
entry.setInheritSubmode(false);
|
||||
modeTable.insert(entry);
|
||||
}
|
||||
|
||||
@ -15,6 +15,7 @@ class PowerSwitchIF;
|
||||
|
||||
class SusAssembly : AssemblyBase {
|
||||
public:
|
||||
static constexpr uint8_t NUMBER_SUN_SENSORS_ONE_SIDE = 6;
|
||||
static constexpr uint8_t NUMBER_SUN_SENSORS = 12;
|
||||
|
||||
static constexpr Submode_t NOMINAL = 0;
|
||||
@ -41,6 +42,8 @@ class SusAssembly : AssemblyBase {
|
||||
ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
|
||||
ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
|
||||
ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
|
||||
void handleModeReached() override;
|
||||
void handleModeTransitionFailed(ReturnValue_t result) override;
|
||||
|
||||
/**
|
||||
* Check whether it makes sense to send mode commands to the device
|
||||
@ -50,6 +53,8 @@ class SusAssembly : AssemblyBase {
|
||||
*/
|
||||
bool isUseable(object_id_t object, Mode_t mode);
|
||||
void powerStateMachine(Mode_t mode, Submode_t submode);
|
||||
ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode);
|
||||
void initModeTableEntry(object_id_t id, ModeListEntry& entry);
|
||||
void refreshHelperModes();
|
||||
};
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user