eive-obsw/mission/acs/SusHandler.cpp

155 lines
5.1 KiB
C++
Raw Normal View History

2022-02-22 20:34:25 +01:00
#include "SusHandler.h"
2023-03-26 16:42:00 +02:00
#include <mission/tcs/max1227.h>
2022-02-22 20:34:25 +01:00
2023-03-01 16:36:21 +01:00
#include <cmath>
2022-02-22 20:34:25 +01:00
2023-03-01 16:36:21 +01:00
#include "fsfw/datapool/PoolReadGuard.h"
2022-02-22 20:34:25 +01:00
2023-03-01 16:36:21 +01:00
SusHandler::SusHandler(object_id_t objectId, uint8_t susIdx, object_id_t deviceCommunication,
CookieIF *comCookie)
: DeviceHandlerBase(objectId, deviceCommunication, comCookie), dataset(this), susIdx(susIdx) {}
SusHandler::~SusHandler() = default;
2022-02-22 20:34:25 +01:00
void SusHandler::doStartUp() {
2023-03-01 16:36:21 +01:00
if (internalState != InternalState::STARTUP) {
2022-02-22 20:34:25 +01:00
commandExecuted = false;
2023-03-01 16:36:21 +01:00
updatePeriodicReply(true, REPLY);
internalState = InternalState::STARTUP;
2022-02-22 20:34:25 +01:00
}
2023-03-01 16:36:21 +01:00
if (internalState == InternalState::STARTUP) {
2022-02-22 20:34:25 +01:00
if (commandExecuted) {
2023-09-07 16:03:36 +02:00
if (waitingForRecovery) {
waitingForRecovery = false;
}
2023-03-27 15:03:54 +02:00
setMode(MODE_ON);
2023-03-01 16:36:21 +01:00
internalState = InternalState::NONE;
2022-02-22 20:34:25 +01:00
commandExecuted = false;
}
}
}
2022-04-03 20:35:49 +02:00
void SusHandler::doShutDown() {
2023-03-01 16:36:21 +01:00
if (internalState != InternalState::SHUTDOWN) {
2023-04-19 18:09:57 +02:00
PoolReadGuard pg(&dataset);
2023-06-22 11:33:54 +02:00
dataset.tempC = thermal::INVALID_TEMPERATURE;
2023-03-01 16:36:21 +01:00
dataset.setValidity(false, true);
internalState = InternalState::SHUTDOWN;
commandExecuted = false;
}
if (internalState == InternalState::SHUTDOWN and commandExecuted) {
updatePeriodicReply(false, REPLY);
commandExecuted = false;
internalState = InternalState::NONE;
2023-03-02 18:27:27 +01:00
setMode(MODE_OFF);
2022-02-22 20:34:25 +01:00
}
}
ReturnValue_t SusHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
2023-03-01 16:36:21 +01:00
if (internalState == InternalState::STARTUP) {
*id = REQUEST;
return prepareRequest(acs::SimpleSensorMode::NORMAL);
} else if (internalState == InternalState::SHUTDOWN) {
*id = REQUEST;
return prepareRequest(acs::SimpleSensorMode::OFF);
2022-02-22 20:34:25 +01:00
}
2022-03-22 11:07:31 +01:00
return NOTHING_TO_SEND;
2022-02-22 20:34:25 +01:00
}
2023-03-01 16:36:21 +01:00
ReturnValue_t SusHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
*id = REQUEST;
return prepareRequest(acs::SimpleSensorMode::NORMAL);
}
2022-02-22 20:34:25 +01:00
ReturnValue_t SusHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t *commandData,
size_t commandDataLen) {
2023-03-01 16:36:21 +01:00
return NOTHING_TO_SEND;
2022-02-22 20:34:25 +01:00
}
2023-03-01 16:36:21 +01:00
ReturnValue_t SusHandler::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId,
size_t *foundLen) {
if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
return IGNORE_FULL_PACKET;
}
if (len != sizeof(acs::SusReply)) {
*foundLen = len;
return returnvalue::FAILED;
}
*foundId = REPLY;
*foundLen = len;
if (internalState == InternalState::SHUTDOWN) {
commandExecuted = true;
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-02-22 20:34:25 +01:00
}
ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
2023-03-01 16:36:21 +01:00
const auto *reply = reinterpret_cast<const acs::SusReply *>(packet);
2023-09-07 16:07:59 +02:00
if (!reply->dataWasSet) {
return returnvalue::OK;
}
if (internalState == InternalState::STARTUP) {
commandExecuted = true;
}
PoolReadGuard pg(&dataset);
// Simple FDIR variant to make the handler more robust to invalid messages which
// appear sometimes for the SUS device: Allow invalid message up to a certain threshold
// before triggering FDIR reactions.
if (reply->tempRaw == 0xfff and not waitingForRecovery) {
if (invalidMsgCounter == 0) {
triggerEvent(TEMPERATURE_ALL_ONES_START);
} else if (invalidMsgCounter == susMax1227::MAX_INVALID_MSG_COUNT) {
triggerEvent(DeviceHandlerIF::DEVICE_WANTS_HARD_REBOOT);
waitingForRecovery = true;
2023-06-13 07:48:32 +02:00
}
2023-09-07 16:09:43 +02:00
invalidMsgCounter++;
2023-09-07 16:07:59 +02:00
dataset.setValidity(false, true);
dataset.tempC = thermal::INVALID_TEMPERATURE;
std::memset(dataset.channels.value, 0, sizeof(dataset.channels.value));
return returnvalue::OK;
}
if (invalidMsgCounter > 0) {
triggerEvent(TEMPERATURE_ALL_ONES_RECOVERY, invalidMsgCounter);
invalidMsgCounter = 0;
2022-02-22 20:34:25 +01:00
}
2023-09-07 16:07:59 +02:00
dataset.setValidity(true, true);
dataset.tempC = max1227::getTemperature(reply->tempRaw);
std::memcpy(dataset.channels.value, reply->channelsRaw, sizeof(reply->channelsRaw));
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-02-22 20:34:25 +01:00
}
2023-03-01 16:36:21 +01:00
void SusHandler::fillCommandAndReplyMap() {
insertInCommandMap(REQUEST);
insertInReplyMap(REPLY, 5, nullptr, 0, true);
}
void SusHandler::setToGoToNormalMode(bool enable) { this->goToNormalMode = enable; }
uint32_t SusHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { return transitionDelay; }
void SusHandler::modeChanged(void) { internalState = InternalState::NONE; }
2022-02-22 20:34:25 +01:00
ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
2023-02-28 19:14:15 +01:00
localDataPoolMap.emplace(susMax1227::TEMPERATURE_C, &tempC);
localDataPoolMap.emplace(susMax1227::CHANNEL_VEC, &channelVec);
2022-08-15 11:57:57 +02:00
poolManager.subscribeForDiagPeriodicPacket(
subdp::DiagnosticsHkPeriodicParams(dataset.getSid(), false, 5.0));
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-02-22 20:34:25 +01:00
}
2023-03-01 16:36:21 +01:00
ReturnValue_t SusHandler::prepareRequest(acs::SimpleSensorMode mode) {
request.mode = mode;
rawPacket = reinterpret_cast<uint8_t *>(&request);
rawPacketLen = sizeof(acs::SusRequest);
return returnvalue::OK;
2022-02-22 20:34:25 +01:00
}
2023-03-01 16:36:21 +01:00
LocalPoolDataSetBase *SusHandler::getDataSetHandle(sid_t sid) {
if (sid == dataset.getSid()) {
return &dataset;
}
return nullptr;
2022-02-22 20:34:25 +01:00
}