eive-obsw/mission/system/EiveSystem.cpp

472 lines
17 KiB
C++
Raw Normal View History

2023-04-06 16:50:33 +02:00
#include "EiveSystem.h"
#include <eive/objects.h>
#include <fsfw/events/EventManager.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/power/PowerSwitchIF.h>
2023-04-14 20:04:51 +02:00
#include <fsfw/tasks/TaskFactory.h>
2023-04-06 16:50:33 +02:00
#include <mission/acs/defs.h>
#include <mission/com/defs.h>
#include <mission/controller/tcsDefs.h>
2023-04-14 13:11:11 +02:00
#include "linux/ipcore/pdec.h"
2023-04-06 16:50:33 +02:00
#include "mission/power/bpxBattDefs.h"
#include "mission/power/defs.h"
#include "mission/sysDefs.h"
EiveSystem::EiveSystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
2023-04-06 17:49:34 +02:00
uint32_t maxNumberOfTables, std::atomic_uint16_t& i2cErrors)
2023-04-06 16:50:33 +02:00
: Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables),
2023-04-06 17:49:34 +02:00
actionHelper(this, commandQueue),
i2cErrors(i2cErrors) {
2023-04-06 16:50:33 +02:00
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 (satsystem::Mode::PTG_NADIR): {
modeStr = "POINTING NADIR";
2023-04-06 16:50:33 +02:00
break;
}
case (satsystem::Mode::PTG_TARGET): {
2023-04-06 16:50:33 +02:00
modeStr = "POINTING TARGET";
break;
}
case (satsystem::Mode::PTG_TARGET_GS): {
2023-04-06 16:50:33 +02:00
modeStr = "POINTING TARGET GS";
break;
}
case (satsystem::Mode::PTG_INERTIAL): {
modeStr = "POINTING INERTIAL";
break;
}
2023-04-06 16:50:33 +02:00
}
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;
}
2023-04-14 13:26:44 +02:00
pdecRecoveryLogic();
2023-04-06 17:01:28 +02:00
i2cRecoveryLogic();
2023-04-06 16:50:33 +02:00
}
ReturnValue_t EiveSystem::initialize() {
if (powerSwitcher == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
ReturnValue_t result = actionHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
2023-04-06 22:35:23 +02:00
2023-09-29 14:18:42 +02:00
auto* plSs = ObjectManager::instance()->get<HasModesIF>(objects::PL_SUBSYSTEM);
if (plSs == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
plSsQueueId = plSs->getCommandQueue();
auto* plPcdu = ObjectManager::instance()->get<HasHealthIF>(objects::PLPCDU_HANDLER);
if (plPcdu == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
plPcduQueueId = plPcdu->getCommandQueue();
auto* plocMpsoc = ObjectManager::instance()->get<HasHealthIF>(objects::PLOC_MPSOC_HANDLER);
if (plocMpsoc == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
plocMpsocQueueId = plocMpsoc->getCommandQueue();
auto* plocSupervisor =
ObjectManager::instance()->get<HasHealthIF>(objects::PLOC_SUPERVISOR_HANDLER);
if (plocSupervisor == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
plocSupervisorQueueId = plocSupervisor->getCommandQueue();
auto* camera = ObjectManager::instance()->get<HasHealthIF>(objects::CAM_SWITCHER);
if (camera == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
cameraQueueId = camera->getCommandQueue();
auto* scex = ObjectManager::instance()->get<HasHealthIF>(objects::SCEX);
if (scex == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
scexQueueId = scex->getCommandQueue();
auto* radSensor = ObjectManager::instance()->get<HasHealthIF>(objects::RAD_SENSOR);
if (radSensor == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
radSensorQueueId = radSensor->getCommandQueue();
auto* str = ObjectManager::instance()->get<HasHealthIF>(objects::STAR_TRACKER);
if (str == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
strQueueId = str->getCommandQueue();
2023-04-06 16:50:33 +02:00
auto* bpxDest = ObjectManager::instance()->get<HasActionsIF>(objects::BPX_BATT_HANDLER);
if (bpxDest == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
bpxBattQueueId = bpxDest->getCommandQueue();
2023-04-06 22:35:23 +02:00
auto* coreCtrl = ObjectManager::instance()->get<HasActionsIF>(objects::CORE_CONTROLLER);
if (coreCtrl == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
coreCtrlQueueId = coreCtrl->getCommandQueue();
2023-04-14 13:26:44 +02:00
auto* pdecHandler = ObjectManager::instance()->get<HasActionsIF>(objects::PDEC_HANDLER);
if (pdecHandler == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
pdecHandlerQueueId = pdecHandler->getCommandQueue();
2023-04-06 16:50:33 +02:00
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));
2023-04-17 11:35:10 +02:00
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::MGT_OVERHEATING));
2023-04-14 13:11:11 +02:00
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(pdec::INVALID_TC_FRAME));
2023-09-29 14:18:42 +02:00
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_LOW));
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_CRITICAL));
2023-04-06 16:50:33 +02:00
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()) {
2023-04-14 13:11:11 +02:00
case pdec::INVALID_TC_FRAME: {
2023-04-14 19:58:22 +02:00
if (event.getParameter1() == pdec::FRAME_DIRTY_RETVAL) {
2023-04-14 13:11:11 +02:00
frameDirtyErrorCounter++;
2023-08-14 10:34:06 +02:00
// Check whether threshold was reached after 10 seconds.
2023-08-15 13:49:06 +02:00
if (frameDirtyErrorCounter == 1) {
2023-08-14 10:34:06 +02:00
frameDirtyCheckCd.resetTimer();
}
2023-04-14 13:11:11 +02:00
}
break;
}
2023-04-06 16:50:33 +02:00
case tcsCtrl::OBC_OVERHEATING:
2023-04-17 11:35:10 +02:00
case tcsCtrl::MGT_OVERHEATING:
2023-04-06 16:50:33 +02:00
case tcsCtrl::PCDU_SYSTEM_OVERHEATING: {
if (isInTransition) {
performSafeRecovery = true;
return;
}
commandSelfToSafe();
break;
}
2023-09-29 14:18:42 +02:00
case power::POWER_LEVEL_LOW: {
forceOffPayload();
break;
}
case power::POWER_LEVEL_CRITICAL:
CommandMessage msg;
HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HasHealthIF::FAULTY);
ReturnValue_t result = MessageQueueSenderIF::sendMessage(
strQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to STR Assembly failed"
<< std::endl;
}
break;
2023-04-06 16:50:33 +02:00
}
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): {
2023-04-06 19:29:51 +02:00
triggerEvent(core::TRYING_I2C_RECOVERY);
2023-04-06 16:50:33 +02:00
performI2cReboot = true;
i2cRebootState = I2cRebootState::SYSTEM_MODE_BOOT;
this->actionCommandedBy = commandedBy;
return returnvalue::OK;
}
default: {
return HasActionsIF::INVALID_ACTION_ID;
}
}
return returnvalue::OK;
}
2023-04-06 17:49:34 +02:00
void EiveSystem::setI2cRecoveryParams(PowerSwitchIF* pwrSwitcher) {
this->powerSwitcher = pwrSwitcher;
}
2023-04-06 17:01:28 +02:00
void EiveSystem::i2cRecoveryLogic() {
ReturnValue_t result;
2023-04-06 17:49:34 +02:00
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;
2023-04-06 22:39:57 +02:00
i2cRebootHandlingCountdown.resetTimer();
2023-04-06 17:49:34 +02:00
}
// 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 {
2023-08-02 09:19:43 +02:00
if (waitingForI2cReboot) {
return;
}
2023-04-14 20:04:51 +02:00
triggerEvent(core::I2C_REBOOT);
2023-08-01 09:26:06 +02:00
// Some delay to ensure that the event is stored in the persistent TM store as well.
TaskFactory::delayTask(500);
2023-04-06 17:49:34 +02:00
// We already tried an I2C recovery but the bus is still broken.
2023-08-01 09:26:06 +02:00
// Send reboot request to core controller.
2023-08-02 09:00:45 +02:00
result = sendSelfRebootCommand();
if (result != returnvalue::OK) {
2023-08-01 09:36:08 +02:00
sif::error << "Sending a reboot command has failed" << std::endl;
2023-08-02 09:00:45 +02:00
// If the previous operation failed, it should be re-attempted the next task cycle.
return;
2023-08-01 09:36:08 +02:00
}
2023-08-02 09:19:43 +02:00
waitingForI2cReboot = true;
2023-04-14 13:26:44 +02:00
return;
2023-04-06 17:49:34 +02:00
}
}
2023-04-06 17:01:28 +02:00
}
if (not isInTransition and performI2cReboot) {
2023-04-06 22:35:23 +02:00
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): {
2023-04-06 17:01:28 +02:00
result = powerSwitcher->sendSwitchCommand(power::Switches::P60_DOCK_3V3_STACK,
2023-04-06 22:35:23 +02:00
PowerSwitchIF::SWITCH_ON);
2023-04-06 17:01:28 +02:00
if (result != returnvalue::OK) {
actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, result);
2023-04-06 17:49:34 +02:00
commonI2cRecoverySequenceFinish();
return;
2023-04-06 17:01:28 +02:00
}
2023-04-06 22:35:23 +02:00
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
2023-04-06 17:49:34 +02:00
commonI2cRecoverySequenceFinish();
2023-04-06 22:35:23 +02:00
actionHelper.finish(true, actionCommandedBy, EXECUTE_I2C_REBOOT);
2023-04-06 17:01:28 +02:00
}
2023-04-06 22:35:23 +02:00
break;
2023-04-06 17:01:28 +02:00
}
2023-04-06 22:35:23 +02:00
default: {
sif::error << "EiveSystem: Unexpected I2C reboot state" << std::endl;
break;
2023-04-06 17:01:28 +02:00
}
}
// Timeout handling for the internal procedure.
2023-04-06 17:49:34 +02:00
if (i2cRebootState != I2cRebootState::NONE and i2cRebootHandlingCountdown.hasTimedOut()) {
2023-04-06 17:01:28 +02:00
actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, returnvalue::FAILED);
2023-04-06 17:49:34 +02:00
// Command stack back on in any case.
2023-04-06 17:01:28 +02:00
powerSwitcher->sendSwitchCommand(power::Switches::P60_DOCK_3V3_STACK,
PowerSwitchIF::SWITCH_ON);
2023-04-06 17:49:34 +02:00
commonI2cRecoverySequenceFinish();
2023-04-06 17:01:28 +02:00
}
}
}
2023-04-06 16:50:33 +02:00
void EiveSystem::commandSelfToSafe() { startTransition(satsystem::Mode::SAFE, 0); }
2023-04-06 17:49:34 +02:00
2023-04-14 13:26:44 +02:00
ReturnValue_t EiveSystem::sendFullRebootCommand() {
CommandMessage msg;
ActionMessage::setCommand(&msg, core::REBOOT_OBC, store_address_t());
return commandQueue->sendMessage(coreCtrlQueueId, &msg);
}
void EiveSystem::pdecRecoveryLogic() {
2023-08-04 14:28:58 +02:00
// PDEC reset has happened too often in the last time. Perform reboot to same image.
2023-08-04 14:23:10 +02:00
if (pdecResetCounter >= PDEC_RESET_MAX_COUNT_BEFORE_REBOOT) {
if (waitingForPdecReboot) {
return;
}
triggerEvent(core::PDEC_REBOOT);
// Some delay to ensure that the event is stored in the persistent TM store as well.
TaskFactory::delayTask(500);
// Send reboot command.
ReturnValue_t result = sendSelfRebootCommand();
if (result != returnvalue::OK) {
sif::error << "Sending a reboot command has failed" << std::endl;
// If the previous operation failed, it should be re-attempted the next task cycle.
pdecResetCounterResetCd.resetTimer();
return;
}
waitingForPdecReboot = true;
return;
}
if (pdecResetCounterResetCd.hasTimedOut()) {
pdecResetCounter = 0;
}
2023-08-14 10:30:46 +02:00
if (frameDirtyCheckCd.hasTimedOut() and frameDirtyErrorCounter > 0) {
2023-04-14 19:30:40 +02:00
if (frameDirtyErrorCounter >= FRAME_DIRTY_COM_REBOOT_LIMIT) {
2023-08-04 14:23:10 +02:00
// Try one full PDEC reset.
CommandMessage msg;
store_address_t dummy{};
ActionMessage::setCommand(&msg, pdec::RESET_PDEC_WITH_REINIITALIZATION, dummy);
commandQueue->sendMessage(pdecHandlerQueueId, &msg);
pdecResetCounterResetCd.resetTimer();
pdecResetCounter++;
2023-04-14 13:26:44 +02:00
}
frameDirtyErrorCounter = 0;
}
}
2023-09-29 14:18:42 +02:00
void EiveSystem::forceOffPayload() {
CommandMessage msg;
// set PL to faulty
HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HasHealthIF::FAULTY);
ReturnValue_t result = commandQueue->sendMessage(plPcduQueueId, &msg);
2023-09-29 14:18:42 +02:00
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to PL PCDU failed" << std::endl;
}
result = commandQueue->sendMessage(plocMpsocQueueId, &msg);
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to PLOC MPSOC failed" << std::endl;
}
result = commandQueue->sendMessage(plocSupervisorQueueId, &msg);
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to PLOC SUPERVISOR failed" << std::endl;
}
result = commandQueue->sendMessage(cameraQueueId, &msg);
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to PL CAM failed" << std::endl;
}
result = commandQueue->sendMessage(scexQueueId, &msg);
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to SCEX failed" << std::endl;
}
result = commandQueue->sendMessage(radSensorQueueId, &msg);
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to RAD SENSOR failed" << std::endl;
}
}
2023-04-06 17:49:34 +02:00
void EiveSystem::commonI2cRecoverySequenceFinish() {
alreadyTriedI2cRecovery = true;
performI2cReboot = false;
i2cRebootState = I2cRebootState::NONE;
2023-04-07 17:24:19 +02:00
// 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.
2023-04-06 17:49:34 +02:00
i2cErrors = 0;
2023-04-07 17:24:19 +02:00
i2cRecoveryClearCountdown.resetTimer();
2023-04-06 17:49:34 +02:00
// This should always be accepted
commandSelfToSafe();
}
2023-04-06 22:35:23 +02:00
ReturnValue_t EiveSystem::handleCommandMessage(CommandMessage* message) {
if (message->getMessageType() == messagetypes::ACTION) {
return actionHelper.handleActionMessage(message);
}
return Subsystem::handleCommandMessage(message);
}
2023-08-01 09:26:06 +02:00
ReturnValue_t EiveSystem::sendSelfRebootCommand() {
CommandMessage msg;
uint8_t data[1];
// This option is used to target the same image.
data[0] = true;
store_address_t storeId;
ReturnValue_t result = IPCStore->addData(&storeId, data, sizeof(data));
if (result != returnvalue::OK) {
return result;
}
ActionMessage::setCommand(&msg, core::XSC_REBOOT_OBC, storeId);
return commandQueue->sendMessage(coreCtrlQueueId, &msg);
}