1563 lines
53 KiB
C++
1563 lines
53 KiB
C++
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
|
|
|
|
#include "fsfw/datapoollocal/LocalPoolVariable.h"
|
|
#include "fsfw/devicehandlers/AcceptsDeviceResponsesIF.h"
|
|
#include "fsfw/devicehandlers/DeviceTmReportingWrapper.h"
|
|
#include "fsfw/globalfunctions/CRC.h"
|
|
#include "fsfw/housekeeping/HousekeepingMessage.h"
|
|
#include "fsfw/ipc/MessageQueueMessage.h"
|
|
#include "fsfw/ipc/QueueFactory.h"
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
|
#include "fsfw/serialize/SerialBufferAdapter.h"
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
|
#include "fsfw/storagemanager/StorageManagerIF.h"
|
|
#include "fsfw/subsystem/SubsystemBase.h"
|
|
#include "fsfw/thermal/ThermalComponentIF.h"
|
|
|
|
object_id_t DeviceHandlerBase::powerSwitcherId = objects::NO_OBJECT;
|
|
object_id_t DeviceHandlerBase::rawDataReceiverId = objects::NO_OBJECT;
|
|
object_id_t DeviceHandlerBase::defaultFdirParentId = objects::NO_OBJECT;
|
|
|
|
DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication,
|
|
CookieIF* comCookie, FailureIsolationBase* fdirInstance,
|
|
size_t cmdQueueSize)
|
|
: SystemObject(setObjectId),
|
|
mode(MODE_OFF),
|
|
submode(SUBMODE_NONE),
|
|
wiretappingMode(OFF),
|
|
storedRawData(StorageManagerIF::INVALID_ADDRESS),
|
|
deviceCommunicationId(deviceCommunication),
|
|
comCookie(comCookie),
|
|
healthHelper(this, setObjectId),
|
|
modeHelper(this),
|
|
parameterHelper(this),
|
|
actionHelper(this, nullptr),
|
|
poolManager(this, nullptr),
|
|
childTransitionFailure(returnvalue::OK),
|
|
fdirInstance(fdirInstance),
|
|
defaultFDIRUsed(fdirInstance == nullptr),
|
|
switchOffWasReported(false),
|
|
childTransitionDelay(5000),
|
|
transitionSourceMode(_MODE_POWER_DOWN),
|
|
transitionSourceSubMode(SUBMODE_NONE) {
|
|
commandQueue = QueueFactory::instance()->createMessageQueue(
|
|
cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE);
|
|
insertInCommandMap(RAW_COMMAND_ID);
|
|
cookieInfo.state = COOKIE_UNUSED;
|
|
cookieInfo.pendingCommand = deviceCommandMap.end();
|
|
if (comCookie == nullptr) {
|
|
printWarningOrError(sif::OutputTypes::OUT_ERROR, "DeviceHandlerBase", returnvalue::FAILED,
|
|
"Invalid cookie");
|
|
}
|
|
if (this->fdirInstance == nullptr) {
|
|
this->fdirInstance = new DeviceHandlerFailureIsolation(setObjectId, defaultFdirParentId);
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::setHkDestination(object_id_t hkDestination) {
|
|
this->hkDestination = hkDestination;
|
|
}
|
|
|
|
void DeviceHandlerBase::setThermalStateRequestPoolIds(lp_id_t thermalStatePoolId,
|
|
lp_id_t heaterRequestPoolId,
|
|
uint32_t thermalSetId) {
|
|
thermalSet =
|
|
new DeviceHandlerThermalSet(this, thermalSetId, thermalStatePoolId, heaterRequestPoolId);
|
|
}
|
|
|
|
DeviceHandlerBase::~DeviceHandlerBase() {
|
|
if (comCookie != nullptr) {
|
|
delete comCookie;
|
|
}
|
|
if (defaultFDIRUsed) {
|
|
delete fdirInstance;
|
|
}
|
|
QueueFactory::instance()->deleteMessageQueue(commandQueue);
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::performOperation(uint8_t counter) {
|
|
this->pstStep = counter;
|
|
this->lastStep = this->pstStep;
|
|
|
|
if (getComAction() == CommunicationAction::NOTHING) {
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
if (getComAction() == CommunicationAction::PERFORM_OPERATION) {
|
|
cookieInfo.state = COOKIE_UNUSED;
|
|
readCommandQueue();
|
|
doStateMachine();
|
|
checkSwitchState();
|
|
decrementDeviceReplyMap();
|
|
fdirInstance->checkForFailures();
|
|
performOperationHook();
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
if (mode == MODE_OFF) {
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
switch (getComAction()) {
|
|
case CommunicationAction::SEND_WRITE:
|
|
if (cookieInfo.state == COOKIE_UNUSED) {
|
|
/* If no external command was specified, build internal command. */
|
|
buildInternalCommand();
|
|
}
|
|
doSendWrite();
|
|
break;
|
|
case CommunicationAction::GET_WRITE:
|
|
doGetWrite();
|
|
break;
|
|
case CommunicationAction::SEND_READ:
|
|
doSendRead();
|
|
break;
|
|
case CommunicationAction::GET_READ:
|
|
doGetRead();
|
|
/* This will be performed after datasets have been updated by the
|
|
custom device implementation. */
|
|
poolManager.performHkOperation();
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::initialize() {
|
|
ReturnValue_t result = SystemObject::initialize();
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
|
|
communicationInterface =
|
|
ObjectManager::instance()->get<DeviceCommunicationIF>(deviceCommunicationId);
|
|
if (communicationInterface == nullptr) {
|
|
printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize",
|
|
ObjectManagerIF::CHILD_INIT_FAILED, "Passed communication IF invalid");
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
|
|
result = communicationInterface->initializeInterface(comCookie);
|
|
if (result != returnvalue::OK) {
|
|
printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize",
|
|
ObjectManagerIF::CHILD_INIT_FAILED, "ComIF initialization failed");
|
|
return result;
|
|
}
|
|
|
|
IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
|
|
if (IPCStore == nullptr) {
|
|
printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize",
|
|
ObjectManagerIF::CHILD_INIT_FAILED, "IPC Store not set up");
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
|
|
if (rawDataReceiverId != objects::NO_OBJECT) {
|
|
AcceptsDeviceResponsesIF* rawReceiver =
|
|
ObjectManager::instance()->get<AcceptsDeviceResponsesIF>(rawDataReceiverId);
|
|
|
|
if (rawReceiver == nullptr) {
|
|
printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize",
|
|
ObjectManagerIF::CHILD_INIT_FAILED,
|
|
"Raw receiver object ID set but no valid object found.");
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "Make sure the raw receiver object is set up properly"
|
|
" and implements AcceptsDeviceResponsesIF"
|
|
<< std::endl;
|
|
#else
|
|
sif::printError(
|
|
"Make sure the raw receiver object is set up "
|
|
"properly and implements AcceptsDeviceResponsesIF\n");
|
|
#endif
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
defaultRawReceiver = rawReceiver->getDeviceQueue();
|
|
}
|
|
|
|
if (powerSwitcherId != objects::NO_OBJECT) {
|
|
powerSwitcher = ObjectManager::instance()->get<PowerSwitchIF>(powerSwitcherId);
|
|
if (powerSwitcher == nullptr) {
|
|
printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize",
|
|
ObjectManagerIF::CHILD_INIT_FAILED,
|
|
"Power switcher set but no valid object found.");
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "Make sure the power switcher object is set up "
|
|
<< "properly and implements PowerSwitchIF" << std::endl;
|
|
#else
|
|
sif::printError(
|
|
"Make sure the power switcher object is set up "
|
|
"properly and implements PowerSwitchIF\n");
|
|
#endif
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
}
|
|
|
|
result = healthHelper.initialize();
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
|
|
result = modeHelper.initialize();
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
result = actionHelper.initialize(commandQueue);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
result = fdirInstance->initialize();
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
|
|
result = parameterHelper.initialize();
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
|
|
result = poolManager.initialize(commandQueue);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
|
|
fillCommandAndReplyMap();
|
|
|
|
if (thermalSet != nullptr) {
|
|
// Set temperature target state to NON_OP.
|
|
result = thermalSet->read();
|
|
if (result == returnvalue::OK) {
|
|
thermalSet->heaterRequest.value = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL;
|
|
thermalSet->heaterRequest.setValid(true);
|
|
thermalSet->commit();
|
|
}
|
|
}
|
|
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void DeviceHandlerBase::decrementDeviceReplyMap() {
|
|
bool timedOut = false;
|
|
for (std::pair<const DeviceCommandId_t, DeviceReplyInfo>& replyPair : deviceReplyMap) {
|
|
if (replyPair.second.countdown != nullptr && replyPair.second.active) {
|
|
if (replyPair.second.countdown->hasTimedOut()) {
|
|
resetTimeoutControlledReply(&replyPair.second);
|
|
timedOut = true;
|
|
}
|
|
}
|
|
if (replyPair.second.delayCycles != 0 && replyPair.second.countdown == nullptr) {
|
|
replyPair.second.delayCycles--;
|
|
if (replyPair.second.delayCycles == 0) {
|
|
resetDelayCyclesControlledReply(&replyPair.second);
|
|
timedOut = true;
|
|
}
|
|
}
|
|
if (timedOut) {
|
|
replyToReply(replyPair.first, replyPair.second, TIMEOUT);
|
|
missedReply(replyPair.first);
|
|
timedOut = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::readCommandQueue() {
|
|
if (dontCheckQueue()) {
|
|
return;
|
|
}
|
|
|
|
CommandMessage command;
|
|
ReturnValue_t result = commandQueue->receiveMessage(&command);
|
|
if (result != returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
result = healthHelper.handleHealthCommand(&command);
|
|
if (result == returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
result = modeHelper.handleModeCommand(&command);
|
|
if (result == returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
result = actionHelper.handleActionMessage(&command);
|
|
if (result == returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
result = parameterHelper.handleParameterMessage(&command);
|
|
if (result == returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
result = poolManager.handleHousekeepingMessage(&command);
|
|
if (result == returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
result = handleDeviceHandlerMessage(&command);
|
|
if (result == returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
result = letChildHandleMessage(&command);
|
|
if (result == returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
replyReturnvalueToCommand(CommandMessage::UNKNOWN_COMMAND);
|
|
}
|
|
|
|
void DeviceHandlerBase::doStateMachine() {
|
|
switch (mode) {
|
|
case _MODE_START_UP:
|
|
case _MODE_SHUT_DOWN:
|
|
case _MODE_TO_NORMAL:
|
|
case _MODE_TO_ON:
|
|
case _MODE_TO_RAW: {
|
|
Mode_t currentMode = mode;
|
|
callChildStatemachine();
|
|
// Only do timeout if child did not change anything
|
|
if (mode != currentMode) {
|
|
break;
|
|
}
|
|
uint32_t currentUptime;
|
|
Clock::getUptime(¤tUptime);
|
|
if (currentUptime - timeoutStart >= childTransitionDelay) {
|
|
#if FSFW_VERBOSE_LEVEL >= 1 && FSFW_OBJ_EVENT_TRANSLATION == 0
|
|
char printout[60];
|
|
sprintf(printout, "Transition timeout (%lu) occured !",
|
|
static_cast<unsigned long>(childTransitionDelay));
|
|
/* Common configuration error for development, so print it */
|
|
printWarningOrError(sif::OutputTypes::OUT_WARNING, "doStateMachine", returnvalue::FAILED,
|
|
printout);
|
|
#endif
|
|
triggerEvent(MODE_TRANSITION_FAILED, childTransitionFailure, 0);
|
|
setMode(transitionSourceMode, transitionSourceSubMode);
|
|
break;
|
|
}
|
|
} break;
|
|
case _MODE_POWER_DOWN:
|
|
commandSwitch(PowerSwitchIF::SWITCH_OFF);
|
|
setMode(_MODE_WAIT_OFF);
|
|
break;
|
|
case _MODE_POWER_ON:
|
|
commandSwitch(PowerSwitchIF::SWITCH_ON);
|
|
setMode(_MODE_WAIT_ON);
|
|
break;
|
|
case _MODE_WAIT_ON: {
|
|
uint32_t currentUptime;
|
|
Clock::getUptime(¤tUptime);
|
|
if (powerSwitcher != nullptr and
|
|
currentUptime - timeoutStart >= powerSwitcher->getSwitchDelayMs()) {
|
|
triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT, 0);
|
|
setMode(_MODE_POWER_DOWN);
|
|
callChildStatemachine();
|
|
break;
|
|
}
|
|
ReturnValue_t switchState = getStateOfSwitches();
|
|
if ((switchState == PowerSwitchIF::SWITCH_ON) || (switchState == NO_SWITCH)) {
|
|
// NOTE: TransitionSourceMode and -SubMode are set by handleCommandedModeTransition
|
|
childTransitionFailure = CHILD_TIMEOUT;
|
|
setMode(_MODE_START_UP);
|
|
callChildStatemachine();
|
|
}
|
|
} break;
|
|
case _MODE_WAIT_OFF: {
|
|
uint32_t currentUptime;
|
|
Clock::getUptime(¤tUptime);
|
|
|
|
if (powerSwitcher == nullptr) {
|
|
setMode(MODE_OFF);
|
|
break;
|
|
}
|
|
if (currentUptime - timeoutStart >= powerSwitcher->getSwitchDelayMs()) {
|
|
triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT, 0);
|
|
setMode(MODE_ERROR_ON);
|
|
break;
|
|
}
|
|
ReturnValue_t switchState = getStateOfSwitches();
|
|
if ((switchState == PowerSwitchIF::SWITCH_OFF) || (switchState == NO_SWITCH)) {
|
|
setMode(_MODE_SWITCH_IS_OFF);
|
|
}
|
|
} break;
|
|
case MODE_OFF:
|
|
doOffActivity();
|
|
break;
|
|
case MODE_ON:
|
|
doOnActivity();
|
|
break;
|
|
case MODE_RAW:
|
|
case MODE_NORMAL:
|
|
case MODE_ERROR_ON:
|
|
break;
|
|
case _MODE_SWITCH_IS_OFF:
|
|
setMode(MODE_OFF, SUBMODE_NONE);
|
|
break;
|
|
default:
|
|
triggerEvent(OBJECT_IN_INVALID_MODE, mode, submode);
|
|
setMode(_MODE_POWER_DOWN, 0);
|
|
break;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::isModeCombinationValid(Mode_t mode, Submode_t submode) {
|
|
switch (mode) {
|
|
case MODE_OFF:
|
|
case MODE_ON:
|
|
case MODE_NORMAL:
|
|
case MODE_RAW:
|
|
if (submode == SUBMODE_NONE) {
|
|
return returnvalue::OK;
|
|
} else {
|
|
return INVALID_SUBMODE;
|
|
}
|
|
default:
|
|
return HasModesIF::INVALID_MODE;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::insertInCommandAndReplyMap(
|
|
DeviceCommandId_t deviceCommand, uint16_t maxDelayCycles, LocalPoolDataSetBase* replyDataSet,
|
|
size_t replyLen, bool periodic, bool hasDifferentReplyId, DeviceCommandId_t replyId,
|
|
Countdown* countdown) {
|
|
// No need to check, as we may try to insert multiple times.
|
|
insertInCommandMap(deviceCommand, hasDifferentReplyId, replyId);
|
|
if (hasDifferentReplyId) {
|
|
return insertInReplyMap(replyId, maxDelayCycles, replyDataSet, replyLen, periodic, countdown);
|
|
} else {
|
|
return insertInReplyMap(deviceCommand, maxDelayCycles, replyDataSet, replyLen, periodic,
|
|
countdown);
|
|
}
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId,
|
|
uint16_t maxDelayCycles,
|
|
LocalPoolDataSetBase* dataSet, size_t replyLen,
|
|
bool periodic, Countdown* countdown) {
|
|
DeviceReplyInfo info;
|
|
info.maxDelayCycles = maxDelayCycles;
|
|
info.periodic = periodic;
|
|
info.delayCycles = 0;
|
|
info.replyLen = replyLen;
|
|
info.dataSet = dataSet;
|
|
info.command = deviceCommandMap.end();
|
|
info.countdown = countdown;
|
|
auto resultPair = deviceReplyMap.emplace(replyId, info);
|
|
if (resultPair.second) {
|
|
return returnvalue::OK;
|
|
} else {
|
|
return returnvalue::FAILED;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceCommand,
|
|
bool useAlternativeReply,
|
|
DeviceCommandId_t alternativeReplyId) {
|
|
DeviceCommandInfo info;
|
|
info.expectedReplies = 0;
|
|
info.isExecuting = false;
|
|
info.sendReplyTo = NO_COMMANDER;
|
|
info.useAlternativeReplyId = useAlternativeReply;
|
|
info.alternativeReplyId = alternativeReplyId;
|
|
auto resultPair = deviceCommandMap.emplace(deviceCommand, info);
|
|
if (resultPair.second) {
|
|
return returnvalue::OK;
|
|
} else {
|
|
return returnvalue::FAILED;
|
|
}
|
|
}
|
|
|
|
size_t DeviceHandlerBase::getNextReplyLength(DeviceCommandId_t commandId) {
|
|
DeviceCommandId_t replyId = NO_COMMAND_ID;
|
|
DeviceCommandMap::iterator command = cookieInfo.pendingCommand;
|
|
if (command->second.useAlternativeReplyId) {
|
|
replyId = command->second.alternativeReplyId;
|
|
} else {
|
|
replyId = commandId;
|
|
}
|
|
DeviceReplyIter iter = deviceReplyMap.find(replyId);
|
|
if (iter != deviceReplyMap.end()) {
|
|
if ((iter->second.delayCycles != 0 && iter->second.countdown == nullptr) ||
|
|
(iter->second.active && iter->second.countdown != nullptr)) {
|
|
return iter->second.replyLen;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::updateReplyMapEntry(DeviceCommandId_t deviceReply,
|
|
uint16_t delayCycles, uint16_t maxDelayCycles,
|
|
bool periodic) {
|
|
auto replyIter = deviceReplyMap.find(deviceReply);
|
|
if (replyIter == deviceReplyMap.end()) {
|
|
triggerEvent(INVALID_DEVICE_COMMAND, deviceReply);
|
|
return COMMAND_NOT_SUPPORTED;
|
|
} else {
|
|
DeviceReplyInfo* info = &(replyIter->second);
|
|
if (maxDelayCycles != 0) {
|
|
info->maxDelayCycles = maxDelayCycles;
|
|
}
|
|
info->delayCycles = delayCycles;
|
|
info->periodic = periodic;
|
|
return returnvalue::OK;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::updatePeriodicReply(bool enable, DeviceCommandId_t deviceReply) {
|
|
auto replyIter = deviceReplyMap.find(deviceReply);
|
|
if (replyIter == deviceReplyMap.end()) {
|
|
triggerEvent(INVALID_DEVICE_COMMAND, deviceReply);
|
|
return COMMAND_NOT_SUPPORTED;
|
|
} else {
|
|
DeviceReplyInfo* info = &(replyIter->second);
|
|
if (not info->periodic) {
|
|
return COMMAND_NOT_SUPPORTED;
|
|
}
|
|
if (enable) {
|
|
info->active = true;
|
|
if (info->countdown != nullptr) {
|
|
info->countdown->resetTimer();
|
|
} else {
|
|
info->delayCycles = info->maxDelayCycles;
|
|
}
|
|
} else {
|
|
info->active = false;
|
|
if (info->countdown != nullptr) {
|
|
info->countdown->timeOut();
|
|
} else {
|
|
info->delayCycles = 0;
|
|
}
|
|
}
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::setReplyDataset(DeviceCommandId_t replyId,
|
|
LocalPoolDataSetBase* dataSet) {
|
|
auto replyIter = deviceReplyMap.find(replyId);
|
|
if (replyIter == deviceReplyMap.end()) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
replyIter->second.dataSet = dataSet;
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void DeviceHandlerBase::callChildStatemachine() {
|
|
if (mode == _MODE_START_UP) {
|
|
doStartUp();
|
|
} else if (mode == _MODE_SHUT_DOWN) {
|
|
doShutDown();
|
|
} else if (mode & TRANSITION_MODE_CHILD_ACTION_MASK) {
|
|
doTransition(transitionSourceMode, transitionSourceSubMode);
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::setTransition(Mode_t modeTo, Submode_t submodeTo) {
|
|
triggerEvent(CHANGING_MODE, modeTo, submodeTo);
|
|
childTransitionDelay = getTransitionDelayMs(mode, modeTo);
|
|
transitionSourceMode = mode;
|
|
transitionSourceSubMode = submode;
|
|
childTransitionFailure = CHILD_TIMEOUT;
|
|
|
|
// transitionTargetMode is set by setMode
|
|
setMode((modeTo | TRANSITION_MODE_CHILD_ACTION_MASK), submodeTo);
|
|
}
|
|
|
|
void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) {
|
|
/* TODO: This will probably be done by the LocalDataPoolManager now */
|
|
// changeHK(mode, submode, false);
|
|
submode = newSubmode;
|
|
mode = newMode;
|
|
modeChanged();
|
|
setNormalDatapoolEntriesInvalid();
|
|
if (!isTransitionalMode()) {
|
|
modeHelper.modeChanged(newMode, newSubmode);
|
|
announceMode(false);
|
|
}
|
|
Clock::getUptime(&timeoutStart);
|
|
|
|
if (mode == MODE_OFF and thermalSet != nullptr) {
|
|
ReturnValue_t result = thermalSet->read();
|
|
if (result == returnvalue::OK) {
|
|
if (thermalSet->heaterRequest.value != ThermalComponentIF::STATE_REQUEST_IGNORE) {
|
|
thermalSet->heaterRequest.value = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL;
|
|
}
|
|
thermalSet->heaterRequest.commit(PoolVariableIF::VALID);
|
|
}
|
|
}
|
|
/* TODO: This will probably be done by the LocalDataPoolManager now */
|
|
// changeHK(mode, submode, true);
|
|
}
|
|
|
|
void DeviceHandlerBase::setMode(Mode_t newMode) { setMode(newMode, submode); }
|
|
|
|
void DeviceHandlerBase::replyReturnvalueToCommand(ReturnValue_t status, uint32_t parameter) {
|
|
// This is actually the reply protocol for raw and misc DH commands.
|
|
if (status == returnvalue::OK) {
|
|
CommandMessage reply(CommandMessage::REPLY_COMMAND_OK, 0, parameter);
|
|
commandQueue->reply(&reply);
|
|
} else {
|
|
CommandMessage reply(CommandMessage::REPLY_REJECTED, status, parameter);
|
|
commandQueue->reply(&reply);
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::replyToCommand(ReturnValue_t status, uint32_t parameter) {
|
|
// Check if we reply to a raw command.
|
|
if (cookieInfo.pendingCommand->first == RAW_COMMAND_ID) {
|
|
if (status == NO_REPLY_EXPECTED) {
|
|
status = returnvalue::OK;
|
|
}
|
|
replyReturnvalueToCommand(status, parameter);
|
|
// Always delete data from a raw command.
|
|
IPCStore->deleteData(storedRawData);
|
|
return;
|
|
}
|
|
// Check if we were externally commanded.
|
|
if (cookieInfo.pendingCommand->second.sendReplyTo != NO_COMMANDER) {
|
|
MessageQueueId_t queueId = cookieInfo.pendingCommand->second.sendReplyTo;
|
|
if (status == NO_REPLY_EXPECTED) {
|
|
actionHelper.finish(true, queueId, cookieInfo.pendingCommand->first, returnvalue::OK);
|
|
} else {
|
|
actionHelper.step(1, queueId, cookieInfo.pendingCommand->first, status);
|
|
}
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::replyToReply(const DeviceCommandId_t command, DeviceReplyInfo& replyInfo,
|
|
ReturnValue_t status) {
|
|
// No need to check if iter exists, as this is checked by callers.
|
|
// If someone else uses the method, add check.
|
|
if (replyInfo.command == deviceCommandMap.end()) {
|
|
// Is most likely periodic reply. Silent return.
|
|
return;
|
|
}
|
|
DeviceCommandInfo* info = &replyInfo.command->second;
|
|
if (info == nullptr) {
|
|
printWarningOrError(sif::OutputTypes::OUT_ERROR, "replyToReply", returnvalue::FAILED,
|
|
"Command pointer not found");
|
|
return;
|
|
}
|
|
|
|
if (info->expectedReplies > 0) {
|
|
// Check before to avoid underflow
|
|
info->expectedReplies--;
|
|
}
|
|
// Check if more replies are expected. If so, do nothing.
|
|
if (info->expectedReplies == 0) {
|
|
// Check if it was transition or internal command.
|
|
// Don't send any replies in that case.
|
|
if (info->sendReplyTo != NO_COMMANDER) {
|
|
bool success = false;
|
|
if (status == returnvalue::OK) {
|
|
success = true;
|
|
}
|
|
actionHelper.finish(success, info->sendReplyTo, command, status);
|
|
}
|
|
info->isExecuting = false;
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::doSendWrite() {
|
|
if (cookieInfo.state == COOKIE_WRITE_READY) {
|
|
ReturnValue_t result = communicationInterface->sendMessage(comCookie, rawPacket, rawPacketLen);
|
|
|
|
if (result == returnvalue::OK) {
|
|
cookieInfo.state = COOKIE_WRITE_SENT;
|
|
} else {
|
|
// always generate a failure event, so that FDIR knows what's up
|
|
triggerEvent(DEVICE_SENDING_COMMAND_FAILED, result, cookieInfo.pendingCommand->first);
|
|
replyToCommand(result);
|
|
cookieInfo.state = COOKIE_UNUSED;
|
|
cookieInfo.pendingCommand->second.isExecuting = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::doGetWrite() {
|
|
if (cookieInfo.state != COOKIE_WRITE_SENT) {
|
|
return;
|
|
}
|
|
cookieInfo.state = COOKIE_UNUSED;
|
|
ReturnValue_t result = communicationInterface->getSendSuccess(comCookie);
|
|
if (result == returnvalue::OK) {
|
|
if (wiretappingMode == RAW) {
|
|
replyRawData(rawPacket, rawPacketLen, requestedRawTraffic, true);
|
|
}
|
|
|
|
// We need to distinguish here, because a raw command never expects a reply.
|
|
//(Could be done in eRIRM, but then child implementations need to be careful.
|
|
DeviceCommandMap::iterator command = cookieInfo.pendingCommand;
|
|
result = enableReplyInReplyMap(command, 1, command->second.useAlternativeReplyId,
|
|
command->second.alternativeReplyId);
|
|
} else {
|
|
// always generate a failure event, so that FDIR knows what's up
|
|
triggerEvent(DEVICE_SENDING_COMMAND_FAILED, result, cookieInfo.pendingCommand->first);
|
|
}
|
|
if (result != returnvalue::OK) {
|
|
cookieInfo.pendingCommand->second.isExecuting = false;
|
|
}
|
|
replyToCommand(result);
|
|
}
|
|
|
|
void DeviceHandlerBase::doSendRead() {
|
|
ReturnValue_t result;
|
|
|
|
result = doSendReadHook();
|
|
if (result != returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
size_t replyLen = 0;
|
|
if (cookieInfo.pendingCommand != deviceCommandMap.end()) {
|
|
replyLen = getNextReplyLength(cookieInfo.pendingCommand->first);
|
|
}
|
|
|
|
result = communicationInterface->requestReceiveMessage(comCookie, replyLen);
|
|
|
|
if (result == returnvalue::OK) {
|
|
cookieInfo.state = COOKIE_READ_SENT;
|
|
} else {
|
|
triggerEvent(DEVICE_REQUESTING_REPLY_FAILED, result);
|
|
// We can't inform anyone, because we don't know which command was sent last.
|
|
// So, we need to wait for a timeout.
|
|
// but I think we can allow to ignore one missedReply.
|
|
ignoreMissedRepliesCount++;
|
|
cookieInfo.state = COOKIE_UNUSED;
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::doGetRead() {
|
|
size_t receivedDataLen = 0;
|
|
uint8_t* receivedData = nullptr;
|
|
|
|
if (cookieInfo.state != COOKIE_READ_SENT) {
|
|
cookieInfo.state = COOKIE_UNUSED;
|
|
return;
|
|
}
|
|
|
|
cookieInfo.state = COOKIE_UNUSED;
|
|
|
|
ReturnValue_t result =
|
|
communicationInterface->readReceivedMessage(comCookie, &receivedData, &receivedDataLen);
|
|
|
|
if (result != returnvalue::OK) {
|
|
triggerEvent(DEVICE_REQUESTING_REPLY_FAILED, result);
|
|
// I think we can allow to ignore one missedReply.
|
|
ignoreMissedRepliesCount++;
|
|
return;
|
|
}
|
|
|
|
if (receivedDataLen == 0 or result == DeviceCommunicationIF::NO_REPLY_RECEIVED) return;
|
|
|
|
if (wiretappingMode == RAW) {
|
|
replyRawData(receivedData, receivedDataLen, requestedRawTraffic);
|
|
}
|
|
|
|
if (mode == MODE_RAW) {
|
|
if (defaultRawReceiver != MessageQueueIF::NO_QUEUE) {
|
|
replyRawReplyIfnotWiretapped(receivedData, receivedDataLen);
|
|
}
|
|
} else {
|
|
parseReply(receivedData, receivedDataLen);
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::parseReply(const uint8_t* receivedData, size_t receivedDataLen) {
|
|
ReturnValue_t result = returnvalue::FAILED;
|
|
DeviceCommandId_t foundId = DeviceHandlerIF::NO_COMMAND_ID;
|
|
size_t foundLen = 0;
|
|
/* The loop may not execute more often than the number of received bytes
|
|
(worst case). This approach avoids infinite loops due to buggy scanForReply routines. */
|
|
uint32_t remainingLength = receivedDataLen;
|
|
for (uint32_t count = 0; count < receivedDataLen; count++) {
|
|
result = scanForReply(receivedData, remainingLength, &foundId, &foundLen);
|
|
switch (result) {
|
|
case returnvalue::OK:
|
|
handleReply(receivedData, foundId, foundLen);
|
|
if (foundLen == 0) {
|
|
printWarningOrError(sif::OutputTypes::OUT_WARNING, "parseReply",
|
|
ObjectManagerIF::CHILD_INIT_FAILED,
|
|
"Found length is one, parsing might be stuck");
|
|
}
|
|
break;
|
|
case APERIODIC_REPLY: {
|
|
result = interpretDeviceReply(foundId, receivedData);
|
|
if (result != returnvalue::OK) {
|
|
replyRawReplyIfnotWiretapped(receivedData, foundLen);
|
|
triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, foundId);
|
|
}
|
|
if (foundLen == 0) {
|
|
printWarningOrError(sif::OutputTypes::OUT_ERROR, "parseReply",
|
|
ObjectManagerIF::CHILD_INIT_FAILED,
|
|
"Power switcher set but no valid object found.");
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::warning << "DeviceHandlerBase::parseReply: foundLen is 0!"
|
|
" Packet parsing will be stuck."
|
|
<< std::endl;
|
|
#endif
|
|
}
|
|
break;
|
|
}
|
|
case IGNORE_REPLY_DATA:
|
|
continue;
|
|
case IGNORE_FULL_PACKET:
|
|
return;
|
|
default:
|
|
// We need to wait for timeout.. don't know what command failed
|
|
// and who sent it.
|
|
replyRawReplyIfnotWiretapped(receivedData, foundLen);
|
|
triggerEvent(DEVICE_READING_REPLY_FAILED, result, foundLen);
|
|
break;
|
|
}
|
|
receivedData += foundLen;
|
|
if (remainingLength > foundLen) {
|
|
remainingLength -= foundLen;
|
|
} else {
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::handleReply(const uint8_t* receivedData, DeviceCommandId_t foundId,
|
|
uint32_t foundLen) {
|
|
ReturnValue_t result;
|
|
DeviceReplyMap::iterator iter = deviceReplyMap.find(foundId);
|
|
|
|
if (iter == deviceReplyMap.end()) {
|
|
replyRawReplyIfnotWiretapped(receivedData, foundLen);
|
|
triggerEvent(DEVICE_UNKNOWN_REPLY, foundId);
|
|
return;
|
|
}
|
|
|
|
DeviceReplyInfo* info = &(iter->second);
|
|
|
|
if ((info->delayCycles != 0 && info->countdown == nullptr) ||
|
|
(info->active && info->countdown != nullptr)) {
|
|
result = interpretDeviceReply(foundId, receivedData);
|
|
|
|
if (result == IGNORE_REPLY_DATA) {
|
|
return;
|
|
}
|
|
|
|
if (info->active && info->countdown != nullptr) {
|
|
resetTimeoutControlledReply(info);
|
|
} else if (info->delayCycles != 0) {
|
|
resetDelayCyclesControlledReply(info);
|
|
}
|
|
|
|
if (result != returnvalue::OK) {
|
|
// Report failed interpretation to FDIR.
|
|
replyRawReplyIfnotWiretapped(receivedData, foundLen);
|
|
triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, foundId);
|
|
}
|
|
replyToReply(iter->first, iter->second, result);
|
|
} else {
|
|
/* Other completion failure messages are created by timeout.
|
|
Powering down the device might take some time during which periodic
|
|
replies may still come in. */
|
|
if (mode != _MODE_WAIT_OFF) {
|
|
triggerEvent(DEVICE_UNREQUESTED_REPLY, foundId);
|
|
}
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::resetTimeoutControlledReply(DeviceReplyInfo* info) {
|
|
if (info->periodic) {
|
|
info->countdown->resetTimer();
|
|
} else {
|
|
info->active = false;
|
|
info->countdown->timeOut();
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::resetDelayCyclesControlledReply(DeviceReplyInfo* info) {
|
|
if (info->periodic) {
|
|
info->delayCycles = info->maxDelayCycles;
|
|
} else {
|
|
info->delayCycles = 0;
|
|
info->active = false;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::getStorageData(store_address_t storageAddress, uint8_t** data,
|
|
size_t* len) {
|
|
size_t lenTmp;
|
|
|
|
if (IPCStore == nullptr) {
|
|
*data = nullptr;
|
|
*len = 0;
|
|
return returnvalue::FAILED;
|
|
}
|
|
ReturnValue_t result = IPCStore->modifyData(storageAddress, data, &lenTmp);
|
|
if (result == returnvalue::OK) {
|
|
*len = lenTmp;
|
|
return returnvalue::OK;
|
|
} else {
|
|
triggerEvent(StorageManagerIF::GET_DATA_FAILED, result, storageAddress.raw);
|
|
*data = nullptr;
|
|
*len = 0;
|
|
return result;
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::replyRawData(const uint8_t* data, size_t len, MessageQueueId_t sendTo,
|
|
bool isCommand) {
|
|
if (IPCStore == nullptr or len == 0 or sendTo == MessageQueueIF::NO_QUEUE) {
|
|
return;
|
|
}
|
|
store_address_t address;
|
|
ReturnValue_t result = IPCStore->addData(&address, data, len);
|
|
|
|
if (result != returnvalue::OK) {
|
|
triggerEvent(StorageManagerIF::STORE_DATA_FAILED, result);
|
|
return;
|
|
}
|
|
|
|
CommandMessage command;
|
|
|
|
DeviceHandlerMessage::setDeviceHandlerRawReplyMessage(&command, getObjectId(), address,
|
|
isCommand);
|
|
|
|
result = commandQueue->sendMessage(sendTo, &command);
|
|
|
|
if (result != returnvalue::OK) {
|
|
IPCStore->deleteData(address);
|
|
// Silently discard data, this indicates heavy TM traffic which
|
|
// should not be increased by additional events.
|
|
}
|
|
}
|
|
|
|
// Default child implementations
|
|
DeviceHandlerIF::CommunicationAction DeviceHandlerBase::getComAction() {
|
|
switch (pstStep) {
|
|
case 0:
|
|
return CommunicationAction::PERFORM_OPERATION;
|
|
break;
|
|
case 1:
|
|
return CommunicationAction::SEND_WRITE;
|
|
break;
|
|
case 2:
|
|
return CommunicationAction::GET_WRITE;
|
|
break;
|
|
case 3:
|
|
return CommunicationAction::SEND_READ;
|
|
break;
|
|
case 4:
|
|
return CommunicationAction::GET_READ;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return CommunicationAction::NOTHING;
|
|
}
|
|
|
|
MessageQueueId_t DeviceHandlerBase::getCommandQueue() const { return commandQueue->getId(); }
|
|
|
|
void DeviceHandlerBase::buildRawDeviceCommand(CommandMessage* commandMessage) {
|
|
storedRawData = DeviceHandlerMessage::getStoreAddress(commandMessage);
|
|
ReturnValue_t result = getStorageData(storedRawData, &rawPacket, &rawPacketLen);
|
|
if (result != returnvalue::OK) {
|
|
replyReturnvalueToCommand(result, RAW_COMMAND_ID);
|
|
storedRawData.raw = StorageManagerIF::INVALID_ADDRESS;
|
|
} else {
|
|
cookieInfo.pendingCommand = deviceCommandMap.find((DeviceCommandId_t)RAW_COMMAND_ID);
|
|
cookieInfo.pendingCommand->second.isExecuting = true;
|
|
cookieInfo.state = COOKIE_WRITE_READY;
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::commandSwitch(ReturnValue_t onOff) {
|
|
if (powerSwitcher == nullptr) {
|
|
return;
|
|
}
|
|
const uint8_t* switches;
|
|
uint8_t numberOfSwitches = 0;
|
|
ReturnValue_t result = getSwitches(&switches, &numberOfSwitches);
|
|
if (result == returnvalue::OK) {
|
|
while (numberOfSwitches > 0) {
|
|
powerSwitcher->sendSwitchCommand(switches[numberOfSwitches - 1], onOff);
|
|
numberOfSwitches--;
|
|
}
|
|
}
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::doSendReadHook() { return returnvalue::OK; }
|
|
|
|
ReturnValue_t DeviceHandlerBase::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) {
|
|
return DeviceHandlerBase::NO_SWITCH;
|
|
}
|
|
|
|
void DeviceHandlerBase::modeChanged(void) {}
|
|
|
|
ReturnValue_t DeviceHandlerBase::enableReplyInReplyMap(DeviceCommandMap::iterator command,
|
|
uint8_t expectedReplies,
|
|
bool useAlternativeId,
|
|
DeviceCommandId_t alternativeReply) {
|
|
DeviceReplyMap::iterator iter;
|
|
if (useAlternativeId) {
|
|
iter = deviceReplyMap.find(alternativeReply);
|
|
} else {
|
|
iter = deviceReplyMap.find(command->first);
|
|
}
|
|
if (iter != deviceReplyMap.end()) {
|
|
DeviceReplyInfo* info = &(iter->second);
|
|
// If a countdown has been set, the delay cycles will be ignored and the reply times out
|
|
// as soon as the countdown has expired
|
|
info->delayCycles = info->maxDelayCycles;
|
|
info->command = command;
|
|
command->second.expectedReplies = expectedReplies;
|
|
if (info->countdown != nullptr) {
|
|
info->countdown->resetTimer();
|
|
}
|
|
info->active = true;
|
|
return returnvalue::OK;
|
|
} else {
|
|
return NO_REPLY_EXPECTED;
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
|
|
setMode(getBaseMode(mode));
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::getStateOfSwitches(void) {
|
|
if (powerSwitcher == nullptr) {
|
|
return NO_SWITCH;
|
|
}
|
|
uint8_t numberOfSwitches = 0;
|
|
const uint8_t* switches;
|
|
|
|
ReturnValue_t result = getSwitches(&switches, &numberOfSwitches);
|
|
if ((result == returnvalue::OK) && (numberOfSwitches != 0)) {
|
|
while (numberOfSwitches > 0) {
|
|
if (powerSwitcher->getSwitchState(switches[numberOfSwitches - 1]) ==
|
|
PowerSwitchIF::SWITCH_OFF) {
|
|
return PowerSwitchIF::SWITCH_OFF;
|
|
}
|
|
numberOfSwitches--;
|
|
}
|
|
return PowerSwitchIF::SWITCH_ON;
|
|
}
|
|
|
|
return NO_SWITCH;
|
|
}
|
|
|
|
Mode_t DeviceHandlerBase::getBaseMode(Mode_t transitionMode) {
|
|
// only child action special modes are handled, as a child should
|
|
// never see any base action modes
|
|
if (transitionMode == _MODE_START_UP) {
|
|
return _MODE_TO_ON;
|
|
}
|
|
if (transitionMode == _MODE_SHUT_DOWN) {
|
|
return _MODE_POWER_DOWN;
|
|
}
|
|
return transitionMode & ~(TRANSITION_MODE_BASE_ACTION_MASK | TRANSITION_MODE_CHILD_ACTION_MASK);
|
|
}
|
|
|
|
// SHOULDDO: Allow transition from OFF to NORMAL to reduce complexity in assemblies. And, by the
|
|
// way, throw away DHB and write a new one:
|
|
// - Include power and thermal completely, but more modular :-)
|
|
// - Don't use modes for state transitions, reduce FSM (Finte State Machine) complexity.
|
|
// - Modularization?
|
|
ReturnValue_t DeviceHandlerBase::checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
|
|
uint32_t* msToReachTheMode) {
|
|
if (isTransitionalMode()) {
|
|
return IN_TRANSITION;
|
|
}
|
|
if ((mode == MODE_ERROR_ON) && (commandedMode != MODE_OFF)) {
|
|
return TRANS_NOT_ALLOWED;
|
|
}
|
|
if ((commandedMode == MODE_NORMAL) && (mode == MODE_OFF)) {
|
|
return TRANS_NOT_ALLOWED;
|
|
}
|
|
|
|
if ((commandedMode == MODE_ON) && (mode == MODE_OFF) and (thermalSet != nullptr)) {
|
|
ReturnValue_t result = thermalSet->read();
|
|
if (result == returnvalue::OK) {
|
|
if ((thermalSet->heaterRequest.value != ThermalComponentIF::STATE_REQUEST_IGNORE) and
|
|
(not ThermalComponentIF::isOperational(thermalSet->thermalState.value))) {
|
|
triggerEvent(ThermalComponentIF::TEMP_NOT_IN_OP_RANGE, thermalSet->thermalState.value);
|
|
return NON_OP_TEMPERATURE;
|
|
}
|
|
}
|
|
}
|
|
|
|
return isModeCombinationValid(commandedMode, commandedSubmode);
|
|
}
|
|
|
|
void DeviceHandlerBase::startTransition(Mode_t commandedMode, Submode_t commandedSubmode) {
|
|
switch (commandedMode) {
|
|
case MODE_ON:
|
|
handleTransitionToOnMode(commandedMode, commandedSubmode);
|
|
break;
|
|
case MODE_OFF:
|
|
if (mode == MODE_OFF) {
|
|
triggerEvent(CHANGING_MODE, commandedMode, commandedSubmode);
|
|
setMode(_MODE_POWER_DOWN, commandedSubmode);
|
|
} else {
|
|
// already set the delay for the child transition
|
|
// so we don't need to call it twice
|
|
childTransitionDelay = getTransitionDelayMs(mode, _MODE_POWER_DOWN);
|
|
transitionSourceMode = _MODE_POWER_DOWN;
|
|
transitionSourceSubMode = commandedSubmode;
|
|
childTransitionFailure = CHILD_TIMEOUT;
|
|
setMode(_MODE_SHUT_DOWN, commandedSubmode);
|
|
triggerEvent(CHANGING_MODE, commandedMode, commandedSubmode);
|
|
}
|
|
break;
|
|
case MODE_RAW:
|
|
if (mode != MODE_OFF) {
|
|
setTransition(MODE_RAW, commandedSubmode);
|
|
} else {
|
|
setMode(MODE_RAW, commandedSubmode);
|
|
}
|
|
break;
|
|
case MODE_NORMAL:
|
|
if (mode != MODE_OFF) {
|
|
setTransition(MODE_NORMAL, commandedSubmode);
|
|
} else {
|
|
replyReturnvalueToCommand(HasModesIF::TRANS_NOT_ALLOWED);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::handleTransitionToOnMode(Mode_t commandedMode, Submode_t commandedSubmode) {
|
|
if (mode == MODE_OFF) {
|
|
transitionSourceMode = _MODE_POWER_DOWN;
|
|
transitionSourceSubMode = SUBMODE_NONE;
|
|
setMode(_MODE_POWER_ON, commandedSubmode);
|
|
// already set the delay for the child transition so we don't
|
|
// need to call it twice
|
|
childTransitionDelay = getTransitionDelayMs(_MODE_START_UP, MODE_ON);
|
|
triggerEvent(CHANGING_MODE, commandedMode, commandedSubmode);
|
|
if (thermalSet != nullptr) {
|
|
ReturnValue_t result = thermalSet->read();
|
|
if (result == returnvalue::OK) {
|
|
if (thermalSet->heaterRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) {
|
|
thermalSet->heaterRequest = ThermalComponentIF::STATE_REQUEST_OPERATIONAL;
|
|
thermalSet->commit();
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
setTransition(MODE_ON, commandedSubmode);
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::getMode(Mode_t* mode, Submode_t* submode) {
|
|
*mode = this->mode;
|
|
*submode = this->submode;
|
|
}
|
|
|
|
void DeviceHandlerBase::setToExternalControl() { healthHelper.setHealth(EXTERNAL_CONTROL); }
|
|
|
|
void DeviceHandlerBase::announceMode(bool recursive) { triggerEvent(MODE_INFO, mode, submode); }
|
|
|
|
bool DeviceHandlerBase::dontCheckQueue() { return false; }
|
|
|
|
void DeviceHandlerBase::missedReply(DeviceCommandId_t id) {
|
|
if (ignoreMissedRepliesCount > 0) {
|
|
ignoreMissedRepliesCount--;
|
|
} else {
|
|
triggerEvent(DEVICE_MISSED_REPLY, id);
|
|
}
|
|
}
|
|
|
|
HasHealthIF::HealthState DeviceHandlerBase::getHealth() { return healthHelper.getHealth(); }
|
|
|
|
ReturnValue_t DeviceHandlerBase::setHealth(HealthState health) {
|
|
healthHelper.setHealth(health);
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void DeviceHandlerBase::checkSwitchState() {
|
|
if ((mode == MODE_ON || mode == MODE_NORMAL)) {
|
|
// We only check in ON and NORMAL, ignore RAW and ERROR_ON.
|
|
ReturnValue_t result = getStateOfSwitches();
|
|
if (result == PowerSwitchIF::SWITCH_OFF && !switchOffWasReported) {
|
|
triggerEvent(PowerSwitchIF::SWITCH_WENT_OFF);
|
|
switchOffWasReported = true;
|
|
}
|
|
} else {
|
|
switchOffWasReported = false;
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::doOnActivity() {}
|
|
|
|
ReturnValue_t DeviceHandlerBase::acceptExternalDeviceCommands() {
|
|
if ((mode != MODE_ON) && (mode != MODE_NORMAL)) {
|
|
return WRONG_MODE_FOR_COMMAND;
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void DeviceHandlerBase::replyRawReplyIfnotWiretapped(const uint8_t* data, size_t len) {
|
|
if ((wiretappingMode == RAW) && (defaultRawReceiver == requestedRawTraffic)) {
|
|
// The raw packet was already sent by the wiretapping service
|
|
} else {
|
|
replyRawData(data, len, defaultRawReceiver);
|
|
}
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::handleDeviceHandlerMessage(CommandMessage* message) {
|
|
switch (message->getCommand()) {
|
|
case DeviceHandlerMessage::CMD_WIRETAPPING:
|
|
switch (DeviceHandlerMessage::getWiretappingMode(message)) {
|
|
case RAW:
|
|
wiretappingMode = RAW;
|
|
requestedRawTraffic = commandQueue->getLastPartner();
|
|
break;
|
|
case TM:
|
|
wiretappingMode = TM;
|
|
requestedRawTraffic = commandQueue->getLastPartner();
|
|
break;
|
|
case OFF:
|
|
wiretappingMode = OFF;
|
|
break;
|
|
default:
|
|
replyReturnvalueToCommand(INVALID_COMMAND_PARAMETER);
|
|
wiretappingMode = OFF;
|
|
return returnvalue::OK;
|
|
}
|
|
replyReturnvalueToCommand(returnvalue::OK);
|
|
return returnvalue::OK;
|
|
case DeviceHandlerMessage::CMD_RAW:
|
|
if ((mode != MODE_RAW)) {
|
|
DeviceHandlerMessage::clear(message);
|
|
replyReturnvalueToCommand(WRONG_MODE_FOR_COMMAND);
|
|
} else {
|
|
buildRawDeviceCommand(message);
|
|
}
|
|
return returnvalue::OK;
|
|
default:
|
|
return returnvalue::FAILED;
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::setParentQueue(MessageQueueId_t parentQueueId) {
|
|
modeHelper.setParentQueue(parentQueueId);
|
|
healthHelper.setParentQueue(parentQueueId);
|
|
}
|
|
|
|
bool DeviceHandlerBase::isAwaitingReply() {
|
|
std::map<DeviceCommandId_t, DeviceReplyInfo>::iterator iter;
|
|
for (iter = deviceReplyMap.begin(); iter != deviceReplyMap.end(); ++iter) {
|
|
if ((iter->second.delayCycles != 0 && iter->second.countdown == nullptr) ||
|
|
(iter->second.active && iter->second.countdown != nullptr)) {
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::letChildHandleMessage(CommandMessage* message) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
void DeviceHandlerBase::handleDeviceTm(const uint8_t* rawData, size_t rawDataLen,
|
|
DeviceCommandId_t replyId, bool forceDirectTm) {
|
|
SerialBufferAdapter bufferWrapper(rawData, rawDataLen);
|
|
handleDeviceTm(bufferWrapper, replyId, forceDirectTm);
|
|
}
|
|
|
|
void DeviceHandlerBase::handleDeviceTm(const SerializeIF& dataSet, DeviceCommandId_t replyId,
|
|
bool forceDirectTm) {
|
|
auto iter = deviceReplyMap.find(replyId);
|
|
if (iter == deviceReplyMap.end()) {
|
|
triggerEvent(DEVICE_UNKNOWN_REPLY, replyId);
|
|
return;
|
|
}
|
|
|
|
// Regular replies to a command
|
|
if (iter->second.command != deviceCommandMap.end()) {
|
|
MessageQueueId_t queueId = iter->second.command->second.sendReplyTo;
|
|
|
|
if (queueId != NO_COMMANDER) {
|
|
// This may fail, but we'll ignore the fault.
|
|
actionHelper.reportData(queueId, replyId, const_cast<SerializeIF*>(&dataSet));
|
|
}
|
|
|
|
// This check should make sure we get any TM but don't get anything doubled.
|
|
if (wiretappingMode == TM && (requestedRawTraffic != queueId)) {
|
|
DeviceTmReportingWrapper wrapper(getObjectId(), replyId, dataSet);
|
|
actionHelper.reportData(requestedRawTraffic, replyId, &wrapper);
|
|
}
|
|
|
|
else if (forceDirectTm and (defaultRawReceiver != queueId) and
|
|
(defaultRawReceiver != MessageQueueIF::NO_QUEUE)) {
|
|
// hiding of sender needed so the service will handle it as
|
|
// unexpected Data, no matter what state (progress or completed)
|
|
// it is in
|
|
actionHelper.reportData(defaultRawReceiver, replyId, const_cast<SerializeIF*>(&dataSet),
|
|
true);
|
|
}
|
|
}
|
|
// Unrequested or aperiodic replies
|
|
else {
|
|
DeviceTmReportingWrapper wrapper(getObjectId(), replyId, dataSet);
|
|
if (wiretappingMode == TM) {
|
|
actionHelper.reportData(requestedRawTraffic, replyId, &wrapper);
|
|
}
|
|
if (forceDirectTm and defaultRawReceiver != MessageQueueIF::NO_QUEUE) {
|
|
// hiding of sender needed so the service will handle it as
|
|
// unexpected Data, no matter what state (progress or completed)
|
|
// it is in
|
|
actionHelper.reportData(defaultRawReceiver, replyId, &wrapper, true);
|
|
}
|
|
}
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
|
|
const uint8_t* data, size_t size) {
|
|
ReturnValue_t result = acceptExternalDeviceCommands();
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
DeviceCommandMap::iterator iter = deviceCommandMap.find(actionId);
|
|
if (iter == deviceCommandMap.end()) {
|
|
result = COMMAND_NOT_SUPPORTED;
|
|
} else if (iter->second.isExecuting) {
|
|
result = COMMAND_ALREADY_SENT;
|
|
} else {
|
|
iter->second.sendReplyTo = commandedBy;
|
|
result = buildCommandFromCommand(actionId, data, size);
|
|
}
|
|
if (result == returnvalue::OK) {
|
|
iter->second.isExecuting = true;
|
|
cookieInfo.pendingCommand = iter;
|
|
cookieInfo.state = COOKIE_WRITE_READY;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void DeviceHandlerBase::buildInternalCommand(void) {
|
|
/* Neither raw nor direct could build a command */
|
|
ReturnValue_t result = NOTHING_TO_SEND;
|
|
DeviceCommandId_t deviceCommandId = NO_COMMAND_ID;
|
|
if (mode == MODE_NORMAL) {
|
|
result = buildNormalDeviceCommand(&deviceCommandId);
|
|
if (result == BUSY) {
|
|
/* So we can track misconfigurations */
|
|
printWarningOrError(sif::OutputTypes::OUT_WARNING, "buildInternalCommand",
|
|
returnvalue::FAILED, "Busy.");
|
|
/* No need to report this */
|
|
result = NOTHING_TO_SEND;
|
|
}
|
|
} else if (mode == MODE_RAW) {
|
|
result = buildChildRawCommand();
|
|
deviceCommandId = RAW_COMMAND_ID;
|
|
} else if (mode & TRANSITION_MODE_CHILD_ACTION_MASK) {
|
|
result = buildTransitionDeviceCommand(&deviceCommandId);
|
|
} else {
|
|
return;
|
|
}
|
|
|
|
if (result == NOTHING_TO_SEND) {
|
|
return;
|
|
}
|
|
if (result == returnvalue::OK) {
|
|
DeviceCommandMap::iterator iter = deviceCommandMap.find(deviceCommandId);
|
|
if (iter == deviceCommandMap.end()) {
|
|
#if FSFW_VERBOSE_LEVEL >= 1
|
|
char output[36];
|
|
sprintf(output, "Command 0x%08x unknown", static_cast<unsigned int>(deviceCommandId));
|
|
// so we can track misconfigurations
|
|
printWarningOrError(sif::OutputTypes::OUT_WARNING, "buildInternalCommand",
|
|
COMMAND_NOT_SUPPORTED, output);
|
|
#endif
|
|
result = COMMAND_NOT_SUPPORTED;
|
|
} else if (iter->second.isExecuting) {
|
|
#if FSFW_VERBOSE_LEVEL >= 1
|
|
char output[36];
|
|
sprintf(output, "Command 0x%08x is executing", static_cast<unsigned int>(deviceCommandId));
|
|
// so we can track misconfigurations
|
|
printWarningOrError(sif::OutputTypes::OUT_WARNING, "buildInternalCommand",
|
|
returnvalue::FAILED, output);
|
|
#endif
|
|
// this is an internal command, no need to report a failure here,
|
|
// missed reply will track if a reply is too late, otherwise, it's ok
|
|
return;
|
|
} else {
|
|
iter->second.sendReplyTo = NO_COMMANDER;
|
|
iter->second.isExecuting = true;
|
|
cookieInfo.pendingCommand = iter;
|
|
cookieInfo.state = COOKIE_WRITE_READY;
|
|
}
|
|
}
|
|
if (result != returnvalue::OK) {
|
|
triggerEvent(DEVICE_BUILDING_COMMAND_FAILED, result, deviceCommandId);
|
|
}
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::buildChildRawCommand() { return NOTHING_TO_SEND; }
|
|
|
|
uint8_t DeviceHandlerBase::getReplyDelayCycles(DeviceCommandId_t deviceCommand) {
|
|
DeviceReplyMap::iterator iter = deviceReplyMap.find(deviceCommand);
|
|
if (iter == deviceReplyMap.end()) {
|
|
return 0;
|
|
} else if (iter->second.countdown != nullptr) {
|
|
// fake a useful return value for legacy code
|
|
if (iter->second.active) {
|
|
return 1;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
return iter->second.delayCycles;
|
|
}
|
|
|
|
Mode_t DeviceHandlerBase::getTransitionSourceMode() const { return transitionSourceMode; }
|
|
|
|
Submode_t DeviceHandlerBase::getTransitionSourceSubMode() const { return transitionSourceSubMode; }
|
|
|
|
void DeviceHandlerBase::triggerEvent(Event event, uint32_t parameter1, uint32_t parameter2) {
|
|
fdirInstance->triggerEvent(event, parameter1, parameter2);
|
|
}
|
|
|
|
void DeviceHandlerBase::forwardEvent(Event event, uint32_t parameter1, uint32_t parameter2) const {
|
|
fdirInstance->triggerEvent(event, parameter1, parameter2);
|
|
}
|
|
|
|
void DeviceHandlerBase::doOffActivity() {}
|
|
|
|
ReturnValue_t DeviceHandlerBase::getParameter(uint8_t domainId, uint8_t uniqueId,
|
|
ParameterWrapper* parameterWrapper,
|
|
const ParameterWrapper* newValues,
|
|
uint16_t startAtIndex) {
|
|
ReturnValue_t result =
|
|
fdirInstance->getParameter(domainId, uniqueId, parameterWrapper, newValues, startAtIndex);
|
|
if (result != INVALID_DOMAIN_ID) {
|
|
return result;
|
|
}
|
|
return INVALID_DOMAIN_ID;
|
|
}
|
|
|
|
bool DeviceHandlerBase::isTransitionalMode() {
|
|
return ((mode & (TRANSITION_MODE_BASE_ACTION_MASK | TRANSITION_MODE_CHILD_ACTION_MASK)) != 0);
|
|
}
|
|
|
|
bool DeviceHandlerBase::commandIsExecuting(DeviceCommandId_t commandId) {
|
|
auto iter = deviceCommandMap.find(commandId);
|
|
if (iter != deviceCommandMap.end()) {
|
|
return iter->second.isExecuting;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::setTaskIF(PeriodicTaskIF* task) { executingTask = task; }
|
|
|
|
void DeviceHandlerBase::debugInterface(uint8_t positionTracker, object_id_t objectId,
|
|
uint32_t parameter) {}
|
|
|
|
void DeviceHandlerBase::performOperationHook() {}
|
|
|
|
ReturnValue_t DeviceHandlerBase::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
|
LocalDataPoolManager& poolManager) {
|
|
if (thermalSet != nullptr) {
|
|
localDataPoolMap.emplace(thermalSet->thermalStatePoolId,
|
|
new PoolEntry<DeviceHandlerIF::dh_thermal_state_t>);
|
|
localDataPoolMap.emplace(thermalSet->heaterRequestPoolId,
|
|
new PoolEntry<DeviceHandlerIF::dh_heater_request_t>);
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() {
|
|
// In this function, the task handle should be valid if the task
|
|
// was implemented correctly. We still check to be 1000 % sure :-)
|
|
if (executingTask != nullptr) {
|
|
pstIntervalMs = executingTask->getPeriodMs();
|
|
}
|
|
this->poolManager.initializeAfterTaskCreation();
|
|
|
|
if (setStartupImmediately) {
|
|
startTransition(MODE_ON, SUBMODE_NONE);
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
LocalPoolDataSetBase* DeviceHandlerBase::getDataSetHandle(sid_t sid) {
|
|
auto iter = deviceReplyMap.find(sid.ownerSetId);
|
|
if (iter != deviceReplyMap.end()) {
|
|
return iter->second.dataSet;
|
|
} else {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
object_id_t DeviceHandlerBase::getObjectId() const { return SystemObject::getObjectId(); }
|
|
|
|
void DeviceHandlerBase::setStartUpImmediately() { this->setStartupImmediately = true; }
|
|
|
|
dur_millis_t DeviceHandlerBase::getPeriodicOperationFrequency() const { return pstIntervalMs; }
|
|
|
|
DeviceCommandId_t DeviceHandlerBase::getPendingCommand() const {
|
|
if (cookieInfo.pendingCommand != deviceCommandMap.end()) {
|
|
return cookieInfo.pendingCommand->first;
|
|
}
|
|
return DeviceHandlerIF::NO_COMMAND_ID;
|
|
}
|
|
|
|
void DeviceHandlerBase::setNormalDatapoolEntriesInvalid() {
|
|
for (const auto& reply : deviceReplyMap) {
|
|
if (reply.second.dataSet != nullptr) {
|
|
reply.second.dataSet->setValidity(false, true);
|
|
}
|
|
}
|
|
}
|
|
|
|
void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType, const char* functionName,
|
|
ReturnValue_t errorCode, const char* errorPrint) {
|
|
if (errorPrint == nullptr) {
|
|
if (errorCode == ObjectManagerIF::CHILD_INIT_FAILED) {
|
|
errorPrint = "Initialization error";
|
|
} else if (errorCode == returnvalue::FAILED) {
|
|
if (errorType == sif::OutputTypes::OUT_WARNING) {
|
|
errorPrint = "Generic Warning";
|
|
} else {
|
|
errorPrint = "Generic Error";
|
|
}
|
|
} else {
|
|
errorPrint = "Unknown error";
|
|
}
|
|
}
|
|
if (functionName == nullptr) {
|
|
functionName = "unknown function";
|
|
}
|
|
|
|
if (errorType == sif::OutputTypes::OUT_WARNING) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::warning << "DeviceHandlerBase::" << functionName << ": Object ID 0x" << std::hex
|
|
<< std::setw(8) << std::setfill('0') << this->getObjectId() << " | " << errorPrint
|
|
<< std::dec << std::setfill(' ') << std::endl;
|
|
#else
|
|
sif::printWarning("DeviceHandlerBase::%s: Object ID 0x%08x | %s\n", functionName,
|
|
this->getObjectId(), errorPrint);
|
|
#endif
|
|
} else if (errorType == sif::OutputTypes::OUT_ERROR) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "DeviceHandlerBase::" << functionName << ": Object ID 0x" << std::hex
|
|
<< std::setw(8) << std::setfill('0') << this->getObjectId() << " | " << errorPrint
|
|
<< std::dec << std::setfill(' ') << std::endl;
|
|
#else
|
|
sif::printError("DeviceHandlerBase::%s: Object ID 0x%08x | %s\n", functionName,
|
|
this->getObjectId(), errorPrint);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
LocalDataPoolManager* DeviceHandlerBase::getHkManagerHandle() { return &poolManager; }
|
|
|
|
MessageQueueId_t DeviceHandlerBase::getCommanderQueueId(DeviceCommandId_t replyId) const {
|
|
auto commandIter = deviceCommandMap.find(replyId);
|
|
if (commandIter == deviceCommandMap.end()) {
|
|
return MessageQueueIF::NO_QUEUE;
|
|
}
|
|
return commandIter->second.sendReplyTo;
|
|
}
|