eive-obsw/mission/payload/PayloadPcduHandler.cpp

826 lines
30 KiB
C++
Raw Normal View History

2022-03-04 15:26:36 +01:00
#include <fsfw/src/fsfw/datapool/PoolReadGuard.h>
2023-03-26 16:42:00 +02:00
#include <mission/payload/PayloadPcduHandler.h>
2022-03-04 15:26:36 +01:00
2022-04-26 10:37:25 +02:00
#include "OBSWConfig.h"
#ifdef XIPHOS_Q7S
2022-03-04 15:26:36 +01:00
#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, SdCardMountedIF* sdcMan,
2022-12-22 11:10:55 +01:00
Stack5VHandler& stackHandler, bool periodicPrintout)
2022-03-04 15:26:36 +01:00
: DeviceHandlerBase(objectId, comIF, cookie),
adcSet(this),
2022-12-22 11:10:55 +01:00
stackHandler(stackHandler),
2022-03-04 15:26:36 +01:00
periodicPrintout(periodicPrintout),
gpioIF(gpioIF),
2022-12-22 11:10:55 +01:00
sdcMan(sdcMan) {}
2022-03-04 15:26:36 +01:00
void PayloadPcduHandler::doStartUp() {
2023-01-10 15:16:52 +01:00
if (state > States::STACK_5V_CORRECT) {
2022-03-04 15:26:36 +01:00
// Config error
sif::error << "PayloadPcduHandler::doStartUp: Invalid state" << std::endl;
}
2022-10-19 10:46:57 +02:00
clearSetOnOffFlag = true;
2022-12-22 14:13:41 +01:00
if (state == States::PL_PCDU_OFF) {
state = States::STACK_5V_SWITCHING;
}
if (state == States::STACK_5V_SWITCHING) {
2023-01-10 11:13:34 +01:00
ReturnValue_t retval = stackHandler.deviceToOn(StackCommander::PL_PCDU, true);
2022-12-22 14:13:41 +01:00
if (retval == BUSY) {
return;
}
2022-12-22 16:18:30 +01:00
state = States::STACK_5V_PENDING;
2022-12-22 14:13:41 +01:00
}
if (state == States::STACK_5V_PENDING) {
if (stackHandler.isSwitchOn()) {
state = States::STACK_5V_CORRECT;
}
}
if (state == States::STACK_5V_CORRECT) {
2022-03-28 12:23:56 +02:00
quickTransitionAlreadyCalled = false;
setMode(_MODE_TO_ON);
}
}
void PayloadPcduHandler::doShutDown() {
if (not quickTransitionAlreadyCalled) {
quickTransitionBackToOff(false, false);
quickTransitionAlreadyCalled = true;
}
2022-10-19 10:46:57 +02:00
if (clearSetOnOffFlag) {
std::memset(adcSet.processed.value, 0, adcSet.processed.getSerializedSize());
clearSetOnOffFlag = false;
}
2023-01-10 15:16:52 +01:00
ReturnValue_t retval = stackHandler.deviceToOff(StackCommander::PL_PCDU, true);
if (retval == BUSY) {
return;
2022-03-28 12:23:56 +02:00
}
2023-01-10 15:16:52 +01:00
state = States::PL_PCDU_OFF;
// No need to set mode _MODE_POWER_DOWN, power switching was already handled
setMode(MODE_OFF);
2022-03-04 15:26:36 +01:00
}
2022-03-28 15:29:42 +02:00
void PayloadPcduHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
if (getMode() == _MODE_TO_NORMAL) {
2022-03-28 15:29:42 +02:00
stateMachineToNormal(modeFrom, subModeFrom);
return;
2022-12-23 12:50:10 +01:00
} else if (getMode() == _MODE_TO_ON and modeFrom == MODE_NORMAL) {
gpioIF->pullLow(gpioIds::PLPCDU_ENB_HPA);
gpioIF->pullLow(gpioIds::PLPCDU_ENB_MPA);
gpioIF->pullLow(gpioIds::PLPCDU_ENB_TX);
gpioIF->pullLow(gpioIds::PLPCDU_ENB_X8);
gpioIF->pullLow(gpioIds::PLPCDU_ENB_DRO);
gpioIF->pullLow(gpioIds::PLPCDU_ENB_TX);
gpioIF->pullLow(gpioIds::PLPCDU_ENB_VBAT0);
gpioIF->pullLow(gpioIds::PLPCDU_ENB_VBAT1);
state = States::STACK_5V_CORRECT;
2022-03-28 15:29:42 +02:00
}
DeviceHandlerBase::doTransition(modeFrom, subModeFrom);
}
ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_t subModeFrom) {
2022-03-04 15:26:36 +01:00
using namespace plpcdu;
2022-04-07 00:14:18 +02:00
bool doFinish = true;
if (((getSubmode() >> SOLID_STATE_RELAYS_ADC_ON) & 0b1) == 1) {
if (state == States::PL_PCDU_OFF) {
2022-03-28 12:23:56 +02:00
sif::error << "PayloadPcduHandler::stateMachineToNormal: Unexpected state PL_PCDU_OFF"
<< "detected" << std::endl;
2022-03-24 12:48:23 +01:00
setMode(MODE_OFF);
2022-08-24 17:27:47 +02:00
return returnvalue::FAILED;
2022-03-04 15:26:36 +01:00
}
2022-12-22 14:13:41 +01:00
if (state == States::STACK_5V_CORRECT) {
2022-03-30 23:01:38 +02:00
#if OBSW_VERBOSE_LEVEL >= 1
2022-03-31 14:40:42 +02:00
sif::info << "Switching on SSR VBAT0 & VBAT1 GPIOs" << std::endl;
2022-03-30 23:01:38 +02:00
#endif
2022-03-24 12:48:23 +01:00
// Switch on relays here
gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT0);
gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT1);
state = States::ON_TRANS_SSR;
transitionOk = true;
2022-04-07 00:14:18 +02:00
doFinish = false;
2022-03-24 12:48:23 +01:00
}
if (state == States::ON_TRANS_SSR) {
// If necessary, check whether a certain amount of time has elapsed
if (transitionOk) {
transitionOk = false;
state = States::ON_TRANS_ADC_CLOSE_ZERO;
adcCountdown.setTimeout(50);
adcCountdown.resetTimer();
adcState = AdcStates::BOOT_DELAY;
2022-04-07 00:14:18 +02:00
doFinish = false;
2022-03-24 12:48:23 +01:00
// If the values are not close to zero, we should not allow transition
monMode = MonitoringMode::CLOSE_TO_ZERO;
}
}
if (state == States::ON_TRANS_ADC_CLOSE_ZERO) {
2022-03-24 12:48:23 +01:00
if (adcState == AdcStates::BOOT_DELAY) {
2022-04-07 00:14:18 +02:00
doFinish = false;
2022-03-24 12:48:23 +01:00
if (adcCountdown.hasTimedOut()) {
adcState = AdcStates::SEND_SETUP;
adcCmdExecuted = false;
}
}
if (adcState == AdcStates::SEND_SETUP) {
if (adcCmdExecuted) {
adcState = AdcStates::NORMAL;
2022-04-07 00:14:18 +02:00
doFinish = true;
2022-03-24 12:48:23 +01:00
adcCountdown.setTimeout(100);
adcCountdown.resetTimer();
adcCmdExecuted = false;
}
}
2022-03-04 15:26:36 +01:00
}
}
2022-03-28 15:29:42 +02:00
2022-04-07 00:14:18 +02:00
auto switchHandler = [&](NormalSubmodeBits bit, gpioId_t id, std::string info) {
if (((diffMask >> bit) & 1) == 1) {
if (((getSubmode() >> bit) & 1) == 1) {
2022-03-04 15:26:36 +01:00
#if OBSW_VERBOSE_LEVEL >= 1
2022-04-07 00:14:18 +02:00
sif::info << "Enabling PL PCDU " << info << " module" << std::endl;
2022-03-04 15:26:36 +01:00
#endif
2022-04-07 00:14:18 +02:00
// Switch on DRO and start monitoring for negative voltages
updateSwitchGpio(id, gpio::Levels::HIGH);
} else {
2022-03-04 15:26:36 +01:00
#if OBSW_VERBOSE_LEVEL >= 1
2022-04-07 00:14:18 +02:00
sif::info << "Disabling PL PCDU " << info << " module" << std::endl;
2022-03-04 15:26:36 +01:00
#endif
2022-04-07 00:14:18 +02:00
updateSwitchGpio(id, gpio::Levels::LOW);
}
}
};
2022-03-28 15:29:42 +02:00
2022-12-23 12:50:10 +01:00
// sif::debug << "DIFF MASK: " << (int)diffMask << std::endl;
// No handling for the SSRs: If those are pulled low, the ADC is off
// and normal mode does not really make sense anyway
2022-04-07 00:14:18 +02:00
switchHandler(DRO_ON, gpioIds::PLPCDU_ENB_DRO, "DRO");
switchHandler(X8_ON, gpioIds::PLPCDU_ENB_X8, "X8");
switchHandler(TX_ON, gpioIds::PLPCDU_ENB_TX, "TX");
switchHandler(MPA_ON, gpioIds::PLPCDU_ENB_MPA, "MPA");
switchHandler(HPA_ON, gpioIds::PLPCDU_ENB_HPA, "HPA");
if (doFinish) {
setMode(MODE_NORMAL);
2022-03-04 15:26:36 +01:00
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-04 15:26:36 +01:00
}
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);
}
if (getMode() == _MODE_TO_NORMAL) {
2022-03-04 15:26:36 +01:00
return buildNormalDeviceCommand(id);
}
return NOTHING_TO_SEND;
}
2022-04-07 00:14:18 +02:00
void PayloadPcduHandler::updateSwitchGpio(gpioId_t id, gpio::Levels level) {
if (level == gpio::Levels::HIGH) {
gpioIF->pullHigh(id);
} else {
gpioIF->pullLow(id);
}
adcCountdown.setTimeout(100);
adcCountdown.resetTimer();
}
2022-03-04 15:26:36 +01:00
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;
}
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-04 15:26:36 +01:00
}
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;
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-04 15:26:36 +01:00
}
ReturnValue_t PayloadPcduHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t* packet) {
using namespace plpcdu;
switch (id) {
case (SETUP_CMD): {
if (getMode() == _MODE_TO_NORMAL) {
2022-03-04 15:26:36 +01:00
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);
2022-08-24 17:27:47 +02:00
if (pg.getReadResult() != returnvalue::OK) {
2022-03-04 15:26:36 +01:00
return pg.getReadResult();
}
handleExtConvRead(packet);
checkAdcValues();
adcSet.setValidity(true, true);
handlePrintout();
break;
}
case (READ_WITH_TEMP_EXT): {
PoolReadGuard pg(&adcSet);
2022-08-24 17:27:47 +02:00
if (pg.getReadResult() != returnvalue::OK) {
2022-03-04 15:26:36 +01:00
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]);
checkAdcValues();
adcSet.setValidity(true, true);
handlePrintout();
break;
}
default: {
break;
}
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-04 15:26:36 +01:00
}
uint32_t PayloadPcduHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
// 20 minutes transition delay is allowed
2022-03-25 13:12:27 +01:00
return 20 * 60 * 1000;
2022-03-04 15:26:36 +01:00
}
ReturnValue_t PayloadPcduHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
localDataPoolMap.emplace(plpcdu::PlPcduPoolIds::CHANNEL_VEC, &channelValues);
localDataPoolMap.emplace(plpcdu::PlPcduPoolIds::PROCESSED_VEC, &processedValues);
localDataPoolMap.emplace(plpcdu::PlPcduPoolIds::TEMP, &tempC);
2022-08-15 11:57:57 +02:00
poolManager.subscribeForDiagPeriodicPacket(
subdp::DiagnosticsHkPeriodicParams(adcSet.getSid(), false, 5.0));
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-04 15:26:36 +01:00
}
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() {
using namespace plpcdu;
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;
sif::info << "Neg V: " << adcSet.processed[U_NEG_V_FB] << std::endl;
sif::info << "I HPA [mA]: " << adcSet.processed[I_HPA] << std::endl;
sif::info << "U HPA [V]: " << adcSet.processed[U_HPA_DIV_6] << std::endl;
sif::info << "I MPA [mA]: " << adcSet.processed[I_MPA] << std::endl;
sif::info << "U MPA [V]: " << adcSet.processed[U_MPA_DIV_6] << std::endl;
sif::info << "I TX [mA]: " << adcSet.processed[I_TX] << std::endl;
sif::info << "U TX [V]: " << adcSet.processed[U_TX_DIV_6] << std::endl;
sif::info << "I X8 [mA]: " << adcSet.processed[I_X8] << std::endl;
sif::info << "U X8 [V]: " << adcSet.processed[U_X8_DIV_6] << std::endl;
sif::info << "I DRO [mA]: " << adcSet.processed[I_DRO] << std::endl;
sif::info << "U DRO [V]: " << adcSet.processed[U_DRO_DIV_6] << std::endl;
}
}
}
void PayloadPcduHandler::enablePeriodicPrintout(bool enable, uint8_t divider) {
this->periodicPrintout = enable;
opDivider.setDivider(divider);
}
2022-03-28 12:23:56 +02:00
void PayloadPcduHandler::quickTransitionBackToOff(bool startTransitionToOff, bool notifyFdir) {
2022-03-04 15:26:36 +01:00
States currentState = state;
gpioIF->pullLow(gpioIds::PLPCDU_ENB_HPA);
gpioIF->pullLow(gpioIds::PLPCDU_ENB_MPA);
gpioIF->pullLow(gpioIds::PLPCDU_ENB_TX);
gpioIF->pullLow(gpioIds::PLPCDU_ENB_X8);
gpioIF->pullLow(gpioIds::PLPCDU_ENB_DRO);
gpioIF->pullLow(gpioIds::PLPCDU_ENB_TX);
gpioIF->pullLow(gpioIds::PLPCDU_ENB_VBAT0);
gpioIF->pullLow(gpioIds::PLPCDU_ENB_VBAT1);
2022-12-22 14:13:41 +01:00
state = States::STACK_5V_SWITCHING;
2022-03-04 15:26:36 +01:00
adcState = AdcStates::OFF;
2022-03-28 12:23:56 +02:00
if (startTransitionToOff) {
startTransition(MODE_OFF, 0);
}
2022-03-04 15:26:36 +01:00
if (notifyFdir) {
triggerEvent(TRANSITION_BACK_TO_OFF, static_cast<uint32_t>(currentState));
}
}
void PayloadPcduHandler::checkAdcValues() {
using namespace plpcdu;
checkJsonFileInit();
adcSet.processed[U_BAT_DIV_6] =
static_cast<float>(adcSet.channels[0]) * VOLTAGE_DIV / MAX122X_BIT * MAX122X_VREF;
adcSet.processed[U_NEG_V_FB] =
V_POS - VOLTAGE_DIV_U_NEG *
(V_POS - static_cast<float>(adcSet.channels[1]) / MAX122X_BIT * MAX122X_VREF);
adcSet.processed[I_HPA] = static_cast<float>(adcSet.channels[2]) * SCALE_CURRENT_HPA * 1000.0;
adcSet.processed[U_HPA_DIV_6] = static_cast<float>(adcSet.channels[3]) * SCALE_VOLTAGE;
adcSet.processed[I_MPA] = static_cast<float>(adcSet.channels[4]) * SCALE_CURRENT_MPA * 1000.0;
adcSet.processed[U_MPA_DIV_6] = static_cast<float>(adcSet.channels[5]) * SCALE_VOLTAGE;
adcSet.processed[I_TX] = static_cast<float>(adcSet.channels[6]) * SCALE_CURRENT_TX * 1000.0;
adcSet.processed[U_TX_DIV_6] = static_cast<float>(adcSet.channels[7]) * SCALE_VOLTAGE;
adcSet.processed[I_X8] = static_cast<float>(adcSet.channels[8]) * SCALE_CURRENT_X8 * 1000.0;
adcSet.processed[U_X8_DIV_6] = static_cast<float>(adcSet.channels[9]) * SCALE_VOLTAGE;
adcSet.processed[I_DRO] = static_cast<float>(adcSet.channels[10]) * SCALE_CURRENT_DRO * 1000.0;
adcSet.processed[U_DRO_DIV_6] = static_cast<float>(adcSet.channels[11]) * SCALE_VOLTAGE;
float lowerBound = 0.0;
float upperBound = 0.0;
bool adcTransition = false;
adcTransition = state == States::ON_TRANS_DRO and adcCountdown.isBusy();
// Now check against voltage and current limits, depending on state
if (state >= States::ON_TRANS_DRO and not adcTransition) {
if (ssrToDroInjectionRequested) {
handleFailureInjection("SSR to DRO", NEG_V_OUT_OF_BOUNDS);
ssrToDroInjectionRequested = false;
return;
}
params.getValue(PARAM_KEY_MAP[NEG_V_LOWER_BOUND], lowerBound);
params.getValue(PARAM_KEY_MAP[NEG_V_UPPER_BOUND], upperBound);
if (not checkVoltage(adcSet.processed[U_NEG_V_FB], lowerBound, upperBound,
NEG_V_OUT_OF_BOUNDS)) {
return;
}
params.getValue(PARAM_KEY_MAP[DRO_U_LOWER_BOUND], lowerBound);
params.getValue(PARAM_KEY_MAP[DRO_U_UPPER_BOUND], upperBound);
if (not checkVoltage(adcSet.processed[U_DRO_DIV_6], lowerBound, upperBound,
U_DRO_OUT_OF_BOUNDS)) {
return;
}
params.getValue(PARAM_KEY_MAP[DRO_I_UPPER_BOUND], upperBound);
if (not checkCurrent(adcSet.processed[I_DRO], upperBound, I_DRO_OUT_OF_BOUNDS)) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::warning << "Detected out of bounds current for DRO: " << adcSet.processed[I_DRO]
<< ", Raw: " << adcSet.channels[I_DRO] << std::endl;
#endif
return;
}
}
adcTransition = state == States::ON_TRANS_X8 and adcCountdown.isBusy();
if (state >= States::ON_TRANS_X8 and not adcTransition) {
if (droToX8InjectionRequested) {
handleFailureInjection("X8 to TX", U_X8_OUT_OF_BOUNDS);
droToX8InjectionRequested = false;
return;
}
params.getValue(PARAM_KEY_MAP[X8_U_LOWER_BOUND], lowerBound);
params.getValue(PARAM_KEY_MAP[X8_U_UPPER_BOUND], upperBound);
if (not checkVoltage(adcSet.processed[U_X8_DIV_6], lowerBound, upperBound,
U_X8_OUT_OF_BOUNDS)) {
return;
}
params.getValue(PARAM_KEY_MAP[X8_I_UPPER_BOUND], upperBound);
if (not checkCurrent(adcSet.processed[I_X8], upperBound, I_X8_OUT_OF_BOUNDS)) {
return;
}
}
adcTransition = state == States::ON_TRANS_TX and adcCountdown.isBusy();
if (state >= States::ON_TRANS_TX and not adcTransition) {
if (txToMpaInjectionRequested) {
handleFailureInjection("TX to MPA", U_TX_OUT_OF_BOUNDS);
txToMpaInjectionRequested = false;
return;
}
params.getValue(PARAM_KEY_MAP[TX_U_LOWER_BOUND], lowerBound);
params.getValue(PARAM_KEY_MAP[TX_U_UPPER_BOUND], upperBound);
if (not checkVoltage(adcSet.processed[U_TX_DIV_6], lowerBound, upperBound,
U_TX_OUT_OF_BOUNDS)) {
return;
}
params.getValue(PARAM_KEY_MAP[TX_I_UPPER_BOUND], upperBound);
if (not checkCurrent(adcSet.processed[I_TX], upperBound, I_TX_OUT_OF_BOUNDS)) {
return;
}
}
adcTransition = state == States::ON_TRANS_MPA and adcCountdown.isBusy();
if (state >= States::ON_TRANS_MPA and not adcTransition) {
if (mpaToHpaInjectionRequested) {
handleFailureInjection("MPA to HPA", U_HPA_OUT_OF_BOUNDS);
mpaToHpaInjectionRequested = false;
return;
}
params.getValue(PARAM_KEY_MAP[MPA_U_LOWER_BOUND], lowerBound);
params.getValue(PARAM_KEY_MAP[MPA_U_UPPER_BOUND], upperBound);
if (not checkVoltage(adcSet.processed[U_MPA_DIV_6], lowerBound, upperBound,
U_MPA_OUT_OF_BOUNDS)) {
return;
}
params.getValue(PARAM_KEY_MAP[MPA_I_UPPER_BOUND], upperBound);
if (not checkCurrent(adcSet.processed[I_MPA], upperBound, I_MPA_OUT_OF_BOUNDS)) {
return;
}
}
adcTransition = state == States::ON_TRANS_HPA and adcCountdown.isBusy();
if (state >= States::ON_TRANS_HPA and not adcTransition) {
if (allOnInjectRequested) {
handleFailureInjection("All On", U_HPA_OUT_OF_BOUNDS);
allOnInjectRequested = false;
return;
}
params.getValue(PARAM_KEY_MAP[HPA_U_LOWER_BOUND], lowerBound);
params.getValue(PARAM_KEY_MAP[HPA_U_UPPER_BOUND], upperBound);
if (not checkVoltage(adcSet.processed[U_HPA_DIV_6], lowerBound, upperBound,
U_HPA_OUT_OF_BOUNDS)) {
return;
}
params.getValue(PARAM_KEY_MAP[HPA_I_UPPER_BOUND], upperBound);
if (not checkCurrent(adcSet.processed[I_HPA], upperBound, I_HPA_OUT_OF_BOUNDS)) {
2022-03-28 17:46:34 +02:00
sif::warning << "PayloadPcduHandler::checkCurrent: I HPA exceeded limit: Measured "
<< adcSet.processed[I_HPA] << " mA" << std::endl;
2022-03-04 15:26:36 +01:00
return;
}
}
transitionOk = true;
}
void PayloadPcduHandler::checkJsonFileInit() {
if (not jsonFileInitComplete) {
2022-09-12 17:23:43 +02:00
auto activeSd = sdcMan->getActiveSdCard();
2022-09-29 13:53:42 +02:00
if (activeSd and sdcMan->isSdCardUsable(activeSd.value())) {
2023-02-22 18:06:34 +01:00
if (sdcMan->getCurrentMountPrefix() == nullptr) {
return;
}
2022-05-24 00:20:23 +02:00
params.initialize(sdcMan->getCurrentMountPrefix());
2022-03-04 15:26:36 +01:00
jsonFileInitComplete = true;
}
}
}
bool PayloadPcduHandler::checkVoltage(float val, float lowerBound, float upperBound, Event event) {
bool tooLarge = false;
if (val < lowerBound or val > upperBound) {
if (val > upperBound) {
tooLarge = true;
} else {
tooLarge = false;
}
uint32_t p2 = 0;
serializeFloat(p2, val);
triggerEvent(event, tooLarge, p2);
transitionOk = false;
2022-03-28 12:23:56 +02:00
quickTransitionBackToOff(true, true);
quickTransitionAlreadyCalled = true;
2022-03-04 15:26:36 +01:00
return false;
}
return true;
}
bool PayloadPcduHandler::checkCurrent(float val, float upperBound, Event event) {
if (val > upperBound) {
uint32_t p2 = 0;
serializeFloat(p2, val);
triggerEvent(event, true, p2);
transitionOk = false;
2022-03-28 12:23:56 +02:00
quickTransitionBackToOff(true, true);
quickTransitionAlreadyCalled = true;
2022-03-04 15:26:36 +01:00
return false;
}
return true;
}
ReturnValue_t PayloadPcduHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
2022-03-28 15:29:42 +02:00
using namespace plpcdu;
if (mode == MODE_NORMAL) {
uint8_t dhbSubmode = getSubmode();
diffMask = submode ^ dhbSubmode;
2022-03-30 12:09:17 +02:00
// Also deals with the case where the mode is MODE_ON, submode should be 0 here
if ((((submode >> SOLID_STATE_RELAYS_ADC_ON) & 0b1) == SOLID_STATE_RELAYS_ADC_ON) and
(getMode() == MODE_NORMAL and dhbSubmode != ALL_OFF_SUBMODE)) {
2022-03-28 15:29:42 +02:00
return TRANS_NOT_ALLOWED;
}
if (((((submode >> DRO_ON) & 1) == 1) and
((dhbSubmode & 0b1) != (1 << SOLID_STATE_RELAYS_ADC_ON)))) {
2022-03-28 15:29:42 +02:00
return TRANS_NOT_ALLOWED;
}
if ((((submode >> X8_ON) & 1) == 1) and
((dhbSubmode & 0b11) != ((1 << SOLID_STATE_RELAYS_ADC_ON) | (1 << DRO_ON)))) {
2022-03-28 15:29:42 +02:00
return TRANS_NOT_ALLOWED;
}
if (((((submode >> TX_ON) & 1) == 1) and
((dhbSubmode & 0b111) !=
2022-04-07 00:14:18 +02:00
((1 << X8_ON) | (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
2022-03-28 15:29:42 +02:00
return TRANS_NOT_ALLOWED;
}
if ((((submode >> MPA_ON) & 1) == 1 and
((dhbSubmode & 0b1111) !=
((1 << TX_ON) | (1 << X8_ON) | (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
2022-03-28 15:29:42 +02:00
return TRANS_NOT_ALLOWED;
}
if ((((submode >> HPA_ON) & 1) == 1 and
((dhbSubmode & 0b11111) != ((1 << MPA_ON) | (1 << TX_ON) | (1 << X8_ON) | (1 << DRO_ON) |
(1 << SOLID_STATE_RELAYS_ADC_ON))))) {
2022-03-28 15:29:42 +02:00
return TRANS_NOT_ALLOWED;
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-04 15:26:36 +01:00
}
return DeviceHandlerBase::isModeCombinationValid(mode, submode);
}
ReturnValue_t PayloadPcduHandler::serializeFloat(uint32_t& param, float val) {
size_t dummy = 0;
return SerializeAdapter::serialize(&val, reinterpret_cast<uint8_t*>(&param), &dummy, 4,
SerializeIF::Endianness::NETWORK);
}
ReturnValue_t PayloadPcduHandler::getParameter(uint8_t domainId, uint8_t uniqueId,
ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues,
uint16_t startAtIndex) {
using namespace plpcdu;
switch (uniqueId) {
case (PlPcduParamIds::NEG_V_LOWER_BOUND):
case (PlPcduParamIds::NEG_V_UPPER_BOUND):
case (PlPcduParamIds::DRO_U_LOWER_BOUND):
case (PlPcduParamIds::DRO_U_UPPER_BOUND):
case (PlPcduParamIds::DRO_I_UPPER_BOUND):
case (PlPcduParamIds::X8_U_LOWER_BOUND):
case (PlPcduParamIds::X8_U_UPPER_BOUND):
case (PlPcduParamIds::X8_I_UPPER_BOUND):
case (PlPcduParamIds::TX_U_LOWER_BOUND):
case (PlPcduParamIds::TX_U_UPPER_BOUND):
case (PlPcduParamIds::TX_I_UPPER_BOUND):
case (PlPcduParamIds::MPA_U_LOWER_BOUND):
case (PlPcduParamIds::MPA_U_UPPER_BOUND):
case (PlPcduParamIds::MPA_I_UPPER_BOUND):
case (PlPcduParamIds::HPA_U_LOWER_BOUND):
case (PlPcduParamIds::HPA_U_UPPER_BOUND):
case (PlPcduParamIds::HPA_I_UPPER_BOUND):
case (PlPcduParamIds::SSR_TO_DRO_WAIT_TIME):
case (PlPcduParamIds::DRO_TO_X8_WAIT_TIME):
case (PlPcduParamIds::X8_TO_TX_WAIT_TIME):
case (PlPcduParamIds::TX_TO_MPA_WAIT_TIME):
case (PlPcduParamIds::MPA_TO_HPA_WAIT_TIME): {
handleDoubleParamUpdate(PARAM_KEY_MAP[static_cast<PlPcduParamIds>(uniqueId)],
parameterWrapper, newValues);
break;
}
case (PlPcduParamIds::INJECT_SSR_TO_DRO_FAILURE): {
ssrToDroInjectionRequested = true;
break;
}
case (PlPcduParamIds::INJECT_DRO_TO_X8_FAILURE): {
droToX8InjectionRequested = true;
break;
}
case (PlPcduParamIds::INJECT_X8_TO_TX_FAILURE): {
x8ToTxInjectionRequested = true;
break;
}
case (PlPcduParamIds::INJECT_TX_TO_MPA_FAILURE): {
txToMpaInjectionRequested = true;
break;
}
case (PlPcduParamIds::INJECT_MPA_TO_HPA_FAILURE): {
mpaToHpaInjectionRequested = true;
break;
}
case (PlPcduParamIds::INJECT_ALL_ON_FAILURE): {
allOnInjectRequested = true;
break;
}
default: {
return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues,
startAtIndex);
}
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-04 15:26:36 +01:00
}
void PayloadPcduHandler::handleFailureInjection(std::string output, Event event) {
sif::info << "PayloadPcduHandler::handleFailureInjection: " << output
2022-03-04 15:26:36 +01:00
<< " failure injection. "
"Transitioning back to off"
<< std::endl;
triggerEvent(event, 0, 0);
transitionOk = false;
2022-03-28 12:23:56 +02:00
quickTransitionBackToOff(true, true);
quickTransitionAlreadyCalled = true;
2022-03-04 15:26:36 +01:00
droToX8InjectionRequested = false;
}
ReturnValue_t PayloadPcduHandler::handleDoubleParamUpdate(std::string key,
ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues) {
double newValue = 0.0;
ReturnValue_t result = newValues->getElement<double>(&newValue, 0, 0);
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
2022-03-04 15:26:36 +01:00
return result;
}
params.setValue(key, newValue);
// Do this so the dumping and loading with the framework works as well
doubleDummy = newValue;
parameterWrapper->set(doubleDummy);
return params.writeJsonFile();
}
2022-04-26 10:37:25 +02:00
#ifdef XIPHOS_Q7S
2022-03-04 15:26:36 +01:00
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;
2022-08-24 17:27:47 +02:00
return returnvalue::FAILED;
2022-03-04 15:26:36 +01:00
}
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);
}
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-04 15:26:36 +01:00
}
ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cookie,
const uint8_t* sendData, size_t sendLen,
bool tempOnly) {
2022-08-24 17:27:47 +02:00
ReturnValue_t result = returnvalue::OK;
2022-03-04 15:26:36 +01:00
int retval = 0;
// Prepare transfer
int fileDescriptor = 0;
2023-02-18 13:46:00 +01:00
UnixFileGuard fileHelper(comIf->getSpiDev(), fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
2022-08-24 17:27:47 +02:00
if (fileHelper.getOpenResult() != returnvalue::OK) {
2022-03-04 15:26:36 +01:00
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();
2022-10-26 17:11:57 +02:00
GpioIF& gpioIF = comIf->getGpioInterface();
2022-03-04 15:26:36 +01:00
MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
uint32_t timeoutMs = 0;
2022-05-14 11:34:25 +02:00
MutexIF* mutex = comIf->getCsMutex();
2022-10-26 17:11:57 +02:00
if (mutex == nullptr) {
2022-03-04 15:26:36 +01:00
#if OBSW_VERBOSE_LEVEL >= 1
sif::warning << "GyroADIS16507Handler::spiSendCallback: "
"Mutex or GPIO interface invalid"
<< std::endl;
2022-08-24 17:27:47 +02:00
return returnvalue::FAILED;
2022-03-04 15:26:36 +01:00
#endif
}
if (gpioId != gpio::NO_GPIO) {
2022-05-14 11:34:25 +02:00
cookie->getMutexParams(timeoutType, timeoutMs);
2022-03-04 15:26:36 +01:00
result = mutex->lockMutex(timeoutType, timeoutMs);
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
2022-03-04 15:26:36 +01:00
#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) {
2022-10-26 17:11:57 +02:00
gpioIF.pullLow(gpioId);
2022-03-04 15:26:36 +01:00
}
// 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) {
2022-10-26 17:11:57 +02:00
gpioIF.pullHigh(gpioId);
2022-03-04 15:26:36 +01:00
}
transferStruct->tx_buf += transferLen;
transferStruct->rx_buf += transferLen;
transferStruct->len = plpcdu::TEMP_REPLY_SIZE - 1;
if (gpioId != gpio::NO_GPIO) {
2022-10-26 17:11:57 +02:00
gpioIF.pullLow(gpioId);
2022-03-04 15:26:36 +01:00
}
// 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) {
2022-10-26 17:11:57 +02:00
gpioIF.pullHigh(gpioId);
2022-03-04 15:26:36 +01:00
}
transferStruct->tx_buf = origTx;
transferStruct->rx_buf = origRx;
if (gpioId != gpio::NO_GPIO) {
mutex->unlockMutex();
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-04 15:26:36 +01:00
}
#endif