140 lines
4.8 KiB
C++
140 lines
4.8 KiB
C++
#include "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;
|
|
primaryDataset.setValidity(false, true);
|
|
internalState = InternalState::SHUTDOWN;
|
|
}
|
|
if (internalState == InternalState::SHUTDOWN and commandExecuted) {
|
|
updatePeriodicReply(false, REPLY);
|
|
setMode(MODE_OFF);
|
|
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;
|
|
if (internalState == InternalState::SHUTDOWN) {
|
|
commandExecuted = true;
|
|
}
|
|
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);
|
|
primaryDataset.setValidity(true, true);
|
|
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;
|
|
}
|
|
|
|
LocalPoolDataSetBase *MgmRm3100CustomHandler::getDataSetHandle(sid_t sid) {
|
|
if (sid == primaryDataset.getSid()) {
|
|
return &primaryDataset;
|
|
}
|
|
return nullptr;
|
|
}
|