eive-obsw/mission/devices/Max31865PT1000Handler.cpp

490 lines
18 KiB
C++

#include "Max31865PT1000Handler.h"
#include "fsfw/datapool/PoolReadGuard.h"
#include <bitset>
#include <cmath>
Max31865PT1000Handler::Max31865PT1000Handler(object_id_t objectId,
object_id_t comIF, CookieIF *comCookie, uint8_t switchId):
DeviceHandlerBase(objectId, comIF, comCookie), switchId(switchId),
sensorDataset(this), sensorDatasetSid(sensorDataset.getSid()) {
#if OBSW_VERBOSE_LEVEL >= 1
debugDivider = new PeriodicOperationDivider(0);
#endif
}
Max31865PT1000Handler::~Max31865PT1000Handler() {
}
void Max31865PT1000Handler::doStartUp() {
if(internalState == InternalState::NONE) {
internalState = InternalState::WARMUP;
Clock::getUptime(&startTime);
}
if(internalState == InternalState::WARMUP) {
dur_millis_t timeNow = 0;
Clock::getUptime(&timeNow);
if(timeNow - startTime >= 100) {
internalState = InternalState::CONFIGURE;
}
}
if(internalState == InternalState::CONFIGURE) {
if(commandExecuted) {
internalState = InternalState::REQUEST_CONFIG;
commandExecuted = false;
}
}
if(internalState == InternalState::REQUEST_CONFIG) {
if (commandExecuted) {
commandExecuted = false;
internalState = InternalState::CONFIG_HIGH_THRESHOLD;
}
}
if(internalState == InternalState::CONFIG_HIGH_THRESHOLD) {
if(commandExecuted) {
internalState = InternalState::REQUEST_HIGH_THRESHOLD;
commandExecuted = false;
}
}
if(internalState == InternalState::REQUEST_HIGH_THRESHOLD) {
if(commandExecuted) {
internalState = InternalState::CONFIG_LOW_THRESHOLD;
commandExecuted = false;
}
}
if(internalState == InternalState::CONFIG_LOW_THRESHOLD) {
if(commandExecuted) {
internalState = InternalState::REQUEST_LOW_THRESHOLD;
commandExecuted = false;
}
}
if(internalState == InternalState::REQUEST_LOW_THRESHOLD) {
if(commandExecuted) {
setMode(MODE_ON);
setMode(MODE_NORMAL);
internalState = InternalState::RUNNING;
commandExecuted = false;
}
}
}
void Max31865PT1000Handler::doShutDown() {
commandExecuted = false;
setMode(MODE_OFF);
}
ReturnValue_t Max31865PT1000Handler::buildNormalDeviceCommand(
DeviceCommandId_t *id) {
if(internalState == InternalState::RUNNING) {
*id = Max31865Definitions::REQUEST_RTD;
return buildCommandFromCommand(*id, nullptr, 0);
}
else if(internalState == InternalState::REQUEST_FAULT_BYTE) {
*id = Max31865Definitions::REQUEST_FAULT_BYTE;
return buildCommandFromCommand(*id, nullptr, 0);
}
else {
return DeviceHandlerBase::NOTHING_TO_SEND;
}
}
ReturnValue_t Max31865PT1000Handler::buildTransitionDeviceCommand(
DeviceCommandId_t *id) {
switch(internalState) {
case(InternalState::NONE):
case(InternalState::WARMUP):
case(InternalState::RUNNING):
return DeviceHandlerBase::NOTHING_TO_SEND;
case(InternalState::CONFIGURE): {
*id = Max31865Definitions::CONFIG_CMD;
uint8_t config[1] = {DEFAULT_CONFIG};
return buildCommandFromCommand(*id, config, 1);
}
case(InternalState::REQUEST_CONFIG): {
*id = Max31865Definitions::REQUEST_CONFIG;
return buildCommandFromCommand(*id, nullptr, 0);
}
case(InternalState::CONFIG_HIGH_THRESHOLD): {
*id = Max31865Definitions::WRITE_HIGH_THRESHOLD;
return buildCommandFromCommand(*id, nullptr, 0);
}
case(InternalState::REQUEST_HIGH_THRESHOLD): {
*id = Max31865Definitions::REQUEST_HIGH_THRESHOLD;
return buildCommandFromCommand(*id, nullptr, 0);
}
case(InternalState::CONFIG_LOW_THRESHOLD): {
*id = Max31865Definitions::WRITE_LOW_THRESHOLD;
return buildCommandFromCommand(*id, nullptr, 0);
}
case(InternalState::REQUEST_LOW_THRESHOLD): {
*id = Max31865Definitions::REQUEST_LOW_THRESHOLD;
return buildCommandFromCommand(*id, nullptr, 0);
}
default:
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "Max31865PT1000Handler: Invalid internal state" << std::endl;
#else
sif::printError("Max31865PT1000Handler: Invalid internal state\n");
#endif
return HasReturnvaluesIF::RETURN_FAILED;
}
}
ReturnValue_t Max31865PT1000Handler::buildCommandFromCommand(
DeviceCommandId_t deviceCommand, const uint8_t *commandData,
size_t commandDataLen) {
switch(deviceCommand) {
case(Max31865Definitions::CONFIG_CMD) : {
commandBuffer[0] = static_cast<uint8_t>(Max31865Definitions::CONFIG_CMD);
if(commandDataLen == 1) {
commandBuffer[1] = commandData[0];
DeviceHandlerBase::rawPacketLen = 2;
DeviceHandlerBase::rawPacket = commandBuffer.data();
return HasReturnvaluesIF::RETURN_OK;
}
else {
return DeviceHandlerIF::NO_COMMAND_DATA;
}
}
case(Max31865Definitions::REQUEST_CONFIG): {
commandBuffer[0] = static_cast<uint8_t>(
Max31865Definitions::REQUEST_CONFIG);
commandBuffer[1] = 0x00; // dummy byte
DeviceHandlerBase::rawPacketLen = 2;
DeviceHandlerBase::rawPacket = commandBuffer.data();
return HasReturnvaluesIF::RETURN_OK;
}
case(Max31865Definitions::WRITE_HIGH_THRESHOLD): {
commandBuffer[0] = static_cast<uint8_t>(
Max31865Definitions::WRITE_HIGH_THRESHOLD);
commandBuffer[1] = static_cast<uint8_t>(HIGH_THRESHOLD >> 8);
commandBuffer[2] = static_cast<uint8_t>(HIGH_THRESHOLD & 0xFF);
DeviceHandlerBase::rawPacketLen = 3;
DeviceHandlerBase::rawPacket = commandBuffer.data();
return HasReturnvaluesIF::RETURN_OK;
}
case(Max31865Definitions::REQUEST_HIGH_THRESHOLD): {
commandBuffer[0] = static_cast<uint8_t>(
Max31865Definitions::REQUEST_HIGH_THRESHOLD);
commandBuffer[1] = 0x00; //dummy byte
commandBuffer[2] = 0x00; //dummy byte
DeviceHandlerBase::rawPacketLen = 3;
DeviceHandlerBase::rawPacket = commandBuffer.data();
return HasReturnvaluesIF::RETURN_OK;
}
case(Max31865Definitions::WRITE_LOW_THRESHOLD): {
commandBuffer[0] = static_cast<uint8_t>(
Max31865Definitions::WRITE_LOW_THRESHOLD);
commandBuffer[1] = static_cast<uint8_t>(LOW_THRESHOLD >> 8);
commandBuffer[2] = static_cast<uint8_t>(LOW_THRESHOLD & 0xFF);
DeviceHandlerBase::rawPacketLen = 3;
DeviceHandlerBase::rawPacket = commandBuffer.data();
return HasReturnvaluesIF::RETURN_OK;
}
case(Max31865Definitions::REQUEST_LOW_THRESHOLD): {
commandBuffer[0] = static_cast<uint8_t>(
Max31865Definitions::REQUEST_LOW_THRESHOLD);
commandBuffer[1] = 0x00; //dummy byte
commandBuffer[2] = 0x00; //dummy byte
DeviceHandlerBase::rawPacketLen = 3;
DeviceHandlerBase::rawPacket = commandBuffer.data();
return HasReturnvaluesIF::RETURN_OK;
}
case(Max31865Definitions::REQUEST_RTD): {
commandBuffer[0] = static_cast<uint8_t>(
Max31865Definitions::REQUEST_RTD);
// two dummy bytes
commandBuffer[1] = 0x00;
commandBuffer[2] = 0x00;
DeviceHandlerBase::rawPacketLen = 3;
DeviceHandlerBase::rawPacket = commandBuffer.data();
return HasReturnvaluesIF::RETURN_OK;
}
case(Max31865Definitions::REQUEST_FAULT_BYTE): {
commandBuffer[0] = static_cast<uint8_t>(
Max31865Definitions::REQUEST_FAULT_BYTE);
commandBuffer[1] = 0x00;
DeviceHandlerBase::rawPacketLen = 2;
DeviceHandlerBase::rawPacket = commandBuffer.data();
return HasReturnvaluesIF::RETURN_OK;
}
default:
//Unknown DeviceCommand
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
}
void Max31865PT1000Handler::fillCommandAndReplyMap() {
insertInCommandAndReplyMap(Max31865Definitions::CONFIG_CMD, 3);
insertInCommandAndReplyMap(Max31865Definitions::REQUEST_CONFIG, 3);
insertInCommandAndReplyMap(Max31865Definitions::WRITE_LOW_THRESHOLD, 3);
insertInCommandAndReplyMap(Max31865Definitions::REQUEST_LOW_THRESHOLD, 3);
insertInCommandAndReplyMap(Max31865Definitions::WRITE_HIGH_THRESHOLD, 3);
insertInCommandAndReplyMap(Max31865Definitions::REQUEST_HIGH_THRESHOLD, 3);
insertInCommandAndReplyMap(Max31865Definitions::REQUEST_RTD, 3,
&sensorDataset);
insertInCommandAndReplyMap(Max31865Definitions::REQUEST_FAULT_BYTE, 3);
}
ReturnValue_t Max31865PT1000Handler::scanForReply(const uint8_t *start,
size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
size_t rtdReplySize = 3;
size_t configReplySize = 2;
if(remainingSize == rtdReplySize and
internalState == InternalState::RUNNING) {
*foundId = Max31865Definitions::REQUEST_RTD;
*foundLen = rtdReplySize;
return RETURN_OK;
}
if(remainingSize == 3) {
switch(internalState) {
case(InternalState::CONFIG_HIGH_THRESHOLD): {
*foundLen = 3;
*foundId = Max31865Definitions::WRITE_HIGH_THRESHOLD;
commandExecuted = true;
return RETURN_OK;
}
case(InternalState::REQUEST_HIGH_THRESHOLD): {
*foundLen = 3;
*foundId = Max31865Definitions::REQUEST_HIGH_THRESHOLD;
return RETURN_OK;
}
case(InternalState::CONFIG_LOW_THRESHOLD): {
*foundLen = 3;
*foundId = Max31865Definitions::WRITE_LOW_THRESHOLD;
commandExecuted = true;
return RETURN_OK;
}
case(InternalState::REQUEST_LOW_THRESHOLD): {
*foundLen = 3;
*foundId = Max31865Definitions::REQUEST_LOW_THRESHOLD;
return RETURN_OK;
}
default: {
sif::debug << "Max31865PT1000Handler::scanForReply: Unknown internal state"
<< std::endl;
return RETURN_OK;
}
}
}
if(remainingSize == configReplySize) {
if(internalState == InternalState::CONFIGURE) {
commandExecuted = true;
*foundLen = configReplySize;
*foundId = Max31865Definitions::CONFIG_CMD;
}
else if(internalState == InternalState::REQUEST_FAULT_BYTE) {
*foundId = Max31865Definitions::REQUEST_FAULT_BYTE;
*foundLen = 2;
internalState = InternalState::RUNNING;
}
else {
*foundId = Max31865Definitions::REQUEST_CONFIG;
*foundLen = configReplySize;
}
}
return RETURN_OK;
}
ReturnValue_t Max31865PT1000Handler::interpretDeviceReply(
DeviceCommandId_t id, const uint8_t *packet) {
switch(id) {
case(Max31865Definitions::REQUEST_CONFIG): {
if(packet[1] != DEFAULT_CONFIG) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
// it propably would be better if we at least try one restart..
sif::error << "Max31865PT1000Handler: Invalid configuration reply!" << std::endl;
#else
sif::printError("Max31865PT1000Handler: Invalid configuration reply!\n");
#endif
return HasReturnvaluesIF::RETURN_OK;
}
// set to true for invalid configs too for now.
if(internalState == InternalState::REQUEST_CONFIG) {
commandExecuted = true;
}
else if(internalState == InternalState::RUNNING) {
// we should propably generate a telemetry with the config byte
// as payload here.
}
break;
}
case(Max31865Definitions::REQUEST_LOW_THRESHOLD): {
uint16_t readLowThreshold = packet[0] << 8 | packet[1];
if(readLowThreshold != LOW_THRESHOLD) {
#if FSFW_CPP_OSTREAM_ENABLED == 1 && DEBUG_RTD == 1
sif::error
<< "Max31865PT1000Handler::interpretDeviceReply: Missmatch between written "
<< "and readback value of low threshold register"
<< std::endl;
#endif
}
commandExecuted = true;
break;
}
case(Max31865Definitions::REQUEST_HIGH_THRESHOLD): {
uint16_t readHighThreshold = packet[0] << 8 | packet[1];
if(readHighThreshold != HIGH_THRESHOLD) {
#if FSFW_CPP_OSTREAM_ENABLED == 1 && DEBUG_RTD == 1
sif::error
<< "Max31865PT1000Handler::interpretDeviceReply: Missmatch between written "
<< "and readback value of high threshold register"
<< std::endl;
#endif
}
commandExecuted = true;
break;
}
case(Max31865Definitions::REQUEST_RTD): {
// first bit of LSB reply byte is the fault bit
uint8_t faultBit = packet[2] & 0b0000'0001;
if(faultBit == 1) {
// Maybe we should attempt to restart it?
internalState = InternalState::REQUEST_FAULT_BYTE;
}
// RTD value consists of last seven bits of the LSB reply byte and
// the MSB reply byte
uint16_t adcCode = ((packet[1] << 8) | packet[2]) >> 1;
// do something with rtd value, will propably be stored in
// dataset.
float rtdValue = adcCode * RTD_RREF_PT1000 / INT16_MAX;
// calculate approximation
float approxTemp = adcCode / 32.0 - 256.0;
#if OBSW_VERBOSE_LEVEL >= 1
if(debugDivider->checkAndIncrement()) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::info << "Max31865PT1000Handler::interpretDeviceReply: Measured "
<< "resistance is " << rtdValue << " Ohms." << std::endl;
sif::info << "Approximated temperature is " << approxTemp << " C"
<< std::endl;
#else
sif::printInfo("Max31865PT1000Handler::interpretDeviceReply: Measured resistance is %f"
" Ohms.\n", rtdValue);
sif::printInfo("Approximated temperature is %f C\n", approxTemp);
#endif
sensorDataset.setChanged(true);
}
#endif
PoolReadGuard pg(&sensorDataset);
if(pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
// Configuration error
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::debug << "Max31865PT1000Handler::interpretDeviceReply: Error reading dataset!"
<< std::endl;
#else
sif::printDebug("Max31865PT1000Handler::interpretDeviceReply: Error reading dataset!\n");
#endif
return pg.getReadResult();
}
if(not sensorDataset.isValid()) {
sensorDataset.setValidity(true, false);
sensorDataset.rtdValue.setValid(true);
sensorDataset.temperatureCelcius.setValid(true);
}
sensorDataset.rtdValue = rtdValue;
sensorDataset.temperatureCelcius = approxTemp;
break;
}
case(Max31865Definitions::REQUEST_FAULT_BYTE): {
faultByte = packet[1];
#if OBSW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::info << "Max31865PT1000Handler::interpretDeviceReply: Fault byte"
" is: 0b" << std::bitset<8>(faultByte) << std::endl;
#else
sif::printInfo("Max31865PT1000Handler::interpretDeviceReply: Fault byte"
" is: 0b" BYTE_TO_BINARY_PATTERN "\n", BYTE_TO_BINARY(faultByte));
#endif
#endif
ReturnValue_t result = sensorDataset.read();
if(result != HasReturnvaluesIF::RETURN_OK) {
// Configuration error
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::debug << "Max31865PT1000Handler::interpretDeviceReply: "
"Error reading dataset!" << std::endl;
#else
sif::printDebug("Max31865PT1000Handler::interpretDeviceReply: "
"Error reading dataset!\n");
#endif
return result;
}
sensorDataset.errorByte.setValid(true);
sensorDataset.errorByte = faultByte;
if(faultByte != 0) {
sensorDataset.temperatureCelcius.setValid(false);
}
result = sensorDataset.commit();
if(result != HasReturnvaluesIF::RETURN_OK) {
// Configuration error
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::debug << "Max31865PT1000Handler::interpretDeviceReply: "
"Error commiting dataset!" << std::endl;
#else
sif::printDebug("Max31865PT1000Handler::interpretDeviceReply: "
"Error commiting dataset!\n");
#endif
return result;
}
break;
}
default:
break;
}
return HasReturnvaluesIF::RETURN_OK;
}
void Max31865PT1000Handler::debugInterface(uint8_t positionTracker,
object_id_t objectId, uint32_t parameter) {
}
uint32_t Max31865PT1000Handler::getTransitionDelayMs(
Mode_t modeFrom, Mode_t modeTo) {
return 20000;
}
ReturnValue_t Max31865PT1000Handler::getSwitches(
const uint8_t **switches, uint8_t *numberOfSwitches) {
return DeviceHandlerBase::NO_SWITCH;
}
void Max31865PT1000Handler::doTransition(Mode_t modeFrom,
Submode_t subModeFrom) {
DeviceHandlerBase::doTransition(modeFrom, subModeFrom);
}
ReturnValue_t Max31865PT1000Handler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
localDataPoolMap.emplace(Max31865Definitions::PoolIds::RTD_VALUE, new PoolEntry<float>({0}));
localDataPoolMap.emplace(Max31865Definitions::PoolIds::TEMPERATURE_C,
new PoolEntry<float>({0}, 1, true));
localDataPoolMap.emplace(Max31865Definitions::PoolIds::FAULT_BYTE,
new PoolEntry<uint8_t>({0}));
// poolManager.subscribeForPeriodicPacket(sensorDatasetSid,
// false, 4.0, false);
return HasReturnvaluesIF::RETURN_OK;
}
void Max31865PT1000Handler::modeChanged() {
internalState = InternalState::NONE;
}