#include "SusHandler.h" #include #include #include "OBSWConfig.h" SusHandler::SusHandler(object_id_t objectId, uint8_t susIdx, object_id_t comIF, CookieIF *comCookie) : DeviceHandlerBase(objectId, comIF, comCookie), divider(5), dataset(this), susIdx(susIdx) {} SusHandler::~SusHandler() {} void SusHandler::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 SusHandler::doShutDown() { setMode(_MODE_POWER_DOWN); comState = ComStates::IDLE; } ReturnValue_t SusHandler::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 SusHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { if (comState == ComStates::WRITE_SETUP) { *id = susMax1227::WRITE_SETUP; return buildCommandFromCommand(*id, nullptr, 0); } return NOTHING_TO_SEND; } ReturnValue_t SusHandler::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 SusHandler::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 SusHandler::scanForReply(const uint8_t *start, size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) { *foundId = this->getPendingCommand(); *foundLen = remainingSize; return returnvalue::OK; } ReturnValue_t SusHandler::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.temperatureCelcius = 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.temperatureCelcius = max1227::getTemperature(((packet[23] & 0x0f) << 8) | packet[24]); dataset.temperatureCelcius.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 SusHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 3000; } 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; } void SusHandler::setToGoToNormalMode(bool enable) { this->goToNormalModeImmediately = enable; } void SusHandler::printDataset() { if (periodicPrintout) { if (divider.checkAndIncrement()) { sif::info << "SUS ADC " << static_cast(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.temperatureCelcius.value << std::endl; } } } void SusHandler::enablePeriodicPrintout(bool enable, uint8_t divider) { this->periodicPrintout = enable; this->divider.setDivider(divider); }