204 lines
7.3 KiB
C++
204 lines
7.3 KiB
C++
#include <fsfw/datapool/PoolReadGuard.h>
|
|
#include <mission/devices/SusHandler.h>
|
|
#include <OBSWConfig.h>
|
|
|
|
SusHandler::SusHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie,
|
|
LinuxLibgpioIF* gpioComIF, gpioId_t chipSelectId) :
|
|
DeviceHandlerBase(objectId, comIF, comCookie), gpioComIF(gpioComIF), chipSelectId(
|
|
chipSelectId), dataset(this) {
|
|
if (comCookie == NULL) {
|
|
sif::error << "SusHandler: Invalid com cookie" << std::endl;
|
|
}
|
|
}
|
|
|
|
SusHandler::~SusHandler() {
|
|
}
|
|
|
|
|
|
void SusHandler::doStartUp(){
|
|
if (internalState == InternalState::CONFIGURED) {
|
|
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
|
|
setMode(MODE_NORMAL);
|
|
#else
|
|
setMode(_MODE_TO_ON);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void SusHandler::doShutDown(){
|
|
setMode(_MODE_POWER_DOWN);
|
|
}
|
|
|
|
ReturnValue_t SusHandler::buildNormalDeviceCommand(
|
|
DeviceCommandId_t * id) {
|
|
if (communicationStep == CommunicationStep::WRITE_SETUP) {
|
|
*id = SUS::WRITE_SETUP;
|
|
communicationStep = CommunicationStep::REQUEST_TEMP;
|
|
// communicationStep = CommunicationStep::PERFORM_CONVERSIONS;
|
|
}
|
|
else if (communicationStep == CommunicationStep::PERFORM_CONVERSIONS) {
|
|
*id = SUS::PERFORM_CONVERSIONS;
|
|
communicationStep = CommunicationStep::WRITE_SETUP;
|
|
// communicationStep = CommunicationStep::PERFORM_CONVERSIONS;
|
|
}
|
|
else if (communicationStep == CommunicationStep::REQUEST_TEMP) {
|
|
*id = SUS::RQUEST_TEMP;
|
|
communicationStep = CommunicationStep::READ_TEMP;
|
|
}
|
|
else if (communicationStep == CommunicationStep::READ_TEMP) {
|
|
*id = SUS::READ_TEMP;
|
|
communicationStep = CommunicationStep::PERFORM_CONVERSIONS;
|
|
}
|
|
return buildCommandFromCommand(*id, nullptr, 0);
|
|
}
|
|
|
|
ReturnValue_t SusHandler::buildTransitionDeviceCommand(
|
|
DeviceCommandId_t * id){
|
|
if (internalState == InternalState::SETUP) {
|
|
*id = SUS::WRITE_SETUP;
|
|
}
|
|
else {
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
return buildCommandFromCommand(*id, nullptr, 0);
|
|
}
|
|
|
|
ReturnValue_t SusHandler::buildCommandFromCommand(
|
|
DeviceCommandId_t deviceCommand, const uint8_t * commandData,
|
|
size_t commandDataLen) {
|
|
switch(deviceCommand) {
|
|
case(SUS::WRITE_SETUP): {
|
|
gpioComIF->pullLow(chipSelectId);
|
|
cmdBuffer[0] = SUS::SETUP_DEFINITION;
|
|
rawPacket = cmdBuffer;
|
|
rawPacketLen = 1;
|
|
internalState = InternalState::CONFIGURED;
|
|
return RETURN_OK;
|
|
}
|
|
case(SUS::PERFORM_CONVERSIONS): {
|
|
// gpioComIF->pullLow(chipSelectId);
|
|
std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
|
|
/**
|
|
* The sun sensor ADC is shutdown when CS is pulled high so each time requesting a
|
|
* measurement the setup has to be rewritten
|
|
*/
|
|
// cmdBuffer[0] = SUS::RESET_FIFO;
|
|
// cmdBuffer[1] = SUS::SETUP_DEFINITION;
|
|
// wirte one dummy byte here
|
|
// cmdBuffer[2] = SUS::CONVERT_TEMPERATURE;
|
|
cmdBuffer[0] = SUS::CONVERT_DIFF_CHANNEL_0;
|
|
cmdBuffer[1] = SUS::DUMMY_BYTE;
|
|
cmdBuffer[2] = SUS::CONVERT_DIFF_CHANNEL_1;
|
|
cmdBuffer[3] = SUS::DUMMY_BYTE;
|
|
cmdBuffer[4] = SUS::CONVERT_DIFF_CHANNEL_2;
|
|
cmdBuffer[5] = SUS::DUMMY_BYTE;
|
|
cmdBuffer[6] = SUS::CONVERT_DIFF_CHANNEL_3;
|
|
cmdBuffer[7] = SUS::DUMMY_BYTE;
|
|
cmdBuffer[9] = SUS::CONVERT_DIFF_CHANNEL_4;
|
|
cmdBuffer[10] = SUS::DUMMY_BYTE;
|
|
cmdBuffer[11] = SUS::CONVERT_DIFF_CHANNEL_5;
|
|
cmdBuffer[12] = SUS::DUMMY_BYTE;
|
|
cmdBuffer[13] = SUS::DUMMY_BYTE;
|
|
// cmdBuffer[0] = SUS::SETUP_DEFINITION;
|
|
// cmdBuffer[1] = SUS::UNIPOLAR_CONFIG;
|
|
// cmdBuffer[2] = SUS::CONVERT_TEMPERATURE;
|
|
// cmdBuffer[26] = SUS::CONVERT_DIFF_CHANNEL_0_1;
|
|
// cmdBuffer[28] = SUS::CONVERT_DIFF_CHANNEL_2_3;
|
|
// cmdBuffer[30] = SUS::CONVERT_DIFF_CHANNEL_4_5;
|
|
rawPacket = cmdBuffer;
|
|
// rawPacketLen = SUS::SIZE_PERFORM_CONVERSIONS;
|
|
rawPacketLen = 14;
|
|
return RETURN_OK;
|
|
}
|
|
case(SUS::RQUEST_TEMP): {
|
|
// gpioComIF->pullLow(chipSelectId);
|
|
std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
|
|
// cmdBuffer[0] = SUS::RESET_FIFO;
|
|
// cmdBuffer[1] = SUS::SETUP_DEFINITION;
|
|
cmdBuffer[0] = SUS::CONVERT_TEMPERATURE;
|
|
rawPacket = cmdBuffer;
|
|
rawPacketLen = 1;
|
|
return RETURN_OK;
|
|
}
|
|
case(SUS::READ_TEMP): {
|
|
std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
|
|
rawPacket = cmdBuffer;
|
|
rawPacketLen = 24;
|
|
return RETURN_OK;
|
|
}
|
|
default:
|
|
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
|
|
void SusHandler::fillCommandAndReplyMap() {
|
|
this->insertInCommandMap(SUS::WRITE_SETUP);
|
|
// this->insertInCommandAndReplyMap(SUS::READ_TEMP, 1, &dataset, SUS::SIZE_PERFORM_CONVERSIONS);
|
|
this->insertInCommandAndReplyMap(SUS::PERFORM_CONVERSIONS, 1, &dataset, SUS::SIZE_PERFORM_CONVERSIONS);
|
|
this->insertInCommandAndReplyMap(SUS::READ_TEMP, 1, nullptr, SUS::SIZE_PERFORM_CONVERSIONS);
|
|
// this->insertInCommandAndReplyMap(SUS::PERFORM_CONVERSIONS, 1, &dataset,
|
|
// SUS::SIZE_PERFORM_CONVERSIONS);
|
|
this->insertInCommandMap(SUS::RQUEST_TEMP);
|
|
}
|
|
|
|
ReturnValue_t SusHandler::scanForReply(const uint8_t *start,
|
|
size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
|
|
*foundId = this->getPendingCommand();
|
|
*foundLen = remainingSize;
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id,
|
|
const uint8_t *packet) {
|
|
switch (id) {
|
|
case SUS::PERFORM_CONVERSIONS: {
|
|
gpioComIF->pullHigh(chipSelectId);
|
|
PoolReadGuard readSet(&dataset);
|
|
dataset.temperatureCelcius = (*(packet + 25) << 8 | *(packet + 26)) * 0.125;
|
|
dataset.diffScanChannel0_1 = (*(packet + 29) << 8 | *(packet + 30));
|
|
dataset.diffScanChannel2_3 = (*(packet + 31) << 8 | *(packet + 32));
|
|
dataset.diffScanChannel4_5 = (*(packet + 33) << 8 | *(packet + 34));
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && DEBUG_SUS
|
|
sif::info << "SUS with object id " << std::hex << this->getObjectId() << ", temperature: "
|
|
<< dataset.temperatureCelcius << " °C" << std::endl;
|
|
sif::info << "SUS with object id " << std::hex << this->getObjectId() << ", channel 0/1: "
|
|
<< dataset.diffScanChannel0_1 << std::endl;
|
|
sif::info << "SUS with object id " << std::hex << this->getObjectId() << ", channel 2/3: "
|
|
<< dataset.diffScanChannel2_3 << std::endl;
|
|
sif::info << "SUS with object id " << std::hex << this->getObjectId() << ", channel 4/5: "
|
|
<< dataset.diffScanChannel4_5 << std::endl;
|
|
#endif
|
|
break;
|
|
}
|
|
case SUS::READ_TEMP: {
|
|
// gpioComIF->pullHigh(chipSelectId);
|
|
break;
|
|
}
|
|
default: {
|
|
sif::debug << "SusHandler::interpretDeviceReply: Unknown reply id" << std::endl;
|
|
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
|
|
}
|
|
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
void SusHandler::setNormalDatapoolEntriesInvalid(){
|
|
|
|
}
|
|
|
|
uint32_t SusHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){
|
|
return 5000;
|
|
}
|
|
|
|
ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
|
LocalDataPoolManager& poolManager) {
|
|
localDataPoolMap.emplace(SUS::TEMPERATURE_C, new PoolEntry<float>( { 0.0 }));
|
|
localDataPoolMap.emplace(SUS::DIFF_SCAN_CHANNEL_0_1, new PoolEntry<uint16_t>( { 0 }));
|
|
localDataPoolMap.emplace(SUS::DIFF_SCAN_CHANNEL_2_3, new PoolEntry<uint16_t>( { 0 }));
|
|
localDataPoolMap.emplace(SUS::DIFF_SCAN_CHANNEL_4_5, new PoolEntry<uint16_t>( { 0 }));
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|