eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests Releases 118 Wiki Activity

finished most of sus handler
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller 2022-03-03 20:11:12 +01:00
parent a7c1dafce5
commit 0595e29100
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
4 changed files with 161 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
mission/system/AcsBoardAssembly.cpp
View File

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

3
mission/system/AcsBoardAssembly.h
View File

@ -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();
};

113
mission/system/SusAssembly.cpp
View File

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

5
mission/system/SusAssembly.h
View File

@ -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();
};