eive-obsw/mission/system/tcs/TcsBoardAssembly.cpp

180 lines
5.9 KiB
C++
Raw Normal View History

2022-03-22 18:48:03 +01:00
#include "TcsBoardAssembly.h"
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/ipc/QueueFactory.h>
2022-09-29 19:40:00 +02:00
TcsBoardAssembly::TcsBoardAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher,
2023-04-11 22:58:13 +02:00
power::Switch_t theSwitch, TcsBoardHelper helper,
std::atomic_bool& tcsShortlyUnavailable)
: SharedPowerAssemblyBase(objectId, pwrSwitcher, theSwitch, 16),
helper(helper),
tcsShortlyUnavailable(tcsShortlyUnavailable) {
2022-03-22 20:35:37 +01:00
ModeListEntry entry;
for (uint8_t idx = 0; idx < NUMBER_RTDS; idx++) {
entry.setObject(helper.rtdInfos[idx].first);
2022-03-22 20:35:37 +01:00
entry.setMode(MODE_OFF);
entry.setSubmode(SUBMODE_NONE);
modeTable.insert(entry);
}
2022-03-22 18:48:03 +01:00
}
ReturnValue_t TcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) {
2022-08-24 17:27:47 +02:00
ReturnValue_t result = returnvalue::OK;
2022-03-22 18:48:03 +01:00
// Initialize the mode table to ensure all devices are in a defined state
for (uint8_t idx = 0; idx < NUMBER_RTDS; idx++) {
modeTable[idx].setMode(MODE_OFF);
modeTable[idx].setSubmode(SUBMODE_NONE);
}
2023-03-06 15:41:56 +01:00
if (recoveryState == RecoveryState::RECOVERY_IDLE) {
result = checkAndHandleHealthStates(mode, submode);
if (result == NEED_TO_CHANGE_HEALTH) {
return returnvalue::OK;
}
}
2022-03-22 18:48:03 +01:00
if (recoveryState != RecoveryState::RECOVERY_STARTED) {
if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
result = handleNormalOrOnModeCmd(mode, submode);
}
2023-04-11 22:58:13 +02:00
} else {
tcsShortlyUnavailable = true;
2022-03-22 18:48:03 +01:00
}
HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
executeTable(tableIter);
return result;
}
ReturnValue_t TcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
2022-03-22 20:35:37 +01:00
int devsInWrongMode = 0;
try {
for (uint8_t idx = 0; idx < NUMBER_RTDS; idx++) {
if (childrenMap.at(helper.rtdInfos[idx].first).mode != wantedMode) {
2022-03-22 20:35:37 +01:00
devsInWrongMode++;
}
2022-03-22 18:48:03 +01:00
}
2022-03-22 20:35:37 +01:00
} catch (const std::out_of_range& e) {
sif::error << "TcsBoardAssembly: Invalid children map: " << e.what() << std::endl;
}
2023-03-06 17:34:04 +01:00
if (devsInWrongMode == NUMBER_RTDS) {
2022-03-22 20:35:37 +01:00
if (warningSwitch) {
2023-03-06 17:34:04 +01:00
sif::warning << "TcsBoardAssembly::checkChildrenStateOn: All devices in wrong mode"
<< std::endl;
2022-03-22 20:35:37 +01:00
warningSwitch = false;
}
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
2022-03-22 18:48:03 +01:00
}
// TODO: Can't really do something other than power cycling if devices in wrong mode.
// Might attempt one power-cycle. In any case, trigger an event
2022-03-22 20:35:37 +01:00
if (devsInWrongMode > 0) {
if (warningSwitch) {
sif::warning << "TcsBoardAssembly::checkChildrenStateOn: " << devsInWrongMode << " devices in"
<< " wrong mode" << std::endl;
warningSwitch = false;
}
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-22 18:48:03 +01:00
}
ReturnValue_t TcsBoardAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
if (mode == MODE_ON or mode == MODE_OFF or mode == DeviceHandlerIF::MODE_NORMAL) {
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-22 18:48:03 +01:00
}
return HasModesIF::INVALID_MODE;
}
ReturnValue_t TcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
2022-08-24 17:27:47 +02:00
ReturnValue_t result = returnvalue::OK;
2022-03-22 18:48:03 +01:00
bool needsSecondStep = false;
Mode_t devMode = 0;
object_id_t objId = 0;
2022-03-22 20:35:37 +01:00
try {
for (uint8_t idx = 0; idx < NUMBER_RTDS; idx++) {
devMode = childrenMap.at(helper.rtdInfos[idx].first).mode;
objId = helper.rtdInfos[idx].first;
2022-03-22 20:35:37 +01:00
if (mode == devMode) {
modeTable[idx].setMode(mode);
} else if (mode == DeviceHandlerIF::MODE_NORMAL) {
if (isUseable(objId, devMode)) {
if (devMode == MODE_ON) {
modeTable[idx].setMode(mode);
modeTable[idx].setSubmode(SUBMODE_NONE);
} else {
modeTable[idx].setMode(MODE_ON);
modeTable[idx].setSubmode(SUBMODE_NONE);
if (internalState != STATE_SECOND_STEP) {
needsSecondStep = true;
}
}
}
} else if (mode == MODE_ON) {
if (isUseable(objId, devMode)) {
2022-03-22 18:48:03 +01:00
modeTable[idx].setMode(MODE_ON);
modeTable[idx].setSubmode(SUBMODE_NONE);
}
}
}
2022-03-22 20:35:37 +01:00
} catch (const std::out_of_range& e) {
sif::error << "TcsBoardAssembly: Invalid children map: " << e.what() << std::endl;
2022-03-22 18:48:03 +01:00
}
if (needsSecondStep) {
result = NEED_SECOND_STEP;
}
return result;
}
bool TcsBoardAssembly::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;
}
2022-03-22 20:35:37 +01:00
void TcsBoardAssembly::handleChildrenLostMode(ReturnValue_t result) {
2022-03-22 20:40:30 +01:00
triggerEvent(CHILDREN_LOST_MODE, result);
2023-03-06 15:41:56 +01:00
startTransition(mode, submode);
}
ReturnValue_t TcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
Submode_t deviceSubmode) {
ReturnValue_t status = returnvalue::OK;
2023-03-06 17:34:04 +01:00
for (const auto& dev : helper.rtdInfos) {
HealthState health = healthHelper.healthTable->getHealth(dev.first);
if (health == HealthState::HEALTHY) {
return returnvalue::OK;
}
}
for (const auto& dev : helper.rtdInfos) {
HealthState health = healthHelper.healthTable->getHealth(dev.first);
2023-03-06 15:41:56 +01:00
if (health == FAULTY or health == PERMANENT_FAULTY) {
status = NEED_TO_CHANGE_HEALTH;
} else if (health == EXTERNAL_CONTROL) {
modeHelper.setForced(true);
}
}
2023-03-06 17:34:04 +01:00
return status;
2022-03-22 20:35:37 +01:00
}
2023-04-11 22:58:13 +02:00
bool TcsBoardAssembly::checkAndHandleRecovery() {
bool recoveryPending = SharedPowerAssemblyBase::checkAndHandleRecovery();
tcsShortlyUnavailable = recoveryPending;
return recoveryPending;
}
2022-03-22 20:35:37 +01:00
void TcsBoardAssembly::handleModeTransitionFailed(ReturnValue_t result) {
if (targetMode == MODE_OFF) {
AssemblyBase::handleModeTransitionFailed(result);
} else {
// To avoid transitioning back to off
triggerEvent(MODE_TRANSITION_FAILED, result);
}
}