From 165a4ef814ee659b0eaa57552483da6cbd472a73 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 22 Apr 2022 10:28:29 +0200 Subject: [PATCH] continued RW Assembly --- linux/devices/GPSHyperionLinuxController.cpp | 15 +- .../devicedefinitions/PlocMPSoCDefinitions.h | 28 +-- linux/devices/ploc/PlocMPSoCHandler.cpp | 7 +- linux/devices/ploc/PlocMPSoCHandler.h | 2 +- mission/system/RwAssembly.cpp | 184 +++++++++++++++++- mission/system/RwAssembly.h | 39 +++- mission/system/TcsBoardAssembly.cpp | 1 - mission/system/TcsBoardAssembly.h | 3 +- 8 files changed, 244 insertions(+), 35 deletions(-) diff --git a/linux/devices/GPSHyperionLinuxController.cpp b/linux/devices/GPSHyperionLinuxController.cpp index 42bc43a6..9c762842 100644 --- a/linux/devices/GPSHyperionLinuxController.cpp +++ b/linux/devices/GPSHyperionLinuxController.cpp @@ -110,12 +110,12 @@ void GPSHyperionLinuxController::readGpsDataFromGpsd() { // The data from the device will generally be read all at once. Therefore, we // can set all field here if (not myGpsmm.is_open()) { - if(gpsNotOpenSwitch) { + if (gpsNotOpenSwitch) { // Opening failed - #if FSFW_VERBOSE_LEVEL >= 1 - sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM failed | " << - "Error " << errno << " | " << gps_errstr(errno) << std::endl; - #endif +#if FSFW_VERBOSE_LEVEL >= 1 + sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM failed | " + << "Error " << errno << " | " << gps_errstr(errno) << std::endl; +#endif gpsNotOpenSwitch = false; } @@ -124,9 +124,10 @@ void GPSHyperionLinuxController::readGpsDataFromGpsd() { gps_data_t *gps = nullptr; gps = myGpsmm.read(); if (gps == nullptr) { - if(gpsReadFailedSwitch) { + if (gpsReadFailedSwitch) { gpsReadFailedSwitch = false; - sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed" << std::endl; + sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed" + << std::endl; } return; } diff --git a/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h b/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h index fc7fa185..23389208 100644 --- a/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h +++ b/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h @@ -643,24 +643,24 @@ class TcModeIdle : public TcBase { }; class TcCamcmdSend : public TcBase { -public: + public: TcCamcmdSend(uint16_t sequenceCount) : TcBase(apid::TC_CAM_CMD_SEND, sequenceCount) {} -protected: - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) { - if (commandDataLen > MAX_DATA_LENGTH) { - return INVALID_LENGTH; - } - std::memcpy(this->getPacketData(), commandData, commandDataLen); - *(this->getPacketData() + commandDataLen) = CARRIAGE_RETURN; - uint16_t trueLength = commandDataLen + sizeof(CARRIAGE_RETURN) + CRC_SIZE; - this->setPacketDataLength(trueLength - 1); - return HasReturnvaluesIF::RETURN_OK; + protected: + ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) { + if (commandDataLen > MAX_DATA_LENGTH) { + return INVALID_LENGTH; } -private: - static const uint8_t MAX_DATA_LENGTH = 10; - static const uint8_t CARRIAGE_RETURN = 0xD; + std::memcpy(this->getPacketData(), commandData, commandDataLen); + *(this->getPacketData() + commandDataLen) = CARRIAGE_RETURN; + uint16_t trueLength = commandDataLen + sizeof(CARRIAGE_RETURN) + CRC_SIZE; + this->setPacketDataLength(trueLength - 1); + return HasReturnvaluesIF::RETURN_OK; + } + private: + static const uint8_t MAX_DATA_LENGTH = 10; + static const uint8_t CARRIAGE_RETURN = 0xD; }; } // namespace mpsoc diff --git a/linux/devices/ploc/PlocMPSoCHandler.cpp b/linux/devices/ploc/PlocMPSoCHandler.cpp index 07a658bf..52fd5812 100644 --- a/linux/devices/ploc/PlocMPSoCHandler.cpp +++ b/linux/devices/ploc/PlocMPSoCHandler.cpp @@ -255,7 +255,7 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() { this->insertInReplyMap(mpsoc::ACK_REPORT, 3, nullptr, mpsoc::SIZE_ACK_REPORT); this->insertInReplyMap(mpsoc::EXE_REPORT, 3, nullptr, mpsoc::SIZE_EXE_REPORT); this->insertInReplyMap(mpsoc::TM_MEMORY_READ_REPORT, 2, nullptr, mpsoc::SIZE_TM_MEM_READ_REPORT); - this->insertInReplyMap(mpsoc::TM_CAM_CMD_RPT, 2, nullptr, SpacePacket::PACKET_MAX_SIZE); + this->insertInReplyMap(mpsoc::TM_CAM_CMD_RPT, 2, nullptr, SpacePacket::PACKET_MAX_SIZE); } ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remainingSize, @@ -653,8 +653,9 @@ ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) { sif::warning << "PlocMPSoCHandler::handleCamCmdRpt: CRC failure" << std::endl; } const uint8_t* dataFieldPtr = data + mpsoc::SPACE_PACKET_HEADER_SIZE; - std::string camCmdRptMsg(reinterpret_cast( - dataFieldPtr), tmCamCmdRpt.rememberSpacePacketSize - mpsoc::SPACE_PACKET_HEADER_SIZE - 3); + std::string camCmdRptMsg( + reinterpret_cast(dataFieldPtr), + tmCamCmdRpt.rememberSpacePacketSize - mpsoc::SPACE_PACKET_HEADER_SIZE - 3); uint8_t ackValue = *(packet.getPacketData() + packet.getPacketDataLength() - 2); sif::info << "CamCmdRpt message: " << camCmdRptMsg << std::endl; sif::info << "CamCmdRpt Ack value: 0x" << std::hex << static_cast(ackValue) diff --git a/linux/devices/ploc/PlocMPSoCHandler.h b/linux/devices/ploc/PlocMPSoCHandler.h index c4af88d5..d5ea231b 100644 --- a/linux/devices/ploc/PlocMPSoCHandler.h +++ b/linux/devices/ploc/PlocMPSoCHandler.h @@ -4,11 +4,11 @@ #include #include "PlocMPSoCHelper.h" -#include "fsfw/tmtcpacket/SpacePacket.h" #include "fsfw/action/CommandActionHelper.h" #include "fsfw/action/CommandsActionsIF.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" #include "fsfw/ipc/QueueFactory.h" +#include "fsfw/tmtcpacket/SpacePacket.h" #include "fsfw/tmtcservices/SourceSequenceCounter.h" #include "fsfw_hal/linux/gpio/Gpio.h" #include "fsfw_hal/linux/uart/UartComIF.h" diff --git a/mission/system/RwAssembly.cpp b/mission/system/RwAssembly.cpp index 36455c23..2b99759f 100644 --- a/mission/system/RwAssembly.cpp +++ b/mission/system/RwAssembly.cpp @@ -2,6 +2,186 @@ RwAssembly::RwAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, power::Switch_t switcher, RwHelper helper) - : AssemblyBase(objectId, parentId), helper(helper) { - + : AssemblyBase(objectId, parentId), helper(helper), switcher(pwrSwitcher, switcher) { + ModeListEntry entry; + for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) { + entry.setObject(helper.rwIds[idx]); + entry.setMode(MODE_OFF); + entry.setSubmode(SUBMODE_NONE); + entry.setInheritSubmode(false); + modeTable.insert(entry); + } +} + +void RwAssembly::performChildOperation() { + auto state = switcher.getState(); + if (state != PowerSwitcher::WAIT_OFF and state != PowerSwitcher::WAIT_ON) { + AssemblyBase::performChildOperation(); + return; + } + switcher.doStateMachine(); + if (state == PowerSwitcher::WAIT_OFF and switcher.getState() == PowerSwitcher::SWITCH_IS_OFF) { + // Indicator that a transition to off is finished + AssemblyBase::handleModeReached(); + } else if (state == PowerSwitcher::WAIT_ON and + switcher.getState() == PowerSwitcher::SWITCH_IS_ON) { + // Indicator that mode commanding can be performed now + AssemblyBase::startTransition(targetMode, targetSubmode); + } +} + +ReturnValue_t RwAssembly::commandChildren(Mode_t mode, Submode_t submode) { + ReturnValue_t result = RETURN_OK; + // Initialize the mode table to ensure all devices are in a defined state + for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) { + modeTable[idx].setMode(MODE_OFF); + modeTable[idx].setSubmode(SUBMODE_NONE); + } + if (recoveryState != RecoveryState::RECOVERY_STARTED) { + if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) { + result = handleNormalOrOnModeCmd(mode, submode); + } + } + HybridIterator tableIter(modeTable.begin(), modeTable.end()); + executeTable(tableIter); + return result; +} + +ReturnValue_t RwAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) { + int devsInCorrectMode = 0; + try { + for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) { + if (childrenMap.at(helper.rwIds[idx]).mode != wantedMode) { + devsInCorrectMode++; + } + } + } catch (const std::out_of_range& e) { + sif::error << "RwAssembly: Invalid children map: " << e.what() << std::endl; + } + if (devsInCorrectMode < 3) { + if (warningSwitch) { + sif::warning << "RwAssembly::checkChildrenStateOn: Only " << devsInCorrectMode + << " devices in correct mode" << std::endl; + warningSwitch = false; + } + return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; + } + return RETURN_OK; +} + +ReturnValue_t RwAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) { + if (mode == MODE_ON or mode == MODE_OFF or mode == DeviceHandlerIF::MODE_NORMAL) { + return RETURN_OK; + } + return HasModesIF::INVALID_MODE; +} + +void RwAssembly::startTransition(Mode_t mode, Submode_t submode) { + if (mode != MODE_OFF) { + switcher.turnOn(true); + switcher.doStateMachine(); + if (switcher.getState() == PowerSwitcher::SWITCH_IS_ON) { + AssemblyBase::startTransition(mode, submode); + } else { + // Need to wait with mode commanding until power switcher is done + targetMode = mode; + targetSubmode = submode; + } + } else { + // Perform regular mode commanding first + AssemblyBase::startTransition(mode, submode); + } +} + +void RwAssembly::handleModeReached() { + if (targetMode == MODE_OFF) { + switcher.turnOff(true); + switcher.doStateMachine(); + // Need to wait with call to AssemblyBase::handleModeReached until power switcher is done + if (switcher.getState() == PowerSwitcher::SWITCH_IS_OFF) { + AssemblyBase::handleModeReached(); + } + } else { + AssemblyBase::handleModeReached(); + } +} + +void RwAssembly::handleChildrenLostMode(ReturnValue_t result) { + AssemblyBase::handleChildrenLostMode(result); +} + +ReturnValue_t RwAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) { + ReturnValue_t result = RETURN_OK; + bool needsSecondStep = false; + Mode_t devMode = 0; + object_id_t objId = 0; + try { + for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) { + devMode = childrenMap.at(helper.rwIds[idx]).mode; + objId = helper.rwIds[idx]; + if (mode == devMode) { + modeTable[idx].setMode(mode); + } else if (mode == DeviceHandlerIF::MODE_NORMAL) { + if (isUseable(objId, devMode)) { + if (devMode == MODE_ON) { + modeTable[idx].setMode(mode); + modeTable[idx].setSubmode(SUBMODE_NONE); + } else { + modeTable[idx].setMode(MODE_ON); + modeTable[idx].setSubmode(SUBMODE_NONE); + if (internalState != STATE_SECOND_STEP) { + needsSecondStep = true; + } + } + } + } else if (mode == MODE_ON) { + if (isUseable(objId, devMode)) { + modeTable[idx].setMode(MODE_ON); + modeTable[idx].setSubmode(SUBMODE_NONE); + } + } + } + } catch (const std::out_of_range& e) { + sif::error << "TcsBoardAssembly: Invalid children map: " << e.what() << std::endl; + } + if (needsSecondStep) { + result = NEED_SECOND_STEP; + } + return result; +} + +bool RwAssembly::isUseable(object_id_t object, Mode_t mode) { + if (healthHelper.healthTable->isFaulty(object)) { + return false; + } + + // Check if device is already in target mode + if (childrenMap[object].mode == mode) { + return true; + } + + if (healthHelper.healthTable->isCommandable(object)) { + return true; + } + return false; +} + +ReturnValue_t RwAssembly::initialize() { + ReturnValue_t result = RETURN_OK; + for (const auto& obj : helper.rwIds) { + result = registerChild(obj); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + } + return SubsystemBase::initialize(); +} + +void RwAssembly::handleModeTransitionFailed(ReturnValue_t result) { + if (targetMode == MODE_OFF) { + AssemblyBase::handleModeTransitionFailed(result); + } else { + // To avoid transitioning back to off + triggerEvent(MODE_TRANSITION_FAILED, result); + } } diff --git a/mission/system/RwAssembly.h b/mission/system/RwAssembly.h index e8705ed1..1e94c431 100644 --- a/mission/system/RwAssembly.h +++ b/mission/system/RwAssembly.h @@ -2,6 +2,7 @@ #define MISSION_SYSTEM_RWASS_H_ #include +#include struct RwHelper { RwHelper(std::array rwIds) : rwIds(rwIds) {} @@ -9,13 +10,41 @@ struct RwHelper { std::array rwIds = {}; }; -class RwAssembly: public AssemblyBase { -public: +class RwAssembly : public AssemblyBase { + public: RwAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, - power::Switch_t switcher, RwHelper helper); -private: + power::Switch_t switcher, RwHelper helper); + + private: + static constexpr uint8_t NUMBER_RWS = 4; RwHelper helper; - PowerSwitcher pwrSwitcher; + PowerSwitcher switcher; + bool warningSwitch = true; + FixedArrayList modeTable; + + ReturnValue_t initialize() override; + + ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode); + /** + * Check whether it makes sense to send mode commands to the device + * @param object + * @param mode + * @return + */ + bool isUseable(object_id_t object, Mode_t mode); + + // AssemblyBase implementation + void performChildOperation() override; + ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override; + ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override; + ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override; + void startTransition(Mode_t mode, Submode_t submode) override; + void handleModeReached() override; + + // These two overrides prevent a transition of the whole assembly back to off just because + // some devices are not working + void handleChildrenLostMode(ReturnValue_t result) override; + void handleModeTransitionFailed(ReturnValue_t result) override; }; #endif /* MISSION_SYSTEM_RWASS_H_ */ diff --git a/mission/system/TcsBoardAssembly.cpp b/mission/system/TcsBoardAssembly.cpp index f011c931..7655603b 100644 --- a/mission/system/TcsBoardAssembly.cpp +++ b/mission/system/TcsBoardAssembly.cpp @@ -32,7 +32,6 @@ void TcsBoardAssembly::performChildOperation() { switcher.getState() == PowerSwitcher::SWITCH_IS_ON) { // Indicator that mode commanding can be performed now AssemblyBase::startTransition(targetMode, targetSubmode); - // AssemblyBase::performChildOperation(); } } diff --git a/mission/system/TcsBoardAssembly.h b/mission/system/TcsBoardAssembly.h index 832f335d..c23fbd7c 100644 --- a/mission/system/TcsBoardAssembly.h +++ b/mission/system/TcsBoardAssembly.h @@ -21,8 +21,6 @@ class TcsBoardAssembly : public AssemblyBase, public ConfirmsFailuresIF { ReturnValue_t initialize() override; - void performChildOperation() override; - private: static constexpr uint8_t NUMBER_RTDS = 16; PowerSwitcher switcher; @@ -44,6 +42,7 @@ class TcsBoardAssembly : public AssemblyBase, public ConfirmsFailuresIF { MessageQueueId_t getEventReceptionQueue() override; // AssemblyBase implementation + void performChildOperation() override; ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override; ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override; ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;