fsfw/devicehandlers/AssemblyBase.cpp

274 lines
7.3 KiB
C++

#include "AssemblyBase.h"
AssemblyBase::AssemblyBase(object_id_t objectId, object_id_t parentId,
uint16_t commandQueueDepth) :
SubsystemBase(objectId, parentId, MODE_OFF, commandQueueDepth),
internalState(STATE_NONE), recoveryState(RECOVERY_IDLE),
recoveringDevice(childrenMap.end()), targetMode(MODE_OFF),
targetSubmode(SUBMODE_NONE) {
recoveryOffTimer.setTimeout(POWER_OFF_TIME_MS);
}
AssemblyBase::~AssemblyBase() {
}
ReturnValue_t AssemblyBase::handleCommandMessage(CommandMessage* message) {
return handleHealthReply(message);
}
void AssemblyBase::performChildOperation() {
if (isInTransition()) {
handleChildrenTransition();
} else {
handleChildrenChanged();
}
}
void AssemblyBase::startTransition(Mode_t mode, Submode_t submode) {
doStartTransition(mode, submode);
if (modeHelper.isForced()) {
triggerEvent(FORCING_MODE, mode, submode);
} else {
triggerEvent(CHANGING_MODE, mode, submode);
}
}
void AssemblyBase::doStartTransition(Mode_t mode, Submode_t submode) {
targetMode = mode;
targetSubmode = submode;
internalState = STATE_SINGLE_STEP;
ReturnValue_t result = commandChildren(mode, submode);
if (result == NEED_SECOND_STEP) {
internalState = STATE_NEED_SECOND_STEP;
}
}
bool AssemblyBase::isInTransition() {
return (internalState != STATE_NONE) || (recoveryState != RECOVERY_IDLE);
}
bool AssemblyBase::handleChildrenChanged() {
if (childrenChangedMode) {
ReturnValue_t result = checkChildrenState();
if (result != RETURN_OK) {
handleChildrenLostMode(result);
}
return true;
} else {
return handleChildrenChangedHealth();
}
}
void AssemblyBase::handleChildrenLostMode(ReturnValue_t result) {
triggerEvent(CANT_KEEP_MODE, mode, submode);
startTransition(MODE_OFF, SUBMODE_NONE);
}
bool AssemblyBase::handleChildrenChangedHealth() {
auto iter = childrenMap.begin();
for (; iter != childrenMap.end(); iter++) {
if (iter->second.healthChanged) {
iter->second.healthChanged = false;
break;
}
}
if (iter == childrenMap.end()) {
return false;
}
HealthState healthState = healthHelper.healthTable->getHealth(iter->first);
if (healthState == HasHealthIF::NEEDS_RECOVERY) {
triggerEvent(TRYING_RECOVERY);
recoveryState = RECOVERY_STARTED;
recoveringDevice = iter;
doStartTransition(targetMode, targetSubmode);
} else {
triggerEvent(CHILD_CHANGED_HEALTH);
doStartTransition(mode, submode);
}
if (modeHelper.isForced()) {
triggerEvent(FORCING_MODE, targetMode, targetSubmode);
}
return true;
}
void AssemblyBase::handleChildrenTransition() {
if (commandsOutstanding <= 0) {
switch (internalState) {
case STATE_NEED_SECOND_STEP:
internalState = STATE_SECOND_STEP;
commandChildren(targetMode, targetSubmode);
return;
case STATE_OVERWRITE_HEALTH: {
internalState = STATE_SINGLE_STEP;
ReturnValue_t result = commandChildren(mode, submode);
if (result == NEED_SECOND_STEP) {
internalState = STATE_NEED_SECOND_STEP;
}
return;
}
case STATE_NONE:
//Valid state, used in recovery.
case STATE_SINGLE_STEP:
case STATE_SECOND_STEP:
if (checkAndHandleRecovery()) {
return;
}
break;
}
ReturnValue_t result = checkChildrenState();
if (result == RETURN_OK) {
handleModeReached();
} else {
handleModeTransitionFailed(result);
}
}
}
void AssemblyBase::handleModeReached() {
internalState = STATE_NONE;
setMode(targetMode, targetSubmode);
}
void AssemblyBase::handleModeTransitionFailed(ReturnValue_t result) {
//always accept transition to OFF, there is nothing we can do except sending an info event
//In theory this should never happen, but we would risk an infinite loop otherwise
if (targetMode == MODE_OFF) {
triggerEvent(CHILD_PROBLEMS, result);
internalState = STATE_NONE;
setMode(targetMode, targetSubmode);
} else {
if (handleChildrenChangedHealth()) {
//If any health change is pending, handle that first.
return;
}
triggerEvent(MODE_TRANSITION_FAILED, result);
startTransition(MODE_OFF, SUBMODE_NONE);
}
}
void AssemblyBase::sendHealthCommand(MessageQueueId_t sendTo,
HealthState health) {
CommandMessage command;
HealthMessage::setHealthMessage(&command, HealthMessage::HEALTH_SET,
health);
if (commandQueue->sendMessage(sendTo, &command) == RETURN_OK) {
commandsOutstanding++;
}
}
ReturnValue_t AssemblyBase::checkChildrenState() {
if (targetMode == MODE_OFF) {
return checkChildrenStateOff();
} else {
return checkChildrenStateOn(targetMode, targetSubmode);
}
}
ReturnValue_t AssemblyBase::checkChildrenStateOff() {
for (const auto& childIter: childrenMap) {
if (checkChildOff(childIter.first) != RETURN_OK) {
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
}
}
return RETURN_OK;
}
ReturnValue_t AssemblyBase::checkChildOff(uint32_t objectId) {
ChildInfo childInfo = childrenMap.find(objectId)->second;
if (healthHelper.healthTable->isCommandable(objectId)) {
if (childInfo.submode != SUBMODE_NONE) {
return RETURN_FAILED;
} else {
if ((childInfo.mode != MODE_OFF)
&& (childInfo.mode != DeviceHandlerIF::MODE_ERROR_ON)) {
return RETURN_FAILED;
}
}
}
return RETURN_OK;
}
ReturnValue_t AssemblyBase::checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t* msToReachTheMode) {
//always accept transition to OFF
if (mode == MODE_OFF) {
if (submode != SUBMODE_NONE) {
return INVALID_SUBMODE;
}
return RETURN_OK;
}
if ((mode != MODE_ON) && (mode != DeviceHandlerIF::MODE_NORMAL)) {
return INVALID_MODE;
}
if (internalState != STATE_NONE) {
return IN_TRANSITION;
}
return isModeCombinationValid(mode, submode);
}
ReturnValue_t AssemblyBase::handleHealthReply(CommandMessage* message) {
if (message->getCommand() == HealthMessage::HEALTH_INFO) {
HealthState health = HealthMessage::getHealth(message);
if (health != EXTERNAL_CONTROL) {
updateChildChangedHealth(message->getSender(), true);
}
return RETURN_OK;
}
if (message->getCommand() == HealthMessage::REPLY_HEALTH_SET
|| (message->getCommand() == CommandMessage::REPLY_REJECTED
&& message->getParameter2() == HealthMessage::HEALTH_SET)) {
if (isInTransition()) {
commandsOutstanding--;
}
return RETURN_OK;
}
return RETURN_FAILED;
}
bool AssemblyBase::checkAndHandleRecovery() {
switch (recoveryState) {
case RECOVERY_STARTED:
recoveryState = RECOVERY_WAIT;
recoveryOffTimer.resetTimer();
return true;
case RECOVERY_WAIT:
if (recoveryOffTimer.isBusy()) {
return true;
}
triggerEvent(RECOVERY_STEP, 0);
sendHealthCommand(recoveringDevice->second.commandQueue, HEALTHY);
internalState = STATE_NONE;
recoveryState = RECOVERY_ONGOING;
//Don't check state!
return true;
case RECOVERY_ONGOING:
triggerEvent(RECOVERY_STEP, 1);
recoveryState = RECOVERY_ONGOING_2;
recoveringDevice->second.healthChanged = false;
//Device should be healthy again, so restart a transition.
//Might be including second step, but that's already handled.
doStartTransition(targetMode, targetSubmode);
return true;
case RECOVERY_ONGOING_2:
triggerEvent(RECOVERY_DONE);
//Now we're through, but not sure if it was successful.
recoveryState = RECOVERY_IDLE;
return false;
case RECOVERY_IDLE:
default:
return false;
}
}
void AssemblyBase::overwriteDeviceHealth(object_id_t objectId,
HasHealthIF::HealthState oldHealth) {
triggerEvent(OVERWRITING_HEALTH, objectId, oldHealth);
internalState = STATE_OVERWRITE_HEALTH;
modeHelper.setForced(true);
sendHealthCommand(childrenMap[objectId].commandQueue, EXTERNAL_CONTROL);
}