eive-obsw/mission/devices/GyrL3gCustomHandler.cpp
2023-02-26 19:09:12 +01:00

186 lines
6.3 KiB
C++

#include <mission/devices/GyrL3gCustomHandler.h>
#include <cmath>
#include "fsfw/datapool/PoolReadGuard.h"
#include "mission/devices/devicedefinitions/acsPolling.h"
GyrL3gCustomHandler::GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication,
CookieIF *comCookie, uint32_t transitionDelayMs)
: DeviceHandlerBase(objectId, deviceCommunication, comCookie),
transitionDelayMs(transitionDelayMs),
dataset(this) {}
GyrL3gCustomHandler::~GyrL3gCustomHandler() = default;
void GyrL3gCustomHandler::doStartUp() {
if (internalState == InternalState::NONE) {
internalState = InternalState::STARTUP;
updatePeriodicReply(true, l3gd20h::REPLY);
commandExecuted = false;
}
if (internalState == InternalState::STARTUP) {
if (commandExecuted) {
if (goNormalModeImmediately) {
setMode(MODE_NORMAL);
} else {
setMode(_MODE_TO_ON);
}
}
}
}
void GyrL3gCustomHandler::doShutDown() {
if (internalState != InternalState::SHUTDOWN) {
internalState = InternalState::SHUTDOWN;
commandExecuted = false;
}
if (commandExecuted) {
internalState = InternalState::NONE;
updatePeriodicReply(false, l3gd20h::REPLY);
commandExecuted = false;
setMode(_MODE_POWER_DOWN);
}
}
ReturnValue_t GyrL3gCustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
switch (internalState) {
case (InternalState::NONE):
case (InternalState::NORMAL): {
return NOTHING_TO_SEND;
}
case (InternalState::STARTUP): {
*id = l3gd20h::REQUEST;
gyrRequest.ctrlRegs[0] = l3gd20h::CTRL_REG_1_VAL;
gyrRequest.ctrlRegs[1] = l3gd20h::CTRL_REG_2_VAL;
gyrRequest.ctrlRegs[2] = l3gd20h::CTRL_REG_3_VAL;
gyrRequest.ctrlRegs[3] = l3gd20h::CTRL_REG_4_VAL;
gyrRequest.ctrlRegs[4] = l3gd20h::CTRL_REG_5_VAL;
return doPeriodicRequest(acs::SimpleSensorMode::NORMAL);
}
case (InternalState::SHUTDOWN): {
*id = l3gd20h::REQUEST;
return doPeriodicRequest(acs::SimpleSensorMode::OFF);
}
default:
#if FSFW_CPP_OSTREAM_ENABLED == 1
/* Might be a configuration error. */
sif::warning << "GyroL3GD20Handler::buildTransitionDeviceCommand: "
"Unknown internal state!"
<< std::endl;
#else
sif::printDebug(
"GyroL3GD20Handler::buildTransitionDeviceCommand: "
"Unknown internal state!\n");
#endif
return returnvalue::OK;
}
return returnvalue::OK;
}
ReturnValue_t GyrL3gCustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
*id = l3gd20h::REQUEST;
return doPeriodicRequest(acs::SimpleSensorMode::NORMAL);
}
ReturnValue_t GyrL3gCustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t *commandData,
size_t commandDataLen) {
switch (deviceCommand) {
default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
return returnvalue::OK;
}
ReturnValue_t GyrL3gCustomHandler::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::GyroL3gReply)) {
*foundLen = len;
return returnvalue::FAILED;
}
*foundId = l3gd20h::REPLY;
*foundLen = len;
return returnvalue::OK;
}
ReturnValue_t GyrL3gCustomHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
const acs::GyroL3gReply *reply = reinterpret_cast<const acs::GyroL3gReply *>(packet);
if (reply->cfgWasSet) {
if (internalState == InternalState::STARTUP) {
commandExecuted = true;
}
PoolReadGuard pg(&dataset);
dataset.setValidity(true, true);
dataset.angVelocX = static_cast<float>(reply->angVelocities[0]) * reply->sensitivity;
dataset.angVelocY = static_cast<float>(reply->angVelocities[1]) * reply->sensitivity;
dataset.angVelocZ = static_cast<float>(reply->angVelocities[2]) * reply->sensitivity;
if (std::abs(dataset.angVelocX.value) > absLimitX) {
dataset.angVelocX.setValid(false);
}
if (std::abs(dataset.angVelocY.value) > absLimitY) {
dataset.angVelocY.setValid(false);
}
if (std::abs(dataset.angVelocZ.value) > absLimitZ) {
dataset.angVelocZ.setValid(false);
}
dataset.temperature = 25.0 - reply->tempOffsetRaw;
}
return returnvalue::OK;
}
uint32_t GyrL3gCustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
return this->transitionDelayMs;
}
void GyrL3gCustomHandler::setToGoToNormalMode(bool enable) { this->goNormalModeImmediately = true; }
ReturnValue_t GyrL3gCustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry<float>({0.0}));
poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(dataset.getSid(), false, 10.0));
return returnvalue::OK;
}
void GyrL3gCustomHandler::fillCommandAndReplyMap() {
insertInCommandMap(l3gd20h::REQUEST);
insertInReplyMap(l3gd20h::REPLY, 5, nullptr, 0, true);
}
void GyrL3gCustomHandler::modeChanged() { internalState = InternalState::NONE; }
void GyrL3gCustomHandler::setAbsoluteLimits(float limitX, float limitY, float limitZ) {
this->absLimitX = limitX;
this->absLimitY = limitY;
this->absLimitZ = limitZ;
}
void GyrL3gCustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
periodicPrintout = enable;
debugDivider.setDivider(divider);
}
LocalPoolDataSetBase *GyrL3gCustomHandler::getDataSetHandle(sid_t sid) {
if (sid == dataset.getSid()) {
return &dataset;
}
return nullptr;
}
ReturnValue_t GyrL3gCustomHandler::doPeriodicRequest(acs::SimpleSensorMode mode) {
gyrRequest.mode = mode;
rawPacket = reinterpret_cast<uint8_t *>(&gyrRequest);
rawPacketLen = sizeof(acs::GyroL3gRequest);
return returnvalue::OK;
}