eive-obsw/mission/acs/archive/LegacySusHandler.cpp
2023-03-24 20:50:33 +01:00

234 lines
7.9 KiB
C++

#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <mission/devices/LegacySusHandler.h>
#include "OBSWConfig.h"
LegacySusHandler::LegacySusHandler(object_id_t objectId, uint8_t susIdx, object_id_t comIF,
CookieIF *comCookie)
: DeviceHandlerBase(objectId, comIF, comCookie), divider(5), dataset(this), susIdx(susIdx) {}
LegacySusHandler::~LegacySusHandler() {}
void LegacySusHandler::doStartUp() {
if (comState == ComStates::IDLE) {
comState = ComStates::WRITE_SETUP;
commandExecuted = false;
}
if (comState == ComStates::WRITE_SETUP) {
if (commandExecuted) {
if (goToNormalModeImmediately) {
setMode(MODE_NORMAL);
} else {
setMode(_MODE_TO_ON);
}
commandExecuted = false;
if (clkMode == ClkModes::INT_CLOCKED) {
comState = ComStates::START_INT_CLOCKED_CONVERSIONS;
} else {
comState = ComStates::EXT_CLOCKED_CONVERSIONS;
}
}
}
}
void LegacySusHandler::doShutDown() {
setMode(_MODE_POWER_DOWN);
comState = ComStates::IDLE;
}
ReturnValue_t LegacySusHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
switch (comState) {
case (ComStates::IDLE): {
break;
}
case (ComStates::WRITE_SETUP): {
*id = susMax1227::WRITE_SETUP;
return buildCommandFromCommand(*id, nullptr, 0);
}
case (ComStates::EXT_CLOCKED_CONVERSIONS): {
*id = susMax1227::READ_EXT_TIMED_CONVERSIONS;
return buildCommandFromCommand(*id, nullptr, 0);
}
case (ComStates::START_INT_CLOCKED_CONVERSIONS): {
*id = susMax1227::START_INT_TIMED_CONVERSIONS;
comState = ComStates::READ_INT_CLOCKED_CONVERSIONS;
return buildCommandFromCommand(*id, nullptr, 0);
}
case (ComStates::READ_INT_CLOCKED_CONVERSIONS): {
*id = susMax1227::READ_INT_TIMED_CONVERSIONS;
comState = ComStates::START_INT_CLOCKED_CONVERSIONS;
return buildCommandFromCommand(*id, nullptr, 0);
}
case (ComStates::EXT_CLOCKED_TEMP): {
*id = susMax1227::READ_EXT_TIMED_TEMPS;
return buildCommandFromCommand(*id, nullptr, 0);
}
}
return NOTHING_TO_SEND;
}
ReturnValue_t LegacySusHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
if (comState == ComStates::WRITE_SETUP) {
*id = susMax1227::WRITE_SETUP;
return buildCommandFromCommand(*id, nullptr, 0);
}
return NOTHING_TO_SEND;
}
ReturnValue_t LegacySusHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t *commandData,
size_t commandDataLen) {
using namespace max1227;
switch (deviceCommand) {
case (susMax1227::WRITE_SETUP): {
if (clkMode == ClkModes::INT_CLOCKED) {
cmdBuffer[0] = susMax1227::SETUP_INT_CLOKED;
} else {
cmdBuffer[0] = susMax1227::SETUP_EXT_CLOCKED;
}
rawPacket = cmdBuffer;
rawPacketLen = 1;
break;
}
case (susMax1227::START_INT_TIMED_CONVERSIONS): {
std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
cmdBuffer[0] = max1227::buildResetByte(true);
cmdBuffer[1] = susMax1227::CONVERSION;
rawPacket = cmdBuffer;
rawPacketLen = 2;
break;
}
case (susMax1227::READ_INT_TIMED_CONVERSIONS): {
std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
rawPacket = cmdBuffer;
rawPacketLen = susMax1227::SIZE_READ_INT_CONVERSIONS;
break;
}
case (susMax1227::READ_EXT_TIMED_CONVERSIONS): {
std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
rawPacket = cmdBuffer;
for (uint8_t idx = 0; idx < 6; idx++) {
cmdBuffer[idx * 2] = buildConvByte(ScanModes::N_ONCE, idx, false);
cmdBuffer[idx * 2 + 1] = 0;
}
cmdBuffer[12] = 0x00;
rawPacketLen = susMax1227::SIZE_READ_EXT_CONVERSIONS;
break;
}
case (susMax1227::READ_EXT_TIMED_TEMPS): {
cmdBuffer[0] = buildConvByte(ScanModes::N_ONCE, 0, true);
std::memset(cmdBuffer + 1, 0, 24);
rawPacket = cmdBuffer;
rawPacketLen = 25;
break;
}
default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
return returnvalue::OK;
}
void LegacySusHandler::fillCommandAndReplyMap() {
insertInCommandAndReplyMap(susMax1227::WRITE_SETUP, 1);
insertInCommandAndReplyMap(susMax1227::START_INT_TIMED_CONVERSIONS, 1);
insertInCommandAndReplyMap(susMax1227::READ_INT_TIMED_CONVERSIONS, 1, &dataset,
susMax1227::SIZE_READ_INT_CONVERSIONS);
insertInCommandAndReplyMap(susMax1227::READ_EXT_TIMED_CONVERSIONS, 1, &dataset,
susMax1227::SIZE_READ_EXT_CONVERSIONS);
insertInCommandAndReplyMap(susMax1227::READ_EXT_TIMED_TEMPS, 1);
}
ReturnValue_t LegacySusHandler::scanForReply(const uint8_t *start, size_t remainingSize,
DeviceCommandId_t *foundId, size_t *foundLen) {
*foundId = this->getPendingCommand();
*foundLen = remainingSize;
return returnvalue::OK;
}
ReturnValue_t LegacySusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
switch (id) {
case susMax1227::WRITE_SETUP: {
if (getMode() == _MODE_START_UP) {
commandExecuted = true;
}
return returnvalue::OK;
}
case susMax1227::START_INT_TIMED_CONVERSIONS: {
return returnvalue::OK;
}
case susMax1227::READ_INT_TIMED_CONVERSIONS: {
PoolReadGuard readSet(&dataset);
dataset.tempC = max1227::getTemperature(((packet[0] & 0x0f) << 8) | packet[1]);
for (uint8_t idx = 0; idx < 6; idx++) {
dataset.channels[idx] = packet[idx * 2 + 2] << 8 | packet[idx * 2 + 3];
}
dataset.setValidity(true, true);
printDataset();
break;
}
case (susMax1227::READ_EXT_TIMED_CONVERSIONS): {
PoolReadGuard readSet(&dataset);
for (uint8_t idx = 0; idx < 6; idx++) {
dataset.channels[idx] = packet[idx * 2 + 1] << 8 | packet[idx * 2 + 2];
}
dataset.channels.setValid(true);
// Read temperature in next read cycle
if (clkMode == ClkModes::EXT_CLOCKED_WITH_TEMP) {
comState = ComStates::EXT_CLOCKED_TEMP;
}
printDataset();
break;
}
case (susMax1227::READ_EXT_TIMED_TEMPS): {
PoolReadGuard readSet(&dataset);
dataset.tempC = max1227::getTemperature(((packet[23] & 0x0f) << 8) | packet[24]);
dataset.tempC.setValid(true);
comState = ComStates::EXT_CLOCKED_CONVERSIONS;
break;
}
default: {
sif::debug << "SusHandler::interpretDeviceReply: Unknown reply id" << std::endl;
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
}
}
return returnvalue::OK;
}
uint32_t LegacySusHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 3000; }
ReturnValue_t LegacySusHandler::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;
}
void LegacySusHandler::setToGoToNormalMode(bool enable) {
this->goToNormalModeImmediately = enable;
}
void LegacySusHandler::printDataset() {
if (periodicPrintout) {
if (divider.checkAndIncrement()) {
sif::info << "SUS ADC " << static_cast<int>(susIdx) << " hex [" << std::setfill('0')
<< std::hex;
for (uint8_t idx = 0; idx < 6; idx++) {
sif::info << std::setw(3) << dataset.channels[idx];
if (idx < 6 - 1) {
sif::info << ",";
}
}
sif::info << "] | T[C] " << std::dec << dataset.tempC.value << std::endl;
}
}
}
void LegacySusHandler::enablePeriodicPrintout(bool enable, uint8_t divider) {
this->periodicPrintout = enable;
this->divider.setDivider(divider);
}