eive-obsw/mission/system/objects/AcsBoardAssembly.cpp

278 lines
12 KiB
C++
Raw Normal View History

#include "AcsBoardAssembly.h"
2022-03-02 17:56:54 +01:00
2022-03-04 00:55:51 +01:00
#include <devices/gpioIds.h>
2022-03-02 17:56:54 +01:00
#include <fsfw/power/PowerSwitchIF.h>
#include <fsfw/serviceinterface.h>
2022-04-07 19:48:09 +02:00
#include "OBSWConfig.h"
2022-09-29 19:40:00 +02:00
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) {
2022-03-02 17:56:54 +01:00
if (switcher == nullptr) {
sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid Power Switcher "
"IF passed"
<< std::endl;
}
2022-03-04 00:55:51 +01:00
if (gpioIF == nullptr) {
sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid GPIO IF passed" << std::endl;
}
2022-03-02 17:56:54 +01:00
ModeListEntry entry;
2022-03-10 10:54:27 +01:00
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);
2022-03-04 18:12:16 +01:00
}
2022-03-02 17:56:54 +01:00
ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) {
using namespace duallane;
2022-08-24 17:27:47 +02:00
ReturnValue_t result = returnvalue::OK;
2022-03-03 10:28:55 +01:00
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_STARTED) {
if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
result = handleNormalOrOnModeCmd(mode, submode);
}
2022-03-02 17:56:54 +01:00
}
2022-03-07 18:39:33 +01:00
HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
executeTable(tableIter);
2022-03-02 17:56:54 +01:00
return result;
}
ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
using namespace duallane;
2022-03-03 10:28:55 +01:00
refreshHelperModes();
2022-03-04 18:33:23 +01:00
if (wantedSubmode == A_SIDE) {
if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode) or
2022-03-03 10:28:55 +01:00
(helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode) or
helper.gpsMode != MODE_ON) {
2022-03-03 10:28:55 +01:00
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-04 18:33:23 +01:00
} else if (wantedSubmode == B_SIDE) {
if ((helper.gyro2SideBMode != wantedMode and helper.gyro3SideBMode != wantedMode) or
2022-03-03 10:28:55 +01:00
(helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or
helper.gpsMode != MODE_ON) {
2022-03-03 10:28:55 +01:00
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-04 18:33:23 +01:00
} else if (wantedSubmode == DUAL_MODE) {
if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode and
helper.gyro2AdisIdSideB != wantedMode and helper.gyro3SideBMode != wantedMode) or
2022-03-03 10:28:55 +01:00
(helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode and
helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or
helper.gpsMode != MODE_ON) {
2022-03-04 00:55:51 +01:00
// 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;
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-03 10:28:55 +01:00
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-02 17:56:54 +01:00
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-02 17:56:54 +01:00
}
2022-03-03 10:12:59 +01:00
ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
using namespace duallane;
2022-08-24 17:27:47 +02:00
ReturnValue_t result = returnvalue::OK;
bool needsSecondStep = false;
2022-03-03 20:11:12 +01:00
auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, ModeTableIdx tableIdx) {
if (mode == devMode) {
modeTable[tableIdx].setMode(mode);
} else if (mode == DeviceHandlerIF::MODE_NORMAL) {
2022-03-03 20:11:12 +01:00
if (isUseable(objectId, devMode)) {
2022-03-07 14:46:46 +01:00
if (devMode == MODE_ON) {
modeTable[tableIdx].setMode(mode);
2022-03-02 17:56:54 +01:00
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
} else {
2022-03-07 14:46:46 +01:00
modeTable[tableIdx].setMode(MODE_ON);
2022-03-02 17:56:54 +01:00
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
if (internalState != STATE_SECOND_STEP) {
needsSecondStep = true;
}
2022-03-02 17:56:54 +01:00
}
}
2022-03-04 18:33:23 +01:00
} else if (mode == MODE_ON) {
2022-03-03 20:11:12 +01:00
if (isUseable(objectId, devMode)) {
2022-03-03 10:12:59 +01:00
modeTable[tableIdx].setMode(MODE_ON);
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
2022-03-02 17:56:54 +01:00
}
}
2022-03-03 10:12:59 +01:00
};
bool gpsUsable = isUseable(helper.gpsId, helper.gpsMode);
2022-03-04 18:33:23 +01:00
switch (submode) {
2022-03-03 10:12:59 +01:00
case (A_SIDE): {
2022-03-17 10:48:35 +01:00
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);
2022-03-03 20:11:12 +01:00
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);
if (gpsUsable) {
gpioHandler(gpioIds::GNSS_0_NRESET, true,
2022-04-04 13:40:45 +02:00
"AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin"
"of GNSS 0 high (used GNSS)");
gpioHandler(gpioIds::GNSS_1_NRESET, false,
2022-04-04 13:40:45 +02:00
"AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin"
"of GNSS 1 low (unused GNSS)");
gpioHandler(gpioIds::GNSS_SELECT, false,
2022-04-04 13:40:45 +02:00
"AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select low");
2022-03-04 00:55:51 +01:00
}
break;
2022-03-03 10:12:59 +01:00
}
case (B_SIDE): {
2022-03-17 10:48:35 +01:00
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);
2022-03-03 20:11:12 +01:00
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 (gpsUsable) {
gpioHandler(gpioIds::GNSS_0_NRESET, false,
2022-04-04 13:40:45 +02:00
"AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin"
"of GNSS 0 low (unused GNSS)");
gpioHandler(gpioIds::GNSS_1_NRESET, true,
2022-04-04 13:40:45 +02:00
"AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin"
"of GNSS 1 high (used GNSS)");
gpioHandler(gpioIds::GNSS_SELECT, true,
2022-04-04 13:40:45 +02:00
"AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select high");
2022-03-04 00:55:51 +01:00
}
break;
2022-03-03 10:12:59 +01:00
}
case (DUAL_MODE): {
2022-03-03 20:11:12 +01:00
cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
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);
2022-08-24 17:27:47 +02:00
ReturnValue_t status = returnvalue::OK;
if (gpsUsable) {
gpioHandler(gpioIds::GNSS_0_NRESET, true,
2022-04-04 13:40:45 +02:00
"AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin"
"of GNSS 0 high (used GNSS)");
gpioHandler(gpioIds::GNSS_1_NRESET, true,
2022-04-04 13:40:45 +02:00
"AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin"
"of GNSS 1 high (used GNSS)");
if (defaultSubmode == Submodes::A_SIDE) {
status = gpioIF->pullLow(gpioIds::GNSS_SELECT);
} else {
status = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
}
2022-08-24 17:27:47 +02:00
if (status != returnvalue::OK) {
2022-03-04 00:55:51 +01:00
#if OBSW_VERBOSE_LEVEL >= 1
sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select to"
"default side for dual mode"
<< std::endl;
2022-03-04 00:55:51 +01:00
#endif
}
2022-03-04 00:55:51 +01:00
}
break;
2022-03-03 10:12:59 +01:00
}
default: {
sif::error << "AcsBoardAssembly::handleNormalModeCmd: Unknown submode" << std::endl;
}
2022-03-02 17:56:54 +01:00
}
if (gpsUsable) {
modeTable[ModeTableIdx::GPS].setMode(MODE_ON);
modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
2022-03-04 00:55:51 +01:00
}
if (needsSecondStep) {
result = NEED_SECOND_STEP;
2022-03-04 00:55:51 +01:00
}
return result;
2022-03-04 00:55:51 +01:00
}
void AcsBoardAssembly::selectGpsInDualMode(duallane::Submodes side) {
using namespace duallane;
2022-03-04 00:55:51 +01:00
if (submode != Submodes::DUAL_MODE) {
return;
}
2022-08-24 17:27:47 +02:00
ReturnValue_t result = returnvalue::OK;
2022-03-04 00:55:51 +01:00
if (side == Submodes::A_SIDE) {
result = gpioIF->pullLow(gpioIds::GNSS_SELECT);
} else {
result = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
}
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
2022-03-04 00:55:51 +01:00
#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) {
2022-08-24 17:27:47 +02:00
ReturnValue_t result = returnvalue::OK;
2022-04-04 13:40:45 +02:00
if (high) {
result = gpioIF->pullHigh(gpio);
} else {
result = gpioIF->pullLow(gpio);
}
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::error << error << std::endl;
2022-03-04 00:55:51 +01:00
#endif
}
2022-03-03 20:11:12 +01:00
}
2022-03-03 10:28:55 +01:00
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;
}
2022-03-03 10:28:55 +01:00
}
2022-03-03 20:11:12 +01:00
2022-09-30 14:30:30 +02:00
ReturnValue_t AcsBoardAssembly::initialize() { return AssemblyBase::initialize(); }