327 lines
14 KiB
C++
327 lines
14 KiB
C++
#include "AcsBoardAssembly.h"
|
|
|
|
#include <devices/gpioIds.h>
|
|
#include <fsfw/power/PowerSwitchIF.h>
|
|
#include <fsfw/serviceinterface.h>
|
|
|
|
#include "OBSWConfig.h"
|
|
|
|
AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, PowerSwitchIF* switcher,
|
|
AcsBoardHelper helper, GpioIF* gpioIF)
|
|
: DualLaneAssemblyBase(objectId, switcher, SWITCH_A, SWITCH_B, POWER_STATE_MACHINE_TIMEOUT,
|
|
SIDE_SWITCH_TRANSITION_NOT_ALLOWED, TRANSITION_OTHER_SIDE_FAILED),
|
|
helper(helper),
|
|
gpioIF(gpioIF) {
|
|
if (switcher == nullptr) {
|
|
sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid Power Switcher "
|
|
"IF passed"
|
|
<< std::endl;
|
|
}
|
|
if (gpioIF == nullptr) {
|
|
sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid GPIO IF passed" << std::endl;
|
|
}
|
|
ModeListEntry entry;
|
|
initModeTableEntry(helper.mgm0Lis3IdSideA, entry, modeTable);
|
|
initModeTableEntry(helper.mgm1Rm3100IdSideA, entry, modeTable);
|
|
initModeTableEntry(helper.mgm2Lis3IdSideB, entry, modeTable);
|
|
initModeTableEntry(helper.mgm3Rm3100IdSideB, entry, modeTable);
|
|
initModeTableEntry(helper.gyro0AdisIdSideA, entry, modeTable);
|
|
initModeTableEntry(helper.gyro1L3gIdSideA, entry, modeTable);
|
|
initModeTableEntry(helper.gyro2AdisIdSideB, entry, modeTable);
|
|
initModeTableEntry(helper.gyro3L3gIdSideB, entry, modeTable);
|
|
initModeTableEntry(helper.gpsId, entry, modeTable);
|
|
}
|
|
|
|
ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) {
|
|
using namespace duallane;
|
|
ReturnValue_t result = returnvalue::OK;
|
|
refreshHelperModes();
|
|
// Initialize the mode table to ensure all devices are in a defined state
|
|
modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_1_A].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_3_B].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_0_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_0_A].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_1_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_1_A].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_2_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_2_B].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_3_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::GPS].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
|
|
if (recoveryState == RecoveryState::RECOVERY_IDLE) {
|
|
result = checkAndHandleHealthStates(mode, submode);
|
|
if (result == NEED_TO_CHANGE_HEALTH) {
|
|
return returnvalue::OK;
|
|
}
|
|
}
|
|
if (recoveryState != RecoveryState::RECOVERY_STARTED) {
|
|
if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
|
|
result = handleNormalOrOnModeCmd(mode, submode);
|
|
}
|
|
}
|
|
HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
|
|
executeTable(tableIter);
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
|
|
using namespace duallane;
|
|
refreshHelperModes();
|
|
if (wantedSubmode == A_SIDE) {
|
|
if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode) or
|
|
(helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode) or
|
|
(helper.gpsMode != MODE_ON) or
|
|
(healthHelper.healthTable->getHealth(helper.healthDevGps0) == FAULTY)) {
|
|
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
|
|
}
|
|
return returnvalue::OK;
|
|
} else if (wantedSubmode == B_SIDE) {
|
|
if ((helper.gyro2SideBMode != wantedMode and helper.gyro3SideBMode != wantedMode) or
|
|
(helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or
|
|
(helper.gpsMode != MODE_ON) or
|
|
(healthHelper.healthTable->getHealth(helper.healthDevGps1) == FAULTY)) {
|
|
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
|
|
}
|
|
return returnvalue::OK;
|
|
} else if (wantedSubmode == DUAL_MODE) {
|
|
if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode and
|
|
helper.gyro2AdisIdSideB != wantedMode and helper.gyro3SideBMode != wantedMode) or
|
|
(helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode and
|
|
helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or
|
|
helper.gpsMode != MODE_ON) {
|
|
// Trigger event, but don't start any other transitions. This is the last fallback mode.
|
|
if (dualModeErrorSwitch) {
|
|
triggerEvent(NOT_ENOUGH_DEVICES_DUAL_MODE, 0, 0);
|
|
dualModeErrorSwitch = false;
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
|
|
using namespace duallane;
|
|
ReturnValue_t result = returnvalue::OK;
|
|
bool needsSecondStep = false;
|
|
if (sideSwitchState == SideSwitchState::REQUESTED) {
|
|
sideSwitchState = SideSwitchState::TO_DUAL;
|
|
}
|
|
// Switch to dual side first, and later switch back to the otherside
|
|
if (sideSwitchState == SideSwitchState::TO_DUAL) {
|
|
targetSubmodeForSideSwitch = static_cast<duallane::Submodes>(submode);
|
|
submode = Submodes::DUAL_MODE;
|
|
sideSwitchState = SideSwitchState::DISABLE_OTHER_SIDE;
|
|
// TODO: Ugly hack. The base class should support arbitrary number of steps..
|
|
needsSecondStep = true;
|
|
} else if (sideSwitchState == SideSwitchState::DISABLE_OTHER_SIDE) {
|
|
submode = targetSubmodeForSideSwitch;
|
|
}
|
|
auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, ModeTableIdx tableIdx) {
|
|
if (mode == devMode) {
|
|
modeTable[tableIdx].setMode(mode);
|
|
} else if (isModeCommandable(objectId, devMode)) {
|
|
modeTable[tableIdx].setMode(mode);
|
|
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
|
}
|
|
};
|
|
bool gpsUsable = isModeCommandable(helper.gpsId, helper.gpsMode);
|
|
auto gpsCmd = [&](bool gnss0NReset, bool gnss1NReset, uint8_t gnssSelect) {
|
|
if (gpsUsable) {
|
|
if (mode == MODE_ON or mode == DeviceHandlerIF::MODE_NORMAL) {
|
|
modeTable[ModeTableIdx::GPS].setMode(MODE_ON);
|
|
} else if (mode == MODE_OFF) {
|
|
gnss0NReset = true;
|
|
gnss1NReset = true;
|
|
modeTable[ModeTableIdx::GPS].setMode(MODE_OFF);
|
|
}
|
|
modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
|
|
|
|
gpioHandler(gpioIds::GNSS_0_NRESET, gnss0NReset,
|
|
"AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin"
|
|
"of GNSS 0");
|
|
gpioHandler(gpioIds::GNSS_1_NRESET, gnss1NReset,
|
|
"AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin"
|
|
"of GNSS 1");
|
|
gpioHandler(gpioIds::GNSS_SELECT, gnssSelect,
|
|
"AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select");
|
|
}
|
|
};
|
|
switch (submode) {
|
|
case (A_SIDE): {
|
|
modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_3_B].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_2_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_2_B].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_3_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE);
|
|
cmdSeq(helper.gyro0AdisIdSideA, helper.gyro0SideAMode, ModeTableIdx::GYRO_0_A);
|
|
cmdSeq(helper.gyro1L3gIdSideA, helper.gyro1SideAMode, ModeTableIdx::GYRO_1_A);
|
|
cmdSeq(helper.mgm0Lis3IdSideA, helper.mgm0SideAMode, ModeTableIdx::MGM_0_A);
|
|
cmdSeq(helper.mgm1Rm3100IdSideA, helper.mgm1SideAMode, ModeTableIdx::MGM_1_A);
|
|
gpsCmd(true, false, 0);
|
|
break;
|
|
}
|
|
case (B_SIDE): {
|
|
modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_1_A].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_0_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_0_A].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_1_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_1_A].setSubmode(SUBMODE_NONE);
|
|
cmdSeq(helper.gyro2AdisIdSideB, helper.gyro2SideBMode, ModeTableIdx::GYRO_2_B);
|
|
cmdSeq(helper.gyro3L3gIdSideB, helper.gyro3SideBMode, ModeTableIdx::GYRO_3_B);
|
|
cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
|
|
cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
|
|
gpsCmd(false, true, 1);
|
|
break;
|
|
}
|
|
case (DUAL_MODE): {
|
|
cmdSeq(helper.gyro0AdisIdSideA, helper.gyro0SideAMode, ModeTableIdx::GYRO_0_A);
|
|
cmdSeq(helper.gyro1L3gIdSideA, helper.gyro1SideAMode, ModeTableIdx::GYRO_1_A);
|
|
cmdSeq(helper.mgm0Lis3IdSideA, helper.mgm0SideAMode, ModeTableIdx::MGM_0_A);
|
|
cmdSeq(helper.mgm1Rm3100IdSideA, helper.mgm1SideAMode, ModeTableIdx::MGM_1_A);
|
|
cmdSeq(helper.gyro2AdisIdSideB, helper.gyro2SideBMode, ModeTableIdx::GYRO_2_B);
|
|
cmdSeq(helper.gyro3L3gIdSideB, helper.gyro3SideBMode, ModeTableIdx::GYRO_3_B);
|
|
cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
|
|
cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
|
|
if (defaultSubmode == Submodes::A_SIDE) {
|
|
gpsCmd(true, true, 0);
|
|
} else {
|
|
gpsCmd(true, true, 1);
|
|
}
|
|
break;
|
|
}
|
|
default: {
|
|
sif::error << "AcsBoardAssembly::handleNormalModeCmd: Unknown submode" << std::endl;
|
|
}
|
|
}
|
|
if (needsSecondStep) {
|
|
result = NEED_SECOND_STEP;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void AcsBoardAssembly::selectGpsInDualMode(duallane::Submodes side) {
|
|
using namespace duallane;
|
|
if (submode != Submodes::DUAL_MODE) {
|
|
return;
|
|
}
|
|
ReturnValue_t result = returnvalue::OK;
|
|
if (side == Submodes::A_SIDE) {
|
|
result = gpioIF->pullLow(gpioIds::GNSS_SELECT);
|
|
} else {
|
|
result = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
|
|
}
|
|
if (result != returnvalue::OK) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::error << "AcsBoardAssembly::switchGpsInDualMode: Switching GPS failed" << std::endl;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void AcsBoardAssembly::gpioHandler(gpioId_t gpio, bool high, std::string error) {
|
|
ReturnValue_t result = returnvalue::OK;
|
|
if (high) {
|
|
result = gpioIF->pullHigh(gpio);
|
|
} else {
|
|
result = gpioIF->pullLow(gpio);
|
|
}
|
|
if (result != returnvalue::OK) {
|
|
#if OBSW_VERBOSE_LEVEL >= 1
|
|
sif::error << error << std::endl;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void AcsBoardAssembly::refreshHelperModes() {
|
|
try {
|
|
helper.gyro0SideAMode = childrenMap.at(helper.gyro0AdisIdSideA).mode;
|
|
helper.gyro1SideAMode = childrenMap.at(helper.gyro1L3gIdSideA).mode;
|
|
helper.gyro2SideBMode = childrenMap.at(helper.gyro2AdisIdSideB).mode;
|
|
helper.gyro3SideBMode = childrenMap.at(helper.gyro2AdisIdSideB).mode;
|
|
helper.mgm0SideAMode = childrenMap.at(helper.mgm0Lis3IdSideA).mode;
|
|
helper.mgm1SideAMode = childrenMap.at(helper.mgm1Rm3100IdSideA).mode;
|
|
helper.mgm2SideBMode = childrenMap.at(helper.mgm2Lis3IdSideB).mode;
|
|
helper.mgm3SideBMode = childrenMap.at(helper.mgm3Rm3100IdSideB).mode;
|
|
helper.gpsMode = childrenMap.at(helper.gpsId).mode;
|
|
} catch (const std::out_of_range& e) {
|
|
sif::error << "AcsBoardAssembly::refreshHelperModes: Invalid map: " << e.what() << std::endl;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t AcsBoardAssembly::initialize() {
|
|
for (const auto& child : childrenMap) {
|
|
updateChildModeByObjId(child.first, MODE_OFF, 0);
|
|
}
|
|
return AssemblyBase::initialize();
|
|
}
|
|
|
|
ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
|
|
Submode_t deviceSubmode) {
|
|
using namespace returnvalue;
|
|
ReturnValue_t status = returnvalue::OK;
|
|
auto checkAcsBoardSensorGroup = [&](object_id_t o0, object_id_t o1, object_id_t o2,
|
|
object_id_t o3) {
|
|
HealthState h0 = healthHelper.healthTable->getHealth(o0);
|
|
HealthState h1 = healthHelper.healthTable->getHealth(o1);
|
|
HealthState h2 = healthHelper.healthTable->getHealth(o2);
|
|
HealthState h3 = healthHelper.healthTable->getHealth(o3);
|
|
if ((h0 == FAULTY or h0 == PERMANENT_FAULTY) and (h1 == FAULTY or h1 == PERMANENT_FAULTY) and
|
|
(h2 == FAULTY or h2 == PERMANENT_FAULTY) and (h3 == FAULTY or h3 == PERMANENT_FAULTY)) {
|
|
overwriteDeviceHealth(o0, h0);
|
|
overwriteDeviceHealth(o1, h1);
|
|
overwriteDeviceHealth(o2, h2);
|
|
overwriteDeviceHealth(o3, h3);
|
|
}
|
|
if (h0 == EXTERNAL_CONTROL or h1 == EXTERNAL_CONTROL or h2 == EXTERNAL_CONTROL or
|
|
h3 == EXTERNAL_CONTROL) {
|
|
modeHelper.setForced(true);
|
|
}
|
|
};
|
|
if (healthHelper.healthTable->getHealth(helper.gpsId) == EXTERNAL_CONTROL) {
|
|
modeHelper.setForced(true);
|
|
}
|
|
if (deviceSubmode == duallane::DUAL_MODE) {
|
|
checkAcsBoardSensorGroup(helper.mgm0Lis3IdSideA, helper.mgm1Rm3100IdSideA,
|
|
helper.mgm2Lis3IdSideB, helper.mgm3Rm3100IdSideB);
|
|
checkAcsBoardSensorGroup(helper.gyro0AdisIdSideA, helper.gyro1L3gIdSideA,
|
|
helper.gyro2AdisIdSideB, helper.gyro3L3gIdSideB);
|
|
}
|
|
return status;
|
|
}
|
|
|
|
void AcsBoardAssembly::handleChildrenLostMode(ReturnValue_t result) {
|
|
using namespace duallane;
|
|
// Special handling to account for GPS devices being faulty. If the GPS device on the other
|
|
// side is marked faulty, directly to to dual side.
|
|
if (submode == Submodes::A_SIDE) {
|
|
if (healthHelper.healthTable->getHealth(helper.healthDevGps1) == FAULTY or
|
|
healthHelper.healthTable->getHealth(helper.healthDevGps1) == PERMANENT_FAULTY) {
|
|
triggerEvent(DIRECT_TRANSITION_TO_DUAL_OTHER_GPS_FAULTY, submode, 0);
|
|
startTransition(mode, Submodes::DUAL_MODE);
|
|
return;
|
|
}
|
|
} else if (submode == Submodes::B_SIDE) {
|
|
if (healthHelper.healthTable->getHealth(helper.healthDevGps0) == FAULTY or
|
|
healthHelper.healthTable->getHealth(helper.healthDevGps0) == PERMANENT_FAULTY) {
|
|
triggerEvent(DIRECT_TRANSITION_TO_DUAL_OTHER_GPS_FAULTY, submode, 0);
|
|
startTransition(mode, Submodes::DUAL_MODE);
|
|
return;
|
|
}
|
|
}
|
|
DualLaneAssemblyBase::handleChildrenLostMode(result);
|
|
}
|