799 lines
29 KiB
C++
799 lines
29 KiB
C++
#include "PayloadPcduHandler.h"
|
|
|
|
#include <fsfw/src/fsfw/datapool/PoolReadGuard.h>
|
|
|
|
#include "OBSWConfig.h"
|
|
|
|
#ifdef XIPHOS_Q7S
|
|
#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,
|
|
PowerSwitchIF* pwrSwitcher, power::Switch_t switchA,
|
|
power::Switch_t switchB, bool periodicPrintout)
|
|
: DeviceHandlerBase(objectId, comIF, cookie),
|
|
adcSet(this),
|
|
periodicPrintout(periodicPrintout),
|
|
gpioIF(gpioIF),
|
|
sdcMan(sdcMan),
|
|
pwrStateMachine(switchA, switchB, pwrSwitcher) {}
|
|
|
|
void PayloadPcduHandler::doStartUp() {
|
|
if ((state != States::PL_PCDU_OFF) and (state != States::ON_TRANS_SSR)) {
|
|
// Config error
|
|
sif::error << "PayloadPcduHandler::doStartUp: Invalid state" << std::endl;
|
|
}
|
|
if (pwrStateMachine.getState() == power::States::IDLE) {
|
|
pwrStateMachine.start(MODE_ON, pwrSubmode);
|
|
}
|
|
auto opCode = pwrStateMachine.fsm();
|
|
if (opCode == power::OpCodes::TO_NOT_OFF_DONE or opCode == power::OpCodes::TIMEOUT_OCCURED) {
|
|
pwrStateMachine.reset();
|
|
quickTransitionAlreadyCalled = false;
|
|
state = States::POWER_CHANNELS_ON;
|
|
setMode(_MODE_TO_ON);
|
|
}
|
|
}
|
|
|
|
void PayloadPcduHandler::doShutDown() {
|
|
if (not quickTransitionAlreadyCalled) {
|
|
quickTransitionBackToOff(false, false);
|
|
quickTransitionAlreadyCalled = true;
|
|
}
|
|
if (pwrStateMachine.getState() == power::States::IDLE) {
|
|
pwrStateMachine.start(MODE_OFF, 0);
|
|
}
|
|
auto opCode = pwrStateMachine.fsm();
|
|
if (opCode == power::OpCodes::TO_OFF_DONE or opCode == power::OpCodes::TIMEOUT_OCCURED) {
|
|
pwrStateMachine.reset();
|
|
state = States::PL_PCDU_OFF;
|
|
// No need to set mode _MODE_POWER_DOWN, power switching was already handled
|
|
setMode(MODE_OFF);
|
|
}
|
|
}
|
|
|
|
void PayloadPcduHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
|
|
if (mode == _MODE_TO_NORMAL) {
|
|
stateMachineToNormal(modeFrom, subModeFrom);
|
|
return;
|
|
}
|
|
DeviceHandlerBase::doTransition(modeFrom, subModeFrom);
|
|
}
|
|
|
|
ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_t subModeFrom) {
|
|
using namespace plpcdu;
|
|
bool doFinish = true;
|
|
if (((submode >> SOLID_STATE_RELAYS_ADC_ON) & 0b1) == 1) {
|
|
if (state == States::PL_PCDU_OFF) {
|
|
sif::error << "PayloadPcduHandler::stateMachineToNormal: Unexpected state PL_PCDU_OFF"
|
|
<< "detected" << std::endl;
|
|
setMode(MODE_OFF);
|
|
return returnvalue::FAILED;
|
|
}
|
|
if (state == States::POWER_CHANNELS_ON) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::info << "Switching on SSR VBAT0 & VBAT1 GPIOs" << std::endl;
|
|
#endif
|
|
// Switch on relays here
|
|
gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT0);
|
|
gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT1);
|
|
state = States::ON_TRANS_SSR;
|
|
transitionOk = true;
|
|
doFinish = false;
|
|
}
|
|
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;
|
|
doFinish = false;
|
|
// 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) {
|
|
if (adcState == AdcStates::BOOT_DELAY) {
|
|
doFinish = false;
|
|
if (adcCountdown.hasTimedOut()) {
|
|
adcState = AdcStates::SEND_SETUP;
|
|
adcCmdExecuted = false;
|
|
}
|
|
}
|
|
if (adcState == AdcStates::SEND_SETUP) {
|
|
if (adcCmdExecuted) {
|
|
adcState = AdcStates::NORMAL;
|
|
doFinish = true;
|
|
adcCountdown.setTimeout(100);
|
|
adcCountdown.resetTimer();
|
|
adcCmdExecuted = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
auto switchHandler = [&](NormalSubmodeBits bit, gpioId_t id, std::string info) {
|
|
if (((diffMask >> bit) & 1) == 1) {
|
|
if (((submode >> bit) & 1) == 1) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::info << "Enabling PL PCDU " << info << " module" << std::endl;
|
|
#endif
|
|
// Switch on DRO and start monitoring for negative voltages
|
|
updateSwitchGpio(id, gpio::Levels::HIGH);
|
|
} else {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::info << "Disabling PL PCDU " << info << " module" << std::endl;
|
|
#endif
|
|
updateSwitchGpio(id, gpio::Levels::LOW);
|
|
}
|
|
}
|
|
};
|
|
|
|
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, submode);
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
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 (mode == _MODE_TO_NORMAL) {
|
|
return buildNormalDeviceCommand(id);
|
|
}
|
|
return NOTHING_TO_SEND;
|
|
}
|
|
|
|
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();
|
|
}
|
|
|
|
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 returnvalue::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 returnvalue::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() != returnvalue::OK) {
|
|
return pg.getReadResult();
|
|
}
|
|
handleExtConvRead(packet);
|
|
checkAdcValues();
|
|
adcSet.setValidity(true, true);
|
|
handlePrintout();
|
|
break;
|
|
}
|
|
case (READ_WITH_TEMP_EXT): {
|
|
PoolReadGuard pg(&adcSet);
|
|
if (pg.getReadResult() != returnvalue::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]);
|
|
checkAdcValues();
|
|
adcSet.setValidity(true, true);
|
|
handlePrintout();
|
|
break;
|
|
}
|
|
default: {
|
|
break;
|
|
}
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
uint32_t PayloadPcduHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
|
|
// 20 minutes transition delay is allowed
|
|
return 20 * 60 * 1000;
|
|
}
|
|
|
|
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);
|
|
poolManager.subscribeForDiagPeriodicPacket(
|
|
subdp::DiagnosticsHkPeriodicParams(adcSet.getSid(), false, 5.0));
|
|
return returnvalue::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() {
|
|
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);
|
|
}
|
|
|
|
void PayloadPcduHandler::quickTransitionBackToOff(bool startTransitionToOff, bool notifyFdir) {
|
|
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);
|
|
state = States::PL_PCDU_OFF;
|
|
adcState = AdcStates::OFF;
|
|
if (startTransitionToOff) {
|
|
startTransition(MODE_OFF, 0);
|
|
}
|
|
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)) {
|
|
sif::warning << "PayloadPcduHandler::checkCurrent: I HPA exceeded limit: Measured "
|
|
<< adcSet.processed[I_HPA] << " mA" << std::endl;
|
|
return;
|
|
}
|
|
}
|
|
transitionOk = true;
|
|
}
|
|
|
|
void PayloadPcduHandler::checkJsonFileInit() {
|
|
if (not jsonFileInitComplete) {
|
|
sd::SdCard activeSd = sdcMan->getActiveSdCard();
|
|
if (sdcMan->isSdCardUsable(activeSd)) {
|
|
params.initialize(sdcMan->getCurrentMountPrefix());
|
|
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;
|
|
quickTransitionBackToOff(true, true);
|
|
quickTransitionAlreadyCalled = true;
|
|
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;
|
|
quickTransitionBackToOff(true, true);
|
|
quickTransitionAlreadyCalled = true;
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ReturnValue_t PayloadPcduHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
|
|
using namespace plpcdu;
|
|
if (mode == MODE_NORMAL) {
|
|
diffMask = submode ^ this->submode;
|
|
// 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
|
|
(this->mode == MODE_NORMAL and this->submode != ALL_OFF_SUBMODE)) {
|
|
return TRANS_NOT_ALLOWED;
|
|
}
|
|
if (((((submode >> DRO_ON) & 1) == 1) and
|
|
((this->submode & 0b1) != (1 << SOLID_STATE_RELAYS_ADC_ON)))) {
|
|
return TRANS_NOT_ALLOWED;
|
|
}
|
|
if ((((submode >> X8_ON) & 1) == 1) and
|
|
((this->submode & 0b11) != ((1 << SOLID_STATE_RELAYS_ADC_ON) | (1 << DRO_ON)))) {
|
|
return TRANS_NOT_ALLOWED;
|
|
}
|
|
if (((((submode >> TX_ON) & 1) == 1) and
|
|
((this->submode & 0b111) !=
|
|
((1 << X8_ON) | (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
|
|
return TRANS_NOT_ALLOWED;
|
|
}
|
|
if ((((submode >> MPA_ON) & 1) == 1 and
|
|
((this->submode & 0b1111) !=
|
|
((1 << TX_ON) | (1 << X8_ON) | (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
|
|
return TRANS_NOT_ALLOWED;
|
|
}
|
|
if ((((submode >> HPA_ON) & 1) == 1 and
|
|
((this->submode & 0b11111) != ((1 << MPA_ON) | (1 << TX_ON) | (1 << X8_ON) |
|
|
(1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
|
|
return TRANS_NOT_ALLOWED;
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
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*>(¶m), &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);
|
|
}
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void PayloadPcduHandler::handleFailureInjection(std::string output, Event event) {
|
|
sif::info << "PayloadPcduHandler::checkAdcValues: " << output
|
|
<< " failure injection. "
|
|
"Transitioning back to off"
|
|
<< std::endl;
|
|
triggerEvent(event, 0, 0);
|
|
transitionOk = false;
|
|
quickTransitionBackToOff(true, true);
|
|
quickTransitionAlreadyCalled = true;
|
|
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);
|
|
if (result != returnvalue::OK) {
|
|
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();
|
|
}
|
|
|
|
#ifdef XIPHOS_Q7S
|
|
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 returnvalue::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 returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cookie,
|
|
const uint8_t* sendData, size_t sendLen,
|
|
bool tempOnly) {
|
|
ReturnValue_t result = returnvalue::OK;
|
|
int retval = 0;
|
|
// Prepare transfer
|
|
int fileDescriptor = 0;
|
|
UnixFileGuard fileHelper(comIf->getSpiDev(), &fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
|
|
if (fileHelper.getOpenResult() != returnvalue::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->getCsMutex();
|
|
if (mutex == nullptr or gpioIF == nullptr) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::warning << "GyroADIS16507Handler::spiSendCallback: "
|
|
"Mutex or GPIO interface invalid"
|
|
<< std::endl;
|
|
return returnvalue::FAILED;
|
|
#endif
|
|
}
|
|
|
|
if (gpioId != gpio::NO_GPIO) {
|
|
cookie->getMutexParams(timeoutType, timeoutMs);
|
|
result = mutex->lockMutex(timeoutType, timeoutMs);
|
|
if (result != returnvalue::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 returnvalue::OK;
|
|
}
|
|
|
|
#endif
|