451 lines
14 KiB
C++
451 lines
14 KiB
C++
#include "PayloadPcduHandler.h"
|
|
|
|
#include <fsfw/src/fsfw/datapool/PoolReadGuard.h>
|
|
|
|
#ifdef FSFW_OSAL_LINUX
|
|
#include <fsfw_hal/linux/UnixFileGuard.h>
|
|
#include <fsfw_hal/linux/spi/SpiComIF.h>
|
|
#include <fsfw_hal/linux/spi/SpiCookie.h>
|
|
#include <fsfw_hal/linux/utility.h>
|
|
#include <sys/ioctl.h>
|
|
#endif
|
|
|
|
#include "devices/gpioIds.h"
|
|
|
|
PayloadPcduHandler::PayloadPcduHandler(object_id_t objectId, object_id_t comIF, CookieIF* cookie,
|
|
GpioIF* gpioIF, bool periodicPrintout)
|
|
: DeviceHandlerBase(objectId, comIF, cookie),
|
|
adcSet(this),
|
|
periodicPrintout(periodicPrintout),
|
|
gpioIF(gpioIF) {}
|
|
|
|
void PayloadPcduHandler::doStartUp() {
|
|
if ((state != States::PCDU_OFF) and (state != States::ON_TRANS_SSR)) {
|
|
// Config error
|
|
sif::error << "PayloadPcduHandler::doStartUp: Invalid state" << std::endl;
|
|
}
|
|
if (state == States::PCDU_OFF) {
|
|
// Switch on relays here
|
|
gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT0);
|
|
gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT1);
|
|
state = States::ON_TRANS_SSR;
|
|
transitionOk = true;
|
|
}
|
|
if (state == States::ON_TRANS_SSR) {
|
|
// If necessary, check whether a certain amount of time has elapsed
|
|
if (transitionOk) {
|
|
transitionOk = false;
|
|
// We are now in ON mode
|
|
startTransition(MODE_NORMAL, 0);
|
|
adcState = AdcStates::BOOT_DELAY;
|
|
// The ADC can now be read. If the values are not close to zero, we should not allow
|
|
// transition
|
|
monMode = MonitoringMode::CLOSE_TO_ZERO;
|
|
}
|
|
}
|
|
}
|
|
|
|
void PayloadPcduHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
|
|
if (mode == _MODE_TO_NORMAL) {
|
|
stateMachineToNormal();
|
|
}
|
|
}
|
|
|
|
void PayloadPcduHandler::doShutDown() {}
|
|
|
|
ReturnValue_t PayloadPcduHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
|
|
switch (adcState) {
|
|
case (AdcStates::SEND_SETUP): {
|
|
*id = plpcdu::SETUP_CMD;
|
|
return buildCommandFromCommand(*id, nullptr, 0);
|
|
}
|
|
case (AdcStates::NORMAL): {
|
|
*id = plpcdu::READ_WITH_TEMP_EXT;
|
|
return buildCommandFromCommand(*id, nullptr, 0);
|
|
}
|
|
default: {
|
|
break;
|
|
}
|
|
}
|
|
return NOTHING_TO_SEND;
|
|
}
|
|
|
|
ReturnValue_t PayloadPcduHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
|
|
if (adcState == AdcStates::SEND_SETUP) {
|
|
*id = plpcdu::SETUP_CMD;
|
|
return buildCommandFromCommand(*id, nullptr, 0);
|
|
}
|
|
return NOTHING_TO_SEND;
|
|
}
|
|
|
|
void PayloadPcduHandler::fillCommandAndReplyMap() {
|
|
insertInCommandAndReplyMap(plpcdu::READ_CMD, 2, &adcSet);
|
|
insertInCommandAndReplyMap(plpcdu::READ_TEMP_EXT, 1, &adcSet);
|
|
insertInCommandAndReplyMap(plpcdu::READ_WITH_TEMP_EXT, 1, &adcSet);
|
|
insertInCommandAndReplyMap(plpcdu::SETUP_CMD, 1);
|
|
}
|
|
|
|
ReturnValue_t PayloadPcduHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
|
const uint8_t* commandData,
|
|
size_t commandDataLen) {
|
|
switch (deviceCommand) {
|
|
case (plpcdu::SETUP_CMD): {
|
|
cmdBuf[0] = plpcdu::SETUP_BYTE;
|
|
rawPacket = cmdBuf.data();
|
|
rawPacketLen = 1;
|
|
break;
|
|
}
|
|
case (plpcdu::READ_CMD): {
|
|
max1227::prepareExternallyClockedRead0ToN(cmdBuf.data(), plpcdu::CHANNEL_N, rawPacketLen);
|
|
rawPacket = cmdBuf.data();
|
|
break;
|
|
}
|
|
case (plpcdu::READ_TEMP_EXT): {
|
|
max1227::prepareExternallyClockedTemperatureRead(cmdBuf.data(), rawPacketLen);
|
|
rawPacket = cmdBuf.data();
|
|
break;
|
|
}
|
|
case (plpcdu::READ_WITH_TEMP_EXT): {
|
|
size_t sz = 0;
|
|
max1227::prepareExternallyClockedRead0ToN(cmdBuf.data(), plpcdu::CHANNEL_N, sz);
|
|
max1227::prepareExternallyClockedTemperatureRead(cmdBuf.data() + sz, sz);
|
|
rawPacketLen = sz;
|
|
rawPacket = cmdBuf.data();
|
|
break;
|
|
}
|
|
default: {
|
|
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
|
}
|
|
}
|
|
return RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t PayloadPcduHandler::scanForReply(const uint8_t* start, size_t remainingSize,
|
|
DeviceCommandId_t* foundId, size_t* foundLen) {
|
|
// SPI is full duplex
|
|
*foundId = getPendingCommand();
|
|
*foundLen = remainingSize;
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t PayloadPcduHandler::interpretDeviceReply(DeviceCommandId_t id,
|
|
const uint8_t* packet) {
|
|
using namespace plpcdu;
|
|
switch (id) {
|
|
case (SETUP_CMD): {
|
|
if (mode == _MODE_TO_NORMAL) {
|
|
adcCmdExecuted = true;
|
|
}
|
|
break;
|
|
}
|
|
case (READ_TEMP_EXT): {
|
|
uint8_t tempStartIdx = TEMP_REPLY_SIZE - 2;
|
|
adcSet.tempC.value =
|
|
max1227::getTemperature(packet[tempStartIdx] << 8 | packet[tempStartIdx + 1]);
|
|
break;
|
|
}
|
|
case (READ_CMD): {
|
|
PoolReadGuard pg(&adcSet);
|
|
if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
|
|
return pg.getReadResult();
|
|
}
|
|
handleExtConvRead(packet);
|
|
handlePrintout();
|
|
break;
|
|
}
|
|
case (READ_WITH_TEMP_EXT): {
|
|
PoolReadGuard pg(&adcSet);
|
|
if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
|
|
return pg.getReadResult();
|
|
}
|
|
handleExtConvRead(packet);
|
|
uint8_t tempStartIdx = ADC_REPLY_SIZE + TEMP_REPLY_SIZE - 2;
|
|
adcSet.tempC.value =
|
|
max1227::getTemperature(packet[tempStartIdx] << 8 | packet[tempStartIdx + 1]);
|
|
handlePrintout();
|
|
break;
|
|
}
|
|
default: {
|
|
break;
|
|
}
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
uint32_t PayloadPcduHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
|
|
// 20 minutes transition delay is allowed
|
|
return 20 * 60 * 60;
|
|
}
|
|
|
|
ReturnValue_t PayloadPcduHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
|
LocalDataPoolManager& poolManager) {
|
|
localDataPoolMap.emplace(plpcdu::PlPcduPoolIds::CHANNEL_VEC, &channelValues);
|
|
localDataPoolMap.emplace(plpcdu::PlPcduPoolIds::TEMP, &tempC);
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
void PayloadPcduHandler::setToGoToNormalModeImmediately(bool enable) {
|
|
this->goToNormalMode = enable;
|
|
}
|
|
|
|
void PayloadPcduHandler::handleExtConvRead(const uint8_t* bufStart) {
|
|
for (uint8_t idx = 0; idx < 12; idx++) {
|
|
adcSet.channels[idx] = bufStart[idx * 2 + 1] << 8 | bufStart[idx * 2 + 2];
|
|
}
|
|
}
|
|
|
|
void PayloadPcduHandler::handlePrintout() {
|
|
if (periodicPrintout) {
|
|
if (opDivider.checkAndIncrement()) {
|
|
sif::info << "PL PCDU ADC hex [" << std::setfill('0') << std::hex;
|
|
for (uint8_t idx = 0; idx < 12; idx++) {
|
|
sif::info << std::setw(3) << adcSet.channels[idx];
|
|
if (idx < 11) {
|
|
sif::info << ",";
|
|
}
|
|
}
|
|
sif::info << "] | T[C] " << std::dec << adcSet.tempC.value << std::endl;
|
|
}
|
|
}
|
|
}
|
|
|
|
void PayloadPcduHandler::enablePeriodicPrintout(bool enable, uint8_t divider) {
|
|
this->periodicPrintout = enable;
|
|
opDivider.setDivider(divider);
|
|
}
|
|
|
|
void PayloadPcduHandler::stateMachineToNormal() {
|
|
if (adcState == AdcStates::BOOT_DELAY) {
|
|
if (adcCountdown.hasTimedOut()) {
|
|
adcState = AdcStates::SEND_SETUP;
|
|
adcCmdExecuted = false;
|
|
}
|
|
}
|
|
if (adcState == AdcStates::SEND_SETUP) {
|
|
if (adcCmdExecuted) {
|
|
adcState = AdcStates::NORMAL;
|
|
setMode(MODE_NORMAL, plpcdu::NORMAL_ADC_ONLY);
|
|
adcCmdExecuted = false;
|
|
}
|
|
}
|
|
if (submode == plpcdu::NORMAL_ALL_ON) {
|
|
if (state == States::ON_TRANS_ADC_CLOSE_ZERO) {
|
|
if (not commandExecuted) {
|
|
countdown.resetTimer();
|
|
commandExecuted = true;
|
|
}
|
|
// ADC values are ok, 5 seconds have elapsed
|
|
if (transitionOk and countdown.hasTimedOut()) {
|
|
state = States::ON_TRANS_DRO;
|
|
// Now start monitoring for negative voltages instead
|
|
monMode = MonitoringMode::NEGATIVE;
|
|
countdown.resetTimer();
|
|
commandExecuted = false;
|
|
transitionOk = false;
|
|
}
|
|
}
|
|
if (state == States::ON_TRANS_DRO) {
|
|
if (not commandExecuted) {
|
|
// Switch on DRO and start monitoring for negative voltagea
|
|
gpioIF->pullHigh(gpioIds::PLPCDU_ENB_DRO);
|
|
commandExecuted = true;
|
|
}
|
|
// ADC values are ok, 5 seconds have elapsed
|
|
if (transitionOk and countdown.hasTimedOut()) {
|
|
state = States::ON_TRANS_X8;
|
|
countdown.resetTimer();
|
|
commandExecuted = false;
|
|
transitionOk = false;
|
|
}
|
|
}
|
|
if (state == States::ON_TRANS_X8) {
|
|
if (not commandExecuted) {
|
|
// Switch on X8
|
|
gpioIF->pullHigh(gpioIds::PLPCDU_ENB_X8);
|
|
commandExecuted = true;
|
|
}
|
|
// ADC values are ok, 5 seconds have elapsed
|
|
if (transitionOk and countdown.hasTimedOut()) {
|
|
state = States::ON_TRANS_TX;
|
|
countdown.resetTimer();
|
|
commandExecuted = false;
|
|
transitionOk = false;
|
|
}
|
|
}
|
|
if (state == States::ON_TRANS_TX) {
|
|
if (not commandExecuted) {
|
|
// Switch on TX
|
|
gpioIF->pullHigh(gpioIds::PLPCDU_ENB_TX);
|
|
commandExecuted = true;
|
|
}
|
|
// ADC values are ok, 5 seconds have elapsed
|
|
if (transitionOk and countdown.hasTimedOut()) {
|
|
state = States::ON_TRANS_MPA;
|
|
countdown.resetTimer();
|
|
commandExecuted = false;
|
|
transitionOk = false;
|
|
}
|
|
}
|
|
if (state == States::ON_TRANS_MPA) {
|
|
if (not commandExecuted) {
|
|
// Switch on MPA
|
|
gpioIF->pullHigh(gpioIds::PLPCDU_ENB_MPA);
|
|
commandExecuted = true;
|
|
}
|
|
// ADC values are ok, 5 seconds have elapsed
|
|
if (transitionOk and countdown.hasTimedOut()) {
|
|
state = States::ON_TRANS_HPA;
|
|
countdown.resetTimer();
|
|
commandExecuted = false;
|
|
transitionOk = false;
|
|
}
|
|
}
|
|
if (state == States::ON_TRANS_HPA) {
|
|
if (not commandExecuted) {
|
|
// Switch on HPA
|
|
gpioIF->pullHigh(gpioIds::PLPCDU_ENB_HPA);
|
|
commandExecuted = true;
|
|
}
|
|
// ADC values are ok, 5 seconds have elapsed
|
|
if (transitionOk and countdown.hasTimedOut()) {
|
|
state = States::PCDU_ON;
|
|
setMode(MODE_NORMAL, plpcdu::NORMAL_ALL_ON);
|
|
countdown.resetTimer();
|
|
commandExecuted = false;
|
|
transitionOk = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#ifdef FSFW_OSAL_LINUX
|
|
ReturnValue_t PayloadPcduHandler::extConvAsTwoCallback(SpiComIF* comIf, SpiCookie* cookie,
|
|
const uint8_t* sendData, size_t sendLen,
|
|
void* args) {
|
|
auto handler = reinterpret_cast<PayloadPcduHandler*>(args);
|
|
if (handler == nullptr) {
|
|
sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
|
|
<< std::endl;
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
DeviceCommandId_t currentCommand = handler->getPendingCommand();
|
|
switch (currentCommand) {
|
|
case (plpcdu::READ_WITH_TEMP_EXT): {
|
|
return transferAsTwo(comIf, cookie, sendData, sendLen, false);
|
|
}
|
|
case (plpcdu::READ_TEMP_EXT): {
|
|
return transferAsTwo(comIf, cookie, sendData, sendLen, true);
|
|
}
|
|
default: {
|
|
return comIf->performRegularSendOperation(cookie, sendData, sendLen);
|
|
}
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cookie,
|
|
const uint8_t* sendData, size_t sendLen,
|
|
bool tempOnly) {
|
|
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
|
|
int retval = 0;
|
|
// Prepare transfer
|
|
int fileDescriptor = 0;
|
|
std::string device = cookie->getSpiDevice();
|
|
UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
|
|
if (fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
|
|
return SpiComIF::OPENING_FILE_FAILED;
|
|
}
|
|
spi::SpiModes spiMode = spi::SpiModes::MODE_0;
|
|
uint32_t spiSpeed = 0;
|
|
cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
|
|
comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
|
|
cookie->assignWriteBuffer(sendData);
|
|
size_t transferLen = plpcdu::TEMP_REPLY_SIZE;
|
|
if (not tempOnly) {
|
|
transferLen += plpcdu::ADC_REPLY_SIZE;
|
|
}
|
|
cookie->setTransferSize(transferLen);
|
|
|
|
gpioId_t gpioId = cookie->getChipSelectPin();
|
|
GpioIF* gpioIF = comIf->getGpioInterface();
|
|
MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
|
|
uint32_t timeoutMs = 0;
|
|
MutexIF* mutex = comIf->getMutex(&timeoutType, &timeoutMs);
|
|
if (mutex == nullptr or gpioIF == nullptr) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::warning << "GyroADIS16507Handler::spiSendCallback: "
|
|
"Mutex or GPIO interface invalid"
|
|
<< std::endl;
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
#endif
|
|
}
|
|
|
|
if (gpioId != gpio::NO_GPIO) {
|
|
result = mutex->lockMutex(timeoutType, timeoutMs);
|
|
if (result != RETURN_OK) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
|
|
#endif
|
|
return result;
|
|
}
|
|
}
|
|
spi_ioc_transfer* transferStruct = cookie->getTransferStructHandle();
|
|
uint64_t origTx = transferStruct->tx_buf;
|
|
uint64_t origRx = transferStruct->rx_buf;
|
|
if (tempOnly) {
|
|
transferLen = 1;
|
|
} else {
|
|
transferLen = plpcdu::ADC_REPLY_SIZE + 1;
|
|
}
|
|
transferStruct->len = transferLen;
|
|
// Pull SPI CS low. For now, no support for active high given
|
|
if (gpioId != gpio::NO_GPIO) {
|
|
gpioIF->pullLow(gpioId);
|
|
}
|
|
|
|
// Execute transfer
|
|
// Initiate a full duplex SPI transfer.
|
|
retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
|
|
if (retval < 0) {
|
|
utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
|
|
result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
|
|
}
|
|
#if FSFW_HAL_SPI_WIRETAPPING == 1
|
|
comIf->performSpiWiretapping(cookie);
|
|
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
|
|
|
|
if (gpioId != gpio::NO_GPIO) {
|
|
gpioIF->pullHigh(gpioId);
|
|
}
|
|
|
|
transferStruct->tx_buf += transferLen;
|
|
transferStruct->rx_buf += transferLen;
|
|
transferStruct->len = plpcdu::TEMP_REPLY_SIZE - 1;
|
|
if (gpioId != gpio::NO_GPIO) {
|
|
gpioIF->pullLow(gpioId);
|
|
}
|
|
|
|
// Execute transfer
|
|
// Initiate a full duplex SPI transfer.
|
|
retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
|
|
if (retval < 0) {
|
|
utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
|
|
result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
|
|
}
|
|
#if FSFW_HAL_SPI_WIRETAPPING == 1
|
|
comIf->performSpiWiretapping(cookie);
|
|
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
|
|
|
|
if (gpioId != gpio::NO_GPIO) {
|
|
gpioIF->pullHigh(gpioId);
|
|
}
|
|
|
|
transferStruct->tx_buf = origTx;
|
|
transferStruct->rx_buf = origRx;
|
|
if (gpioId != gpio::NO_GPIO) {
|
|
mutex->unlockMutex();
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
#endif
|