eive-obsw/mission/devices/MgmRm3100CustomHandler.cpp

140 lines
4.8 KiB
C++
Raw Normal View History

2023-02-27 11:44:52 +01:00
#include <mission/devices/MgmRm3100CustomHandler.h>
#include "fsfw/datapool/PoolReadGuard.h"
#include "fsfw/devicehandlers/DeviceHandlerMessage.h"
#include "fsfw/globalfunctions/bitutility.h"
#include "fsfw/objectmanager/SystemObjectIF.h"
#include "fsfw/returnvalues/returnvalue.h"
MgmRm3100CustomHandler::MgmRm3100CustomHandler(object_id_t objectId,
object_id_t deviceCommunication, CookieIF *comCookie,
uint32_t transitionDelay)
: DeviceHandlerBase(objectId, deviceCommunication, comCookie),
primaryDataset(this),
transitionDelay(transitionDelay) {}
MgmRm3100CustomHandler::~MgmRm3100CustomHandler() = default;
void MgmRm3100CustomHandler::doStartUp() {
if (internalState != InternalState::STARTUP) {
commandExecuted = false;
updatePeriodicReply(true, REPLY);
internalState = InternalState::STARTUP;
}
if (internalState == InternalState::STARTUP) {
if (commandExecuted) {
commandExecuted = false;
internalState = InternalState::NONE;
setMode(MODE_NORMAL);
}
}
}
void MgmRm3100CustomHandler::doShutDown() {
if (internalState != InternalState::SHUTDOWN) {
commandExecuted = false;
2023-02-28 01:25:25 +01:00
primaryDataset.setValidity(false, true);
2023-02-27 11:44:52 +01:00
internalState = InternalState::SHUTDOWN;
}
if (internalState == InternalState::SHUTDOWN and commandExecuted) {
updatePeriodicReply(false, REPLY);
2023-03-06 14:25:26 +01:00
setMode(MODE_OFF);
2023-02-27 11:44:52 +01:00
commandExecuted = false;
}
}
ReturnValue_t MgmRm3100CustomHandler::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 MgmRm3100CustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t *commandData,
size_t commandDataLen) {
return NOTHING_TO_SEND;
}
ReturnValue_t MgmRm3100CustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
*id = REQUEST;
return prepareRequest(acs::SimpleSensorMode::NORMAL);
}
ReturnValue_t MgmRm3100CustomHandler::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::MgmRm3100Reply)) {
*foundLen = len;
return returnvalue::FAILED;
}
*foundId = REPLY;
*foundLen = len;
2023-02-28 01:25:25 +01:00
if (internalState == InternalState::SHUTDOWN) {
commandExecuted = true;
}
2023-02-27 11:44:52 +01:00
return returnvalue::OK;
}
ReturnValue_t MgmRm3100CustomHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
const auto *reply = reinterpret_cast<const acs::MgmRm3100Reply *>(packet);
if (reply->dataWasRead) {
if (internalState == InternalState::STARTUP) {
commandExecuted = true;
}
PoolReadGuard pg(&primaryDataset);
2023-03-02 13:39:29 +01:00
primaryDataset.setValidity(true, true);
2023-02-27 11:44:52 +01:00
for (uint8_t idx = 0; idx < 3; idx++) {
primaryDataset.fieldStrengths[idx] = reply->mgmValuesRaw[idx] * reply->scaleFactors[idx];
}
}
return returnvalue::OK;
}
void MgmRm3100CustomHandler::fillCommandAndReplyMap() {
insertInCommandMap(REQUEST);
insertInReplyMap(REPLY, 5, nullptr, 0, true);
}
void MgmRm3100CustomHandler::modeChanged() { internalState = InternalState::NONE; }
ReturnValue_t MgmRm3100CustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(mgmRm3100::FIELD_STRENGTHS, &mgmXYZ);
poolManager.subscribeForRegularPeriodicPacket({primaryDataset.getSid(), false, 10.0});
return returnvalue::OK;
}
uint32_t MgmRm3100CustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
return this->transitionDelay;
}
void MgmRm3100CustomHandler::setToGoToNormalMode(bool enable) { goToNormalModeAtStartup = enable; }
void MgmRm3100CustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
periodicPrintout = enable;
debugDivider.setDivider(divider);
}
ReturnValue_t MgmRm3100CustomHandler::prepareRequest(acs::SimpleSensorMode mode) {
request.mode = mode;
rawPacket = reinterpret_cast<uint8_t *>(&request);
rawPacketLen = sizeof(acs::MgmRm3100Request);
return returnvalue::OK;
}
2023-02-28 01:25:25 +01:00
LocalPoolDataSetBase *MgmRm3100CustomHandler::getDataSetHandle(sid_t sid) {
if (sid == primaryDataset.getSid()) {
return &primaryDataset;
}
return nullptr;
}