215 lines
8.2 KiB
C++
215 lines
8.2 KiB
C++
#include "SusHandler.h"
|
|
|
|
#include <fsfw/datapool/PoolReadGuard.h>
|
|
#include <fsfw_hal/linux/spi/SpiComIF.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;
|
|
}
|
|
if (gpioComIF == NULL) {
|
|
sif::error << "SusHandler: Invalid GpioComIF" << std::endl;
|
|
}
|
|
}
|
|
|
|
SusHandler::~SusHandler() {}
|
|
|
|
ReturnValue_t SusHandler::performOperation(uint8_t counter) {
|
|
if (counter != FIRST_WRITE) {
|
|
DeviceHandlerBase::performOperation(counter);
|
|
return RETURN_OK;
|
|
}
|
|
|
|
if (mode != MODE_NORMAL) {
|
|
DeviceHandlerBase::performOperation(DeviceHandlerIF::SEND_WRITE);
|
|
return RETURN_OK;
|
|
}
|
|
|
|
/* If device is in normale mode the communication sequence is initiated here */
|
|
if (communicationStep == CommunicationStep::IDLE) {
|
|
communicationStep = CommunicationStep::WRITE_SETUP;
|
|
}
|
|
|
|
DeviceHandlerBase::performOperation(DeviceHandlerIF::SEND_WRITE);
|
|
|
|
return RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t SusHandler::initialize() {
|
|
ReturnValue_t result = RETURN_OK;
|
|
result = DeviceHandlerBase::initialize();
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
auto spiComIF = dynamic_cast<SpiComIF *>(communicationInterface);
|
|
if (spiComIF == nullptr) {
|
|
sif::debug << "SusHandler::initialize: Invalid communication interface" << std::endl;
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
spiMutex = spiComIF->getMutex();
|
|
if (spiMutex == nullptr) {
|
|
sif::debug << "SusHandler::initialize: Failed to get spi mutex" << std::endl;
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
return RETURN_OK;
|
|
}
|
|
|
|
void SusHandler::doStartUp() {
|
|
#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::IDLE) {
|
|
return NOTHING_TO_SEND;
|
|
}
|
|
|
|
if (communicationStep == CommunicationStep::WRITE_SETUP) {
|
|
*id = SUS::WRITE_SETUP;
|
|
communicationStep = CommunicationStep::START_CONVERSIONS;
|
|
} else if (communicationStep == CommunicationStep::START_CONVERSIONS) {
|
|
*id = SUS::START_CONVERSIONS;
|
|
communicationStep = CommunicationStep::READ_CONVERSIONS;
|
|
} else if (communicationStep == CommunicationStep::READ_CONVERSIONS) {
|
|
*id = SUS::READ_CONVERSIONS;
|
|
communicationStep = CommunicationStep::IDLE;
|
|
}
|
|
return buildCommandFromCommand(*id, nullptr, 0);
|
|
}
|
|
|
|
ReturnValue_t SusHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t SusHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
|
const uint8_t *commandData,
|
|
size_t commandDataLen) {
|
|
switch (deviceCommand) {
|
|
case (SUS::WRITE_SETUP): {
|
|
/**
|
|
* The sun sensor ADC is shutdown when CS is pulled high, so each time requesting a
|
|
* measurement the setup has to be rewritten. There must also be a little delay between
|
|
* the transmission of the setup byte and the first conversion. Thus the conversion
|
|
* will be performed in an extra step.
|
|
* Because the chip select is driven manually by the SusHandler the SPI bus must be
|
|
* protected with a mutex here.
|
|
*/
|
|
ReturnValue_t result = spiMutex->lockMutex(timeoutType, timeoutMs);
|
|
if (result == MutexIF::MUTEX_TIMEOUT) {
|
|
sif::error << "SusHandler::buildCommandFromCommand: Mutex timeout" << std::endl;
|
|
return ERROR_LOCK_MUTEX;
|
|
} else if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
sif::error << "SusHandler::buildCommandFromCommand: Failed to lock spi mutex" << std::endl;
|
|
return ERROR_LOCK_MUTEX;
|
|
}
|
|
|
|
gpioComIF->pullLow(chipSelectId);
|
|
cmdBuffer[0] = SUS::SETUP;
|
|
rawPacket = cmdBuffer;
|
|
rawPacketLen = 1;
|
|
return RETURN_OK;
|
|
}
|
|
case (SUS::START_CONVERSIONS): {
|
|
std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
|
|
cmdBuffer[0] = SUS::CONVERSION;
|
|
rawPacket = cmdBuffer;
|
|
rawPacketLen = 2;
|
|
return RETURN_OK;
|
|
}
|
|
case (SUS::READ_CONVERSIONS): {
|
|
std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
|
|
rawPacket = cmdBuffer;
|
|
rawPacketLen = SUS::SIZE_READ_CONVERSIONS;
|
|
return RETURN_OK;
|
|
}
|
|
default:
|
|
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
|
|
void SusHandler::fillCommandAndReplyMap() {
|
|
this->insertInCommandMap(SUS::WRITE_SETUP);
|
|
this->insertInCommandMap(SUS::START_CONVERSIONS);
|
|
this->insertInCommandAndReplyMap(SUS::READ_CONVERSIONS, 1, &dataset, SUS::SIZE_READ_CONVERSIONS);
|
|
}
|
|
|
|
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::READ_CONVERSIONS: {
|
|
PoolReadGuard readSet(&dataset);
|
|
dataset.temperatureCelcius = (*(packet) << 8 | *(packet + 1)) * 0.125;
|
|
dataset.ain0 = (*(packet + 2) << 8 | *(packet + 3));
|
|
dataset.ain1 = (*(packet + 4) << 8 | *(packet + 5));
|
|
dataset.ain2 = (*(packet + 6) << 8 | *(packet + 7));
|
|
dataset.ain3 = (*(packet + 8) << 8 | *(packet + 9));
|
|
dataset.ain4 = (*(packet + 10) << 8 | *(packet + 11));
|
|
dataset.ain5 = (*(packet + 12) << 8 | *(packet + 13));
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_SUS
|
|
sif::info << "SUS object id 0x" << std::hex << this->getObjectId()
|
|
<< ", Temperature: " << dataset.temperatureCelcius << " °C" << std::endl;
|
|
sif::info << "SUS object id 0x" << std::hex << this->getObjectId() << ", AIN0: " << std::dec
|
|
<< dataset.ain0 << std::endl;
|
|
sif::info << "SUS object id 0x" << std::hex << this->getObjectId() << ", AIN1: " << std::dec
|
|
<< dataset.ain1 << std::endl;
|
|
sif::info << "SUS object id 0x" << std::hex << this->getObjectId() << ", AIN2: " << std::dec
|
|
<< dataset.ain2 << std::endl;
|
|
sif::info << "SUS object id 0x" << std::hex << this->getObjectId() << ", AIN3: " << std::dec
|
|
<< dataset.ain3 << std::endl;
|
|
sif::info << "SUS object id 0x" << std::hex << this->getObjectId() << ", AIN4: " << std::dec
|
|
<< dataset.ain4 << std::endl;
|
|
sif::info << "SUS object id 0x" << std::hex << this->getObjectId() << ", AIN5: " << std::dec
|
|
<< dataset.ain5 << std::endl;
|
|
#endif
|
|
/** SUS can now be shutdown and thus the SPI bus released again */
|
|
gpioComIF->pullHigh(chipSelectId);
|
|
ReturnValue_t result = spiMutex->unlockMutex();
|
|
if (result != RETURN_OK) {
|
|
sif::error << "SusHandler::interpretDeviceReply: Failed to unlock spi mutex" << std::endl;
|
|
return ERROR_UNLOCK_MUTEX;
|
|
}
|
|
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 1000; }
|
|
|
|
ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
|
LocalDataPoolManager &poolManager) {
|
|
localDataPoolMap.emplace(SUS::TEMPERATURE_C, new PoolEntry<float>({0.0}));
|
|
localDataPoolMap.emplace(SUS::AIN0, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(SUS::AIN1, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(SUS::AIN2, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(SUS::AIN3, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(SUS::AIN4, new PoolEntry<uint16_t>({0}));
|
|
localDataPoolMap.emplace(SUS::AIN5, new PoolEntry<uint16_t>({0}));
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|