129 lines
4.1 KiB
C++
129 lines
4.1 KiB
C++
#include "SusHandler.h"
|
|
|
|
#include <mission/tcs/max1227.h>
|
|
|
|
#include <cmath>
|
|
|
|
#include "fsfw/datapool/PoolReadGuard.h"
|
|
|
|
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;
|
|
|
|
void SusHandler::doStartUp() {
|
|
if (internalState != InternalState::STARTUP) {
|
|
commandExecuted = false;
|
|
updatePeriodicReply(true, REPLY);
|
|
internalState = InternalState::STARTUP;
|
|
}
|
|
if (internalState == InternalState::STARTUP) {
|
|
if (commandExecuted) {
|
|
setMode(MODE_NORMAL);
|
|
internalState = InternalState::NONE;
|
|
commandExecuted = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
void SusHandler::doShutDown() {
|
|
if (internalState != InternalState::SHUTDOWN) {
|
|
dataset.setValidity(false, true);
|
|
internalState = InternalState::SHUTDOWN;
|
|
commandExecuted = false;
|
|
}
|
|
if (internalState == InternalState::SHUTDOWN and commandExecuted) {
|
|
updatePeriodicReply(false, REPLY);
|
|
commandExecuted = false;
|
|
internalState = InternalState::NONE;
|
|
setMode(MODE_OFF);
|
|
}
|
|
}
|
|
|
|
ReturnValue_t SusHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
|
if (internalState == InternalState::STARTUP) {
|
|
*id = REQUEST;
|
|
return prepareRequest(acs::SimpleSensorMode::NORMAL);
|
|
} else if (internalState == InternalState::SHUTDOWN) {
|
|
*id = REQUEST;
|
|
return prepareRequest(acs::SimpleSensorMode::OFF);
|
|
}
|
|
return NOTHING_TO_SEND;
|
|
}
|
|
|
|
ReturnValue_t SusHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
|
*id = REQUEST;
|
|
return prepareRequest(acs::SimpleSensorMode::NORMAL);
|
|
}
|
|
|
|
ReturnValue_t SusHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
|
const uint8_t *commandData,
|
|
size_t commandDataLen) {
|
|
return NOTHING_TO_SEND;
|
|
}
|
|
|
|
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;
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
|
|
const auto *reply = reinterpret_cast<const acs::SusReply *>(packet);
|
|
if (reply->dataWasSet) {
|
|
if (internalState == InternalState::STARTUP) {
|
|
commandExecuted = true;
|
|
}
|
|
PoolReadGuard pg(&dataset);
|
|
dataset.setValidity(true, true);
|
|
dataset.tempC = max1227::getTemperature(reply->tempRaw);
|
|
std::memcpy(dataset.channels.value, reply->channelsRaw, sizeof(reply->channelsRaw));
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
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; }
|
|
|
|
ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
|
LocalDataPoolManager &poolManager) {
|
|
localDataPoolMap.emplace(susMax1227::TEMPERATURE_C, &tempC);
|
|
localDataPoolMap.emplace(susMax1227::CHANNEL_VEC, &channelVec);
|
|
poolManager.subscribeForDiagPeriodicPacket(
|
|
subdp::DiagnosticsHkPeriodicParams(dataset.getSid(), false, 5.0));
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t SusHandler::prepareRequest(acs::SimpleSensorMode mode) {
|
|
request.mode = mode;
|
|
rawPacket = reinterpret_cast<uint8_t *>(&request);
|
|
rawPacketLen = sizeof(acs::SusRequest);
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
LocalPoolDataSetBase *SusHandler::getDataSetHandle(sid_t sid) {
|
|
if (sid == dataset.getSid()) {
|
|
return &dataset;
|
|
}
|
|
return nullptr;
|
|
}
|