328 lines
11 KiB
C++
328 lines
11 KiB
C++
#include "EiveSystem.h"
|
|
|
|
#include <eive/objects.h>
|
|
#include <fsfw/events/EventManager.h>
|
|
#include <fsfw/ipc/QueueFactory.h>
|
|
#include <fsfw/power/PowerSwitchIF.h>
|
|
#include <fsfw/tasks/TaskFactory.h>
|
|
#include <mission/acs/defs.h>
|
|
#include <mission/com/defs.h>
|
|
#include <mission/controller/tcsDefs.h>
|
|
|
|
#include "linux/ipcore/pdec.h"
|
|
#include "mission/power/bpxBattDefs.h"
|
|
#include "mission/power/defs.h"
|
|
#include "mission/sysDefs.h"
|
|
|
|
EiveSystem::EiveSystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
|
|
uint32_t maxNumberOfTables, std::atomic_uint16_t& i2cErrors)
|
|
: Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables),
|
|
actionHelper(this, commandQueue),
|
|
i2cErrors(i2cErrors) {
|
|
auto mqArgs = MqArgs(SubsystemBase::getObjectId(), static_cast<void*>(this));
|
|
eventQueue =
|
|
QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
|
|
}
|
|
|
|
void EiveSystem::announceMode(bool recursive) {
|
|
const char* modeStr = "UNKNOWN";
|
|
switch (mode) {
|
|
case (satsystem::Mode::BOOT): {
|
|
modeStr = "OFF/BOOT";
|
|
break;
|
|
}
|
|
case (satsystem::Mode::SAFE): {
|
|
modeStr = "SAFE";
|
|
break;
|
|
}
|
|
case (satsystem::Mode::PTG_IDLE): {
|
|
modeStr = "POINTING IDLE";
|
|
break;
|
|
}
|
|
case (acs::AcsMode::PTG_INERTIAL): {
|
|
modeStr = "POINTING INERTIAL";
|
|
break;
|
|
}
|
|
case (acs::AcsMode::PTG_TARGET): {
|
|
modeStr = "POINTING TARGET";
|
|
break;
|
|
}
|
|
case (acs::AcsMode::PTG_TARGET_GS): {
|
|
modeStr = "POINTING TARGET GS";
|
|
break;
|
|
}
|
|
}
|
|
sif::info << "EIVE system is now in " << modeStr << " mode" << std::endl;
|
|
return Subsystem::announceMode(recursive);
|
|
}
|
|
|
|
void EiveSystem::performChildOperation() {
|
|
Subsystem::performChildOperation();
|
|
handleEventMessages();
|
|
if (not isInTransition and performSafeRecovery) {
|
|
commandSelfToSafe();
|
|
performSafeRecovery = false;
|
|
return;
|
|
}
|
|
pdecRecoveryLogic();
|
|
i2cRecoveryLogic();
|
|
}
|
|
|
|
ReturnValue_t EiveSystem::initialize() {
|
|
if (powerSwitcher == nullptr) {
|
|
return ObjectManager::CHILD_INIT_FAILED;
|
|
}
|
|
ReturnValue_t result = actionHelper.initialize();
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
|
|
auto* bpxDest = ObjectManager::instance()->get<HasActionsIF>(objects::BPX_BATT_HANDLER);
|
|
if (bpxDest == nullptr) {
|
|
return ObjectManager::CHILD_INIT_FAILED;
|
|
}
|
|
bpxBattQueueId = bpxDest->getCommandQueue();
|
|
|
|
auto* coreCtrl = ObjectManager::instance()->get<HasActionsIF>(objects::CORE_CONTROLLER);
|
|
if (coreCtrl == nullptr) {
|
|
return ObjectManager::CHILD_INIT_FAILED;
|
|
}
|
|
coreCtrlQueueId = coreCtrl->getCommandQueue();
|
|
|
|
auto* pdecHandler = ObjectManager::instance()->get<HasActionsIF>(objects::PDEC_HANDLER);
|
|
if (pdecHandler == nullptr) {
|
|
return ObjectManager::CHILD_INIT_FAILED;
|
|
}
|
|
pdecHandlerQueueId = pdecHandler->getCommandQueue();
|
|
|
|
auto* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
|
|
if (manager == nullptr) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "AcsSubsystem::initialize: Invalid event manager" << std::endl;
|
|
#endif
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
result = manager->registerListener(eventQueue->getId());
|
|
if (result != returnvalue::OK) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::warning << "AcsSubsystem::registerListener: Failed to register as "
|
|
"listener"
|
|
<< std::endl;
|
|
#endif
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
manager->subscribeToEvent(eventQueue->getId(),
|
|
event::getEventId(tcsCtrl::PCDU_SYSTEM_OVERHEATING));
|
|
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::OBC_OVERHEATING));
|
|
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::MGT_OVERHEATING));
|
|
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(pdec::INVALID_TC_FRAME));
|
|
return Subsystem::initialize();
|
|
}
|
|
|
|
void EiveSystem::handleEventMessages() {
|
|
EventMessage event;
|
|
for (ReturnValue_t status = eventQueue->receiveMessage(&event); status == returnvalue::OK;
|
|
status = eventQueue->receiveMessage(&event)) {
|
|
switch (event.getMessageId()) {
|
|
case EventMessage::EVENT_MESSAGE:
|
|
switch (event.getEvent()) {
|
|
case pdec::INVALID_TC_FRAME: {
|
|
if (event.getParameter1() == pdec::FRAME_DIRTY_RETVAL) {
|
|
frameDirtyErrorCounter++;
|
|
}
|
|
break;
|
|
}
|
|
case tcsCtrl::OBC_OVERHEATING:
|
|
case tcsCtrl::MGT_OVERHEATING:
|
|
case tcsCtrl::PCDU_SYSTEM_OVERHEATING: {
|
|
if (isInTransition) {
|
|
performSafeRecovery = true;
|
|
return;
|
|
}
|
|
|
|
commandSelfToSafe();
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
default:
|
|
sif::debug << "EiveSystem: Did not subscribe to event " << event.getEvent() << std::endl;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
MessageQueueId_t EiveSystem::getCommandQueue() const { return Subsystem::getCommandQueue(); }
|
|
|
|
ReturnValue_t EiveSystem::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
|
|
const uint8_t* data, size_t size) {
|
|
switch (actionId) {
|
|
case (EXECUTE_I2C_REBOOT): {
|
|
triggerEvent(core::TRYING_I2C_RECOVERY);
|
|
performI2cReboot = true;
|
|
i2cRebootState = I2cRebootState::SYSTEM_MODE_BOOT;
|
|
this->actionCommandedBy = commandedBy;
|
|
return returnvalue::OK;
|
|
}
|
|
default: {
|
|
return HasActionsIF::INVALID_ACTION_ID;
|
|
}
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void EiveSystem::setI2cRecoveryParams(PowerSwitchIF* pwrSwitcher) {
|
|
this->powerSwitcher = pwrSwitcher;
|
|
}
|
|
|
|
void EiveSystem::i2cRecoveryLogic() {
|
|
ReturnValue_t result;
|
|
if (not performI2cReboot) {
|
|
// If a recovery worked, need to reset these flags and the error count after some time.
|
|
if (i2cRecoveryClearCountdown.hasTimedOut()) {
|
|
i2cErrors = 0;
|
|
alreadyTriedI2cRecovery = false;
|
|
i2cRebootHandlingCountdown.resetTimer();
|
|
}
|
|
// If an I2C recovery is not ongoing and the I2C error counter is above a threshold, try
|
|
// recovery or reboot if recovery was already attempted.
|
|
if (i2cErrors >= 5) {
|
|
if (not alreadyTriedI2cRecovery) {
|
|
// Try recovery.
|
|
executeAction(EXECUTE_I2C_REBOOT, MessageQueueIF::NO_QUEUE, nullptr, 0);
|
|
} else {
|
|
triggerEvent(core::I2C_REBOOT);
|
|
// We already tried an I2C recovery but the bus is still broken.
|
|
// Send full reboot request to core controller.
|
|
sendFullRebootCommand();
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
if (not isInTransition and performI2cReboot) {
|
|
switch (i2cRebootState) {
|
|
case (I2cRebootState::NONE): {
|
|
break;
|
|
}
|
|
case (I2cRebootState::SYSTEM_MODE_BOOT): {
|
|
startTransition(satsystem::Mode::BOOT, 0);
|
|
i2cRebootState = I2cRebootState::SWITCH_3V3_STACK_OFF_AND_BATT_REBOOT;
|
|
i2cRebootHandlingCountdown.resetTimer();
|
|
break;
|
|
}
|
|
case (I2cRebootState::SWITCH_3V3_STACK_OFF_AND_BATT_REBOOT): {
|
|
if (mode == satsystem::Mode::BOOT) {
|
|
result = powerSwitcher->sendSwitchCommand(power::Switches::P60_DOCK_3V3_STACK,
|
|
PowerSwitchIF::SWITCH_OFF);
|
|
if (result != returnvalue::OK) {
|
|
actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, result);
|
|
commonI2cRecoverySequenceFinish();
|
|
return;
|
|
}
|
|
CommandMessage msg;
|
|
store_address_t dummy{};
|
|
ActionMessage::setCommand(&msg, bpxBat::REBOOT, dummy);
|
|
result = commandQueue->sendMessage(bpxBattQueueId, &msg);
|
|
if (result != returnvalue::OK) {
|
|
actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, result);
|
|
commonI2cRecoverySequenceFinish();
|
|
return;
|
|
}
|
|
i2cRebootState = I2cRebootState::WAIT_CYCLE;
|
|
}
|
|
break;
|
|
}
|
|
case (I2cRebootState::WAIT_CYCLE): {
|
|
i2cRebootState = I2cRebootState::SWITCH_3V3_STACK_ON;
|
|
break;
|
|
}
|
|
case (I2cRebootState::SWITCH_3V3_STACK_ON): {
|
|
result = powerSwitcher->sendSwitchCommand(power::Switches::P60_DOCK_3V3_STACK,
|
|
PowerSwitchIF::SWITCH_ON);
|
|
if (result != returnvalue::OK) {
|
|
actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, result);
|
|
commonI2cRecoverySequenceFinish();
|
|
return;
|
|
}
|
|
i2cRebootState = I2cRebootState::SYSTEM_MODE_SAFE;
|
|
break;
|
|
}
|
|
case (I2cRebootState::SYSTEM_MODE_SAFE): {
|
|
if (powerSwitcher->getSwitchState(power::Switches::P60_DOCK_3V3_STACK) ==
|
|
PowerSwitchIF::SWITCH_ON) {
|
|
// This should always be accepted
|
|
commonI2cRecoverySequenceFinish();
|
|
actionHelper.finish(true, actionCommandedBy, EXECUTE_I2C_REBOOT);
|
|
}
|
|
break;
|
|
}
|
|
default: {
|
|
sif::error << "EiveSystem: Unexpected I2C reboot state" << std::endl;
|
|
break;
|
|
}
|
|
}
|
|
// Timeout handling for the internal procedure.
|
|
if (i2cRebootState != I2cRebootState::NONE and i2cRebootHandlingCountdown.hasTimedOut()) {
|
|
actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, returnvalue::FAILED);
|
|
// Command stack back on in any case.
|
|
powerSwitcher->sendSwitchCommand(power::Switches::P60_DOCK_3V3_STACK,
|
|
PowerSwitchIF::SWITCH_ON);
|
|
commonI2cRecoverySequenceFinish();
|
|
}
|
|
}
|
|
}
|
|
|
|
void EiveSystem::commandSelfToSafe() { startTransition(satsystem::Mode::SAFE, 0); }
|
|
|
|
ReturnValue_t EiveSystem::sendFullRebootCommand() {
|
|
CommandMessage msg;
|
|
ActionMessage::setCommand(&msg, core::REBOOT_OBC, store_address_t());
|
|
return commandQueue->sendMessage(coreCtrlQueueId, &msg);
|
|
}
|
|
|
|
void EiveSystem::pdecRecoveryLogic() {
|
|
if (ptmeResetWasAttempted and ptmeResetWasAttemptedCd.hasTimedOut()) {
|
|
ptmeResetWasAttempted = false;
|
|
}
|
|
if (frameDirtyCheckCd.hasTimedOut()) {
|
|
if (frameDirtyErrorCounter >= FRAME_DIRTY_COM_REBOOT_LIMIT) {
|
|
// If a PTME reset was already attempted and there is still an issue receiving TC frames,
|
|
// reboot the system.
|
|
if (ptmeResetWasAttempted) {
|
|
triggerEvent(core::PDEC_REBOOT);
|
|
// Send reboot command.
|
|
sendFullRebootCommand();
|
|
} else {
|
|
// Try one full PDEC reset.
|
|
CommandMessage msg;
|
|
store_address_t dummy{};
|
|
ActionMessage::setCommand(&msg, pdec::RESET_PDEC_WITH_REINIITALIZATION, dummy);
|
|
commandQueue->sendMessage(pdecHandlerQueueId, &msg);
|
|
ptmeResetWasAttemptedCd.resetTimer();
|
|
ptmeResetWasAttempted = true;
|
|
}
|
|
}
|
|
frameDirtyErrorCounter = 0;
|
|
frameDirtyCheckCd.resetTimer();
|
|
}
|
|
}
|
|
|
|
void EiveSystem::commonI2cRecoverySequenceFinish() {
|
|
alreadyTriedI2cRecovery = true;
|
|
performI2cReboot = false;
|
|
i2cRebootState = I2cRebootState::NONE;
|
|
// Reset this counter and the recovery clear countdown. If I2C devices are still problematic,
|
|
// we will get a full reboot next time this count goes above 5.
|
|
i2cErrors = 0;
|
|
i2cRecoveryClearCountdown.resetTimer();
|
|
// This should always be accepted
|
|
commandSelfToSafe();
|
|
}
|
|
|
|
ReturnValue_t EiveSystem::handleCommandMessage(CommandMessage* message) {
|
|
if (message->getMessageType() == messagetypes::ACTION) {
|
|
return actionHelper.handleActionMessage(message);
|
|
}
|
|
return Subsystem::handleCommandMessage(message);
|
|
}
|