From 120153e9c72ac71c19c5f2a4f0d869caaae18ca5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Mar 2022 00:55:51 +0100 Subject: [PATCH] added some missing acs board ass code --- common/config/commonSubsystemIds.h | 1 + fsfw | 2 +- mission/system/AcsBoardAssembly.cpp | 121 +++++++++++++++++++++++++--- mission/system/AcsBoardAssembly.h | 28 +++++-- 4 files changed, 133 insertions(+), 19 deletions(-) diff --git a/common/config/commonSubsystemIds.h b/common/config/commonSubsystemIds.h index 9f293ec6..554f4812 100644 --- a/common/config/commonSubsystemIds.h +++ b/common/config/commonSubsystemIds.h @@ -20,6 +20,7 @@ enum: uint8_t { PDEC_HANDLER = 119, STR_HELPER = 120, PL_PCDU_HANDLER = 121, + ACS_BOARD_ASS = 122, COMMON_SUBSYSTEM_ID_END }; } diff --git a/fsfw b/fsfw index 75c56280..4e6c1cb7 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 75c56280ad139640d2c12ac4ab78ce66c25fb495 +Subproject commit 4e6c1cb72ad623208ea8e08349f6ab39bfa45ed0 diff --git a/mission/system/AcsBoardAssembly.cpp b/mission/system/AcsBoardAssembly.cpp index 0dd2f99c..7cecf66b 100644 --- a/mission/system/AcsBoardAssembly.cpp +++ b/mission/system/AcsBoardAssembly.cpp @@ -1,16 +1,20 @@ #include "AcsBoardAssembly.h" +#include #include #include AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId, - PowerSwitchIF* switcher, AcsBoardHelper helper) - : AssemblyBase(objectId, parentId), pwrSwitcher(switcher), helper(helper) { + PowerSwitchIF* switcher, AcsBoardHelper helper, GpioIF* gpioIF) + : AssemblyBase(objectId, parentId), pwrSwitcher(switcher), 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); initModeTableEntry(helper.mgm1Rm3100IdSideA, entry); @@ -59,27 +63,34 @@ ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) { refreshHelperModes(); - if (wantedSubmode == A_SIDE) { + if (submode == A_SIDE) { if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode) or (helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode) or helper.gpsMode != wantedMode) { + submode = B_SIDE; return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; } return RETURN_OK; - } else if (wantedSubmode == B_SIDE) { + } else if (submode == B_SIDE) { if ((helper.gyro2SideBMode != wantedMode and helper.gyro3SideBMode != wantedMode) or (helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or helper.gpsMode != wantedMode) { + submode = DUAL_MODE; return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; } return RETURN_OK; - } else if (wantedSubmode == DUAL_MODE) { + } else if (submode == 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 != wantedMode) { - return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; + // 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 RETURN_OK; } return RETURN_OK; } @@ -107,13 +118,20 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s } } }; - switch (submode) { + switch (this->submode) { case (A_SIDE): { 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.gpsId, helper.gpsMode, ModeTableIdx::GPS); + ReturnValue_t result = gpioIF->pullLow(gpioIds::GNSS_SELECT); + if (result != HasReturnvaluesIF::RETURN_OK) { +#if OBSW_VERBOSE_LEVEL >= 1 + sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select low" + << std::endl; +#endif + } modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF); modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE); modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF); @@ -130,6 +148,13 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B); cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B); cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS); + gpioIF->pullHigh(gpioIds::GNSS_SELECT); + if (result != HasReturnvaluesIF::RETURN_OK) { +#if OBSW_VERBOSE_LEVEL >= 1 + sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select high" + << std::endl; +#endif + } modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF); modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE); modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF); @@ -150,6 +175,18 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s 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) { + result = gpioIF->pullLow(gpioIds::GNSS_SELECT); + } else { + result = gpioIF->pullHigh(gpioIds::GNSS_SELECT); + } + if (result != HasReturnvaluesIF::RETURN_OK) { +#if OBSW_VERBOSE_LEVEL >= 1 + sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select to" + "default side for dual mode" + << std::endl; +#endif + } return result; } default: { @@ -174,7 +211,7 @@ void AcsBoardAssembly::powerStateMachine(Mode_t mode, Submode_t submode) { return; } } else { - switch (submode) { + switch (this->submode) { case (A_SIDE): { if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_OFF) { @@ -307,14 +344,72 @@ bool AcsBoardAssembly::isUseable(object_id_t object, Mode_t mode) { void AcsBoardAssembly::handleModeReached() { AssemblyBase::handleModeReached(); state = States::IDLE; + tryingOtherSide = false; + dualModeErrorSwitch = true; +} + +void AcsBoardAssembly::handleChildrenLostMode(ReturnValue_t result) { + // Some ACS board components are required for Safe-Mode. It would be good if the software + // transitions from A side to B side and from B side to dual mode autonomously + // to ensure that that enough sensors are available without an operators intervention. + // Therefore, the lost mode handler was overwritten to start these transitions + Submode_t nextSubmode = Submodes::A_SIDE; + if (submode == Submodes::A_SIDE) { + nextSubmode = Submodes::B_SIDE; + } + if (not tryingOtherSide) { + triggerEvent(CANT_KEEP_MODE, mode, submode); + startTransition(mode, nextSubmode); + tryingOtherSide = true; + } else { + // Not sure when this would happen. This flag is reset if the mode was reached. If it + // was not reached, the transition failure handler should be called. + sif::error << "AcsBoardAssembly::handleChildrenLostMode: Wrong handler called" << std::endl; + triggerEvent(TRANSITION_OTHER_SIDE_FAILED, mode, targetSubmode); + startTransition(mode, Submodes::DUAL_MODE); + } } void AcsBoardAssembly::handleModeTransitionFailed(ReturnValue_t result) { - // Some ACS board components are required for Safe-Mode. It would be good if the software - // transitions from A side to B side and from B side to dual mode autonomously - // to ensure that that enough sensors are available witout an operators intervention. - // Therefore, the failure handler is overriden to perform these steps. - // TODO: Implement transitions mentioned above + Submode_t nextSubmode = Submodes::A_SIDE; + if (submode == Submodes::A_SIDE) { + nextSubmode = Submodes::B_SIDE; + } + // Check whether the transition was started because the mode could not be kept (not commanded). + // If this is not the case, start transition to other side. If it is the case, start + // transition to dual mode. + if (not tryingOtherSide) { + triggerEvent(CANT_KEEP_MODE, mode, submode); + startTransition(mode, nextSubmode); + tryingOtherSide = true; + } else { + triggerEvent(TRANSITION_OTHER_SIDE_FAILED, mode, targetSubmode); + startTransition(mode, Submodes::DUAL_MODE); + } +} + +void AcsBoardAssembly::setPreferredSide(Submodes submode) { + if (submode != Submodes::A_SIDE and submode != Submodes::B_SIDE) { + return; + } + this->defaultSubmode = submode; +} + +void AcsBoardAssembly::selectGpsInDualMode(Submodes side) { + if (submode != Submodes::DUAL_MODE) { + return; + } + ReturnValue_t result = RETURN_OK; + if (side == Submodes::A_SIDE) { + result = gpioIF->pullLow(gpioIds::GNSS_SELECT); + } else { + result = gpioIF->pullHigh(gpioIds::GNSS_SELECT); + } + if (result != HasReturnvaluesIF::RETURN_OK) { +#if OBSW_VERBOSE_LEVEL >= 1 + sif::error << "AcsBoardAssembly::switchGpsInDualMode: Switching GPS failed" << std::endl; +#endif + } } void AcsBoardAssembly::refreshHelperModes() { diff --git a/mission/system/AcsBoardAssembly.h b/mission/system/AcsBoardAssembly.h index 3fc4a277..37da581e 100644 --- a/mission/system/AcsBoardAssembly.h +++ b/mission/system/AcsBoardAssembly.h @@ -1,6 +1,7 @@ #ifndef MISSION_SYSTEM_ACSBOARDASSEMBLY_H_ #define MISSION_SYSTEM_ACSBOARDASSEMBLY_H_ +#include #include #include #include @@ -54,17 +55,30 @@ enum ModeTableIdx : uint8_t { }; class PowerSwitchIF; +class GpioIF; class AcsBoardAssembly : public AssemblyBase { public: + static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_BOARD_ASS; + static constexpr Event TRANSITION_OTHER_SIDE_FAILED = + event::makeEvent(SUBSYSTEM_ID, 0, severity::HIGH); + static constexpr Event NOT_ENOUGH_DEVICES_DUAL_MODE = + event::makeEvent(SUBSYSTEM_ID, 1, severity::HIGH); static constexpr uint8_t NUMBER_DEVICES_MODE_TABLE = 9; - static constexpr Submode_t A_SIDE = 0; - static constexpr Submode_t B_SIDE = 1; - static constexpr Submode_t DUAL_MODE = 2; + enum Submodes : Submode_t { A_SIDE = 0, B_SIDE = 1, DUAL_MODE = 2 }; AcsBoardAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, - AcsBoardHelper helper); + AcsBoardHelper helper, GpioIF* gpioIF); + + void setPreferredSide(Submodes submode); + + /** + * In dual mode, the A side or the B side GPS device can be used, but not both. + * This function can be used to switch the used GPS device. + * @param side + */ + void selectGpsInDualMode(Submodes side); private: static constexpr pcduSwitches::Switches SWITCH_A = @@ -75,8 +89,11 @@ class AcsBoardAssembly : public AssemblyBase { enum class States { IDLE, SWITCHING_POWER, MODE_COMMANDING } state = States::IDLE; PowerSwitchIF* pwrSwitcher = nullptr; - + bool tryingOtherSide = false; AcsBoardHelper helper; + GpioIF* gpioIF = nullptr; + Submodes defaultSubmode = Submodes::A_SIDE; + bool dualModeErrorSwitch = true; FixedArrayList modeTable; ReturnValue_t initialize() override; @@ -87,6 +104,7 @@ class AcsBoardAssembly : public AssemblyBase { ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override; void handleModeReached() override; void handleModeTransitionFailed(ReturnValue_t result) override; + void handleChildrenLostMode(ReturnValue_t result) override; /** * Check whether it makes sense to send mode commands to the device