diff --git a/CHANGELOG.md b/CHANGELOG.md index 6d2e94db..348e7023 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,14 +10,41 @@ list yields a list of all related PRs for each release. # [unreleased] -# [v1.14.1] +- ACS Subsystem. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/231 +- Payload Subsystem. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/231 +- Add IRQ mode for PDEC handler. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/310 +- Extended TM funnels to allow multiple TM recipients. + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/312 +- DHB: Transitions to normal mode now possible directly, which simplifies subsystem implementations + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/313 +- MAX3185 Low Level Handler and Device Handler: Simplifications and bugfixes to allow switching + off without triggering unrequested replies + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/313 + +# [v1.15.0] 27.10.2022 + +- Consistent device file naming +- Remove rad sensor from EM build, lead to weird bugs on EM which + prevented `xsc_boot_copy` from working properly +- CFDP closure handling is now working + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/307 +- Safety mechanism for SD card handling on graceful reboots + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/308 +- Solar Array Deployment handler update + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/305 +- IMTQ updates as preparation for ACS controller expansion + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/306 +- P60 Module: Reduce number of set IDs, use same set IDs for core, auxiliary + and config HK set across the three PCDU modules + +# [v1.14.1] 11.10.2022 - Various bugfixes and regression fixes - General file handling at program initialization now works properly again - Scratch buffer preferred SD card handling works again - Use scoped locks in TCS controller to avoid deadlocks -# [v1.14.0] +# [v1.14.0] 10.10.2022 - Provide full SW update capability for the OBSW. This includes very basic CFDP integration, a software update @@ -36,6 +63,7 @@ list yields a list of all related PRs for each release. - Fix for EM SW: Always create ACS Task - Added Scex device handler and Scex uart reader PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/303 +- ACS Subsystem. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/228 # [v1.13.0] 24.08.2022 diff --git a/CMakeLists.txt b/CMakeLists.txt index 831a7064..910abafe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,8 +10,8 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR_IF_GIT_FAILS 1) -set(OBSW_VERSION_MINOR_IF_GIT_FAILS 14) -set(OBSW_VERSION_REVISION_IF_GIT_FAILS 1) +set(OBSW_VERSION_MINOR_IF_GIT_FAILS 15) +set(OBSW_VERSION_REVISION_IF_GIT_FAILS 0) # set(CMAKE_VERBOSE TRUE) @@ -98,9 +98,30 @@ set(OBSW_ADD_ACS_BOARD set(OBSW_ADD_GPS_CTRL ${INIT_VAL} CACHE STRING "Add GPS controllers") +set(OBSW_ADD_CCSDS_IP_CORES + 1 + CACHE STRING "Add CCSDS IP cores") +set(OBSW_TM_TO_PTME + 1 + CACHE STRING "Send telemetry to PTME IP core") +set(OBSW_TC_FROM_PDEC + 1 + CACHE STRING "Poll telecommand from PDEC IP core") set(OBSW_ADD_TCS_CTRL ${INIT_VAL} CACHE STRING "Add TCS controllers") +set(OBSW_ADD_HEATERS + ${INIT_VAL} + CACHE STRING "Add TCS heaters") +set(OBSW_ADD_PLOC_SUPERVISOR + ${INIT_VAL} + CACHE STRING "Add PLOC supervisor handler") +set(OBSW_ADD_SA_DEPL + ${INIT_VAL} + CACHE STRING "Add SA deployment handler") +set(OBSW_ADD_PLOC_MPSOC + ${INIT_VAL} + CACHE STRING "Add MPSoC handler") set(OBSW_ADD_ACS_CTRL ${INIT_VAL} CACHE STRING "Add ACS controller") @@ -128,6 +149,9 @@ set(OBSW_ADD_RW set(OBSW_ADD_SCEX_DEVICE ${INIT_VAL} CACHE STRING "Add Solar Cell Experiment module") +set(OBSW_SYRLINKS_SIMULATED + ${OBSW_Q7S_EM} + CACHE STRING "Syrlinks is simulated") # ############################################################################## # Pre-Sources preparation diff --git a/bsp_egse/ObjectFactory.cpp b/bsp_egse/ObjectFactory.cpp index 8d5e3416..b13af722 100644 --- a/bsp_egse/ObjectFactory.cpp +++ b/bsp_egse/ObjectFactory.cpp @@ -1,8 +1,8 @@ #include "ObjectFactory.h" #include -#include -#include +#include +#include #include "OBSWConfig.h" #include "busConf.h" @@ -39,7 +39,7 @@ void ObjectFactory::produce(void* args) { UartCookie* starTrackerCookie = new UartCookie(objects::STAR_TRACKER, egse::STAR_TRACKER_UART, UartModes::NON_CANONICAL, uart::STAR_TRACKER_BAUD, startracker::MAX_FRAME_SIZE * 2 + 2); - new UartComIF(objects::UART_COM_IF); + newSerialComIF(objects::UART_COM_IF); starTrackerCookie->setNoFixedSizeReply(); StrHelper* strHelper = new StrHelper(objects::STR_HELPER); StarTrackerHandler* starTrackerHandler = new StarTrackerHandler( diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index f4607de4..a3c94fe1 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include #include @@ -59,12 +59,14 @@ void Factory::setStaticFrameworkObjectIds() { void ObjectFactory::produce(void* args) { Factory::setStaticFrameworkObjectIds(); - ObjectFactory::produceGenericObjects(); + PusTmFunnel* pusFunnel; + CfdpTmFunnel* cfdpFunnel; + ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel); dummy::DummyCfg cfg; dummy::createDummies(cfg); new TemperatureSensorsDummy(); new SusDummy(); - new ThermalController(objects::THERMAL_CONTROLLER, objects::NO_OBJECT); + new ThermalController(objects::THERMAL_CONTROLLER); new TestTask(objects::TEST_TASK); } diff --git a/bsp_hosted/fsfwconfig/OBSWConfig.h.in b/bsp_hosted/fsfwconfig/OBSWConfig.h.in index 69029c23..b7de30f0 100644 --- a/bsp_hosted/fsfwconfig/OBSWConfig.h.in +++ b/bsp_hosted/fsfwconfig/OBSWConfig.h.in @@ -16,7 +16,7 @@ debugging. */ #define OBSW_VEBOSE_LEVEL 1 -#define OBSW_USE_CCSDS_IP_CORE 0 +#define OBSW_ADD_CCSDS_IP_CORES 0 // Set to 1 if all telemetry should be sent to the PTME IP Core #define OBSW_TM_TO_PTME 0 // Set to 1 if telecommands are received via the PDEC IP Core diff --git a/bsp_linux_board/OBSWConfig.h.in b/bsp_linux_board/OBSWConfig.h.in index f17c3d48..04b90ed9 100644 --- a/bsp_linux_board/OBSWConfig.h.in +++ b/bsp_linux_board/OBSWConfig.h.in @@ -103,7 +103,7 @@ /*******************************************************************/ #cmakedefine EIVE_BUILD_GPSD_GPS_HANDLER -#define OBSW_USE_CCSDS_IP_CORE 0 +#define OBSW_ADD_CCSDS_IP_CORES 0 // Set to 1 if all telemetry should be sent to the PTME IP Core #define OBSW_TM_TO_PTME 0 // Set to 1 if telecommands are received via the PDEC IP Core diff --git a/bsp_linux_board/ObjectFactory.cpp b/bsp_linux_board/ObjectFactory.cpp index 8ebcdc8c..53db052c 100644 --- a/bsp_linux_board/ObjectFactory.cpp +++ b/bsp_linux_board/ObjectFactory.cpp @@ -34,8 +34,8 @@ #include "fsfw/osal/common/UdpTmTcBridge.h" #endif -#include -#include +#include +#include #include "fsfw_hal/common/gpio/GpioCookie.h" #include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h" @@ -202,7 +202,7 @@ void ObjectFactory::createTestTasks() { #if OBSW_ADD_UART_TEST_CODE == 1 new UartTestClass(objects::UART_TEST); #else - new UartComIF(objects::UART_COM_IF); + newSerialComIF(objects::UART_COM_IF); #endif #if RPI_LOOPBACK_TEST_GPIO == 1 diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index f3cfa907..840e2575 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -14,18 +14,12 @@ /** All of the following flags should be enabled for mission code */ /*******************************************************************/ -#define OBSW_USE_CCSDS_IP_CORE 1 -// Set to 1 if all telemetry should be sent to the PTME IP Core -#define OBSW_TM_TO_PTME 0 -// Set to 1 if telecommands are received via the PDEC IP Core -#define OBSW_TC_FROM_PDEC 0 - #define OBSW_ADD_GOMSPACE_PCDU @OBSW_ADD_GOMSPACE_PCDU@ #define OBSW_ADD_MGT @OBSW_ADD_MGT@ #define OBSW_ADD_BPX_BATTERY_HANDLER @OBSW_ADD_BPX_BATTERY_HANDLER@ #define OBSW_ADD_STAR_TRACKER @OBSW_ADD_STAR_TRACKER@ -#define OBSW_ADD_PLOC_SUPERVISOR 1 -#define OBSW_ADD_PLOC_MPSOC 1 +#define OBSW_ADD_PLOC_SUPERVISOR @OBSW_ADD_PLOC_SUPERVISOR@ +#define OBSW_ADD_PLOC_MPSOC @OBSW_ADD_PLOC_MPSOC@ #define OBSW_ADD_SUN_SENSORS @OBSW_ADD_SUN_SENSORS@ #define OBSW_ADD_SUS_BOARD_ASS @OBSW_ADD_SUS_BOARD_ASS@ #define OBSW_ADD_ACS_BOARD @OBSW_ADD_ACS_BOARD@ @@ -34,11 +28,18 @@ #define OBSW_ADD_TCS_CTRL @OBSW_ADD_TCS_CTRL@ #define OBSW_ADD_RW @OBSW_ADD_RW@ #define OBSW_ADD_RTD_DEVICES @OBSW_ADD_RTD_DEVICES@ +#define OBSW_ADD_SA_DEPL @OBSW_ADD_SA_DEPL@ #define OBSW_ADD_SCEX_DEVICE @OBSW_ADD_SCEX_DEVICE@ +#define OBSW_ADD_HEATERS @OBSW_ADD_HEATERS@ #define OBSW_ADD_TMP_DEVICES @OBSW_ADD_TMP_DEVICES@ #define OBSW_ADD_RAD_SENSORS @OBSW_ADD_RAD_SENSORS@ #define OBSW_ADD_PL_PCDU @OBSW_ADD_PL_PCDU@ #define OBSW_ADD_SYRLINKS @OBSW_ADD_SYRLINKS@ +#define OBSW_ADD_CCSDS_IP_CORES @OBSW_ADD_CCSDS_IP_CORES@ +// Set to 1 if all telemetry should be sent to the PTME IP Core +#define OBSW_TM_TO_PTME @OBSW_TM_TO_PTME@ +// Set to 1 if telecommands are received via the PDEC IP Core +#define OBSW_TC_FROM_PDEC @OBSW_TC_FROM_PDEC@ #define OBSW_ENABLE_SYRLINKS_TRANSMIT_TIMEOUT 0 // Configuration parameter which causes the core controller to try to keep at least one SD card @@ -59,12 +60,12 @@ /*******************************************************************/ // Can be used to switch device to NORMAL mode immediately -#define OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP 1 +#define OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP 0 #define OBSW_PRINT_MISSED_DEADLINES 1 #define OBSW_MPSOC_JTAG_BOOT 0 #define OBSW_STAR_TRACKER_GROUND_CONFIG 1 -#define OBSW_SYRLINKS_SIMULATED 1 +#define OBSW_SYRLINKS_SIMULATED @OBSW_SYRLINKS_SIMULATED@ #define OBSW_ADD_TEST_CODE 0 #define OBSW_ADD_TEST_TASK 0 #define OBSW_ADD_TEST_PST 0 diff --git a/bsp_q7s/boardconfig/busConf.h b/bsp_q7s/boardconfig/busConf.h index 4ccdc78d..1db53d3e 100644 --- a/bsp_q7s/boardconfig/busConf.h +++ b/bsp_q7s/boardconfig/busConf.h @@ -3,24 +3,25 @@ namespace q7s { -static constexpr char SPI_DEFAULT_DEV[] = "/dev/spi-main"; +static constexpr char SPI_DEFAULT_DEV[] = "/dev/spi_main"; static constexpr uint32_t SPI_MAIN_BUS_LOCK_TIMEOUT = 50; -static constexpr char SPI_RW_DEV[] = "/dev/spi-rw"; +static constexpr char SPI_RW_DEV[] = "/dev/spi_rw"; -static constexpr char I2C_DEFAULT_DEV[] = "/dev/i2c-eive"; +static constexpr char I2C_DEFAULT_DEV[] = "/dev/i2c_eive"; static constexpr char UART_GNSS_DEV[] = "/dev/gps0"; -static constexpr char UART_PLOC_MPSOC_DEV[] = "/dev/ul-plmpsoc"; -static constexpr char UART_PLOC_SUPERVSIOR_DEV[] = "/dev/ul-plsv"; -static constexpr char UART_SYRLINKS_DEV[] = "/dev/ul-syrlinks"; -static constexpr char UART_STAR_TRACKER_DEV[] = "/dev/ul-str"; -static constexpr char UART_SCEX_DEV[] = "/dev/ttyS-SCEX"; +static constexpr char UART_PLOC_MPSOC_DEV[] = "/dev/ul_plmpsoc"; +static constexpr char UART_PLOC_SUPERVSIOR_DEV[] = "/dev/ul_plsv"; +static constexpr char UART_SYRLINKS_DEV[] = "/dev/ul_syrlinks"; +static constexpr char UART_STAR_TRACKER_DEV[] = "/dev/ul_str"; +static constexpr char UART_SCEX_DEV[] = "/dev/scex"; -static constexpr char UIO_PDEC_REGISTERS[] = "/dev/uio0"; -static constexpr char UIO_PTME[] = "/dev/uio1"; -static constexpr char UIO_PDEC_CONFIG_MEMORY[] = "/dev/uio2"; -static constexpr char UIO_PDEC_RAM[] = "/dev/uio3"; +static constexpr char UIO_PDEC_REGISTERS[] = "/dev/uio_pdec_regs"; +static constexpr char UIO_PTME[] = "/dev/uio_ptme"; +static constexpr char UIO_PDEC_CONFIG_MEMORY[] = "/dev/uio_pdec_cfg_mem"; +static constexpr char UIO_PDEC_RAM[] = "/dev/uio_pdec_ram"; +static constexpr char UIO_PDEC_IRQ[] = "/dev/uio_pdec_irq"; static constexpr int MAP_ID_PTME_CONFIG = 3; namespace uiomapids { diff --git a/bsp_q7s/boardtest/Q7STestTask.cpp b/bsp_q7s/boardtest/Q7STestTask.cpp index a782ff6d..50a34284 100644 --- a/bsp_q7s/boardtest/Q7STestTask.cpp +++ b/bsp_q7s/boardtest/Q7STestTask.cpp @@ -75,8 +75,10 @@ ReturnValue_t Q7STestTask::performOneShotAction() { if (doTestProtHandler) { testProtHandler(); } - FsOpCodes opCode = FsOpCodes::CREATE_EMPTY_FILE_IN_TMP; - testFileSystemHandlerDirect(opCode); + if (DO_TEST_FS_HANDLER) { + FsOpCodes opCode = FsOpCodes::CREATE_EMPTY_FILE_IN_TMP; + testFileSystemHandlerDirect(opCode); + } return TestTask::performOneShotAction(); } diff --git a/bsp_q7s/boardtest/Q7STestTask.h b/bsp_q7s/boardtest/Q7STestTask.h index 9e39e8b3..dcfc3e96 100644 --- a/bsp_q7s/boardtest/Q7STestTask.h +++ b/bsp_q7s/boardtest/Q7STestTask.h @@ -18,6 +18,7 @@ class Q7STestTask : public TestTask { bool doTestScratchApi = false; static constexpr bool DO_TEST_GOMSPACE_API = false; static constexpr bool DO_TEST_GOMSPACE_GET_CONFIG = false; + static constexpr bool DO_TEST_FS_HANDLER = false; bool doTestGpsShm = false; bool doTestGpsSocket = false; bool doTestProtHandler = false; diff --git a/bsp_q7s/callbacks/rwSpiCallback.cpp b/bsp_q7s/callbacks/rwSpiCallback.cpp index ae23d64d..74ede602 100644 --- a/bsp_q7s/callbacks/rwSpiCallback.cpp +++ b/bsp_q7s/callbacks/rwSpiCallback.cpp @@ -41,19 +41,19 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen uint8_t writeSize = 0; gpioId_t gpioId = cookie->getChipSelectPin(); - GpioIF* gpioIF = comIf->getGpioInterface(); + GpioIF& gpioIF = comIf->getGpioInterface(); MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; uint32_t timeoutMs = 0; MutexIF* mutex = comIf->getCsMutex(); cookie->getMutexParams(timeoutType, timeoutMs); - if (mutex == nullptr or gpioIF == nullptr) { + if (mutex == nullptr) { sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl; return returnvalue::FAILED; } int fileDescriptor = 0; const std::string& dev = comIf->getSpiDev(); - result = openSpi(dev, O_RDWR, gpioIF, gpioId, mutex, timeoutType, timeoutMs, fileDescriptor); + result = openSpi(dev, O_RDWR, &gpioIF, gpioId, mutex, timeoutType, timeoutMs, fileDescriptor); if (result != returnvalue::OK) { return result; } @@ -75,7 +75,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (write(fileDescriptor, writeBuffer, writeSize) != static_cast(writeSize)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; - closeSpi(fileDescriptor, gpioId, gpioIF, mutex); + closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); return RwHandler::SPI_WRITE_FAILURE; } @@ -100,7 +100,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen } if (write(fileDescriptor, writeBuffer, writeSize) != static_cast(writeSize)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; - closeSpi(fileDescriptor, gpioId, gpioIF, mutex); + closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); return RwHandler::SPI_WRITE_FAILURE; } idx++; @@ -112,14 +112,14 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (write(fileDescriptor, writeBuffer, writeSize) != static_cast(writeSize)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; - closeSpi(fileDescriptor, gpioId, gpioIF, mutex); + closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); return RwHandler::SPI_WRITE_FAILURE; } uint8_t* rxBuf = nullptr; result = comIf->getReadBuffer(cookie->getSpiAddress(), &rxBuf); if (result != returnvalue::OK) { - closeSpi(fileDescriptor, gpioId, gpioIF, mutex); + closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); return result; } @@ -127,9 +127,9 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen // There must be a delay of at least 20 ms after sending the command. // Delay for 70 ms here and release the SPI bus for that duration. - closeSpi(fileDescriptor, gpioId, gpioIF, mutex); + closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); usleep(RwDefinitions::SPI_REPLY_DELAY); - result = openSpi(dev, O_RDWR, gpioIF, gpioId, mutex, timeoutType, timeoutMs, fileDescriptor); + result = openSpi(dev, O_RDWR, &gpioIF, gpioId, mutex, timeoutType, timeoutMs, fileDescriptor); if (result != returnvalue::OK) { return result; } @@ -142,13 +142,13 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen for (int idx = 0; idx < 10; idx++) { if (read(fileDescriptor, &byteRead, 1) != 1) { sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; - closeSpi(fileDescriptor, gpioId, gpioIF, mutex); + closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); return RwHandler::SPI_READ_FAILURE; } if (idx == 0) { if (byteRead != FLAG_BYTE) { sif::error << "Invalid data, expected start marker" << std::endl; - closeSpi(fileDescriptor, gpioId, gpioIF, mutex); + closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); return RwHandler::NO_START_MARKER; } } @@ -159,7 +159,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (idx == 9) { sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl; - closeSpi(fileDescriptor, gpioId, gpioIF, mutex); + closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); return RwHandler::NO_REPLY; } } @@ -199,7 +199,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen continue; } else { sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl; - closeSpi(fileDescriptor, gpioId, gpioIF, mutex); + closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); result = RwHandler::INVALID_SUBSTITUTE; break; } @@ -233,7 +233,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen cookie->setTransferSize(decodedFrameLen); - closeSpi(fileDescriptor, gpioId, gpioIF, mutex); + closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); return result; } diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 95700148..05660705 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include "OBSWVersion.h" #include "fsfw/serviceinterface/ServiceInterface.h" @@ -30,10 +31,7 @@ xsc::Chip CoreController::CURRENT_CHIP = xsc::Chip::NO_CHIP; xsc::Copy CoreController::CURRENT_COPY = xsc::Copy::NO_COPY; CoreController::CoreController(object_id_t objectId) - : ExtendedControllerBase(objectId, objects::NO_OBJECT, 5), - opDivider5(5), - opDivider10(10), - hkSet(this) { + : ExtendedControllerBase(objectId, 5), opDivider5(5), opDivider10(10), hkSet(this) { ReturnValue_t result = returnvalue::OK; try { result = initWatchdogFifo(); @@ -49,10 +47,7 @@ CoreController::CoreController(object_id_t objectId) sdcMan->setBlocking(false); } - result = initBootCopy(); - if (result != returnvalue::OK) { - sif::warning << "CoreController::CoreController: Boot copy init" << std::endl; - } + getCurrentBootCopy(CURRENT_CHIP, CURRENT_COPY); } catch (const std::filesystem::filesystem_error &e) { sif::error << "CoreController::CoreController: Failed with exception " << e.what() << std::endl; } @@ -81,6 +76,12 @@ void CoreController::performControlOperation() { sdStateMachine(); performMountedSdCardOperations(); if (sdCardCheckCd.hasTimedOut()) { + if (shortSdCardCdCounter < 2) { + shortSdCardCdCounter++; + } + if (shortSdCardCdCounter == 2) { + sdCardCheckCd.setTimeout(DEFAULT_SD_CARD_CHECK_TIMEOUT); + } performSdCardCheck(); sdCardCheckCd.resetTimer(); } @@ -91,9 +92,9 @@ void CoreController::performControlOperation() { ReturnValue_t CoreController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(core::TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(core::PS_VOLTAGE, new PoolEntry({0})); - localDataPoolMap.emplace(core::PL_VOLTAGE, new PoolEntry({0})); + localDataPoolMap.emplace(core::TEMPERATURE, &tempPoolEntry); + localDataPoolMap.emplace(core::PS_VOLTAGE, &psVoltageEntry); + localDataPoolMap.emplace(core::PL_VOLTAGE, &plVoltageEntry); poolManager.subscribeForRegularPeriodicPacket({hkSet.getSid(), false, 10.0}); return returnvalue::OK; } @@ -171,8 +172,7 @@ ReturnValue_t CoreController::initializeAfterTaskCreation() { setenv("PATH", updatedEnvPath.c_str(), true); updateProtInfo(); initPrint(); - ExtendedControllerBase::initializeAfterTaskCreation(); - return result; + return ExtendedControllerBase::initializeAfterTaskCreation(); } ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, @@ -793,7 +793,7 @@ ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId, return returnvalue::OK; } -ReturnValue_t CoreController::initBootCopy() { +ReturnValue_t CoreController::initBootCopyFile() { if (not std::filesystem::exists(CURR_COPY_FILE)) { // This file is created by the systemd service eive-early-config so this should // not happen normally @@ -803,8 +803,6 @@ ReturnValue_t CoreController::initBootCopy() { utility::handleSystemError(result, "CoreController::initBootCopy"); } } - - getCurrentBootCopy(CURRENT_CHIP, CURRENT_COPY); return returnvalue::OK; } @@ -932,6 +930,9 @@ ReturnValue_t CoreController::actionReboot(const uint8_t *data, size_t size) { ReturnValue_t CoreController::gracefulShutdownTasks(xsc::Chip chip, xsc::Copy copy, bool &protOpPerformed) { sdcMan->setBlocking(true); + sdcMan->markUnusable(); + // Wait two seconds to ensure no one uses the SD cards + TaskFactory::delayTask(2000); // Attempt graceful shutdown by unmounting and switching off SD cards sdcMan->switchOffSdCard(sd::SdCard::SLOT_0); sdcMan->switchOffSdCard(sd::SdCard::SLOT_1); @@ -1237,6 +1238,10 @@ void CoreController::performMountedSdCardOperations() { std::filesystem::create_directory(path.str()); } initVersionFile(); + ReturnValue_t result = initBootCopyFile(); + if (result != returnvalue::OK) { + sif::warning << "CoreController::CoreController: Boot copy init" << std::endl; + } initClockFromTimeFile(); performRebootFileHandling(false); } diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index 460c0f8a..4d9d5ee8 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -70,6 +70,9 @@ class CoreController : public ExtendedControllerBase { static constexpr char CHIP_1_COPY_0_MOUNT_DIR[] = "/tmp/mntupdate-xdi-qspi1-nom-rootfs"; static constexpr char CHIP_1_COPY_1_MOUNT_DIR[] = "/tmp/mntupdate-xdi-qspi1-gold-rootfs"; + static constexpr dur_millis_t INIT_SD_CARD_CHECK_TIMEOUT = 5000; + static constexpr dur_millis_t DEFAULT_SD_CARD_CHECK_TIMEOUT = 60000; + static constexpr ActionId_t LIST_DIRECTORY_INTO_FILE = 0; static constexpr ActionId_t SWITCH_REBOOT_FILE_HANDLING = 5; static constexpr ActionId_t RESET_REBOOT_COUNTERS = 6; @@ -210,6 +213,8 @@ class CoreController : public ExtendedControllerBase { RebootFile rebootFile = {}; std::string currMntPrefix; bool performOneShotSdCardOpsSwitch = false; + uint8_t shortSdCardCdCounter = 0; + Countdown sdCardCheckCd = Countdown(INIT_SD_CARD_CHECK_TIMEOUT); /** * Index 0: Chip 0 Copy 0 @@ -221,6 +226,10 @@ class CoreController : public ExtendedControllerBase { PeriodicOperationDivider opDivider5; PeriodicOperationDivider opDivider10; + PoolEntry tempPoolEntry = PoolEntry(0.0); + PoolEntry psVoltageEntry = PoolEntry(0.0); + PoolEntry plVoltageEntry = PoolEntry(0.0); + core::HkSet hkSet; #if OBSW_SD_CARD_MUST_BE_ON == 1 @@ -229,7 +238,7 @@ class CoreController : public ExtendedControllerBase { ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) override; - Countdown sdCardCheckCd = Countdown(120000); + LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t* msToReachTheMode); void performMountedSdCardOperations(); @@ -238,7 +247,7 @@ class CoreController : public ExtendedControllerBase { ReturnValue_t initClockFromTimeFile(); ReturnValue_t performSdCardCheck(); ReturnValue_t timeFileHandler(); - ReturnValue_t initBootCopy(); + ReturnValue_t initBootCopyFile(); ReturnValue_t initWatchdogFifo(); ReturnValue_t initSdCardBlocking(); bool startSdStateMachine(sd::SdCard targetActiveSd, SdCfgMode mode, MessageQueueId_t commander, diff --git a/bsp_q7s/core/InitMission.cpp b/bsp_q7s/core/InitMission.cpp index df36df33..bbeaded8 100644 --- a/bsp_q7s/core/InitMission.cpp +++ b/bsp_q7s/core/InitMission.cpp @@ -67,12 +67,7 @@ void initmission::initTasks() { void (*missedDeadlineFunc)(void) = nullptr; #endif - PeriodicTaskIF* sysCtrlTask = factory->createPeriodicTask( - "CORE_CTRL", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.4, missedDeadlineFunc); - result = sysCtrlTask->addComponent(objects::CORE_CONTROLLER); - if (result != returnvalue::OK) { - initmission::printAddObjectError("CORE_CTRL", objects::CORE_CONTROLLER); - } +#if OBSW_ADD_SA_DEPL == 1 // Could add this to the core controller but the core controller does so many thing that I would // prefer to have the solar array deployment in a seprate task. PeriodicTaskIF* solarArrayDeplTask = factory->createPeriodicTask( @@ -81,6 +76,18 @@ void initmission::initTasks() { if (result != returnvalue::OK) { initmission::printAddObjectError("SOLAR_ARRAY_DEPL", objects::SOLAR_ARRAY_DEPL_HANDLER); } +#endif + + PeriodicTaskIF* sysTask = factory->createPeriodicTask( + "CORE_CTRL", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.4, missedDeadlineFunc); + result = sysTask->addComponent(objects::CORE_CONTROLLER); + if (result != returnvalue::OK) { + initmission::printAddObjectError("CORE_CTRL", objects::CORE_CONTROLLER); + } + result = sysTask->addComponent(objects::PL_SUBSYSTEM); + if (result != returnvalue::OK) { + initmission::printAddObjectError("PL_SUBSYSTEM", objects::PL_SUBSYSTEM); + } /* TMTC Distribution */ PeriodicTaskIF* tmTcDistributor = factory->createPeriodicTask( @@ -117,7 +124,7 @@ void initmission::initTasks() { } #endif -#if OBSW_USE_CCSDS_IP_CORE == 1 +#if OBSW_ADD_CCSDS_IP_CORES == 1 PeriodicTaskIF* ccsdsHandlerTask = factory->createPeriodicTask( "CCSDS_HANDLER", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = ccsdsHandlerTask->addComponent(objects::CCSDS_HANDLER); @@ -125,16 +132,14 @@ void initmission::initTasks() { initmission::printAddObjectError("CCSDS Handler", objects::CCSDS_HANDLER); } - // Minimal distance between two received TCs amounts to 0.6 seconds - // If a command has not been read before the next one arrives, the old command will be - // overwritten by the PDEC. + // Runs in IRQ mode, frequency does not really matter PeriodicTaskIF* pdecHandlerTask = factory->createPeriodicTask( - "PDEC_HANDLER", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); + "PDEC_HANDLER", 75, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); result = pdecHandlerTask->addComponent(objects::PDEC_HANDLER); if (result != returnvalue::OK) { initmission::printAddObjectError("PDEC Handler", objects::PDEC_HANDLER); } -#endif /* OBSW_USE_CCSDS_IP_CORE == 1 */ +#endif /* OBSW_ADD_CCSDS_IP_CORE == 1 */ #if OBSW_ADD_CFDP_COMPONENTS == 1 PeriodicTaskIF* cfdpTask = factory->createPeriodicTask( @@ -182,6 +187,10 @@ void initmission::initTasks() { initmission::printAddObjectError("SUS_BOARD_ASS", objects::SUS_BOARD_ASS); } #endif + result = acsSysTask->addComponent(objects::ACS_SUBSYSTEM); + if (result != returnvalue::OK) { + initmission::printAddObjectError("ACS_SUBSYSTEM", objects::ACS_SUBSYSTEM); + } #if OBSW_ADD_RTD_DEVICES == 1 PeriodicTaskIF* tcsPollingTask = factory->createPeriodicTask( @@ -190,6 +199,7 @@ void initmission::initTasks() { if (result != returnvalue::OK) { initmission::printAddObjectError("SPI_RTD_POLLING", objects::SPI_RTD_COM_IF); } + PeriodicTaskIF* tcsTask = factory->createPeriodicTask( "TCS_TASK", 45, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc); std::array rtdIds = { @@ -234,10 +244,12 @@ void initmission::initTasks() { initmission::printAddObjectError("THERMAL_CONTROLLER", objects::THERMAL_CONTROLLER); } #endif +#if OBSW_ADD_HEATERS == 1 result = tcsSystemTask->addComponent(objects::HEATER_HANDLER); if (result != returnvalue::OK) { initmission::printAddObjectError("HEATER_HANDLER", objects::HEATER_HANDLER); } +#endif #if OBSW_ADD_STAR_TRACKER == 1 PeriodicTaskIF* strHelperTask = factory->createPeriodicTask( @@ -266,14 +278,11 @@ void initmission::initTasks() { } #endif /* OBSW_ADD_PLOC_SUPERVISOR */ -#if OBSW_TEST_CCSDS_BRIDGE == 1 - PeriodicTaskIF* ptmeTestTask = factory->createPeriodicTask( - "PTME_TEST", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); - result = ptmeTestTask->addComponent(objects::CCSDS_IP_CORE_BRIDGE); - if (result != returnvalue::OK) { - initmission::printAddObjectError("PTME_TEST", objects::CCSDS_IP_CORE_BRIDGE); - } -#endif + PeriodicTaskIF* plTask = factory->createPeriodicTask( + "PL_TASK", 25, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); + scheduling::addMpsocSupvHandlers(plTask); + plTask->addComponent(objects::CAM_SWITCHER); + #if OBSW_ADD_SCEX_DEVICE == 1 PeriodicTaskIF* scexDevHandler; PeriodicTaskIF* scexReaderTask; @@ -286,6 +295,14 @@ void initmission::initTasks() { createPstTasks(*factory, missedDeadlineFunc, pstTasks); #if OBSW_ADD_TEST_CODE == 1 +#if OBSW_TEST_CCSDS_BRIDGE == 1 + PeriodicTaskIF* ptmeTestTask = factory->createPeriodicTask( + "PTME_TEST", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); + result = ptmeTestTask->addComponent(objects::CCSDS_IP_CORE_BRIDGE); + if (result != returnvalue::OK) { + initmission::printAddObjectError("PTME_TEST", objects::CCSDS_IP_CORE_BRIDGE); + } +#endif std::vector testTasks; createTestTasks(*factory, missedDeadlineFunc, testTasks); #endif @@ -307,13 +324,15 @@ void initmission::initTasks() { tmtcPollingTask->startTask(); #endif -#if OBSW_USE_CCSDS_IP_CORE == 1 +#if OBSW_ADD_CCSDS_IP_CORES == 1 ccsdsHandlerTask->startTask(); pdecHandlerTask->startTask(); -#endif /* OBSW_USE_CCSDS_IP_CORE == 1 */ +#endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ - sysCtrlTask->startTask(); + sysTask->startTask(); +#if OBSW_ADD_SA_DEPL == 1 solarArrayDeplTask->startTask(); +#endif taskStarter(pstTasks, "PST task vector"); taskStarter(pusTasks, "PUS task vector"); @@ -340,7 +359,9 @@ void initmission::initTasks() { tcsPollingTask->startTask(); tcsTask->startTask(); #endif /* OBSW_ADD_RTD_DEVICES == 1 */ - tcsSystemTask->startTask(); + if (not tcsSystemTask->isEmpty()) { + tcsSystemTask->startTask(); + } #if OBSW_ADD_PLOC_SUPERVISOR == 1 supvHelperTask->startTask(); #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ @@ -454,7 +475,7 @@ void initmission::createPusTasks(TaskFactory& factory, taskVec.push_back(pusEvents); PeriodicTaskIF* pusHighPrio = factory.createPeriodicTask( - "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); + "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS); if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_2", objects::PUS_SERVICE_2_DEVICE_ACCESS); @@ -463,6 +484,7 @@ void initmission::createPusTasks(TaskFactory& factory, if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_9", objects::PUS_SERVICE_9_TIME_MGMT); } + taskVec.push_back(pusHighPrio); PeriodicTaskIF* pusMedPrio = factory.createPeriodicTask( @@ -492,14 +514,15 @@ void initmission::createPusTasks(TaskFactory& factory, if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_201", objects::PUS_SERVICE_201_HEALTH); } + // Used for connection tests, therefore use higher priority + result = pusMedPrio->addComponent(objects::PUS_SERVICE_17_TEST); + if (result != returnvalue::OK) { + initmission::printAddObjectError("PUS_17", objects::PUS_SERVICE_17_TEST); + } taskVec.push_back(pusMedPrio); PeriodicTaskIF* pusLowPrio = factory.createPeriodicTask( "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, missedDeadlineFunc); - result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST); - if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS_17", objects::PUS_SERVICE_17_TEST); - } result = pusLowPrio->addComponent(objects::INTERNAL_ERROR_REPORTER); if (result != returnvalue::OK) { initmission::printAddObjectError("ERROR_REPORTER", objects::INTERNAL_ERROR_REPORTER); diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 4ffdd48e..f5397e04 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -1,6 +1,7 @@ #include "ObjectFactory.h" -#include +#include +#include #include "OBSWConfig.h" #include "bsp_q7s/boardtest/Q7STestTask.h" @@ -23,6 +24,7 @@ #include "linux/callbacks/gpioCallbacks.h" #include "linux/csp/CspComIF.h" #include "linux/devices/GPSHyperionLinuxController.h" +#include "linux/devices/ScexUartReader.h" #include "linux/devices/devicedefinitions/PlocMPSoCDefinitions.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" #include "linux/devices/ploc/PlocMPSoCHandler.h" @@ -31,22 +33,28 @@ #include "linux/devices/ploc/PlocSupervisorHandler.h" #include "linux/devices/startracker/StarTrackerHandler.h" #include "linux/devices/startracker/StrHelper.h" -#include "linux/obc/AxiPtmeConfig.h" -#include "linux/obc/PapbVcInterface.h" -#include "linux/obc/PdecHandler.h" -#include "linux/obc/Ptme.h" -#include "linux/obc/PtmeConfig.h" +#include "linux/ipcore/AxiPtmeConfig.h" +#include "linux/ipcore/PapbVcInterface.h" +#include "linux/ipcore/PdecHandler.h" +#include "linux/ipcore/Ptme.h" +#include "linux/ipcore/PtmeConfig.h" #include "mission/csp/CspCookie.h" -#include "mission/system/RwAssembly.h" #include "mission/system/fdir/AcsBoardFdir.h" #include "mission/system/fdir/GomspacePowerFdir.h" #include "mission/system/fdir/RtdFdir.h" #include "mission/system/fdir/SusFdir.h" #include "mission/system/fdir/SyrlinksFdir.h" +#include "mission/system/objects/AcsSubsystem.h" +#include "mission/system/objects/RwAssembly.h" +#include "mission/system/objects/TcsBoardAssembly.h" +#include "mission/system/tree/acsModeTree.h" +#include "mission/system/tree/payloadModeTree.h" #include "tmtc/pusIds.h" #if OBSW_TEST_LIBGPIOD == 1 #include "linux/boardtest/LibgpiodTest.h" #endif +#include + #include #include "fsfw/datapoollocal/LocalDataPoolManager.h" @@ -61,16 +69,15 @@ #include "fsfw_hal/linux/gpio/LinuxLibgpioIF.h" #include "fsfw_hal/linux/i2c/I2cComIF.h" #include "fsfw_hal/linux/i2c/I2cCookie.h" +#include "fsfw_hal/linux/serial/SerialComIF.h" +#include "fsfw_hal/linux/serial/SerialCookie.h" #include "fsfw_hal/linux/spi/SpiComIF.h" #include "fsfw_hal/linux/spi/SpiCookie.h" -#include "fsfw_hal/linux/uart/UartComIF.h" -#include "fsfw_hal/linux/uart/UartCookie.h" #include "mission/core/GenericFactory.h" #include "mission/devices/ACUHandler.h" #include "mission/devices/BpxBatteryHandler.h" #include "mission/devices/GyroADIS1650XHandler.h" #include "mission/devices/HeaterHandler.h" -#include "mission/devices/IMTQHandler.h" #include "mission/devices/Max31865PT1000Handler.h" #include "mission/devices/P60DockHandler.h" #include "mission/devices/PCDUHandler.h" @@ -88,9 +95,9 @@ #include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/SyrlinksDefinitions.h" #include "mission/devices/devicedefinitions/payloadPcduDefinitions.h" -#include "mission/system/AcsBoardAssembly.h" -#include "mission/tmtc/CCSDSHandler.h" -#include "mission/tmtc/TmFunnel.h" +#include "mission/system/objects/AcsBoardAssembly.h" +#include "mission/tmtc/CcsdsIpCoreHandler.h" +#include "mission/tmtc/TmFunnelHandler.h" #include "mission/tmtc/VirtualChannel.h" ResetArgs RESET_ARGS_GNSS; @@ -130,9 +137,9 @@ void ObjectFactory::createTmpComponents() { (void)tmp1075Handler_2; } -void ObjectFactory::createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, UartComIF** uartComIF, - SpiComIF** spiMainComIF, I2cComIF** i2cComIF, - SpiComIF** spiRWComIF) { +void ObjectFactory::createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, + SerialComIF** uartComIF, SpiComIF** spiMainComIF, + I2cComIF** i2cComIF, SpiComIF** spiRWComIF) { if (gpioComIF == nullptr or uartComIF == nullptr or spiMainComIF == nullptr or spiRWComIF == nullptr) { sif::error << "ObjectFactory::createCommunicationInterfaces: Invalid passed ComIF pointer" @@ -143,11 +150,9 @@ void ObjectFactory::createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, Ua /* Communication interfaces */ new CspComIF(objects::CSP_COM_IF); *i2cComIF = new I2cComIF(objects::I2C_COM_IF); - *uartComIF = new UartComIF(objects::UART_COM_IF); - *spiMainComIF = new SpiComIF(objects::SPI_MAIN_COM_IF, q7s::SPI_DEFAULT_DEV, *gpioComIF); - *spiRWComIF = new SpiComIF(objects::SPI_RW_COM_IF, q7s::SPI_RW_DEV, *gpioComIF); - /* Adding gpios for chip select decoding to the gpioComIf */ - q7s::gpioCallbacks::initSpiCsDecoder(*gpioComIF); + *uartComIF = new SerialComIF(objects::UART_COM_IF); + *spiMainComIF = new SpiComIF(objects::SPI_MAIN_COM_IF, q7s::SPI_DEFAULT_DEV, **gpioComIF); + *spiRWComIF = new SpiComIF(objects::SPI_RW_COM_IF, q7s::SPI_RW_DEV, **gpioComIF); } void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher) { @@ -192,8 +197,11 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI #endif } -void ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF) { +ReturnValue_t ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF) { using namespace gpio; + if (gpioComIF == nullptr) { + return returnvalue::FAILED; + } GpioCookie* gpioCookieRadSensor = new GpioCookie; std::stringstream consumer; consumer << "0x" << std::hex << objects::RAD_SENSOR; @@ -219,12 +227,14 @@ void ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF) { #if OBSW_DEBUG_RAD_SENSOR == 1 radSensor->enablePeriodicDataPrint(true); #endif + return returnvalue::OK; } -void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComIF* uartComIF, +void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, PowerSwitchIF* pwrSwitcher) { using namespace gpio; GpioCookie* gpioCookieAcsBoard = new GpioCookie(); + std::vector> assemblyChildren; std::stringstream consumer; GpiodRegularByLineName* gpio = nullptr; @@ -329,11 +339,11 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, MGMLIS3MDL::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); - auto mgmLis3Handler = new MgmLIS3MDLHandler(objects::MGM_0_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, - spiCookie, spi::LIS3_TRANSITION_DELAY); + auto mgmLis3Handler0 = new MgmLIS3MDLHandler( + objects::MGM_0_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_0_LIS3_HANDLER); - mgmLis3Handler->setCustomFdir(fdir); - static_cast(mgmLis3Handler); + mgmLis3Handler0->setCustomFdir(fdir); + assemblyChildren.push_back(*mgmLis3Handler0); #if OBSW_TEST_ACS == 1 mgmLis3Handler->setStartUpImmediately(); mgmLis3Handler->setToGoToNormalMode(true); @@ -344,13 +354,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI spiCookie = new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, RM3100::MAX_BUFFER_SIZE, spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); - auto mgmRm3100Handler = + auto mgmRm3100Handler1 = new MgmRM3100Handler(objects::MGM_1_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::RM3100_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_1_RM3100_HANDLER); - mgmRm3100Handler->setCustomFdir(fdir); - mgmRm3100Handler->setParent(objects::ACS_BOARD_ASS); - static_cast(mgmRm3100Handler); + mgmRm3100Handler1->setCustomFdir(fdir); + assemblyChildren.push_back(*mgmRm3100Handler1); #if OBSW_TEST_ACS == 1 mgmRm3100Handler->setStartUpImmediately(); mgmRm3100Handler->setToGoToNormalMode(true); @@ -361,12 +370,11 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI spiCookie = new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, MGMLIS3MDL::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); - mgmLis3Handler = new MgmLIS3MDLHandler(objects::MGM_2_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, - spiCookie, spi::LIS3_TRANSITION_DELAY); + auto* mgmLis3Handler2 = new MgmLIS3MDLHandler( + objects::MGM_2_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_2_LIS3_HANDLER); - mgmLis3Handler->setCustomFdir(fdir); - mgmLis3Handler->setParent(objects::ACS_BOARD_ASS); - static_cast(mgmLis3Handler); + mgmLis3Handler2->setCustomFdir(fdir); + assemblyChildren.push_back(*mgmLis3Handler2); #if OBSW_TEST_ACS == 1 mgmLis3Handler->setStartUpImmediately(); mgmLis3Handler->setToGoToNormalMode(true); @@ -377,11 +385,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI spiCookie = new SpiCookie(addresses::MGM_3_RM3100, gpioIds::MGM_3_RM3100_CS, RM3100::MAX_BUFFER_SIZE, spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); - mgmRm3100Handler = new MgmRM3100Handler(objects::MGM_3_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, - spiCookie, spi::RM3100_TRANSITION_DELAY); + auto* mgmRm3100Handler3 = + new MgmRM3100Handler(objects::MGM_3_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, + spi::RM3100_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_3_RM3100_HANDLER); - mgmRm3100Handler->setCustomFdir(fdir); - mgmRm3100Handler->setParent(objects::ACS_BOARD_ASS); + mgmRm3100Handler3->setCustomFdir(fdir); + assemblyChildren.push_back(*mgmRm3100Handler3); #if OBSW_TEST_ACS == 1 mgmRm3100Handler->setStartUpImmediately(); mgmRm3100Handler->setToGoToNormalMode(true); @@ -399,8 +408,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI ADIS1650X::Type::ADIS16505); fdir = new AcsBoardFdir(objects::GYRO_0_ADIS_HANDLER); adisHandler->setCustomFdir(fdir); - adisHandler->setParent(objects::ACS_BOARD_ASS); - static_cast(adisHandler); + assemblyChildren.push_back(*adisHandler); #if OBSW_TEST_ACS == 1 adisHandler->setStartUpImmediately(); adisHandler->setToGoToNormalModeImmediately(); @@ -411,12 +419,11 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI // Gyro 1 Side A spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, L3GD20H::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED); - auto gyroL3gHandler = new GyroHandlerL3GD20H( + auto gyroL3gHandler1 = new GyroHandlerL3GD20H( objects::GYRO_1_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::GYRO_1_L3G_HANDLER); - gyroL3gHandler->setCustomFdir(fdir); - gyroL3gHandler->setParent(objects::ACS_BOARD_ASS); - static_cast(gyroL3gHandler); + gyroL3gHandler1->setCustomFdir(fdir); + assemblyChildren.push_back(*gyroL3gHandler1); #if OBSW_TEST_ACS == 1 gyroL3gHandler->setStartUpImmediately(); gyroL3gHandler->setToGoToNormalMode(true); @@ -432,7 +439,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI spiCookie, ADIS1650X::Type::ADIS16505); fdir = new AcsBoardFdir(objects::GYRO_2_ADIS_HANDLER); adisHandler->setCustomFdir(fdir); - adisHandler->setParent(objects::ACS_BOARD_ASS); + assemblyChildren.push_back(*adisHandler); #if OBSW_TEST_ACS == 1 adisHandler->setStartUpImmediately(); adisHandler->setToGoToNormalModeImmediately(); @@ -440,11 +447,11 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI // Gyro 3 Side B spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, L3GD20H::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED); - gyroL3gHandler = new GyroHandlerL3GD20H(objects::GYRO_3_L3G_HANDLER, objects::SPI_MAIN_COM_IF, - spiCookie, spi::L3G_TRANSITION_DELAY); + auto gyroL3gHandler3 = new GyroHandlerL3GD20H( + objects::GYRO_3_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::GYRO_3_L3G_HANDLER); - gyroL3gHandler->setCustomFdir(fdir); - gyroL3gHandler->setParent(objects::ACS_BOARD_ASS); + gyroL3gHandler3->setCustomFdir(fdir); + assemblyChildren.push_back(*gyroL3gHandler3); #if OBSW_TEST_ACS == 1 gyroL3gHandler->setStartUpImmediately(); gyroL3gHandler->setToGoToNormalMode(true); @@ -466,9 +473,18 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI objects::MGM_0_LIS3_HANDLER, objects::MGM_1_RM3100_HANDLER, objects::MGM_2_LIS3_HANDLER, objects::MGM_3_RM3100_HANDLER, objects::GYRO_0_ADIS_HANDLER, objects::GYRO_1_L3G_HANDLER, objects::GYRO_2_ADIS_HANDLER, objects::GYRO_3_L3G_HANDLER, objects::GPS_CONTROLLER); - auto acsAss = new AcsBoardAssembly(objects::ACS_BOARD_ASS, objects::NO_OBJECT, pwrSwitcher, - acsBoardHelper, gpioComIF); + auto acsAss = + new AcsBoardAssembly(objects::ACS_BOARD_ASS, pwrSwitcher, acsBoardHelper, gpioComIF); static_cast(acsAss); + for (auto& assChild : assemblyChildren) { + ReturnValue_t result = assChild.get().connectModeTreeParent(*acsAss); + if (result != returnvalue::OK) { + sif::error << "Connecting assembly for ACS board component " << assChild.get().getObjectId() + << " failed" << std::endl; + } + } + gpsCtrl->connectModeTreeParent(*acsAss); + acsAss->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); #endif /* OBSW_ADD_ACS_HANDLERS == 1 */ } @@ -571,9 +587,13 @@ void ObjectFactory::createSyrlinksComponents(PowerSwitchIF* pwrSwitcher) { #endif } -void ObjectFactory::createPayloadComponents(LinuxLibgpioIF* gpioComIF) { +void ObjectFactory::createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF& pwrSwitch) { using namespace gpio; std::stringstream consumer; + auto* camSwitcher = + new CamSwitcher(objects::CAM_SWITCHER, pwrSwitch, pcdu::PDU2_CH8_PAYLOAD_CAMERA); + camSwitcher->connectModeTreeParent(satsystem::pl::SUBSYSTEM); + // camSwitcher-> #if OBSW_ADD_PLOC_MPSOC == 1 consumer << "0x" << std::hex << objects::PLOC_MPSOC_HANDLER; auto gpioConfigMPSoC = new GpiodRegularByLineName(q7s::gpioNames::ENABLE_MPSOC_UART, @@ -586,9 +606,10 @@ void ObjectFactory::createPayloadComponents(LinuxLibgpioIF* gpioComIF) { mpsoc::MAX_REPLY_SIZE, UartModes::NON_CANONICAL); mpsocCookie->setNoFixedSizeReply(); auto plocMpsocHelper = new PlocMPSoCHelper(objects::PLOC_MPSOC_HELPER); - new PlocMPSoCHandler(objects::PLOC_MPSOC_HANDLER, objects::UART_COM_IF, mpsocCookie, - plocMpsocHelper, Gpio(gpioIds::ENABLE_MPSOC_UART, gpioComIF), - objects::PLOC_SUPERVISOR_HANDLER); + auto* mpsocHandler = new PlocMPSoCHandler( + objects::PLOC_MPSOC_HANDLER, objects::UART_COM_IF, mpsocCookie, plocMpsocHelper, + Gpio(gpioIds::ENABLE_MPSOC_UART, gpioComIF), objects::PLOC_SUPERVISOR_HANDLER); + mpsocHandler->connectModeTreeParent(satsystem::pl::SUBSYSTEM); #endif /* OBSW_ADD_PLOC_MPSOC == 1 */ #if OBSW_ADD_PLOC_SUPERVISOR == 1 consumer << "0x" << std::hex << objects::PLOC_SUPERVISOR_HANDLER; @@ -602,9 +623,10 @@ void ObjectFactory::createPayloadComponents(LinuxLibgpioIF* gpioComIF) { uart::PLOC_SUPV_BAUD, supv::MAX_PACKET_SIZE * 20, UartModes::NON_CANONICAL); supervisorCookie->setNoFixedSizeReply(); auto supvHelper = new PlocSupvHelper(objects::PLOC_SUPERVISOR_HELPER); - new PlocSupervisorHandler(objects::PLOC_SUPERVISOR_HANDLER, objects::UART_COM_IF, - supervisorCookie, Gpio(gpioIds::ENABLE_SUPV_UART, gpioComIF), - pcdu::PDU1_CH6_PLOC_12V, supvHelper); + auto* supvHandler = new PlocSupervisorHandler( + objects::PLOC_SUPERVISOR_HANDLER, objects::UART_COM_IF, supervisorCookie, + Gpio(gpioIds::ENABLE_SUPV_UART, gpioComIF), pcdu::PDU1_CH6_PLOC_12V, supvHelper); + supvHandler->connectModeTreeParent(satsystem::pl::SUBSYSTEM); #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ static_cast(consumer); } @@ -681,13 +703,21 @@ void ObjectFactory::createReactionWheelComponents(LinuxLibgpioIF* gpioComIF, } RwHelper rwHelper(rwIds); - auto* rwAss = new RwAssembly(objects::RW_ASS, objects::NO_OBJECT, pwrSwitcher, - pcdu::Switches::PDU2_CH2_RW_5V, rwHelper); - static_cast(rwAss); + auto* rwAss = + new RwAssembly(objects::RW_ASS, pwrSwitcher, pcdu::Switches::PDU2_CH2_RW_5V, rwHelper); + for (uint8_t idx = 0; idx < rws.size(); idx++) { + ReturnValue_t result = rws[idx]->connectModeTreeParent(*rwAss); + if (result != returnvalue::OK) { + sif::error << "Connecting RW " << static_cast(idx) << " to RW assembly failed" + << std::endl; + } + } + rwAss->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); #endif /* OBSW_ADD_RW == 1 */ } -void ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF) { +ReturnValue_t ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF, + CcsdsIpCoreHandler** ipCoreHandler) { using namespace gpio; // GPIO definitions of signals connected to the virtual channel interfaces of the PTME IP Core GpioCookie* gpioCookiePtmeIp = new GpioCookie; @@ -750,18 +780,19 @@ void ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF) { #else static const uint32_t TRANSMITTER_TIMEOUT = 900000; // 15 minutes #endif - CCSDSHandler* ccsdsHandler = new CCSDSHandler( + *ipCoreHandler = new CcsdsIpCoreHandler( objects::CCSDS_HANDLER, objects::PTME, objects::CCSDS_PACKET_DISTRIBUTOR, ptmeConfig, gpioComIF, gpioIds::RS485_EN_TX_CLOCK, gpioIds::RS485_EN_TX_DATA, TRANSMITTER_TIMEOUT); VirtualChannel* vc = nullptr; vc = new VirtualChannel(ccsds::VC0, common::VC0_QUEUE_SIZE, objects::CCSDS_HANDLER); - ccsdsHandler->addVirtualChannel(ccsds::VC0, vc); + (*ipCoreHandler)->addVirtualChannel(ccsds::VC0, vc); vc = new VirtualChannel(ccsds::VC1, common::VC1_QUEUE_SIZE, objects::CCSDS_HANDLER); - ccsdsHandler->addVirtualChannel(ccsds::VC1, vc); + (*ipCoreHandler)->addVirtualChannel(ccsds::VC1, vc); vc = new VirtualChannel(ccsds::VC2, common::VC2_QUEUE_SIZE, objects::CCSDS_HANDLER); - ccsdsHandler->addVirtualChannel(ccsds::VC2, vc); + (*ipCoreHandler)->addVirtualChannel(ccsds::VC2, vc); vc = new VirtualChannel(ccsds::VC3, common::VC3_QUEUE_SIZE, objects::CCSDS_HANDLER); - ccsdsHandler->addVirtualChannel(ccsds::VC3, vc); + (*ipCoreHandler)->addVirtualChannel(ccsds::VC3, vc); + GpioCookie* gpioCookiePdec = new GpioCookie; consumer.str(""); consumer << "0x" << std::hex << objects::PDEC_HANDLER; @@ -770,8 +801,13 @@ void ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF) { Levels::LOW); gpioCookiePdec->addGpio(gpioIds::PDEC_RESET, gpio); gpioChecker(gpioComIF->addGpios(gpioCookiePdec), "PDEC"); + struct UioNames uioNames {}; + uioNames.configMemory = q7s::UIO_PDEC_CONFIG_MEMORY; + uioNames.ramMemory = q7s::UIO_PDEC_RAM; + uioNames.registers = q7s::UIO_PDEC_REGISTERS; + uioNames.irq = q7s::UIO_PDEC_IRQ; new PdecHandler(objects::PDEC_HANDLER, objects::CCSDS_HANDLER, gpioComIF, gpioIds::PDEC_RESET, - q7s::UIO_PDEC_CONFIG_MEMORY, q7s::UIO_PDEC_RAM, q7s::UIO_PDEC_REGISTERS); + uioNames); GpioCookie* gpioRS485Chip = new GpioCookie; gpio = new GpiodRegularByLineName(q7s::gpioNames::RS485_EN_TX_CLOCK, "RS485 Transceiver", Direction::OUT, Levels::LOW); @@ -787,6 +823,7 @@ void ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF) { Direction::OUT, Levels::LOW); gpioRS485Chip->addGpio(gpioIds::RS485_EN_RX_DATA, gpio); gpioChecker(gpioComIF->addGpios(gpioRS485Chip), "RS485 Transceiver"); + return returnvalue::OK; } void ObjectFactory::createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF, @@ -850,6 +887,7 @@ void ObjectFactory::createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* plPcduHandler->setToGoToNormalModeImmediately(true); plPcduHandler->enablePeriodicPrintout(true, 10); #endif + plPcduHandler->connectModeTreeParent(satsystem::pl::SUBSYSTEM); } void ObjectFactory::createTestComponents(LinuxLibgpioIF* gpioComIF) { @@ -876,14 +914,16 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher) { new StarTrackerHandler(objects::STAR_TRACKER, objects::UART_COM_IF, starTrackerCookie, strHelper, pcdu::PDU1_CH2_STAR_TRACKER_5V); starTracker->setPowerSwitcher(pwrSwitcher); + starTracker->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); } void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher) { I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, IMTQ::MAX_REPLY_SIZE, q7s::I2C_DEFAULT_DEV); - auto imtqHandler = new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie, + auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie, pcdu::Switches::PDU1_CH3_MGT_5V); imtqHandler->setPowerSwitcher(pwrSwitcher); + imtqHandler->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); static_cast(imtqHandler); #if OBSW_TEST_IMTQ == 1 imtqHandler->setStartUpImmediately(); diff --git a/bsp_q7s/core/ObjectFactory.h b/bsp_q7s/core/ObjectFactory.h index 88881090..c67353c4 100644 --- a/bsp_q7s/core/ObjectFactory.h +++ b/bsp_q7s/core/ObjectFactory.h @@ -2,11 +2,14 @@ #define BSP_Q7S_OBJECTFACTORY_H_ #include +#include +#include +#include #include class LinuxLibgpioIF; -class UartComIF; +class SerialComIF; class SpiComIF; class I2cComIF; class PowerSwitchIF; @@ -19,15 +22,15 @@ namespace ObjectFactory { void setStatics(); void produce(void* args); -void createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, UartComIF** uartComIF, +void createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, SerialComIF** uartComIF, SpiComIF** spiMainComIF, I2cComIF** i2cComIF, SpiComIF** spiRwComIF); void createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher); void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF, PowerSwitchIF* pwrSwitcher); void createTmpComponents(); -void createRadSensorComponent(LinuxLibgpioIF* gpioComIF); -void createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComIF* uartComIF, +ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF); +void createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, PowerSwitchIF* pwrSwitcher); void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTableIF* healthTable); void createImtqComponents(PowerSwitchIF* pwrSwitcher); @@ -35,9 +38,9 @@ void createBpxBatteryComponent(); void createStrComponents(PowerSwitchIF* pwrSwitcher); void createSolarArrayDeploymentComponents(PowerSwitchIF& pwrSwitcher, GpioIF& gpioIF); void createSyrlinksComponents(PowerSwitchIF* pwrSwitcher); -void createPayloadComponents(LinuxLibgpioIF* gpioComIF); +void createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF& pwrSwitcher); void createReactionWheelComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF* pwrSwitcher); -void createCcsdsComponents(LinuxLibgpioIF* gpioComIF); +ReturnValue_t createCcsdsComponents(LinuxLibgpioIF* gpioComIF, CcsdsIpCoreHandler** ipCoreHandler); void createMiscComponents(); void createTestComponents(LinuxLibgpioIF* gpioComIF); diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 4202cfdd..aba884f1 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -16,25 +17,33 @@ void ObjectFactory::produce(void* args) { ObjectFactory::setStatics(); HealthTableIF* healthTable = nullptr; - ObjectFactory::produceGenericObjects(&healthTable); + PusTmFunnel* pusFunnel = nullptr; + CfdpTmFunnel* cfdpFunnel = nullptr; + ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel); LinuxLibgpioIF* gpioComIF = nullptr; - UartComIF* uartComIF = nullptr; + SerialComIF* uartComIF = nullptr; SpiComIF* spiMainComIF = nullptr; I2cComIF* i2cComIF = nullptr; SpiComIF* spiRwComIF = nullptr; createCommunicationInterfaces(&gpioComIF, &uartComIF, &spiMainComIF, &i2cComIF, &spiRwComIF); + /* Adding gpios for chip select decoding to the gpioComIf */ + q7s::gpioCallbacks::initSpiCsDecoder(gpioComIF); + gpioCallbacks::disableAllDecoder(gpioComIF); // Hardware is usually not connected to EM, so we need to create dummies which replace lower // level components. dummy::DummyCfg dummyCfg; dummyCfg.addCoreCtrlCfg = false; +#if OBSW_ADD_SYRLINKS == 1 + dummyCfg.addSyrlinksDummies = false; +#endif dummy::createDummies(dummyCfg); new CoreController(objects::CORE_CONTROLLER); - gpioCallbacks::disableAllDecoder(gpioComIF); PowerSwitchIF* pwrSwitcher = new DummyPowerSwitcher(objects::PCDU_HANDLER, 18, 0); + static_cast(pwrSwitcher); // Regular FM code, does not work for EM if the hardware is not connected // createPcduComponents(gpioComIF, &pwrSwitcher); @@ -44,19 +53,28 @@ void ObjectFactory::produce(void* args) { // createRtdComponents(q7s::SPI_DEFAULT_DEV, gpioComIF, pwrSwitcher, spiMainComIF); // createTmpComponents(); // createSolarArrayDeploymentComponents(); - createRadSensorComponent(gpioComIF); + // createPayloadComponents(gpioComIF); + // createHeaterComponents(gpioComIF, pwrSwitcher, healthTable); + + // TODO: Careful! Switching this on somehow messes with the communication with the ProASIC + // and will cause xsc_boot_copy commands to always boot to 0 0 + // createRadSensorComponent(gpioComIF); #if OBSW_ADD_ACS_BOARD == 1 createAcsBoardComponents(gpioComIF, uartComIF, pwrSwitcher); #endif - createHeaterComponents(gpioComIF, pwrSwitcher, healthTable); - - createPayloadComponents(gpioComIF); #if OBSW_ADD_MGT == 1 createImtqComponents(pwrSwitcher); #endif + +#if OBSW_ADD_SYRLINKS == 1 + createSyrlinksComponents(pwrSwitcher); +#endif /* OBSW_ADD_SYRLINKS == 1 */ + +#if OBSW_ADD_RW == 1 createReactionWheelComponents(gpioComIF, pwrSwitcher); +#endif #if OBSW_ADD_BPX_BATTERY_HANDLER == 1 createBpxBatteryComponent(); @@ -65,9 +83,13 @@ void ObjectFactory::produce(void* args) { #if OBSW_ADD_STAR_TRACKER == 1 createStrComponents(pwrSwitcher); #endif /* OBSW_ADD_STAR_TRACKER == 1 */ -#if OBSW_USE_CCSDS_IP_CORE == 1 - createCcsdsComponents(gpioComIF); -#endif /* OBSW_USE_CCSDS_IP_CORE == 1 */ +#if OBSW_ADD_CCSDS_IP_CORES == 1 + CcsdsIpCoreHandler* ipCoreHandler = nullptr; + createCcsdsComponents(gpioComIF, &ipCoreHandler); +#if OBSW_TM_TO_PTME == 1 + ObjectFactory::addTmtcIpCoresToFunnels(*ipCoreHandler, *pusFunnel, *cfdpFunnel); +#endif +#endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ /* Test Task */ #if OBSW_ADD_TEST_CODE == 1 createTestComponents(gpioComIF); @@ -76,7 +98,5 @@ void ObjectFactory::produce(void* args) { createScexComponents(q7s::UART_SCEX_DEV, pwrSwitcher, *SdCardManager::instance(), true, std::nullopt); #endif - - createMiscComponents(); - createAcsController(); + createAcsController(true); } diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index d28b7248..c56dcf8d 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -1,3 +1,4 @@ +#include #include #include "OBSWConfig.h" @@ -10,23 +11,27 @@ #include "linux/ObjectFactory.h" #include "linux/callbacks/gpioCallbacks.h" #include "mission/core/GenericFactory.h" +#include "mission/system/tree/system.h" void ObjectFactory::produce(void* args) { ObjectFactory::setStatics(); HealthTableIF* healthTable = nullptr; - ObjectFactory::produceGenericObjects(&healthTable); + PusTmFunnel* pusFunnel = nullptr; + CfdpTmFunnel* cfdpFunnel = nullptr; + ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel); LinuxLibgpioIF* gpioComIF = nullptr; - UartComIF* uartComIF = nullptr; + SerialComIF* uartComIF = nullptr; SpiComIF* spiMainComIF = nullptr; I2cComIF* i2cComIF = nullptr; PowerSwitchIF* pwrSwitcher = nullptr; SpiComIF* spiRwComIF = nullptr; createCommunicationInterfaces(&gpioComIF, &uartComIF, &spiMainComIF, &i2cComIF, &spiRwComIF); - createTmpComponents(); - new CoreController(objects::CORE_CONTROLLER); - + /* Adding gpios for chip select decoding to the gpioComIf */ + q7s::gpioCallbacks::initSpiCsDecoder(gpioComIF); gpioCallbacks::disableAllDecoder(gpioComIF); + + new CoreController(objects::CORE_CONTROLLER); createPcduComponents(gpioComIF, &pwrSwitcher); createRadSensorComponent(gpioComIF); createSunSensorComponents(gpioComIF, spiMainComIF, pwrSwitcher, q7s::SPI_DEFAULT_DEV); @@ -35,13 +40,16 @@ void ObjectFactory::produce(void* args) { createAcsBoardComponents(gpioComIF, uartComIF, pwrSwitcher); #endif createHeaterComponents(gpioComIF, pwrSwitcher, healthTable); +#if OBSW_ADD_TMP_DEVICES == 1 + createTmpComponents(); +#endif createSolarArrayDeploymentComponents(*pwrSwitcher, *gpioComIF); createPlPcduComponents(gpioComIF, spiMainComIF, pwrSwitcher); #if OBSW_ADD_SYRLINKS == 1 createSyrlinksComponents(pwrSwitcher); #endif /* OBSW_ADD_SYRLINKS == 1 */ createRtdComponents(q7s::SPI_DEFAULT_DEV, gpioComIF, pwrSwitcher, spiMainComIF); - createPayloadComponents(gpioComIF); + createPayloadComponents(gpioComIF, *pwrSwitcher); #if OBSW_ADD_MGT == 1 createImtqComponents(pwrSwitcher); @@ -55,9 +63,13 @@ void ObjectFactory::produce(void* args) { #if OBSW_ADD_STAR_TRACKER == 1 createStrComponents(pwrSwitcher); #endif /* OBSW_ADD_STAR_TRACKER == 1 */ -#if OBSW_USE_CCSDS_IP_CORE == 1 - createCcsdsComponents(gpioComIF); -#endif /* OBSW_USE_CCSDS_IP_CORE == 1 */ +#if OBSW_ADD_CCSDS_IP_CORES == 1 + CcsdsIpCoreHandler* ipCoreHandler = nullptr; + createCcsdsComponents(gpioComIF, &ipCoreHandler); +#if OBSW_TM_TO_PTME == 1 + addTmtcIpCoresToFunnels(*ipCoreHandler, *pusFunnel, *cfdpFunnel); +#endif +#endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ #if OBSW_ADD_SCEX_DEVICE == 1 createScexComponents(q7s::UART_SCEX_DEV, pwrSwitcher, *SdCardManager::instance(), false, @@ -70,5 +82,6 @@ void ObjectFactory::produce(void* args) { createMiscComponents(); createThermalController(); - createAcsController(); + createAcsController(true); + satsystem::init(); } diff --git a/bsp_q7s/fs/SdCardManager.cpp b/bsp_q7s/fs/SdCardManager.cpp index 42182829..9d5fcd6e 100644 --- a/bsp_q7s/fs/SdCardManager.cpp +++ b/bsp_q7s/fs/SdCardManager.cpp @@ -459,7 +459,14 @@ void SdCardManager::setBlocking(bool blocking) { this->blocking = blocking; } void SdCardManager::setPrintCommandOutput(bool print) { this->printCmdOutput = print; } -bool SdCardManager::isSdCardUsable(sd::SdCard sdCard) { +bool SdCardManager::isSdCardUsable(std::optional sdCard) { + { + MutexGuard mg(mutex); + if (markedUnusable) { + return false; + } + } + SdCardManager::SdStatePair active; ReturnValue_t result = this->getSdCardsStatus(active); @@ -467,20 +474,30 @@ bool SdCardManager::isSdCardUsable(sd::SdCard sdCard) { sif::debug << "SdCardManager::isSdCardMounted: Failed to get SD card active state"; return false; } + if (not sdCard) { + if (active.first == sd::MOUNTED or active.second == sd::MOUNTED) { + return true; + } + return false; + } if (sdCard == sd::SLOT_0) { if (active.first == sd::MOUNTED) { return true; } else { return false; } - } else if (sdCard == sd::SLOT_1) { + } + if (sdCard == sd::SLOT_1) { if (active.second == sd::MOUNTED) { return true; } else { return false; } - } else { - sif::debug << "SdCardManager::isSdCardMounted: Unknown SD card specified" << std::endl; + } + if (sdCard == sd::BOTH) { + if (active.first == sd::MOUNTED && active.second == sd::MOUNTED) { + return true; + } } return false; } @@ -560,5 +577,13 @@ void SdCardManager::setActiveSdCard(sd::SdCard sdCard) { std::optional SdCardManager::getActiveSdCard() const { MutexGuard mg(mutex); + if (markedUnusable) { + return std::nullopt; + } return sdInfo.active; } + +void SdCardManager::markUnusable() { + MutexGuard mg(mutex); + markedUnusable = true; +} diff --git a/bsp_q7s/fs/SdCardManager.h b/bsp_q7s/fs/SdCardManager.h index 52c8ed9b..749cae62 100644 --- a/bsp_q7s/fs/SdCardManager.h +++ b/bsp_q7s/fs/SdCardManager.h @@ -206,7 +206,7 @@ class SdCardManager : public SystemObject, public SdCardMountedIF { * * @return true if mounted, otherwise false */ - bool isSdCardUsable(sd::SdCard sdCard) override; + bool isSdCardUsable(std::optional sdCard) override; ReturnValue_t isSdCardMountedReadOnly(sd::SdCard sdcard, bool& readOnly); @@ -214,12 +214,15 @@ class SdCardManager : public SystemObject, public SdCardMountedIF { ReturnValue_t performFsck(sd::SdCard sdcard, bool printOutput, int& linuxError); + void markUnusable(); + private: CommandExecutor cmdExecutor; Operations currentOp = Operations::IDLE; bool blocking = false; bool sdCardActive = true; bool printCmdOutput = true; + bool markedUnusable = false; MutexIF* mutex = nullptr; SdCardManager(); diff --git a/bsp_te0720_1cfa/InitMission.cpp b/bsp_te0720_1cfa/InitMission.cpp index b7c6b683..a3e3a004 100644 --- a/bsp_te0720_1cfa/InitMission.cpp +++ b/bsp_te0720_1cfa/InitMission.cpp @@ -107,7 +107,7 @@ void initmission::initTasks() { } #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ -#if OBSW_USE_CCSDS_IP_CORE == 1 +#if OBSW_ADD_CCSDS_IP_CORES == 1 PeriodicTaskIF* ccsdsHandlerTask = factory->createPeriodicTask( "CCSDS_HANDLER", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = ccsdsHandlerTask->addComponent(objects::CCSDS_HANDLER); @@ -124,7 +124,7 @@ void initmission::initTasks() { if (result != returnvalue::OK) { initmission::printAddObjectError("PDEC Handler", objects::PDEC_HANDLER); } -#endif /* OBSW_USE_CCSDS_IP_CORE == 1 */ +#endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ auto taskStarter = [](std::vector& taskVector, std::string name) { for (const auto& task : taskVector) { @@ -140,10 +140,10 @@ void initmission::initTasks() { tmtcDistributor->startTask(); tmtcBridgeTask->startTask(); tmtcPollingTask->startTask(); -#if OBSW_USE_CCSDS_IP_CORE == 1 +#if OBSW_ADD_CCSDS_IP_CORE == 1 pdecHandlerTask->startTask(); ccsdsHandlerTask->startTask(); -#endif /* #if OBSW_USE_CCSDS_IP_CORE == 1 */ +#endif /* #if OBSW_ADD_CCSDS_IP_CORE == 1 */ #if OBSW_ADD_PLOC_SUPERVISOR == 1 supvHelperTask->startTask(); #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ diff --git a/bsp_te0720_1cfa/OBSWConfig.h.in b/bsp_te0720_1cfa/OBSWConfig.h.in index 2f887daa..d2efda38 100644 --- a/bsp_te0720_1cfa/OBSWConfig.h.in +++ b/bsp_te0720_1cfa/OBSWConfig.h.in @@ -13,7 +13,7 @@ /** All of the following flags should be enabled for mission code */ /*******************************************************************/ -#define OBSW_USE_CCSDS_IP_CORE 0 +#define OBSW_ADD_CCSDS_IP_CORE 0 // Set to 1 if all telemetry should be sent to the PTME IP Core #define OBSW_TM_TO_PTME 0 // Set to 1 if telecommands are received via the PDEC IP Core diff --git a/bsp_te0720_1cfa/ObjectFactory.cpp b/bsp_te0720_1cfa/ObjectFactory.cpp index fe699e50..2877c4e0 100644 --- a/bsp_te0720_1cfa/ObjectFactory.cpp +++ b/bsp_te0720_1cfa/ObjectFactory.cpp @@ -12,8 +12,8 @@ #include "fsfw/tmtcservices/PusServiceBase.h" #include "fsfw_hal/linux/i2c/I2cComIF.h" #include "fsfw_hal/linux/i2c/I2cCookie.h" -#include "fsfw_hal/linux/uart/UartComIF.h" -#include "fsfw_hal/linux/uart/UartCookie.h" +#include "fsfw_hal/linux/serial/SerialComIF.h" +#include "fsfw_hal/linux/serial/SerialCookie.h" #include "fsfw_hal/common/gpio/GpioCookie.h" #include "linux/ObjectFactory.h" #include "linux/devices/ploc/PlocMPSoCHandler.h" @@ -59,7 +59,7 @@ void ObjectFactory::produce(void* args) { ObjectFactory::produceGenericObjects(); LinuxLibgpioIF* gpioComIF = new LinuxLibgpioIF(objects::GPIO_IF);; - new UartComIF(objects::UART_COM_IF); + newSerialComIF(objects::UART_COM_IF); #if OBSW_ADD_PLOC_MPSOC == 1 UartCookie* mpsocUartCookie = new UartCookie(objects::PLOC_MPSOC_HANDLER, te0720_1cfa::MPSOC_UART, diff --git a/common/config/devConf.h b/common/config/devConf.h index 68a3e199..e5188324 100644 --- a/common/config/devConf.h +++ b/common/config/devConf.h @@ -4,8 +4,8 @@ #include #include "fsfw/timemanager/clockDefinitions.h" +#include "fsfw_hal/linux/serial/SerialCookie.h" #include "fsfw_hal/linux/spi/spiDefinitions.h" -#include "fsfw_hal/linux/uart/UartCookie.h" /** * SPI configuration will be contained here to let the device handlers remain independent @@ -55,7 +55,7 @@ namespace uart { static constexpr size_t HYPERION_GPS_REPLY_MAX_BUFFER = 1024; static constexpr UartBaudRate SYRLINKS_BAUD = UartBaudRate::RATE_38400; -static constexpr UartBaudRate SCEX_BAUD = UartBaudRate::RATE_57600; +static constexpr UartBaudRate SCEX_BAUD = UartBaudRate::RATE_38400; static constexpr UartBaudRate GNSS_BAUD = UartBaudRate::RATE_9600; static constexpr UartBaudRate PLOC_MPSOC_BAUD = UartBaudRate::RATE_115200; static constexpr UartBaudRate PLOC_SUPV_BAUD = UartBaudRate::RATE_115200; diff --git a/common/config/eive/objects.h b/common/config/eive/objects.h index 25bebe25..5b9ec8a0 100644 --- a/common/config/eive/objects.h +++ b/common/config/eive/objects.h @@ -127,11 +127,15 @@ enum commonObjects : uint32_t { SUS_BOARD_ASS = 0x73000002, TCS_BOARD_ASS = 0x73000003, RW_ASS = 0x73000004, - CFDP_HANDLER = 0x73000005, - CFDP_DISTRIBUTOR = 0x73000006, + CAM_SWITCHER = 0x73000006, + ACS_SUBSYSTEM = 0x73010001, + PL_SUBSYSTEM = 0x73010002, + EIVE_SYSTEM = 0x73010000, TM_FUNNEL = 0x73000100, PUS_TM_FUNNEL = 0x73000101, CFDP_TM_FUNNEL = 0x73000102, + CFDP_HANDLER = 0x73000205, + CFDP_DISTRIBUTOR = 0x73000206, }; } diff --git a/dummies/CoreControllerDummy.cpp b/dummies/CoreControllerDummy.cpp index fd2e3f63..8a027dbf 100644 --- a/dummies/CoreControllerDummy.cpp +++ b/dummies/CoreControllerDummy.cpp @@ -6,8 +6,7 @@ #include #include -CoreControllerDummy::CoreControllerDummy(object_id_t objectId) - : ExtendedControllerBase(objectId, objects::NO_OBJECT) {} +CoreControllerDummy::CoreControllerDummy(object_id_t objectId) : ExtendedControllerBase(objectId) {} ReturnValue_t CoreControllerDummy::initialize() { static bool done = false; diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index 6dc451b3..aad1478b 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -1,6 +1,6 @@ #include "ImtqDummy.h" -#include +#include ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie) {} diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index 2a2a1bda..6d02ba69 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -5,8 +5,7 @@ #include #include -SusDummy::SusDummy() - : ExtendedControllerBase(objects::SUS_0_N_LOC_XFYFZM_PT_XF, objects::NO_OBJECT), susSet(this) { +SusDummy::SusDummy() : ExtendedControllerBase(objects::SUS_0_N_LOC_XFYFZM_PT_XF), susSet(this) { ObjectManager::instance()->insert(objects::SUS_6_R_LOC_XFYBZM_PT_XF, this); ObjectManager::instance()->insert(objects::SUS_1_N_LOC_XBYFZM_PT_XB, this); ObjectManager::instance()->insert(objects::SUS_7_R_LOC_XBYBZM_PT_XB, this); diff --git a/dummies/TemperatureSensorsDummy.cpp b/dummies/TemperatureSensorsDummy.cpp index 49194e91..c580473a 100644 --- a/dummies/TemperatureSensorsDummy.cpp +++ b/dummies/TemperatureSensorsDummy.cpp @@ -6,7 +6,7 @@ #include TemperatureSensorsDummy::TemperatureSensorsDummy() - : ExtendedControllerBase(objects::RTD_0_IC3_PLOC_HEATSPREADER, objects::NO_OBJECT), + : ExtendedControllerBase(objects::RTD_0_IC3_PLOC_HEATSPREADER), max31865Set(this, MAX31865::MAX31865_SET_ID) { ObjectManager::instance()->insert(objects::RTD_1_IC4_PLOC_MISSIONBOARD, this); ObjectManager::instance()->insert(objects::RTD_2_IC5_4K_CAMERA, this); diff --git a/dummies/helpers.cpp b/dummies/helpers.cpp index 2b18cffd..495d6f6c 100644 --- a/dummies/helpers.cpp +++ b/dummies/helpers.cpp @@ -38,7 +38,9 @@ void dummy::createDummies(DummyCfg cfg) { new RwDummy(objects::RW4, objects::DUMMY_COM_IF, comCookieDummy); new SaDeplDummy(objects::SOLAR_ARRAY_DEPL_HANDLER); new StarTrackerDummy(objects::STAR_TRACKER, objects::DUMMY_COM_IF, comCookieDummy); - new SyrlinksDummy(objects::SYRLINKS_HK_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + if (cfg.addSyrlinksDummies) { + new SyrlinksDummy(objects::SYRLINKS_HK_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + } new ImtqDummy(objects::IMTQ_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); if (cfg.addPowerDummies) { new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); diff --git a/dummies/helpers.h b/dummies/helpers.h index 680f97c1..f509f316 100644 --- a/dummies/helpers.h +++ b/dummies/helpers.h @@ -5,6 +5,7 @@ namespace dummy { struct DummyCfg { bool addCoreCtrlCfg = true; bool addPowerDummies = true; + bool addSyrlinksDummies = true; bool addAcsBoardDummies = true; bool addSusDummies = true; bool addTempSensorDummies = true; diff --git a/fsfw b/fsfw index 819a2bfa..39946bff 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 819a2bfac49babfc6a78452bb83cd581bf902bc8 +Subproject commit 39946bff58db7c5ac9016ca3156abb059560d9cb diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 8a69c7a0..7dace6d3 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -110,14 +110,14 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 11604;0x2d54;MPSOC_HANDLER_CRC_FAILURE;LOW;PLOC reply has invalid crc;linux/devices/ploc/PlocMPSoCHandler.h 11605;0x2d55;MPSOC_HANDLER_SEQUENCE_COUNT_MISMATCH;LOW;Packet sequence count in received space packet does not match expected count P1: Expected sequence count P2: Received sequence count;linux/devices/ploc/PlocMPSoCHandler.h 11606;0x2d56;MPSOC_SHUTDOWN_FAILED;HIGH;Supervisor fails to shutdown MPSoC. Requires to power off the PLOC and thus also to shutdown the supervisor.;linux/devices/ploc/PlocMPSoCHandler.h -11701;0x2db5;SELF_TEST_I2C_FAILURE;LOW;Get self test result returns I2C failure P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;mission/devices/IMTQHandler.h -11702;0x2db6;SELF_TEST_SPI_FAILURE;LOW;Get self test result returns SPI failure. This concerns the MTM connectivity. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;mission/devices/IMTQHandler.h -11703;0x2db7;SELF_TEST_ADC_FAILURE;LOW;Get self test result returns failure in measurement of current and temperature. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;mission/devices/IMTQHandler.h -11704;0x2db8;SELF_TEST_PWM_FAILURE;LOW;Get self test result returns PWM failure which concerns the coil actuation. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;mission/devices/IMTQHandler.h -11705;0x2db9;SELF_TEST_TC_FAILURE;LOW;Get self test result returns TC failure (system failure) P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;mission/devices/IMTQHandler.h -11706;0x2dba;SELF_TEST_MTM_RANGE_FAILURE;LOW;Get self test result returns failure that MTM values were outside of the expected range. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;mission/devices/IMTQHandler.h -11707;0x2dbb;SELF_TEST_COIL_CURRENT_FAILURE;LOW;Get self test result returns failure indicating that the coil current was outside of the expected range P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;mission/devices/IMTQHandler.h -11708;0x2dbc;INVALID_ERROR_BYTE;LOW;Received invalid error byte. This indicates an error of the communication link between IMTQ and OBC.;mission/devices/IMTQHandler.h +11701;0x2db5;SELF_TEST_I2C_FAILURE;LOW;Get self test result returns I2C failure P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;mission/devices/ImtqHandler.h +11702;0x2db6;SELF_TEST_SPI_FAILURE;LOW;Get self test result returns SPI failure. This concerns the MTM connectivity. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;mission/devices/ImtqHandler.h +11703;0x2db7;SELF_TEST_ADC_FAILURE;LOW;Get self test result returns failure in measurement of current and temperature. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;mission/devices/ImtqHandler.h +11704;0x2db8;SELF_TEST_PWM_FAILURE;LOW;Get self test result returns PWM failure which concerns the coil actuation. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;mission/devices/ImtqHandler.h +11705;0x2db9;SELF_TEST_TC_FAILURE;LOW;Get self test result returns TC failure (system failure) P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;mission/devices/ImtqHandler.h +11706;0x2dba;SELF_TEST_MTM_RANGE_FAILURE;LOW;Get self test result returns failure that MTM values were outside of the expected range. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;mission/devices/ImtqHandler.h +11707;0x2dbb;SELF_TEST_COIL_CURRENT_FAILURE;LOW;Get self test result returns failure indicating that the coil current was outside of the expected range P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;mission/devices/ImtqHandler.h +11708;0x2dbc;INVALID_ERROR_BYTE;LOW;Received invalid error byte. This indicates an error of the communication link between IMTQ and OBC.;mission/devices/ImtqHandler.h 11801;0x2e19;ERROR_STATE;HIGH;Reaction wheel signals an error state;mission/devices/devicedefinitions/RwDefinitions.h 11802;0x2e1a;RESET_OCCURED;LOW;;mission/devices/devicedefinitions/RwDefinitions.h 11901;0x2e7d;BOOTING_FIRMWARE_FAILED;LOW;Failed to boot firmware;linux/devices/startracker/StarTrackerHandler.h @@ -132,12 +132,13 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 12300;0x300c;SEND_MRAM_DUMP_FAILED;LOW;Failed to send mram dump command to supervisor handler P1: Return value of commandAction function P2: Start address of MRAM to dump with this command;linux/devices/ploc/PlocMemoryDumper.h 12301;0x300d;MRAM_DUMP_FAILED;LOW;Received completion failure report form PLOC supervisor handler P1: MRAM start address of failing dump command;linux/devices/ploc/PlocMemoryDumper.h 12302;0x300e;MRAM_DUMP_FINISHED;LOW;MRAM dump finished successfully;linux/devices/ploc/PlocMemoryDumper.h -12401;0x3071;INVALID_TC_FRAME;HIGH;;linux/obc/PdecHandler.h -12402;0x3072;INVALID_FAR;HIGH;Read invalid FAR from PDEC after startup;linux/obc/PdecHandler.h -12403;0x3073;CARRIER_LOCK;INFO;Carrier lock detected;linux/obc/PdecHandler.h -12404;0x3074;BIT_LOCK_PDEC;INFO;Bit lock detected (data valid);linux/obc/PdecHandler.h -12405;0x3075;LOST_CARRIER_LOCK_PDEC;INFO;Lost carrier lock;linux/obc/PdecHandler.h -12406;0x3076;LOST_BIT_LOCK_PDEC;INFO;Lost bit lock;linux/obc/PdecHandler.h +12401;0x3071;INVALID_TC_FRAME;HIGH;;linux/ipcore/PdecHandler.h +12402;0x3072;INVALID_FAR;HIGH;Read invalid FAR from PDEC after startup;linux/ipcore/PdecHandler.h +12403;0x3073;CARRIER_LOCK;INFO;Carrier lock detected;linux/ipcore/PdecHandler.h +12404;0x3074;BIT_LOCK_PDEC;INFO;Bit lock detected (data valid);linux/ipcore/PdecHandler.h +12405;0x3075;LOST_CARRIER_LOCK_PDEC;INFO;Lost carrier lock;linux/ipcore/PdecHandler.h +12406;0x3076;LOST_BIT_LOCK_PDEC;INFO;Lost bit lock;linux/ipcore/PdecHandler.h +12407;0x3077;POLL_ERROR_PDEC;MEDIUM;;linux/ipcore/PdecHandler.h 12500;0x30d4;IMAGE_UPLOAD_FAILED;LOW;Image upload failed;linux/devices/startracker/StrHelper.h 12501;0x30d5;IMAGE_DOWNLOAD_FAILED;LOW;Image download failed;linux/devices/startracker/StrHelper.h 12502;0x30d6;IMAGE_UPLOAD_SUCCESSFUL;LOW;Uploading image to star tracker was successfulop;linux/devices/startracker/StrHelper.h @@ -181,15 +182,15 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 12709;0x31a5;I_MPA_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/devices/PayloadPcduHandler.h 12710;0x31a6;U_HPA_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/devices/PayloadPcduHandler.h 12711;0x31a7;I_HPA_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/devices/PayloadPcduHandler.h -12800;0x3200;TRANSITION_OTHER_SIDE_FAILED;HIGH;;mission/system/AcsBoardAssembly.h -12801;0x3201;NOT_ENOUGH_DEVICES_DUAL_MODE;HIGH;;mission/system/AcsBoardAssembly.h -12802;0x3202;POWER_STATE_MACHINE_TIMEOUT;MEDIUM;;mission/system/AcsBoardAssembly.h -12803;0x3203;SIDE_SWITCH_TRANSITION_NOT_ALLOWED;LOW;Not implemented, would increase already high complexity. Operator should instead command the assembly off first and then command the assembly on into the desired mode/submode combination;mission/system/AcsBoardAssembly.h -12900;0x3264;TRANSITION_OTHER_SIDE_FAILED;HIGH;;mission/system/SusAssembly.h -12901;0x3265;NOT_ENOUGH_DEVICES_DUAL_MODE;HIGH;;mission/system/SusAssembly.h -12902;0x3266;POWER_STATE_MACHINE_TIMEOUT;MEDIUM;;mission/system/SusAssembly.h -12903;0x3267;SIDE_SWITCH_TRANSITION_NOT_ALLOWED;LOW;Not implemented, would increase already high complexity. Operator should instead command the assembly off first and then command the assembly on into the desired mode/submode combination;mission/system/SusAssembly.h -13000;0x32c8;CHILDREN_LOST_MODE;MEDIUM;;mission/system/TcsBoardAssembly.h +12800;0x3200;TRANSITION_OTHER_SIDE_FAILED;HIGH;;mission/system/objects/AcsBoardAssembly.h +12801;0x3201;NOT_ENOUGH_DEVICES_DUAL_MODE;HIGH;;mission/system/objects/AcsBoardAssembly.h +12802;0x3202;POWER_STATE_MACHINE_TIMEOUT;MEDIUM;;mission/system/objects/AcsBoardAssembly.h +12803;0x3203;SIDE_SWITCH_TRANSITION_NOT_ALLOWED;LOW;Not implemented, would increase already high complexity. Operator should instead command the assembly off first and then command the assembly on into the desired mode/submode combination;mission/system/objects/AcsBoardAssembly.h +12900;0x3264;TRANSITION_OTHER_SIDE_FAILED;HIGH;;mission/system/objects/SusAssembly.h +12901;0x3265;NOT_ENOUGH_DEVICES_DUAL_MODE;HIGH;;mission/system/objects/SusAssembly.h +12902;0x3266;POWER_STATE_MACHINE_TIMEOUT;MEDIUM;;mission/system/objects/SusAssembly.h +12903;0x3267;SIDE_SWITCH_TRANSITION_NOT_ALLOWED;LOW;Not implemented, would increase already high complexity. Operator should instead command the assembly off first and then command the assembly on into the desired mode/submode combination;mission/system/objects/SusAssembly.h +13000;0x32c8;CHILDREN_LOST_MODE;MEDIUM;;mission/system/objects/TcsBoardAssembly.h 13100;0x332c;GPS_FIX_CHANGE;INFO;Fix has changed. P1: Old fix. P2: New fix 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix;mission/devices/devicedefinitions/GPSDefinitions.h 13200;0x3390;P60_BOOT_COUNT;INFO;P60 boot count is broadcasted once at SW startup. P1: Boot count;mission/devices/P60DockHandler.h 13201;0x3391;BATT_MODE;INFO;Battery mode is broadcasted at startup. P1: Mode;mission/devices/P60DockHandler.h diff --git a/generators/bsp_q7s_objects.csv b/generators/bsp_q7s_objects.csv index 1e44c67b..10ac10ed 100644 --- a/generators/bsp_q7s_objects.csv +++ b/generators/bsp_q7s_objects.csv @@ -131,10 +131,14 @@ 0x73000002;SUS_BOARD_ASS 0x73000003;TCS_BOARD_ASS 0x73000004;RW_ASS -0x73000005;CFDP_HANDLER -0x73000006;CFDP_DISTRIBUTOR +0x73000006;CAM_SWITCHER 0x73000100;TM_FUNNEL 0x73000101;PUS_TM_FUNNEL 0x73000102;CFDP_TM_FUNNEL +0x73000205;CFDP_HANDLER +0x73000206;CFDP_DISTRIBUTOR +0x73010000;EIVE_SYSTEM +0x73010001;ACS_SUBSYSTEM +0x73010002;PL_SUBSYSTEM 0x73500000;CCSDS_IP_CORE_BRIDGE 0xFFFFFFFF;NO_OBJECT diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index 7fccee95..72c805b5 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -9,6 +9,14 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x66a2;SADPL_MainSwitchTimeoutFailure;;162;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h 0x66a3;SADPL_SwitchingDeplSa1Failed;;163;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h 0x66a4;SADPL_SwitchingDeplSa2Failed;;164;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x51a0;IMTQ_InvalidCommandCode;;160;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x51a1;IMTQ_ParameterMissing;;161;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x51a2;IMTQ_ParameterInvalid;;162;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x51a3;IMTQ_CcUnavailable;;163;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x51a4;IMTQ_InternalProcessingError;;164;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x51a5;IMTQ_RejectedWithoutReason;;165;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x51a6;IMTQ_CmdErrUnknown;;166;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/ImtqHandler.h 0x52b0;RWHA_SpiWriteFailure;;176;RW_HANDLER;mission/devices/RwHandler.h 0x52b1;RWHA_SpiReadFailure;Used by the spi send function to tell a failing read call;177;RW_HANDLER;mission/devices/RwHandler.h 0x52b2;RWHA_MissingStartSign;Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E;178;RW_HANDLER;mission/devices/RwHandler.h @@ -27,14 +35,6 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x5d03;GOMS_InvalidParamSize;;3;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h 0x5d04;GOMS_InvalidPayloadSize;;4;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h 0x5d05;GOMS_UnknownReplyId;;5;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x51a0;IMTQ_InvalidCommandCode;;160;IMTQ_HANDLER;mission/devices/IMTQHandler.h -0x51a1;IMTQ_ParameterMissing;;161;IMTQ_HANDLER;mission/devices/IMTQHandler.h -0x51a2;IMTQ_ParameterInvalid;;162;IMTQ_HANDLER;mission/devices/IMTQHandler.h -0x51a3;IMTQ_CcUnavailable;;163;IMTQ_HANDLER;mission/devices/IMTQHandler.h -0x51a4;IMTQ_InternalProcessingError;;164;IMTQ_HANDLER;mission/devices/IMTQHandler.h -0x51a5;IMTQ_RejectedWithoutReason;;165;IMTQ_HANDLER;mission/devices/IMTQHandler.h -0x51a6;IMTQ_CmdErrUnknown;;166;IMTQ_HANDLER;mission/devices/IMTQHandler.h -0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/IMTQHandler.h 0x50a0;SYRLINKS_CrcFailure;;160;SYRLINKS_HANDLER;mission/devices/SyrlinksHkHandler.h 0x50a1;SYRLINKS_UartFraminOrParityErrorAck;;161;SYRLINKS_HANDLER;mission/devices/SyrlinksHkHandler.h 0x50a2;SYRLINKS_BadCharacterAck;;162;SYRLINKS_HANDLER;mission/devices/SyrlinksHkHandler.h @@ -49,7 +49,7 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x4fa3;HEATER_InvalidSwitchNr;;163;HEATER_HANDLER;mission/devices/HeaterHandler.h 0x4fa4;HEATER_MainSwitchSetTimeout;;164;HEATER_HANDLER;mission/devices/HeaterHandler.h 0x4fa5;HEATER_CommandAlreadyWaiting;;165;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CCSDSHandler.h +0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h 0x4500;HSPI_OpeningFileFailed;;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4501;HSPI_FullDuplexTransferFailed;;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4502;HSPI_HalfDuplexTransferFailed;;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h @@ -60,9 +60,9 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x4805;HGIO_GpioDuplicateDetected;;5;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h 0x4806;HGIO_GpioInitFailed;;6;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h 0x4807;HGIO_GpioGetValueFailed;;7;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4601;HURT_UartReadFailure;;1;HAL_UART;fsfw/src/fsfw_hal/linux/uart/UartComIF.h -0x4602;HURT_UartReadSizeMissmatch;;2;HAL_UART;fsfw/src/fsfw_hal/linux/uart/UartComIF.h -0x4603;HURT_UartRxBufferTooSmall;;3;HAL_UART;fsfw/src/fsfw_hal/linux/uart/UartComIF.h +0x4601;HURT_UartReadFailure;;1;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h +0x4602;HURT_UartReadSizeMissmatch;;2;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h +0x4603;HURT_UartRxBufferTooSmall;;3;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h 0x4400;UXOS_ExecutionFinished;Execution of the current command has finished;0;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h 0x4401;UXOS_CommandPending;Command is pending. This will also be returned if the user tries to load another command but a command is still pending;1;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h 0x4402;UXOS_BytesRead;Some bytes have been read from the executing process;2;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h @@ -101,10 +101,6 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x38a6;SGP4_SatelliteHasDecayed;;166;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h 0x38b1;SGP4_TleTooOld;;177;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h 0x38b2;SGP4_TleNotInitialized;;178;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x1101;AL_Full;;1;ARRAY_LIST;fsfw/src/fsfw/container/ArrayList.h -0x1501;FM_KeyAlreadyExists;;1;FIXED_MAP;fsfw/src/fsfw/container/FixedMap.h -0x1502;FM_MapFull;;2;FIXED_MAP;fsfw/src/fsfw/container/FixedMap.h -0x1503;FM_KeyDoesNotExist;;3;FIXED_MAP;fsfw/src/fsfw/container/FixedMap.h 0x1801;FF_Full;;1;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h 0x1802;FF_Empty;;2;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h 0x1601;FMM_MapFull;;1;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h @@ -325,6 +321,7 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x360a;CFDP_InvalidPduFormat;;10;CFDP;fsfw/src/fsfw/cfdp/definitions.h 0x4300;FILS_GenericFileError;;0;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h 0x4301;FILS_GenericDirError;;1;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4302;FILS_FilesystemInactive;;2;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h 0x4303;FILS_GenericRenameError;;3;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h 0x4304;FILS_IsBusy;;4;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h 0x4305;FILS_InvalidParameters;;5;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 69e22777..87c790d7 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 233 translations. + * @brief Auto-generated event translation file. Contains 234 translations. * @details - * Generated on: 2022-10-21 15:41:33 + * Generated on: 2022-11-10 18:07:26 */ #include "translateEvents.h" @@ -144,6 +144,7 @@ const char *CARRIER_LOCK_STRING = "CARRIER_LOCK"; const char *BIT_LOCK_PDEC_STRING = "BIT_LOCK_PDEC"; const char *LOST_CARRIER_LOCK_PDEC_STRING = "LOST_CARRIER_LOCK_PDEC"; const char *LOST_BIT_LOCK_PDEC_STRING = "LOST_BIT_LOCK_PDEC"; +const char *POLL_ERROR_PDEC_STRING = "POLL_ERROR_PDEC"; const char *IMAGE_UPLOAD_FAILED_STRING = "IMAGE_UPLOAD_FAILED"; const char *IMAGE_DOWNLOAD_FAILED_STRING = "IMAGE_DOWNLOAD_FAILED"; const char *IMAGE_UPLOAD_SUCCESSFUL_STRING = "IMAGE_UPLOAD_SUCCESSFUL"; @@ -515,6 +516,8 @@ const char *translateEvents(Event event) { return LOST_CARRIER_LOCK_PDEC_STRING; case (12406): return LOST_BIT_LOCK_PDEC_STRING; + case (12407): + return POLL_ERROR_PDEC_STRING; case (12500): return IMAGE_UPLOAD_FAILED_STRING; case (12501): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index 65f8ea62..cfb7f9a5 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -1,8 +1,8 @@ /** * @brief Auto-generated object translation file. * @details - * Contains 140 translations. - * Generated on: 2022-10-21 15:41:33 + * Contains 144 translations. + * Generated on: 2022-11-10 18:07:26 */ #include "translateObjects.h" @@ -139,11 +139,15 @@ const char *ACS_BOARD_ASS_STRING = "ACS_BOARD_ASS"; const char *SUS_BOARD_ASS_STRING = "SUS_BOARD_ASS"; const char *TCS_BOARD_ASS_STRING = "TCS_BOARD_ASS"; const char *RW_ASS_STRING = "RW_ASS"; -const char *CFDP_HANDLER_STRING = "CFDP_HANDLER"; -const char *CFDP_DISTRIBUTOR_STRING = "CFDP_DISTRIBUTOR"; +const char *CAM_SWITCHER_STRING = "CAM_SWITCHER"; const char *TM_FUNNEL_STRING = "TM_FUNNEL"; const char *PUS_TM_FUNNEL_STRING = "PUS_TM_FUNNEL"; const char *CFDP_TM_FUNNEL_STRING = "CFDP_TM_FUNNEL"; +const char *CFDP_HANDLER_STRING = "CFDP_HANDLER"; +const char *CFDP_DISTRIBUTOR_STRING = "CFDP_DISTRIBUTOR"; +const char *EIVE_SYSTEM_STRING = "EIVE_SYSTEM"; +const char *ACS_SUBSYSTEM_STRING = "ACS_SUBSYSTEM"; +const char *PL_SUBSYSTEM_STRING = "PL_SUBSYSTEM"; const char *CCSDS_IP_CORE_BRIDGE_STRING = "CCSDS_IP_CORE_BRIDGE"; const char *NO_OBJECT_STRING = "NO_OBJECT"; @@ -415,16 +419,24 @@ const char *translateObject(object_id_t object) { return TCS_BOARD_ASS_STRING; case 0x73000004: return RW_ASS_STRING; - case 0x73000005: - return CFDP_HANDLER_STRING; case 0x73000006: - return CFDP_DISTRIBUTOR_STRING; + return CAM_SWITCHER_STRING; case 0x73000100: return TM_FUNNEL_STRING; case 0x73000101: return PUS_TM_FUNNEL_STRING; case 0x73000102: return CFDP_TM_FUNNEL_STRING; + case 0x73000205: + return CFDP_HANDLER_STRING; + case 0x73000206: + return CFDP_DISTRIBUTOR_STRING; + case 0x73010000: + return EIVE_SYSTEM_STRING; + case 0x73010001: + return ACS_SUBSYSTEM_STRING; + case 0x73010002: + return PL_SUBSYSTEM_STRING; case 0x73500000: return CCSDS_IP_CORE_BRIDGE_STRING; case 0xFFFFFFFF: diff --git a/linux/CMakeLists.txt b/linux/CMakeLists.txt index 861dfb5c..d7e6ca93 100644 --- a/linux/CMakeLists.txt +++ b/linux/CMakeLists.txt @@ -4,6 +4,6 @@ add_subdirectory(callbacks) add_subdirectory(boardtest) add_subdirectory(devices) add_subdirectory(fsfwconfig) -add_subdirectory(obc) +add_subdirectory(ipcore) target_sources(${OBSW_NAME} PUBLIC ObjectFactory.cpp InitMission.cpp) diff --git a/linux/InitMission.cpp b/linux/InitMission.cpp index eb8f677a..5bc8698c 100644 --- a/linux/InitMission.cpp +++ b/linux/InitMission.cpp @@ -45,3 +45,21 @@ void scheduling::schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHa printAddObjectError("SCEX_UART_READER", objects::SCEX_UART_READER); } } + +void scheduling::addMpsocSupvHandlers(PeriodicTaskIF* plTask) { +#if OBSW_ADD_PLOC_SUPERVISOR == 1 + plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::PERFORM_OPERATION); + plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::SEND_READ); + plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::GET_READ); + plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::SEND_WRITE); + plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::GET_WRITE); +#endif + +#if OBSW_ADD_PLOC_MPSOC == 1 + plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::PERFORM_OPERATION); + plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::SEND_READ); + plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::GET_READ); + plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::SEND_WRITE); + plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::GET_WRITE); +#endif +} diff --git a/linux/InitMission.h b/linux/InitMission.h index e5a3afff..dd809ff7 100644 --- a/linux/InitMission.h +++ b/linux/InitMission.h @@ -4,4 +4,6 @@ namespace scheduling { void schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHandler, PeriodicTaskIF*& scexReaderTask); -} +void addMpsocSupvHandlers(PeriodicTaskIF* task); + +} // namespace scheduling diff --git a/linux/ObjectFactory.cpp b/linux/ObjectFactory.cpp index ff7060fb..21f884f9 100644 --- a/linux/ObjectFactory.cpp +++ b/linux/ObjectFactory.cpp @@ -1,12 +1,13 @@ #include "ObjectFactory.h" #include +#include #include #include #include +#include #include #include -#include #include #include #include @@ -15,8 +16,6 @@ #include #include #include -#include -#include #include #include @@ -24,6 +23,11 @@ #include "devConf.h" #include "devices/addresses.h" #include "devices/gpioIds.h" +#include "eive/definitions.h" +#include "mission/system/objects/SusAssembly.h" +#include "mission/system/objects/TcsBoardAssembly.h" +#include "mission/system/tree/acsModeTree.h" +#include "mission/system/tree/payloadModeTree.h" void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiComIF, PowerSwitchIF* pwrSwitcher, std::string spiDev) { @@ -78,7 +82,6 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo susHandlers[0] = new SusHandler(objects::SUS_0_N_LOC_XFYFZM_PT_XF, 0, objects::SPI_MAIN_COM_IF, spiCookie); fdir = new SusFdir(objects::SUS_0_N_LOC_XFYFZM_PT_XF); - susHandlers[0]->setParent(objects::SUS_BOARD_ASS); susHandlers[0]->setCustomFdir(fdir); spiCookie = new SpiCookie(addresses::SUS_1, gpioIds::CS_SUS_1, SUS::MAX_CMD_SIZE, @@ -86,7 +89,6 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo susHandlers[1] = new SusHandler(objects::SUS_1_N_LOC_XBYFZM_PT_XB, 1, objects::SPI_MAIN_COM_IF, spiCookie); fdir = new SusFdir(objects::SUS_1_N_LOC_XBYFZM_PT_XB); - susHandlers[1]->setParent(objects::SUS_BOARD_ASS); susHandlers[1]->setCustomFdir(fdir); spiCookie = new SpiCookie(addresses::SUS_2, gpioIds::CS_SUS_2, SUS::MAX_CMD_SIZE, @@ -94,7 +96,6 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo susHandlers[2] = new SusHandler(objects::SUS_2_N_LOC_XFYBZB_PT_YB, 2, objects::SPI_MAIN_COM_IF, spiCookie); fdir = new SusFdir(objects::SUS_2_N_LOC_XFYBZB_PT_YB); - susHandlers[2]->setParent(objects::SUS_BOARD_ASS); susHandlers[2]->setCustomFdir(fdir); spiCookie = new SpiCookie(addresses::SUS_3, gpioIds::CS_SUS_3, SUS::MAX_CMD_SIZE, @@ -102,7 +103,6 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo susHandlers[3] = new SusHandler(objects::SUS_3_N_LOC_XFYBZF_PT_YF, 3, objects::SPI_MAIN_COM_IF, spiCookie); fdir = new SusFdir(objects::SUS_3_N_LOC_XFYBZF_PT_YF); - susHandlers[3]->setParent(objects::SUS_BOARD_ASS); susHandlers[3]->setCustomFdir(fdir); spiCookie = new SpiCookie(addresses::SUS_4, gpioIds::CS_SUS_4, SUS::MAX_CMD_SIZE, @@ -110,7 +110,6 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo susHandlers[4] = new SusHandler(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, 4, objects::SPI_MAIN_COM_IF, spiCookie); fdir = new SusFdir(objects::SUS_4_N_LOC_XMYFZF_PT_ZF); - susHandlers[4]->setParent(objects::SUS_BOARD_ASS); susHandlers[4]->setCustomFdir(fdir); spiCookie = new SpiCookie(addresses::SUS_5, gpioIds::CS_SUS_5, SUS::MAX_CMD_SIZE, @@ -118,7 +117,6 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo susHandlers[5] = new SusHandler(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, 5, objects::SPI_MAIN_COM_IF, spiCookie); fdir = new SusFdir(objects::SUS_5_N_LOC_XFYMZB_PT_ZB); - susHandlers[5]->setParent(objects::SUS_BOARD_ASS); susHandlers[5]->setCustomFdir(fdir); spiCookie = new SpiCookie(addresses::SUS_6, gpioIds::CS_SUS_6, SUS::MAX_CMD_SIZE, @@ -126,7 +124,6 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo susHandlers[6] = new SusHandler(objects::SUS_6_R_LOC_XFYBZM_PT_XF, 6, objects::SPI_MAIN_COM_IF, spiCookie); fdir = new SusFdir(objects::SUS_6_R_LOC_XFYBZM_PT_XF); - susHandlers[6]->setParent(objects::SUS_BOARD_ASS); susHandlers[6]->setCustomFdir(fdir); spiCookie = new SpiCookie(addresses::SUS_7, gpioIds::CS_SUS_7, SUS::MAX_CMD_SIZE, @@ -134,7 +131,6 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo susHandlers[7] = new SusHandler(objects::SUS_7_R_LOC_XBYBZM_PT_XB, 7, objects::SPI_MAIN_COM_IF, spiCookie); fdir = new SusFdir(objects::SUS_7_R_LOC_XBYBZM_PT_XB); - susHandlers[7]->setParent(objects::SUS_BOARD_ASS); susHandlers[7]->setCustomFdir(fdir); spiCookie = new SpiCookie(addresses::SUS_8, gpioIds::CS_SUS_8, SUS::MAX_CMD_SIZE, @@ -142,7 +138,6 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo susHandlers[8] = new SusHandler(objects::SUS_8_R_LOC_XBYBZB_PT_YB, 8, objects::SPI_MAIN_COM_IF, spiCookie); fdir = new SusFdir(objects::SUS_8_R_LOC_XBYBZB_PT_YB); - susHandlers[8]->setParent(objects::SUS_BOARD_ASS); susHandlers[8]->setCustomFdir(fdir); spiCookie = new SpiCookie(addresses::SUS_9, gpioIds::CS_SUS_9, SUS::MAX_CMD_SIZE, @@ -150,7 +145,6 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo susHandlers[9] = new SusHandler(objects::SUS_9_R_LOC_XBYBZB_PT_YF, 9, objects::SPI_MAIN_COM_IF, spiCookie); fdir = new SusFdir(objects::SUS_9_R_LOC_XBYBZB_PT_YF); - susHandlers[9]->setParent(objects::SUS_BOARD_ASS); susHandlers[9]->setCustomFdir(fdir); spiCookie = new SpiCookie(addresses::SUS_10, gpioIds::CS_SUS_10, SUS::MAX_CMD_SIZE, @@ -158,7 +152,6 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo susHandlers[10] = new SusHandler(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, 10, objects::SPI_MAIN_COM_IF, spiCookie); fdir = new SusFdir(objects::SUS_10_N_LOC_XMYBZF_PT_ZF); - susHandlers[10]->setParent(objects::SUS_BOARD_ASS); susHandlers[10]->setCustomFdir(fdir); spiCookie = new SpiCookie(addresses::SUS_11, gpioIds::CS_SUS_11, SUS::MAX_CMD_SIZE, @@ -166,11 +159,24 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo susHandlers[11] = new SusHandler(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, 11, objects::SPI_MAIN_COM_IF, spiCookie); fdir = new SusFdir(objects::SUS_11_R_LOC_XBYMZB_PT_ZB); - susHandlers[11]->setParent(objects::SUS_BOARD_ASS); susHandlers[11]->setCustomFdir(fdir); + std::array susIds = { + objects::SUS_0_N_LOC_XFYFZM_PT_XF, objects::SUS_1_N_LOC_XBYFZM_PT_XB, + objects::SUS_2_N_LOC_XFYBZB_PT_YB, objects::SUS_3_N_LOC_XFYBZF_PT_YF, + objects::SUS_4_N_LOC_XMYFZF_PT_ZF, objects::SUS_5_N_LOC_XFYMZB_PT_ZB, + objects::SUS_6_R_LOC_XFYBZM_PT_XF, objects::SUS_7_R_LOC_XBYBZM_PT_XB, + objects::SUS_8_R_LOC_XBYBZB_PT_YB, objects::SUS_9_R_LOC_XBYBZB_PT_YF, + objects::SUS_10_N_LOC_XMYBZF_PT_ZF, objects::SUS_11_R_LOC_XBYMZB_PT_ZB}; + SusAssHelper susAssHelper = SusAssHelper(susIds); + auto susAss = new SusAssembly(objects::SUS_BOARD_ASS, pwrSwitcher, susAssHelper); for (auto& sus : susHandlers) { if (sus != nullptr) { + ReturnValue_t result = sus->connectModeTreeParent(*susAss); + if (result != returnvalue::OK) { + sif::error << "Connecting SUS " << sus->getObjectId() << " to SUS assembly failed" + << std::endl; + } #if OBSW_TEST_SUS == 1 sus->setStartUpImmediately(); sus->setToGoToNormalMode(true); @@ -180,17 +186,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo #endif } } - std::array susIds = { - objects::SUS_0_N_LOC_XFYFZM_PT_XF, objects::SUS_1_N_LOC_XBYFZM_PT_XB, - objects::SUS_2_N_LOC_XFYBZB_PT_YB, objects::SUS_3_N_LOC_XFYBZF_PT_YF, - objects::SUS_4_N_LOC_XMYFZF_PT_ZF, objects::SUS_5_N_LOC_XFYMZB_PT_ZB, - objects::SUS_6_R_LOC_XFYBZM_PT_XF, objects::SUS_7_R_LOC_XBYBZM_PT_XB, - objects::SUS_8_R_LOC_XBYBZB_PT_YB, objects::SUS_9_R_LOC_XBYBZB_PT_YF, - objects::SUS_10_N_LOC_XMYBZF_PT_ZF, objects::SUS_11_R_LOC_XBYMZB_PT_ZB}; - SusAssHelper susAssHelper = SusAssHelper(susIds); - auto susAss = - new SusAssembly(objects::SUS_BOARD_ASS, objects::NO_OBJECT, pwrSwitcher, susAssHelper); - static_cast(susAss); + susAss->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); #endif /* OBSW_ADD_SUN_SENSORS == 1 */ } @@ -292,6 +288,11 @@ void ObjectFactory::createRtdComponents(std::string spiDev, GpioIF* gpioComIF, std::array rtdCookies = {}; std::array rtds = {}; RtdFdir* rtdFdir = nullptr; + + TcsBoardHelper helper(rtdInfos); + TcsBoardAssembly* tcsBoardAss = new TcsBoardAssembly( + objects::TCS_BOARD_ASS, pwrSwitcher, pcdu::Switches::PDU1_CH0_TCS_BOARD_3V3, helper); + static_cast(tcsBoardAss); // Create special low level reader communication interface new Max31865RtdReader(objects::SPI_RTD_COM_IF, comIF, gpioComIF); for (uint8_t idx = 0; idx < NUM_RTDS; idx++) { @@ -303,7 +304,11 @@ void ObjectFactory::createRtdComponents(std::string spiDev, GpioIF* gpioComIF, rtds[idx] = new Max31865EiveHandler(rtdInfos[idx].first, objects::SPI_RTD_COM_IF, rtdLowLevelCookie); rtds[idx]->setDeviceInfo(idx, rtdInfos[idx].second); - rtds[idx]->setParent(objects::TCS_BOARD_ASS); + ReturnValue_t result = rtds[idx]->connectModeTreeParent(*tcsBoardAss); + if (result != returnvalue::OK) { + sif::error << "Connecting RTD " << static_cast(idx) << " to RTD Assembly failed" + << std::endl; + } rtdFdir = new RtdFdir(rtdInfos[idx].first); rtds[idx]->setCustomFdir(rtdFdir); #if OBSW_DEBUG_RTD == 1 @@ -315,11 +320,6 @@ void ObjectFactory::createRtdComponents(std::string spiDev, GpioIF* gpioComIF, #endif } - TcsBoardHelper helper(rtdInfos); - TcsBoardAssembly* tcsBoardAss = - new TcsBoardAssembly(objects::TCS_BOARD_ASS, objects::NO_OBJECT, pwrSwitcher, - pcdu::Switches::PDU1_CH0_TCS_BOARD_3V3, helper); - static_cast(tcsBoardAss); #endif // OBSW_ADD_RTD_DEVICES == 1 } @@ -337,16 +337,29 @@ void ObjectFactory::createScexComponents(std::string uartDev, PowerSwitchIF* pwr if (switchId) { scexHandler->setPowerSwitcher(*pwrSwitcher, switchId.value()); } + scexHandler->connectModeTreeParent(satsystem::pl::SUBSYSTEM); } void ObjectFactory::createThermalController() { - new ThermalController(objects::THERMAL_CONTROLLER, objects::NO_OBJECT); + new ThermalController(objects::THERMAL_CONTROLLER); } -void ObjectFactory::createAcsController() { new AcsController(objects::ACS_CONTROLLER); } +AcsController* ObjectFactory::createAcsController(bool connectSubsystem) { + auto acsCtrl = new AcsController(objects::ACS_CONTROLLER); + if (connectSubsystem) { + acsCtrl->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); + } + return acsCtrl; +} void ObjectFactory::gpioChecker(ReturnValue_t result, std::string output) { if (result != returnvalue::OK) { sif::error << "ObjectFactory: Adding GPIOs failed for " << output << std::endl; } } + +void ObjectFactory::addTmtcIpCoresToFunnels(CcsdsIpCoreHandler& ipCoreHandler, + PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel) { + cfdpFunnel.addDestination(ipCoreHandler, config::LIVE_TM); + pusFunnel.addDestination(ipCoreHandler, config::LIVE_TM); +} diff --git a/linux/ObjectFactory.h b/linux/ObjectFactory.h index f4ecc6e3..3704e9bd 100644 --- a/linux/ObjectFactory.h +++ b/linux/ObjectFactory.h @@ -4,6 +4,9 @@ #include #include #include +#include +#include +#include #include #include @@ -11,6 +14,7 @@ class GpioIF; class SpiComIF; class PowerSwitchIF; +class AcsController; namespace ObjectFactory { @@ -26,6 +30,9 @@ void createScexComponents(std::string uartDev, PowerSwitchIF* pwrSwitcher, void gpioChecker(ReturnValue_t result, std::string output); void createThermalController(); -void createAcsController(); +AcsController* createAcsController(bool connectSubsystem); + +void addTmtcIpCoresToFunnels(CcsdsIpCoreHandler& ipCoreHandler, PusTmFunnel& pusFunnel, + CfdpTmFunnel& cfdpFunnel); } // namespace ObjectFactory diff --git a/linux/boardtest/UartTestClass.cpp b/linux/boardtest/UartTestClass.cpp index 4f476d4b..2c914d74 100644 --- a/linux/boardtest/UartTestClass.cpp +++ b/linux/boardtest/UartTestClass.cpp @@ -3,7 +3,7 @@ #include // Error integer and strerror() function #include // Contains file controls like O_RDWR #include -#include +#include #include #include #include diff --git a/linux/boardtest/UartTestClass.h b/linux/boardtest/UartTestClass.h index 14d74322..6304724b 100644 --- a/linux/boardtest/UartTestClass.h +++ b/linux/boardtest/UartTestClass.h @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include // Contains POSIX terminal control definitions #include diff --git a/linux/devices/GPSHyperionLinuxController.cpp b/linux/devices/GPSHyperionLinuxController.cpp index 84200baf..1799ef4a 100644 --- a/linux/devices/GPSHyperionLinuxController.cpp +++ b/linux/devices/GPSHyperionLinuxController.cpp @@ -18,9 +18,7 @@ GPSHyperionLinuxController::GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId, bool debugHyperionGps) - : ExtendedControllerBase(objectId, objects::NO_OBJECT), - gpsSet(this), - debugHyperionGps(debugHyperionGps) { + : ExtendedControllerBase(objectId), gpsSet(this), debugHyperionGps(debugHyperionGps) { timeUpdateCd.resetTimer(); } diff --git a/linux/devices/Max31865RtdLowlevelHandler.cpp b/linux/devices/Max31865RtdLowlevelHandler.cpp index 1be2b1c2..38a9ac11 100644 --- a/linux/devices/Max31865RtdLowlevelHandler.cpp +++ b/linux/devices/Max31865RtdLowlevelHandler.cpp @@ -50,7 +50,7 @@ ReturnValue_t Max31865RtdReader::performOperation(uint8_t operationCode) { } bool Max31865RtdReader::rtdIsActive(uint8_t idx) { - if (rtds[idx]->on and rtds[idx]->active and rtds[idx]->configured) { + if (rtds[idx]->on and rtds[idx]->db.active and rtds[idx]->db.configured) { return true; } return false; @@ -69,7 +69,7 @@ bool Max31865RtdReader::periodicInitHandling() { if (rtd == nullptr) { continue; } - if ((rtd->on or rtd->active) and not rtd->configured and rtd->cd.hasTimedOut()) { + if ((rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut()) { ManualCsLockWrapper mg(csLock, gpioIF, rtd->spiCookie, csTimeoutType, csTimeoutMs); if (mg.lockResult != returnvalue::OK or mg.gpioResult != returnvalue::OK) { sif::error << "Max31865RtdReader::periodicInitHandling: Manual CS lock failed" << std::endl; @@ -95,13 +95,7 @@ bool Max31865RtdReader::periodicInitHandling() { if (result != returnvalue::OK) { handleSpiError(rtd, result, "clearFaultStatus"); } - rtd->configured = true; rtd->db.configured = true; - if (rtd->active) { - rtd->db.active = true; - } - } - if (rtd->active and rtd->configured and not rtd->db.active) { rtd->db.active = true; } } @@ -241,8 +235,8 @@ ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* se rtdCookie->cd.setTimeout(MAX31865::WARMUP_MS); rtdCookie->cd.resetTimer(); rtdCookie->on = true; - rtdCookie->active = false; - rtdCookie->configured = false; + rtdCookie->db.active = false; + rtdCookie->db.configured = false; if (sendLen == 5) { thresholdHandler(rtdCookie, sendData); } @@ -254,10 +248,10 @@ ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* se rtdCookie->cd.setTimeout(MAX31865::WARMUP_MS); rtdCookie->cd.resetTimer(); rtdCookie->on = true; - rtdCookie->active = true; - rtdCookie->configured = false; + rtdCookie->db.active = true; + rtdCookie->db.configured = false; } else { - rtdCookie->active = true; + rtdCookie->db.active = true; } if (sendLen == 5) { thresholdHandler(rtdCookie, sendData); @@ -266,8 +260,8 @@ ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* se } case (EiveMax31855::RtdCommands::OFF): { rtdCookie->on = false; - rtdCookie->active = false; - rtdCookie->configured = false; + rtdCookie->db.active = false; + rtdCookie->db.configured = false; break; } case (EiveMax31855::RtdCommands::HIGH_TRESHOLD): { diff --git a/linux/devices/Max31865RtdLowlevelHandler.h b/linux/devices/Max31865RtdLowlevelHandler.h index d3845bd5..89d66350 100644 --- a/linux/devices/Max31865RtdLowlevelHandler.h +++ b/linux/devices/Max31865RtdLowlevelHandler.h @@ -23,8 +23,6 @@ struct Max31865ReaderCookie : public CookieIF { Countdown cd = Countdown(MAX31865::WARMUP_MS); bool on = false; - bool configured = false; - bool active = false; bool writeLowThreshold = false; bool writeHighThreshold = false; uint16_t lowThreshold = 0; diff --git a/linux/devices/ScexUartReader.cpp b/linux/devices/ScexUartReader.cpp index 24461395..ce59472d 100644 --- a/linux/devices/ScexUartReader.cpp +++ b/linux/devices/ScexUartReader.cpp @@ -6,7 +6,7 @@ #include #include #include -#include +#include #include // write(), read(), close() #include // Error integer and strerror() function @@ -58,7 +58,7 @@ ReturnValue_t ScexUartReader::performOperation(uint8_t operationCode) { result = tryDleParsing(); } - TaskFactory::delayTask(400); + TaskFactory::delayTask(150); } else if (bytesRead < 0) { sif::warning << "ScexUartReader::performOperation: read call failed with error [" << errno << ", " << strerror(errno) << "]" << std::endl; @@ -118,10 +118,7 @@ ReturnValue_t ScexUartReader::initializeInterface(CookieIF *cookie) { tty.c_cc[VTIME] = 0; tty.c_cc[VMIN] = 0; - // The SCEX experiment has a fixed baud rate. - if (cfsetispeed(&tty, B38400) != 0) { - sif::warning << "ScexUartReader::initializeInterface: Setting baud rate failed" << std::endl; - } + uart::setBaudrate(tty, uartCookie->getBaudrate()); if (tcsetattr(serialPort, TCSANOW, &tty) != 0) { sif::warning << "ScexUartReader::initializeInterface: tcsetattr call failed with error [" << errno << ", " << strerror(errno) << std::endl; @@ -216,6 +213,10 @@ ReturnValue_t ScexUartReader::tryDleParsing() { void ScexUartReader::reset() { lock->lockMutex(); state = States::FINISH; + ipcRingBuf.clear(); + while (not ipcQueue.empty()) { + ipcQueue.pop(); + } lock->unlockMutex(); } diff --git a/linux/devices/ploc/PlocMPSoCHandler.cpp b/linux/devices/ploc/PlocMPSoCHandler.cpp index a192365f..28513314 100644 --- a/linux/devices/ploc/PlocMPSoCHandler.cpp +++ b/linux/devices/ploc/PlocMPSoCHandler.cpp @@ -31,7 +31,7 @@ ReturnValue_t PlocMPSoCHandler::initialize() { if (result != returnvalue::OK) { return result; } - uartComIf = dynamic_cast(communicationInterface); + uartComIf = dynamic_cast(communicationInterface); if (uartComIf == nullptr) { sif::warning << "PlocMPSoCHandler::initialize: Invalid uart com if" << std::endl; return ObjectManagerIF::CHILD_INIT_FAILED; diff --git a/linux/devices/ploc/PlocMPSoCHandler.h b/linux/devices/ploc/PlocMPSoCHandler.h index fca65c7b..a7d7a427 100644 --- a/linux/devices/ploc/PlocMPSoCHandler.h +++ b/linux/devices/ploc/PlocMPSoCHandler.h @@ -10,7 +10,7 @@ #include "fsfw/ipc/QueueFactory.h" #include "fsfw/tmtcservices/SourceSequenceCounter.h" #include "fsfw_hal/linux/gpio/Gpio.h" -#include "fsfw_hal/linux/uart/UartComIF.h" +#include "fsfw_hal/linux/serial/SerialComIF.h" #include "linux/devices/devicedefinitions/MPSoCReturnValuesIF.h" #include "linux/devices/devicedefinitions/PlocMPSoCDefinitions.h" #include "linux/devices/devicedefinitions/PlocSupervisorDefinitions.h" @@ -123,7 +123,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { */ DeviceCommandId_t nextReplyId = mpsoc::NONE; - UartComIF* uartComIf = nullptr; + SerialComIF* uartComIf = nullptr; PlocMPSoCHelper* plocMPSoCHelper = nullptr; Gpio uartIsolatorSwitch; diff --git a/linux/devices/ploc/PlocMPSoCHelper.cpp b/linux/devices/ploc/PlocMPSoCHelper.cpp index 980b8889..fcd5178b 100644 --- a/linux/devices/ploc/PlocMPSoCHelper.cpp +++ b/linux/devices/ploc/PlocMPSoCHelper.cpp @@ -57,7 +57,7 @@ ReturnValue_t PlocMPSoCHelper::performOperation(uint8_t operationCode) { } ReturnValue_t PlocMPSoCHelper::setComIF(DeviceCommunicationIF* communicationInterface_) { - uartComIF = dynamic_cast(communicationInterface_); + uartComIF = dynamic_cast(communicationInterface_); if (uartComIF == nullptr) { sif::warning << "PlocMPSoCHelper::initialize: Invalid uart com if" << std::endl; return returnvalue::FAILED; diff --git a/linux/devices/ploc/PlocMPSoCHelper.h b/linux/devices/ploc/PlocMPSoCHelper.h index b669b51b..c39cc758 100644 --- a/linux/devices/ploc/PlocMPSoCHelper.h +++ b/linux/devices/ploc/PlocMPSoCHelper.h @@ -9,7 +9,7 @@ #include "fsfw/returnvalues/returnvalue.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw/tmtcservices/SourceSequenceCounter.h" -#include "fsfw_hal/linux/uart/UartComIF.h" +#include "fsfw_hal/linux/serial/SerialComIF.h" #include "linux/devices/devicedefinitions/PlocMPSoCDefinitions.h" #ifdef XIPHOS_Q7S #include "bsp_q7s/fs/SdCardManager.h" @@ -136,7 +136,7 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { * Communication interface of MPSoC responsible for low level access. Must be set by the * MPSoC Handler. */ - UartComIF* uartComIF = nullptr; + SerialComIF* uartComIF = nullptr; // Communication cookie. Must be set by the MPSoC Handler CookieIF* comCookie = nullptr; // Sequence count, must be set by Ploc MPSoC Handler diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index f36c5a8d..6bc224e6 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -46,7 +46,7 @@ ReturnValue_t PlocSupervisorHandler::initialize() { if (result != returnvalue::OK) { return result; } - uartComIf = dynamic_cast(communicationInterface); + uartComIf = dynamic_cast(communicationInterface); if (uartComIf == nullptr) { sif::warning << "PlocSupervisorHandler::initialize: Invalid uart com if" << std::endl; return ObjectManagerIF::CHILD_INIT_FAILED; @@ -1917,6 +1917,11 @@ ReturnValue_t PlocSupervisorHandler::checkMramPacketApid() { } ReturnValue_t PlocSupervisorHandler::handleMramDumpFile(DeviceCommandId_t id) { +#ifdef XIPHOS_Q7S + if (not sdcMan->getActiveSdCard()) { + return HasFileSystemIF::FILESYSTEM_INACTIVE; + } +#endif ReturnValue_t result = returnvalue::OK; uint16_t packetLen = readSpacePacketLength(spacePacketBuffer); uint8_t sequenceFlags = readSequenceFlags(spacePacketBuffer); diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 4bdf2efa..65c23fa6 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -9,7 +9,7 @@ #include "fsfw/timemanager/Countdown.h" #include "fsfw_hal/linux/gpio/Gpio.h" #include "fsfw_hal/linux/gpio/LinuxLibgpioIF.h" -#include "fsfw_hal/linux/uart/UartComIF.h" +#include "fsfw_hal/linux/serial/SerialComIF.h" #include "linux/devices/devicedefinitions/PlocSupervisorDefinitions.h" #include "linux/devices/devicedefinitions/SupvReturnValuesIF.h" @@ -110,7 +110,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { */ DeviceCommandId_t nextReplyId = supv::NONE; - UartComIF* uartComIf = nullptr; + SerialComIF* uartComIf = nullptr; LinuxLibgpioIF* gpioComIF = nullptr; Gpio uartIsolatorSwitch; diff --git a/linux/devices/ploc/PlocSupvHelper.cpp b/linux/devices/ploc/PlocSupvHelper.cpp index f0f94635..5f34cb21 100644 --- a/linux/devices/ploc/PlocSupvHelper.cpp +++ b/linux/devices/ploc/PlocSupvHelper.cpp @@ -1,6 +1,7 @@ #include "PlocSupvHelper.h" #include +#include #include #include @@ -94,7 +95,7 @@ ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { } } -ReturnValue_t PlocSupvHelper::setComIF(UartComIF* uartComIF_) { +ReturnValue_t PlocSupvHelper::setComIF(SerialComIF* uartComIF_) { if (uartComIF_ == nullptr) { sif::warning << "PlocSupvHelper::initialize: Provided invalid uart com if" << std::endl; return returnvalue::FAILED; @@ -748,6 +749,11 @@ uint32_t PlocSupvHelper::getFileSize(std::string filename) { ReturnValue_t PlocSupvHelper::handleEventBufferReception(ploc::SpTmReader& reader) { ReturnValue_t result = returnvalue::OK; +#ifdef XIPHOS_Q7S + if (not sdcMan->getActiveSdCard()) { + return HasFileSystemIF::FILESYSTEM_INACTIVE; + } +#endif std::string filename = Filenaming::generateAbsoluteFilename( eventBufferReq.path, eventBufferReq.filename, timestamping); std::ofstream file(filename, std::ios_base::app | std::ios_base::out); diff --git a/linux/devices/ploc/PlocSupvHelper.h b/linux/devices/ploc/PlocSupvHelper.h index 30152e65..43fda22f 100644 --- a/linux/devices/ploc/PlocSupvHelper.h +++ b/linux/devices/ploc/PlocSupvHelper.h @@ -9,7 +9,7 @@ #include "fsfw/osal/linux/BinarySemaphore.h" #include "fsfw/returnvalues/returnvalue.h" #include "fsfw/tasks/ExecutableObjectIF.h" -#include "fsfw_hal/linux/uart/UartComIF.h" +#include "fsfw_hal/linux/serial/SerialComIF.h" #include "linux/devices/devicedefinitions/PlocSupervisorDefinitions.h" #ifdef XIPHOS_Q7S @@ -104,7 +104,7 @@ class PlocSupvHelper : public SystemObject, public ExecutableObjectIF { ReturnValue_t initialize() override; ReturnValue_t performOperation(uint8_t operationCode = 0) override; - ReturnValue_t setComIF(UartComIF* uartComfIF_); + ReturnValue_t setComIF(SerialComIF* uartComfIF_); void setComCookie(CookieIF* comCookie_); /** @@ -209,7 +209,7 @@ class PlocSupvHelper : public SystemObject, public ExecutableObjectIF { /** * Communication interface responsible for data transactions between OBC and Supervisor. */ - UartComIF* uartComIF = nullptr; + SerialComIF* uartComIF = nullptr; // Communication cookie. Must be set by the supervisor Handler CookieIF* comCookie = nullptr; diff --git a/linux/devices/startracker/StarTrackerHandler.cpp b/linux/devices/startracker/StarTrackerHandler.cpp index a4cd432c..ff632c99 100644 --- a/linux/devices/startracker/StarTrackerHandler.cpp +++ b/linux/devices/startracker/StarTrackerHandler.cpp @@ -240,13 +240,12 @@ void StarTrackerHandler::doStartUp() { // the device handler's submode to the star tracker's mode return; case StartupState::DONE: - submode = SUBMODE_BOOTLOADER; startupState = StartupState::IDLE; break; default: return; } - setMode(_MODE_TO_ON); + setMode(_MODE_TO_ON, SUBMODE_BOOTLOADER); } void StarTrackerHandler::doShutDown() { @@ -654,7 +653,7 @@ void StarTrackerHandler::fillCommandAndReplyMap() { } ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) { - if (this->mode == MODE_NORMAL && mode == MODE_ON) { + if (getMode() == MODE_NORMAL && mode == MODE_ON) { return TRANS_NOT_ALLOWED; } switch (mode) { @@ -678,7 +677,7 @@ ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t } void StarTrackerHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { - switch (mode) { + switch (getMode()) { case _MODE_TO_ON: doOnTransition(subModeFrom); break; @@ -698,17 +697,18 @@ void StarTrackerHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { } void StarTrackerHandler::doOnTransition(Submode_t subModeFrom) { - if (submode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_FIRMWARE) { + uint8_t dhbSubmode = getSubmode(); + if (dhbSubmode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_FIRMWARE) { bootBootloader(); - } else if (submode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_FIRMWARE) { + } else if (dhbSubmode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_FIRMWARE) { setMode(MODE_ON); - } else if (submode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_BOOTLOADER) { + } else if (dhbSubmode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_BOOTLOADER) { bootFirmware(MODE_ON); - } else if (submode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_BOOTLOADER) { + } else if (dhbSubmode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_BOOTLOADER) { setMode(MODE_ON); - } else if (submode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_NONE) { + } else if (dhbSubmode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_NONE) { setMode(MODE_ON); - } else if (submode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_NONE) { + } else if (dhbSubmode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_NONE) { setMode(MODE_ON); } } @@ -2067,13 +2067,13 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) { case startracker::REQ_SUBSCRIPTION: case startracker::REQ_LOG_SUBSCRIPTION: case startracker::REQ_DEBUG_CAMERA: - if (not(mode == MODE_ON && submode == startracker::Program::FIRMWARE)) { + if (not(getMode() == MODE_ON && getSubmode() == startracker::Program::FIRMWARE)) { return STARTRACKER_RUNNING_BOOTLOADER; } break; case startracker::FIRMWARE_UPDATE: case startracker::FLASH_READ: - if (not(mode == MODE_ON && submode == startracker::Program::BOOTLOADER)) { + if (not(getMode() == MODE_ON && getSubmode() == startracker::Program::BOOTLOADER)) { return STARTRACKER_RUNNING_FIRMWARE; } break; diff --git a/linux/devices/startracker/StarTrackerHandler.h b/linux/devices/startracker/StarTrackerHandler.h index beab9d54..029b2bde 100644 --- a/linux/devices/startracker/StarTrackerHandler.h +++ b/linux/devices/startracker/StarTrackerHandler.h @@ -72,7 +72,7 @@ class StarTrackerHandler : public DeviceHandlerBase { ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) override; /** - * @brief Overwritten here to always read all available data from the UartComIF. + * @brief Overwritten here to always read all available data from theSerialComIF. */ virtual size_t getNextReplyLength(DeviceCommandId_t deviceCommand) override; virtual ReturnValue_t doSendReadHook() override; diff --git a/linux/devices/startracker/StrHelper.cpp b/linux/devices/startracker/StrHelper.cpp index 685cf3b3..dcf56b59 100644 --- a/linux/devices/startracker/StrHelper.cpp +++ b/linux/devices/startracker/StrHelper.cpp @@ -1,5 +1,7 @@ #include "StrHelper.h" +#include + #include #include @@ -83,7 +85,7 @@ ReturnValue_t StrHelper::performOperation(uint8_t operationCode) { } ReturnValue_t StrHelper::setComIF(DeviceCommunicationIF* communicationInterface_) { - uartComIF = dynamic_cast(communicationInterface_); + uartComIF = dynamic_cast(communicationInterface_); if (uartComIF == nullptr) { sif::warning << "StrHelper::initialize: Invalid uart com if" << std::endl; return returnvalue::FAILED; @@ -176,6 +178,11 @@ void StrHelper::disableTimestamping() { timestamping = false; } void StrHelper::enableTimestamping() { timestamping = true; } ReturnValue_t StrHelper::performImageDownload() { +#ifdef XIPHOS_Q7S + if (not sdcMan->getActiveSdCard()) { + return HasFileSystemIF::FILESYSTEM_INACTIVE; + } +#endif ReturnValue_t result; #if OBSW_DEBUG_STARTRACKER == 1 ProgressPrinter progressPrinter("Image download", ImageDownload::LAST_POSITION); @@ -244,6 +251,11 @@ ReturnValue_t StrHelper::performImageUpload() { uint32_t imageSize = 0; struct UploadActionRequest uploadReq; uploadReq.position = 0; +#ifdef XIPHOS_Q7S + if (not sdcMan->getActiveSdCard()) { + return HasFileSystemIF::FILESYSTEM_INACTIVE; + } +#endif std::memset(&uploadReq.data, 0, sizeof(uploadReq.data)); if (not std::filesystem::exists(uploadImage.uploadFile)) { triggerEvent(STR_HELPER_FILE_NOT_EXISTS, static_cast(internalState)); @@ -315,6 +327,11 @@ ReturnValue_t StrHelper::performFirmwareUpdate() { } ReturnValue_t StrHelper::performFlashWrite() { +#ifdef XIPHOS_Q7S + if (not sdcMan->getActiveSdCard()) { + return HasFileSystemIF::FILESYSTEM_INACTIVE; + } +#endif ReturnValue_t result = returnvalue::OK; uint32_t size = 0; uint32_t bytesWritten = 0; @@ -394,6 +411,11 @@ ReturnValue_t StrHelper::performFlashWrite() { } ReturnValue_t StrHelper::performFlashRead() { +#ifdef XIPHOS_Q7S + if (not sdcMan->getActiveSdCard()) { + return HasFileSystemIF::FILESYSTEM_INACTIVE; + } +#endif ReturnValue_t result; #if OBSW_DEBUG_STARTRACKER == 1 ProgressPrinter progressPrinter("Flash read", flashRead.size); diff --git a/linux/devices/startracker/StrHelper.h b/linux/devices/startracker/StrHelper.h index a9c80d77..3d78f9a4 100644 --- a/linux/devices/startracker/StrHelper.h +++ b/linux/devices/startracker/StrHelper.h @@ -15,7 +15,7 @@ #include "fsfw/osal/linux/BinarySemaphore.h" #include "fsfw/returnvalues/returnvalue.h" #include "fsfw/tasks/ExecutableObjectIF.h" -#include "fsfw_hal/linux/uart/UartComIF.h" +#include "fsfw_hal/linux/serial/SerialComIF.h" extern "C" { #include "thirdparty/arcsec_star_tracker/client/generated/actionreq.h" @@ -255,7 +255,7 @@ class StrHelper : public SystemObject, public ExecutableObjectIF { * UART communication object responsible for low level access of star tracker * Must be set by star tracker handler */ - UartComIF* uartComIF = nullptr; + SerialComIF* uartComIF = nullptr; // Communication cookie. Must be set by the star tracker handler CookieIF* comCookie = nullptr; diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 69e22777..87c790d7 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 233 translations. + * @brief Auto-generated event translation file. Contains 234 translations. * @details - * Generated on: 2022-10-21 15:41:33 + * Generated on: 2022-11-10 18:07:26 */ #include "translateEvents.h" @@ -144,6 +144,7 @@ const char *CARRIER_LOCK_STRING = "CARRIER_LOCK"; const char *BIT_LOCK_PDEC_STRING = "BIT_LOCK_PDEC"; const char *LOST_CARRIER_LOCK_PDEC_STRING = "LOST_CARRIER_LOCK_PDEC"; const char *LOST_BIT_LOCK_PDEC_STRING = "LOST_BIT_LOCK_PDEC"; +const char *POLL_ERROR_PDEC_STRING = "POLL_ERROR_PDEC"; const char *IMAGE_UPLOAD_FAILED_STRING = "IMAGE_UPLOAD_FAILED"; const char *IMAGE_DOWNLOAD_FAILED_STRING = "IMAGE_DOWNLOAD_FAILED"; const char *IMAGE_UPLOAD_SUCCESSFUL_STRING = "IMAGE_UPLOAD_SUCCESSFUL"; @@ -515,6 +516,8 @@ const char *translateEvents(Event event) { return LOST_CARRIER_LOCK_PDEC_STRING; case (12406): return LOST_BIT_LOCK_PDEC_STRING; + case (12407): + return POLL_ERROR_PDEC_STRING; case (12500): return IMAGE_UPLOAD_FAILED_STRING; case (12501): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index 65f8ea62..cfb7f9a5 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -1,8 +1,8 @@ /** * @brief Auto-generated object translation file. * @details - * Contains 140 translations. - * Generated on: 2022-10-21 15:41:33 + * Contains 144 translations. + * Generated on: 2022-11-10 18:07:26 */ #include "translateObjects.h" @@ -139,11 +139,15 @@ const char *ACS_BOARD_ASS_STRING = "ACS_BOARD_ASS"; const char *SUS_BOARD_ASS_STRING = "SUS_BOARD_ASS"; const char *TCS_BOARD_ASS_STRING = "TCS_BOARD_ASS"; const char *RW_ASS_STRING = "RW_ASS"; -const char *CFDP_HANDLER_STRING = "CFDP_HANDLER"; -const char *CFDP_DISTRIBUTOR_STRING = "CFDP_DISTRIBUTOR"; +const char *CAM_SWITCHER_STRING = "CAM_SWITCHER"; const char *TM_FUNNEL_STRING = "TM_FUNNEL"; const char *PUS_TM_FUNNEL_STRING = "PUS_TM_FUNNEL"; const char *CFDP_TM_FUNNEL_STRING = "CFDP_TM_FUNNEL"; +const char *CFDP_HANDLER_STRING = "CFDP_HANDLER"; +const char *CFDP_DISTRIBUTOR_STRING = "CFDP_DISTRIBUTOR"; +const char *EIVE_SYSTEM_STRING = "EIVE_SYSTEM"; +const char *ACS_SUBSYSTEM_STRING = "ACS_SUBSYSTEM"; +const char *PL_SUBSYSTEM_STRING = "PL_SUBSYSTEM"; const char *CCSDS_IP_CORE_BRIDGE_STRING = "CCSDS_IP_CORE_BRIDGE"; const char *NO_OBJECT_STRING = "NO_OBJECT"; @@ -415,16 +419,24 @@ const char *translateObject(object_id_t object) { return TCS_BOARD_ASS_STRING; case 0x73000004: return RW_ASS_STRING; - case 0x73000005: - return CFDP_HANDLER_STRING; case 0x73000006: - return CFDP_DISTRIBUTOR_STRING; + return CAM_SWITCHER_STRING; case 0x73000100: return TM_FUNNEL_STRING; case 0x73000101: return PUS_TM_FUNNEL_STRING; case 0x73000102: return CFDP_TM_FUNNEL_STRING; + case 0x73000205: + return CFDP_HANDLER_STRING; + case 0x73000206: + return CFDP_DISTRIBUTOR_STRING; + case 0x73010000: + return EIVE_SYSTEM_STRING; + case 0x73010001: + return ACS_SUBSYSTEM_STRING; + case 0x73010002: + return PL_SUBSYSTEM_STRING; case 0x73500000: return CCSDS_IP_CORE_BRIDGE_STRING; case 0xFFFFFFFF: diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 0d75588f..e7f753bd 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -430,11 +430,21 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) { uint32_t length = thisSequence->getPeriodMs(); static_cast(length); #if OBSW_ADD_MGT == 1 - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ); #endif #if OBSW_ADD_BPX_BATTERY_HANDLER == 1 thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -450,32 +460,9 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) { ReturnValue_t pst::pstUart(FixedTimeslotTaskIF *thisSequence) { // Length of a communication cycle uint32_t length = thisSequence->getPeriodMs(); - bool uartPstEmpty = true; - -#if OBSW_ADD_PLOC_MPSOC == 1 - uartPstEmpty = false; - thisSequence->addSlot(objects::PLOC_MPSOC_HANDLER, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::PLOC_MPSOC_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::PLOC_MPSOC_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::PLOC_MPSOC_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::PLOC_MPSOC_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ); -#endif - - thisSequence->addSlot(objects::PLOC_MEMORY_DUMPER, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); -#if OBSW_ADD_PLOC_SUPERVISOR == 1 - thisSequence->addSlot(objects::PLOC_SUPERVISOR_HANDLER, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::PLOC_SUPERVISOR_HANDLER, length * 0.2, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::PLOC_SUPERVISOR_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::PLOC_SUPERVISOR_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::PLOC_SUPERVISOR_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ); -#endif + static_cast(length); #if OBSW_ADD_SYRLINKS == 1 - uartPstEmpty = false; thisSequence->addSlot(objects::SYRLINKS_HK_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::SYRLINKS_HK_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE); @@ -485,22 +472,13 @@ ReturnValue_t pst::pstUart(FixedTimeslotTaskIF *thisSequence) { #endif #if OBSW_ADD_STAR_TRACKER == 1 - uartPstEmpty = false; thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::STAR_TRACKER, length * 0.2, DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::STAR_TRACKER, length * 0.4, DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::STAR_TRACKER, length * 0.6, DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::STAR_TRACKER, length * 0.8, DeviceHandlerIF::GET_READ); #endif - static_cast(length); - if (uartPstEmpty) { - return returnvalue::OK; - } - if (thisSequence->checkSequence() != returnvalue::OK) { - sif::error << "UART PST initialization failed" << std::endl; - return returnvalue::FAILED; - } - return returnvalue::OK; + return thisSequence->checkSequence(); } ReturnValue_t pst::pstGompaceCan(FixedTimeslotTaskIF *thisSequence) { diff --git a/linux/obc/AxiPtmeConfig.cpp b/linux/ipcore/AxiPtmeConfig.cpp similarity index 98% rename from linux/obc/AxiPtmeConfig.cpp rename to linux/ipcore/AxiPtmeConfig.cpp index 7c1f90c6..ead3d691 100644 --- a/linux/obc/AxiPtmeConfig.cpp +++ b/linux/ipcore/AxiPtmeConfig.cpp @@ -4,7 +4,7 @@ #include "fsfw_hal/linux/uio/UioMapper.h" AxiPtmeConfig::AxiPtmeConfig(object_id_t objectId, std::string axiUio, int mapNum) - : SystemObject(objectId), axiUio(axiUio), mapNum(mapNum) { + : SystemObject(objectId), axiUio(std::move(axiUio)), mapNum(mapNum) { mutex = MutexFactory::instance()->createMutex(); if (mutex == nullptr) { sif::warning << "Failed to create mutex" << std::endl; diff --git a/linux/obc/AxiPtmeConfig.h b/linux/ipcore/AxiPtmeConfig.h similarity index 100% rename from linux/obc/AxiPtmeConfig.h rename to linux/ipcore/AxiPtmeConfig.h diff --git a/linux/obc/CMakeLists.txt b/linux/ipcore/CMakeLists.txt similarity index 100% rename from linux/obc/CMakeLists.txt rename to linux/ipcore/CMakeLists.txt diff --git a/linux/obc/PapbVcInterface.cpp b/linux/ipcore/PapbVcInterface.cpp similarity index 97% rename from linux/obc/PapbVcInterface.cpp rename to linux/ipcore/PapbVcInterface.cpp index fc23fed8..b8b12c7a 100644 --- a/linux/obc/PapbVcInterface.cpp +++ b/linux/ipcore/PapbVcInterface.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include "fsfw/serviceinterface/ServiceInterface.h" @@ -8,7 +8,7 @@ PapbVcInterface::PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbBusyId, : gpioComIF(gpioComIF), papbBusyId(papbBusyId), papbEmptyId(papbEmptyId), - uioFile(uioFile), + uioFile(std::move(uioFile)), mapNum(mapNum) {} PapbVcInterface::~PapbVcInterface() {} diff --git a/linux/obc/PapbVcInterface.h b/linux/ipcore/PapbVcInterface.h similarity index 98% rename from linux/obc/PapbVcInterface.h rename to linux/ipcore/PapbVcInterface.h index 6162765e..83081d9d 100644 --- a/linux/obc/PapbVcInterface.h +++ b/linux/ipcore/PapbVcInterface.h @@ -6,7 +6,7 @@ #include "OBSWConfig.h" #include "fsfw/returnvalues/returnvalue.h" -#include "linux/obc/VcInterfaceIF.h" +#include "linux/ipcore/VcInterfaceIF.h" /** * @brief This class handles the transmission of data to a virtual channel of the PTME IP Core diff --git a/linux/obc/PdecConfig.cpp b/linux/ipcore/PdecConfig.cpp similarity index 82% rename from linux/obc/PdecConfig.cpp rename to linux/ipcore/PdecConfig.cpp index ac762a6c..321e5940 100644 --- a/linux/obc/PdecConfig.cpp +++ b/linux/ipcore/PdecConfig.cpp @@ -31,3 +31,8 @@ uint32_t PdecConfig::getConfigWord(uint8_t wordNo) { } return configWords[wordNo]; } + +uint32_t PdecConfig::getImrReg() { + return static_cast(enableNewFarIrq << 2) | + static_cast(enableTcAbortIrq << 1) | static_cast(enableTcNewIrq); +} diff --git a/linux/obc/PdecConfig.h b/linux/ipcore/PdecConfig.h similarity index 92% rename from linux/obc/PdecConfig.h rename to linux/ipcore/PdecConfig.h index e037dd56..284af6ef 100644 --- a/linux/obc/PdecConfig.h +++ b/linux/ipcore/PdecConfig.h @@ -23,6 +23,7 @@ class PdecConfig { * @brief Returns the configuration word by specifying the position. */ uint32_t getConfigWord(uint8_t wordNo); + uint32_t getImrReg(); private: // TC transfer frame configuration parameters @@ -45,6 +46,9 @@ class PdecConfig { static const uint8_t CONFIG_WORDS_NUM = 2; uint32_t configWords[CONFIG_WORDS_NUM]; + bool enableTcNewIrq = true; + bool enableTcAbortIrq = true; + bool enableNewFarIrq = true; void initialize(); }; diff --git a/linux/obc/PdecHandler.cpp b/linux/ipcore/PdecHandler.cpp similarity index 81% rename from linux/obc/PdecHandler.cpp rename to linux/ipcore/PdecHandler.cpp index 6199853f..bc33a220 100644 --- a/linux/obc/PdecHandler.cpp +++ b/linux/ipcore/PdecHandler.cpp @@ -1,7 +1,9 @@ #include "PdecHandler.h" #include +#include #include +#include #include #include @@ -12,18 +14,21 @@ #include "fsfw/serviceinterface/ServiceInterface.h" #include "fsfw/tmtcservices/TmTcMessage.h" #include "fsfw_hal/linux/uio/UioMapper.h" +#include "pdec.h" + +using namespace pdec; + +// If this is ever shared, protect it with a mutex! +uint32_t PdecHandler::CURRENT_FAR = 0; PdecHandler::PdecHandler(object_id_t objectId, object_id_t tcDestinationId, - LinuxLibgpioIF* gpioComIF, gpioId_t pdecReset, std::string uioConfigMemory, - std::string uioRamMemory, std::string uioRegisters) + LinuxLibgpioIF* gpioComIF, gpioId_t pdecReset, UioNames names) : SystemObject(objectId), tcDestinationId(tcDestinationId), gpioComIF(gpioComIF), pdecReset(pdecReset), - uioConfigMemory(uioConfigMemory), - uioRamMemory(uioRamMemory), - uioRegisters(uioRegisters), - actionHelper(this, nullptr) { + actionHelper(this, nullptr), + uioNames(names) { auto mqArgs = MqArgs(objectId, static_cast(this)); commandQueue = QueueFactory::instance()->createMessageQueue( QUEUE_SIZE, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); @@ -47,29 +52,39 @@ ReturnValue_t PdecHandler::initialize() { ReturnValue_t result = returnvalue::OK; - UioMapper regMapper(uioRegisters); + UioMapper regMapper(uioNames.registers); result = regMapper.getMappedAdress(®isterBaseAddress, UioMapper::Permissions::READ_WRITE); if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } - UioMapper configMemMapper(uioConfigMemory); + UioMapper configMemMapper(uioNames.configMemory); result = configMemMapper.getMappedAdress(&memoryBaseAddress, UioMapper::Permissions::READ_WRITE); if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } - UioMapper ramMapper(uioRamMemory); + UioMapper ramMapper(uioNames.ramMemory); result = ramMapper.getMappedAdress(&ramBaseAddress, UioMapper::Permissions::READ_WRITE); if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } - writePdecConfig(); + if (OP_MODE == Modes::IRQ and uioNames.irq == nullptr) { + sif::error << "Can not use IRQ mode if IRQ UIO name is invalid" << std::endl; + return returnvalue::FAILED; + } + PdecConfig pdecConfig; + writePdecConfigDuringReset(pdecConfig); result = releasePdec(); if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } + // This configuration must be done while the PDEC is not held in reset. + if (OP_MODE == Modes::IRQ) { + // Configure interrupt mask register to enable interrupts + *(registerBaseAddress + PDEC_IMR_OFFSET) = pdecConfig.getImrReg(); + } result = actionHelper.initialize(commandQueue); if (result != returnvalue::OK) { return result; @@ -78,59 +93,16 @@ ReturnValue_t PdecHandler::initialize() { return returnvalue::OK; } -MessageQueueId_t PdecHandler::getCommandQueue() const { return commandQueue->getId(); } - -void PdecHandler::writePdecConfig() { - PdecConfig pdecConfig; - - *(memoryBaseAddress + FRAME_HEADER_OFFSET) = pdecConfig.getConfigWord(0); - *(memoryBaseAddress + FRAME_HEADER_OFFSET + 1) = pdecConfig.getConfigWord(1); - - // Configure all MAP IDs as invalid - for (int idx = 0; idx <= MAX_MAP_ADDR; idx += 4) { - *(memoryBaseAddress + MAP_ADDR_LUT_OFFSET + idx + 1 / 4) = - NO_DESTINATION << 24 | NO_DESTINATION << 16 | NO_DESTINATION << 8 | NO_DESTINATION; - } - - // All TCs with MAP ID 7 will be routed to the PM module (can then be read from memory) - uint8_t routeToPm = calcMapAddrEntry(PM_BUFFER); - *(memoryBaseAddress + MAP_ADDR_LUT_OFFSET + 1) = - (NO_DESTINATION << 24) | (NO_DESTINATION << 16) | (NO_DESTINATION << 8) | routeToPm; - - // Write map id clock frequencies - for (int idx = 0; idx <= MAX_MAP_ADDR; idx += 4) { - *(memoryBaseAddress + MAP_CLK_FREQ_OFFSET + idx / 4) = - MAP_CLK_FREQ << 24 | MAP_CLK_FREQ << 16 | MAP_CLK_FREQ << 8 | MAP_CLK_FREQ; - } -} - -ReturnValue_t PdecHandler::resetFarStatFlag() { - uint32_t pdecFar = *(registerBaseAddress + PDEC_FAR_OFFSET); - if (pdecFar != FAR_RESET) { - sif::warning << "PdecHandler::resetFarStatFlag: FAR register did not match expected value." - << " Read value: 0x" << std::hex << static_cast(pdecFar) - << std::endl; - return returnvalue::FAILED; - } -#if OBSW_DEBUG_PDEC_HANDLER == 1 - sif::debug << "PdecHandler::resetFarStatFlag: read FAR with value: 0x" << std::hex << pdecFar - << std::endl; -#endif /* OBSW_DEBUG_PDEC_HANDLER == 1 */ - return returnvalue::OK; -} - -ReturnValue_t PdecHandler::releasePdec() { - ReturnValue_t result = returnvalue::OK; - result = gpioComIF->pullHigh(pdecReset); - if (result != returnvalue::OK) { - sif::error << "PdecHandler::releasePdec: Failed to release PDEC reset signal" << std::endl; - } - return result; -} - ReturnValue_t PdecHandler::performOperation(uint8_t operationCode) { - ReturnValue_t result = returnvalue::OK; + if (OP_MODE == Modes::POLLED) { + return polledOperation(); + } else if (OP_MODE == Modes::IRQ) { + return irqOperation(); + } +} +ReturnValue_t PdecHandler::polledOperation() { + ReturnValue_t result = returnvalue::OK; readCommandQueue(); switch (state) { @@ -153,13 +125,94 @@ ReturnValue_t PdecHandler::performOperation(uint8_t operationCode) { case State::WAIT_FOR_RECOVERY: break; default: - sif::debug << "PdecHandler::performOperation: Invalid state" << std::endl; + sif::error << "PdecHandler::performOperation: Invalid state" << std::endl; break; } return returnvalue::OK; } +ReturnValue_t PdecHandler::irqOperation() { + ReturnValue_t result = returnvalue::OK; + int fd = open(uioNames.irq, O_RDWR); + if (fd < 0) { + sif::error << "PdecHandler::irqOperation: Opening UIO IRQ file" << uioNames.irq << " failed" + << std::endl; + return returnvalue::FAILED; + } + + struct pollfd fds = {.fd = fd, .events = POLLIN, .revents = 0}; + // Used to unmask IRQ + uint32_t info = 1; + ssize_t nb = 0; + int ret = 0; + // Clear interrupts with dummy read before unmasking the interrupt + ret = *(registerBaseAddress + PDEC_PIR_OFFSET); + + while (true) { + readCommandQueue(); + switch (state) { + case State::INIT: + resetFarStatFlag(); + if (result != returnvalue::OK) { + // Requires reconfiguration and reinitialization of PDEC + triggerEvent(INVALID_FAR); + state = State::WAIT_FOR_RECOVERY; + return result; + } + state = State::RUNNING; + break; + case State::RUNNING: { + nb = write(fd, &info, sizeof(info)); + if (nb != static_cast(sizeof(info))) { + sif::error << "PdecHandler::irqOperation: Unmasking IRQ failed" << std::endl; + close(fd); + } + + ret = poll(&fds, 1, IRQ_TIMEOUT_MS); + if (ret == 0) { + // No TCs for timeout period + checkLocks(); + lockCheckCd.resetTimer(); + } else if (ret >= 1) { + nb = read(fd, &info, sizeof(info)); + if (nb == static_cast(sizeof(info))) { + uint32_t pisr = *(registerBaseAddress + PDEC_PISR_OFFSET); + if ((pisr & TC_NEW_MASK) == TC_NEW_MASK) { + // handle TC + handleNewTc(); + } + if ((pisr & TC_ABORT_MASK) == TC_ABORT_MASK) { + tcAbortCounter += 1; + } + if ((pisr & NEW_FAR_MASK) == NEW_FAR_MASK) { + // Read FAR here + CURRENT_FAR = readFar(); + } + if (lockCheckCd.hasTimedOut()) { + checkLocks(); + lockCheckCd.resetTimer(); + } + // Clear interrupts with dummy read + ret = *(registerBaseAddress + PDEC_PIR_OFFSET); + } + } else { + sif::error << "PdecHandler::irqOperation: Poll error with errno " << errno << ": " + << strerror(errno) << std::endl; + triggerEvent(POLL_ERROR_PDEC, errno); + } + break; + } + case State::WAIT_FOR_RECOVERY: + break; + default: + sif::error << "PdecHandler::performOperation: Invalid state" << std::endl; + break; + } + } + return returnvalue::OK; +} + void PdecHandler::readCommandQueue(void) { CommandMessage commandMessage; ReturnValue_t result = returnvalue::FAILED; @@ -177,13 +230,65 @@ void PdecHandler::readCommandQueue(void) { } } +MessageQueueId_t PdecHandler::getCommandQueue() const { return commandQueue->getId(); } + +void PdecHandler::writePdecConfigDuringReset(PdecConfig& pdecConfig) { + *(memoryBaseAddress + FRAME_HEADER_OFFSET) = pdecConfig.getConfigWord(0); + *(memoryBaseAddress + FRAME_HEADER_OFFSET + 1) = pdecConfig.getConfigWord(1); + + // Configure all MAP IDs as invalid + for (int idx = 0; idx <= MAX_MAP_ADDR; idx += 4) { + *(memoryBaseAddress + MAP_ADDR_LUT_OFFSET + idx / 4) = + NO_DESTINATION << 24 | NO_DESTINATION << 16 | NO_DESTINATION << 8 | NO_DESTINATION; + } + + // All TCs with MAP ID 7 will be routed to the PM module (can then be read from memory) + uint8_t routeToPm = calcMapAddrEntry(PM_BUFFER); + *(memoryBaseAddress + MAP_ADDR_LUT_OFFSET + 1) = + (NO_DESTINATION << 24) | (NO_DESTINATION << 16) | (NO_DESTINATION << 8) | routeToPm; + + // Write map id clock frequencies + for (int idx = 0; idx <= MAX_MAP_ADDR; idx += 4) { + *(memoryBaseAddress + MAP_CLK_FREQ_OFFSET + idx / 4) = + MAP_CLK_FREQ << 24 | MAP_CLK_FREQ << 16 | MAP_CLK_FREQ << 8 | MAP_CLK_FREQ; + } +} + +ReturnValue_t PdecHandler::resetFarStatFlag() { + uint32_t pdecFar = readFar(); + if ((pdecFar & FAR_STAT_MASK) != 0) { + sif::warning << "PdecHandler::resetFarStatFlag: FAR register stat bit is not 0." + << " Read value for FAR: 0x" << std::hex << static_cast(pdecFar) + << std::endl; + CURRENT_FAR = pdecFar; + return returnvalue::FAILED; + } +#if OBSW_DEBUG_PDEC_HANDLER == 1 + sif::debug << "PdecHandler::resetFarStatFlag: read FAR with value: 0x" << std::hex << pdecFar + << std::endl; +#endif /* OBSW_DEBUG_PDEC_HANDLER == 1 */ + CURRENT_FAR = pdecFar; + return returnvalue::OK; +} + +ReturnValue_t PdecHandler::releasePdec() { + ReturnValue_t result = returnvalue::OK; + result = gpioComIF->pullHigh(pdecReset); + if (result != returnvalue::OK) { + sif::error << "PdecHandler::releasePdec: Failed to release PDEC reset signal" << std::endl; + } + return result; +} + bool PdecHandler::newTcReceived() { - uint32_t pdecFar = *(registerBaseAddress + PDEC_FAR_OFFSET); + uint32_t pdecFar = readFar(); if (pdecFar >> STAT_POSITION != NEW_FAR_RECEIVED) { + CURRENT_FAR = pdecFar; return false; } if (!checkFrameAna(pdecFar)) { + CURRENT_FAR = pdecFar; return false; } return true; @@ -331,7 +436,6 @@ void PdecHandler::handleNewTc() { printTC(tcLength); #endif /* OBSW_DEBUG_PDEC_HANDLER */ -#if OBSW_TC_FROM_PDEC == 1 store_address_t storeId; result = tcStore->addData(&storeId, tcSegment + 1, tcLength - 1); if (result != returnvalue::OK) { @@ -349,7 +453,6 @@ void PdecHandler::handleNewTc() { tcStore->deleteData(storeId); return; } -#endif /* OBSW_TC_FROM_PDEC == 1 */ return; } @@ -500,6 +603,8 @@ void PdecHandler::printPdecMon() { sif::info << std::setw(30) << std::left << "Start sequence lock: " << lock << std::endl; } +uint32_t PdecHandler::readFar() { return *(registerBaseAddress + PDEC_FAR_OFFSET); } + std::string PdecHandler::getMonStatusString(uint32_t status) { switch (status) { case TC_CHANNEL_INACTIVE: diff --git a/linux/obc/PdecHandler.h b/linux/ipcore/PdecHandler.h similarity index 84% rename from linux/obc/PdecHandler.h rename to linux/ipcore/PdecHandler.h index 57adfa9f..b7089999 100644 --- a/linux/obc/PdecHandler.h +++ b/linux/ipcore/PdecHandler.h @@ -1,6 +1,8 @@ #ifndef LINUX_OBC_PDECHANDLER_H_ #define LINUX_OBC_PDECHANDLER_H_ +#include + #include "OBSWConfig.h" #include "PdecConfig.h" #include "fsfw/action/ActionHelper.h" @@ -13,6 +15,13 @@ #include "fsfw_hal/common/gpio/gpioDefinitions.h" #include "fsfw_hal/linux/gpio/LinuxLibgpioIF.h" +struct UioNames { + const char* configMemory; + const char* ramMemory; + const char* registers; + const char* irq; +}; + /** * @brief This class controls the PDEC IP Core implemented in the programmable logic of the * Zynq-7020. All registers and memories of the PDEC IP Core are accessed via UIO @@ -33,6 +42,10 @@ */ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasActionsIF { public: + static constexpr dur_millis_t IRQ_TIMEOUT_MS = 500; + + enum class Modes { POLLED, IRQ }; + /** * @brief Constructor * @param objectId Object ID of PDEC handler system object @@ -43,8 +56,7 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc * @param uioregsiters String of uio device file same mapped to the PDEC register space */ PdecHandler(object_id_t objectId, object_id_t tcDestinationId, LinuxLibgpioIF* gpioComIF, - gpioId_t pdecReset, std::string uioConfigMemory, std::string uioRamMemory, - std::string uioRegisters); + gpioId_t pdecReset, UioNames names); virtual ~PdecHandler(); @@ -74,10 +86,13 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc static const Event LOST_CARRIER_LOCK_PDEC = MAKE_EVENT(5, severity::INFO); //! [EXPORT] : [COMMENT] Lost bit lock static const Event LOST_BIT_LOCK_PDEC = MAKE_EVENT(6, severity::INFO); + static constexpr Event POLL_ERROR_PDEC = event::makeEvent(SUBSYSTEM_ID, 7, severity::MEDIUM); private: static const uint8_t INTERFACE_ID = CLASS_ID::PDEC_HANDLER; + static constexpr Modes OP_MODE = Modes::IRQ; + static const ReturnValue_t ABANDONED_CLTU = MAKE_RETURN_CODE(0xA0); static const ReturnValue_t FRAME_DIRTY = MAKE_RETURN_CODE(0xA1); static const ReturnValue_t FRAME_ILLEGAL_ONE_REASON = MAKE_RETURN_CODE(0xA2); @@ -112,48 +127,6 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc // Print PDEC monitor register static const ActionId_t PRINT_PDEC_MON = 1; - static const uint8_t STAT_POSITION = 31; - static const uint8_t FRAME_ANA_POSITION = 28; - static const uint8_t IREASON_POSITION = 25; - - static const uint8_t NEW_FAR_RECEIVED = 0; - - static const uint32_t FRAME_ANA_MASK = 0x70000000; - static const uint32_t IREASON_MASK = 0x0E000000; - - static const uint32_t TC_CHANNEL_INACTIVE = 0x0; - static const uint32_t TC_CHANNEL_ACTIVE = 0x1; - static const uint32_t TC_CHANNEL_TIMEDOUT = 0x2; - - static const uint32_t TC0_STATUS_MASK = 0x3; - static const uint32_t TC1_STATUS_MASK = 0xC; - static const uint32_t TC2_STATUS_MASK = 0x300; - static const uint32_t TC3_STATUS_MASK = 0xC00; - static const uint32_t TC4_STATUS_MASK = 0x30000; - static const uint32_t TC5_STATUS_MASK = 0xc00000; - // Lock register set to 1 when start sequence has been found (CLTU is beeing processed) - static const uint32_t LOCK_MASK = 0xc00000; - - static const uint32_t TC0_STATUS_POS = 0; - static const uint32_t TC1_STATUS_POS = 2; - static const uint32_t TC2_STATUS_POS = 4; - static const uint32_t TC3_STATUS_POS = 6; - static const uint32_t TC4_STATUS_POS = 8; - static const uint32_t TC5_STATUS_POS = 10; - // Lock register set to 1 when start sequence has been found (CLTU is beeing processed) - static const uint32_t LOCK_POS = 12; - - /** - * UIO is 4 byte aligned. Thus offset is calculated with "true offset" / 4 - * Example: PDEC_FAR = 0x2840 => Offset in virtual address space is 0xA10 - */ - static const uint32_t PDEC_FAR_OFFSET = 0xA10; - static const uint32_t PDEC_CLCW_OFFSET = 0xA12; - static const uint32_t PDEC_BFREE_OFFSET = 0xA24; - static const uint32_t PDEC_BPTR_OFFSET = 0xA25; - static const uint32_t PDEC_SLEN_OFFSET = 0xA26; - static const uint32_t PDEC_MON_OFFSET = 0xA27; - #ifdef TE0720_1CFA static const int CONFIG_MEMORY_MAP_SIZE = 0x400; static const int RAM_MAP_SIZE = 0x4000; @@ -227,16 +200,69 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc enum class State : uint8_t { INIT, RUNNING, WAIT_FOR_RECOVERY }; + static uint32_t CURRENT_FAR; + + Countdown lockCheckCd = Countdown(IRQ_TIMEOUT_MS); + object_id_t tcDestinationId; + + AcceptsTelecommandsIF* tcDestination = nullptr; + + LinuxLibgpioIF* gpioComIF = nullptr; + + /** + * Reset signal is required to hold PDEC in reset state until the configuration has been + * written to the appropriate memory space. + * Can also be used to reboot PDEC in case of erros. + */ + gpioId_t pdecReset = gpio::NO_GPIO; + + uint32_t tcAbortCounter = 0; + + ActionHelper actionHelper; + + StorageManagerIF* tcStore = nullptr; + + MessageQueueIF* commandQueue = nullptr; + + State state = State::INIT; + + /** + * Pointer pointing to base address of the PDEC memory space. + * This address is equivalent with the base address of the section named configuration area in + * the PDEC datasheet. + */ + uint32_t* memoryBaseAddress = nullptr; + + uint32_t* ramBaseAddress = nullptr; + + // Pointer pointing to base address of register space + uint32_t* registerBaseAddress = nullptr; + + uint8_t tcSegment[TC_SEGMENT_LEN]; + + // Used to check carrier and bit lock changes (default set to no rf and no bitlock) + uint32_t lastClcw = 0xC000; + + bool carrierLock = false; + bool bitLock = false; + + UioNames uioNames; + /** * @brief Reads and handles messages stored in the commandQueue */ void readCommandQueue(void); + ReturnValue_t polledOperation(); + ReturnValue_t irqOperation(); + + uint32_t readFar(); + /** * @brief This functions writes the configuration parameters to the configuration * section of the PDEC. */ - void writePdecConfig(); + void writePdecConfigDuringReset(PdecConfig& config); /** * @brief Reading the FAR resets the set stat flag which signals a new TC. Without clearing @@ -343,58 +369,6 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc void printPdecMon(); std::string getMonStatusString(uint32_t status); - - object_id_t tcDestinationId; - - AcceptsTelecommandsIF* tcDestination = nullptr; - - LinuxLibgpioIF* gpioComIF = nullptr; - - /** - * Reset signal is required to hold PDEC in reset state until the configuration has been - * written to the appropriate memory space. - * Can also be used to reboot PDEC in case of erros. - */ - gpioId_t pdecReset = gpio::NO_GPIO; - - // UIO device file giving access to the PDEC configuration memory section - std::string uioConfigMemory; - - // UIO device file giving access to the PDEC RAM section - std::string uioRamMemory; - - // UIO device file giving access to the PDEC register space - std::string uioRegisters; - - ActionHelper actionHelper; - - StorageManagerIF* tcStore = nullptr; - - MessageQueueIF* commandQueue = nullptr; - - State state = State::INIT; - - /** - * Pointer pointing to base address of the PDEC memory space. - * This address is equivalent with the base address of the section named configuration area in - * the PDEC datasheet. - */ - uint32_t* memoryBaseAddress = nullptr; - - uint32_t* ramBaseAddress = nullptr; - - // Pointer pointing to base address of register space - uint32_t* registerBaseAddress = nullptr; - - uint32_t pdecFar = 0; - - uint8_t tcSegment[TC_SEGMENT_LEN]; - - // Used to check carrier and bit lock changes (default set to no rf and no bitlock) - uint32_t lastClcw = 0xC000; - - bool carrierLock = false; - bool bitLock = false; }; #endif /* LINUX_OBC_PDECHANDLER_H_ */ diff --git a/linux/obc/Ptme.cpp b/linux/ipcore/Ptme.cpp similarity index 98% rename from linux/obc/Ptme.cpp rename to linux/ipcore/Ptme.cpp index 65705c2e..0c475236 100644 --- a/linux/obc/Ptme.cpp +++ b/linux/ipcore/Ptme.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include #include diff --git a/linux/obc/Ptme.h b/linux/ipcore/Ptme.h similarity index 96% rename from linux/obc/Ptme.h rename to linux/ipcore/Ptme.h index f76f7fd1..2de85a38 100644 --- a/linux/obc/Ptme.h +++ b/linux/ipcore/Ptme.h @@ -9,8 +9,8 @@ #include "OBSWConfig.h" #include "fsfw/returnvalues/returnvalue.h" -#include "linux/obc/PtmeIF.h" -#include "linux/obc/VcInterfaceIF.h" +#include "linux/ipcore/PtmeIF.h" +#include "linux/ipcore/VcInterfaceIF.h" /** * @brief This class handles the interfacing to the telemetry (PTME) IP core responsible for the diff --git a/linux/obc/PtmeConfig.cpp b/linux/ipcore/PtmeConfig.cpp similarity index 100% rename from linux/obc/PtmeConfig.cpp rename to linux/ipcore/PtmeConfig.cpp diff --git a/linux/obc/PtmeConfig.h b/linux/ipcore/PtmeConfig.h similarity index 98% rename from linux/obc/PtmeConfig.h rename to linux/ipcore/PtmeConfig.h index 9c5a85af..f57f6fb5 100644 --- a/linux/obc/PtmeConfig.h +++ b/linux/ipcore/PtmeConfig.h @@ -4,7 +4,7 @@ #include "AxiPtmeConfig.h" #include "fsfw/objectmanager/SystemObject.h" #include "fsfw/returnvalues/returnvalue.h" -#include "linux/obc/PtmeConfig.h" +#include "linux/ipcore/PtmeConfig.h" #include "returnvalues/classIds.h" /** diff --git a/linux/obc/PtmeIF.h b/linux/ipcore/PtmeIF.h similarity index 100% rename from linux/obc/PtmeIF.h rename to linux/ipcore/PtmeIF.h diff --git a/linux/obc/VcInterfaceIF.h b/linux/ipcore/VcInterfaceIF.h similarity index 100% rename from linux/obc/VcInterfaceIF.h rename to linux/ipcore/VcInterfaceIF.h diff --git a/linux/ipcore/pdec.h b/linux/ipcore/pdec.h new file mode 100644 index 00000000..16b0c2e6 --- /dev/null +++ b/linux/ipcore/pdec.h @@ -0,0 +1,61 @@ +#ifndef LINUX_OBC_PDEC_H_ +#define LINUX_OBC_PDEC_H_ + +#include + +namespace pdec { + +static const uint8_t STAT_POSITION = 31; +static const uint8_t FRAME_ANA_POSITION = 28; +static const uint8_t IREASON_POSITION = 25; + +static const uint8_t NEW_FAR_RECEIVED = 0; + +static constexpr uint32_t NEW_FAR_MASK = 1 << 2; +static constexpr uint32_t TC_ABORT_MASK = 1 << 1; +static constexpr uint32_t TC_NEW_MASK = 1 << 0; + +static constexpr uint32_t FAR_STAT_MASK = 1 << 31; + +static const uint32_t FRAME_ANA_MASK = 0x70000000; +static const uint32_t IREASON_MASK = 0x0E000000; + +static const uint32_t TC_CHANNEL_INACTIVE = 0x0; +static const uint32_t TC_CHANNEL_ACTIVE = 0x1; +static const uint32_t TC_CHANNEL_TIMEDOUT = 0x2; + +static const uint32_t TC0_STATUS_MASK = 0x3; +static const uint32_t TC1_STATUS_MASK = 0xC; +static const uint32_t TC2_STATUS_MASK = 0x300; +static const uint32_t TC3_STATUS_MASK = 0xC00; +static const uint32_t TC4_STATUS_MASK = 0x30000; +static const uint32_t TC5_STATUS_MASK = 0xc00000; +// Lock register set to 1 when start sequence has been found (CLTU is beeing processed) +static const uint32_t LOCK_MASK = 0xc00000; + +static const uint32_t TC0_STATUS_POS = 0; +static const uint32_t TC1_STATUS_POS = 2; +static const uint32_t TC2_STATUS_POS = 4; +static const uint32_t TC3_STATUS_POS = 6; +static const uint32_t TC4_STATUS_POS = 8; +static const uint32_t TC5_STATUS_POS = 10; +// Lock register set to 1 when start sequence has been found (CLTU is beeing processed) +static const uint32_t LOCK_POS = 12; + +/** + * UIO is 4 byte aligned. Thus offset is calculated with "true offset" / 4 + * Example: PDEC_FAR = 0x2840 => Offset in virtual address space is 0xA10 + */ +static constexpr uint32_t PDEC_PISR_OFFSET = 0xA02; +static constexpr uint32_t PDEC_PIR_OFFSET = 0xA03; +static constexpr uint32_t PDEC_IMR_OFFSET = 0xA04; +static const uint32_t PDEC_FAR_OFFSET = 0xA10; +static const uint32_t PDEC_CLCW_OFFSET = 0xA12; +static const uint32_t PDEC_BFREE_OFFSET = 0xA24; +static const uint32_t PDEC_BPTR_OFFSET = 0xA25; +static const uint32_t PDEC_SLEN_OFFSET = 0xA26; +static const uint32_t PDEC_MON_OFFSET = 0xA27; + +} // namespace pdec + +#endif /* LINUX_OBC_PDEC_H_ */ diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 09e91e26..788ee1c4 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -2,8 +2,10 @@ #include +#include "mission/devices/torquer.h" + AcsController::AcsController(object_id_t objectId) - : ExtendedControllerBase(objectId, objects::NO_OBJECT), mgmData(this) {} + : ExtendedControllerBase(objectId), mgmData(this) {} ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { return returnvalue::OK; @@ -29,6 +31,15 @@ void AcsController::performControlOperation() { break; } + { + // TODO: Calculate actuator output + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // TODO: Insert correct values here + // dipoleSet.setDipoles(500, 500, 500, 150); + } + { PoolReadGuard pg(&mgmData); if (pg.getReadResult() == returnvalue::OK) { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index c20ae94d..0cf38bc7 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -7,8 +7,8 @@ #include "eive/objects.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" -#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" +#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" class AcsController : public ExtendedControllerBase { public: @@ -20,7 +20,6 @@ class AcsController : public ExtendedControllerBase { enum class InternalState { STARTUP, INITIAL_DELAY, READY }; InternalState internalState = InternalState::STARTUP; - ReturnValue_t handleCommandMessage(CommandMessage* message) override; void performControlOperation() override; @@ -44,6 +43,7 @@ class AcsController : public ExtendedControllerBase { RM3100::Rm3100PrimaryDataset mgm3Rm3100Set = RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER); + IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER); PoolEntry mgm0PoolVec = PoolEntry(3); PoolEntry mgm1PoolVec = PoolEntry(3); diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 608b68b7..66fa2187 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -8,14 +8,14 @@ #include #include #include -#include #include #include +#include #include #include -ThermalController::ThermalController(object_id_t objectId, object_id_t parentId) - : ExtendedControllerBase(objectId, parentId), +ThermalController::ThermalController(object_id_t objectId) + : ExtendedControllerBase(objectId), sensorTemperatures(this), susTemperatures(this), deviceTemperatures(this), @@ -573,369 +573,344 @@ void ThermalController::copySus() { void ThermalController::copyDevices() { lp_var_t tempQ7s = lp_var_t(objects::CORE_CONTROLLER, core::PoolIds::TEMPERATURE); - ReturnValue_t result = tempQ7s.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read Q7S temperature" << std::endl; - deviceTemperatures.q7s.setValid(false); - deviceTemperatures.q7s = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.q7s = tempQ7s; - deviceTemperatures.q7s.setValid(tempQ7s.isValid()); + { + PoolReadGuard pg(&tempQ7s, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() == returnvalue::OK) { + deviceTemperatures.q7s = tempQ7s; + deviceTemperatures.q7s.setValid(tempQ7s.isValid()); + } else { + deviceTemperatures.q7s.setValid(false); + deviceTemperatures.q7s = static_cast(INVALID_TEMPERATURE); + } } - result = tempQ7s.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; + + { + lp_var_t battTemp1 = + lp_var_t(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_1); + PoolReadGuard pg(&battTemp1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read battery temperature 1" << std::endl; + deviceTemperatures.batteryTemp1.setValid(false); + deviceTemperatures.batteryTemp1 = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.batteryTemp1 = battTemp1; + deviceTemperatures.batteryTemp1.setValid(battTemp1.isValid()); + } } - lp_var_t battTemp1 = - lp_var_t(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_1); - result = battTemp1.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read battery temperature 1" << std::endl; - deviceTemperatures.batteryTemp1.setValid(false); - deviceTemperatures.batteryTemp1 = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.batteryTemp1 = battTemp1; - deviceTemperatures.batteryTemp1.setValid(battTemp1.isValid()); + + { + lp_var_t battTemp2 = + lp_var_t(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_2); + PoolReadGuard pg(&battTemp2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read battery temperature 2" << std::endl; + deviceTemperatures.batteryTemp2.setValid(false); + deviceTemperatures.batteryTemp2 = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.batteryTemp2 = battTemp2; + deviceTemperatures.batteryTemp2.setValid(battTemp2.isValid()); + } } - result = battTemp1.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; + + { + lp_var_t battTemp3 = + lp_var_t(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_3); + PoolReadGuard pg(&battTemp3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read battery temperature 3" << std::endl; + deviceTemperatures.batteryTemp3.setValid(false); + deviceTemperatures.batteryTemp3 = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.batteryTemp3 = battTemp3; + deviceTemperatures.batteryTemp3.setValid(battTemp3.isValid()); + } } - lp_var_t battTemp2 = - lp_var_t(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_2); - result = battTemp2.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read battery temperature 2" << std::endl; - deviceTemperatures.batteryTemp2.setValid(false); - deviceTemperatures.batteryTemp2 = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.batteryTemp2 = battTemp2; - deviceTemperatures.batteryTemp2.setValid(battTemp2.isValid()); + + { + lp_var_t battTemp4 = + lp_var_t(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_4); + PoolReadGuard pg(&battTemp4, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read battery temperature 4" << std::endl; + deviceTemperatures.batteryTemp4.setValid(false); + deviceTemperatures.batteryTemp4 = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.batteryTemp4 = battTemp4; + deviceTemperatures.batteryTemp4.setValid(battTemp4.isValid()); + } } - result = battTemp2.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; + + { + lp_var_t tempRw1 = lp_var_t(objects::RW1, RwDefinitions::TEMPERATURE_C); + PoolReadGuard pg(&tempRw1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read reaction wheel 1 temperature" << std::endl; + deviceTemperatures.rw1.setValid(false); + deviceTemperatures.rw1 = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.rw1.setValid(tempRw1.isValid()); + deviceTemperatures.rw1 = tempRw1; + } } - lp_var_t battTemp3 = - lp_var_t(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_3); - result = battTemp3.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read battery temperature 3" << std::endl; - deviceTemperatures.batteryTemp3.setValid(false); - deviceTemperatures.batteryTemp3 = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.batteryTemp3 = battTemp3; - deviceTemperatures.batteryTemp3.setValid(battTemp3.isValid()); + + { + lp_var_t tempRw2 = lp_var_t(objects::RW2, RwDefinitions::TEMPERATURE_C); + PoolReadGuard pg(&tempRw2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read reaction wheel 2 temperature" << std::endl; + deviceTemperatures.rw2.setValid(false); + deviceTemperatures.rw2 = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.rw2.setValid(tempRw2.isValid()); + deviceTemperatures.rw2 = tempRw2; + } } - result = battTemp3.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; + + { + lp_var_t tempRw3 = lp_var_t(objects::RW3, RwDefinitions::TEMPERATURE_C); + PoolReadGuard pg(&tempRw3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read reaction wheel 3 temperature" << std::endl; + deviceTemperatures.rw3.setValid(false); + deviceTemperatures.rw3 = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.rw3.setValid(tempRw3.isValid()); + deviceTemperatures.rw3 = tempRw3; + } } - lp_var_t battTemp4 = - lp_var_t(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_4); - result = battTemp4.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read battery temperature 4" << std::endl; - deviceTemperatures.batteryTemp4.setValid(false); - deviceTemperatures.batteryTemp4 = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.batteryTemp4 = battTemp4; - deviceTemperatures.batteryTemp4.setValid(battTemp4.isValid()); + + { + lp_var_t tempRw4 = lp_var_t(objects::RW4, RwDefinitions::TEMPERATURE_C); + PoolReadGuard pg(&tempRw4, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read reaction wheel 4 temperature" << std::endl; + deviceTemperatures.rw4.setValid(false); + deviceTemperatures.rw4 = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.rw4.setValid(tempRw4.isValid()); + deviceTemperatures.rw4 = tempRw4; + } } - result = battTemp4.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; + + { + lp_var_t tempStartracker = + lp_var_t(objects::STAR_TRACKER, startracker::MCU_TEMPERATURE); + PoolReadGuard pg(&tempStartracker, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read startracker temperature" << std::endl; + deviceTemperatures.startracker.setValid(false); + deviceTemperatures.startracker = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.startracker.setValid(tempStartracker.isValid()); + deviceTemperatures.startracker = tempStartracker; + } } - lp_var_t tempRw1 = lp_var_t(objects::RW1, RwDefinitions::TEMPERATURE_C); - result = tempRw1.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read reaction wheel 1 temperature" << std::endl; - deviceTemperatures.rw1.setValid(false); - deviceTemperatures.rw1 = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.rw1.setValid(tempRw1.isValid()); - deviceTemperatures.rw1 = tempRw1; + + { + lp_var_t tempSyrlinksPowerAmplifier = + lp_var_t(objects::SYRLINKS_HK_HANDLER, syrlinks::TEMP_POWER_AMPLIFIER); + PoolReadGuard pg(&tempSyrlinksPowerAmplifier, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read syrlinks power amplifier temperature" + << std::endl; + deviceTemperatures.syrlinksPowerAmplifier.setValid(false); + deviceTemperatures.syrlinksPowerAmplifier = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.syrlinksPowerAmplifier.setValid(tempSyrlinksPowerAmplifier.isValid()); + deviceTemperatures.syrlinksPowerAmplifier = tempSyrlinksPowerAmplifier; + } } - result = tempRw1.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; + + { + lp_var_t tempSyrlinksBasebandBoard = + lp_var_t(objects::SYRLINKS_HK_HANDLER, syrlinks::TEMP_BASEBAND_BOARD); + PoolReadGuard pg(&tempSyrlinksBasebandBoard, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read syrlinks baseband board temperature" + << std::endl; + deviceTemperatures.syrlinksBasebandBoard.setValid(false); + deviceTemperatures.syrlinksBasebandBoard = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.syrlinksBasebandBoard.setValid(tempSyrlinksBasebandBoard.isValid()); + deviceTemperatures.syrlinksBasebandBoard = tempSyrlinksBasebandBoard; + } } - lp_var_t tempRw2 = lp_var_t(objects::RW2, RwDefinitions::TEMPERATURE_C); - result = tempRw2.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read reaction wheel 2 temperature" << std::endl; - deviceTemperatures.rw2.setValid(false); - deviceTemperatures.rw2 = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.rw2.setValid(tempRw2.isValid()); - deviceTemperatures.rw2 = tempRw2; + + { + lp_var_t tempMgt = lp_var_t(objects::IMTQ_HANDLER, IMTQ::MCU_TEMPERATURE); + PoolReadGuard pg(&tempMgt, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read MGT temperature" << std::endl; + deviceTemperatures.mgt.setValid(false); + deviceTemperatures.mgt = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.mgt.setValid(tempMgt.isValid()); + deviceTemperatures.mgt = tempMgt; + } } - result = tempRw2.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; + + { + lp_vec_t tempAcu = + lp_vec_t(objects::ACU_HANDLER, ACU::pool::ACU_TEMPERATURES); + PoolReadGuard pg(&tempAcu, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read ACU temperatures" << std::endl; + deviceTemperatures.acu.setValid(false); + deviceTemperatures.acu[0] = static_cast(INVALID_TEMPERATURE); + deviceTemperatures.acu[1] = static_cast(INVALID_TEMPERATURE); + deviceTemperatures.acu[2] = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.acu.setValid(tempAcu.isValid()); + deviceTemperatures.acu = tempAcu; + } } - lp_var_t tempRw3 = lp_var_t(objects::RW3, RwDefinitions::TEMPERATURE_C); - result = tempRw3.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read reaction wheel 3 temperature" << std::endl; - deviceTemperatures.rw3.setValid(false); - deviceTemperatures.rw3 = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.rw3.setValid(tempRw3.isValid()); - deviceTemperatures.rw3 = tempRw3; + + { + lp_var_t tempPdu1 = lp_var_t(objects::PDU1_HANDLER, PDU::pool::PDU_TEMPERATURE); + PoolReadGuard pg(&tempPdu1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read PDU1 temperature" << std::endl; + deviceTemperatures.pdu1.setValid(false); + deviceTemperatures.pdu1 = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.pdu1.setValid(tempPdu1.isValid()); + deviceTemperatures.pdu1 = tempPdu1; + } } - result = tempRw3.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; + + { + lp_var_t tempPdu2 = lp_var_t(objects::PDU2_HANDLER, PDU::pool::PDU_TEMPERATURE); + PoolReadGuard pg(&tempPdu2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read PDU2 temperature" << std::endl; + deviceTemperatures.pdu2.setValid(false); + deviceTemperatures.pdu2 = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.pdu2.setValid(tempPdu2.isValid()); + deviceTemperatures.pdu2 = tempPdu2; + } } - lp_var_t tempRw4 = lp_var_t(objects::RW4, RwDefinitions::TEMPERATURE_C); - result = tempRw4.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read reaction wheel 4 temperature" << std::endl; - deviceTemperatures.rw4.setValid(false); - deviceTemperatures.rw4 = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.rw4.setValid(tempRw4.isValid()); - deviceTemperatures.rw4 = tempRw4; + + { + lp_var_t temp1P60dock = + lp_var_t(objects::P60DOCK_HANDLER, P60Dock::pool::P60DOCK_TEMPERATURE_1); + PoolReadGuard pg(&temp1P60dock, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read P60 dock temperature 1" << std::endl; + deviceTemperatures.temp1P60dock.setValid(false); + deviceTemperatures.temp1P60dock = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.temp1P60dock.setValid(temp1P60dock.isValid()); + deviceTemperatures.temp1P60dock = temp1P60dock; + } } - result = tempRw4.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; + + { + lp_var_t temp2P60dock = + lp_var_t(objects::P60DOCK_HANDLER, P60Dock::pool::P60DOCK_TEMPERATURE_2); + PoolReadGuard pg(&temp2P60dock, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read P60 dock temperature 2" << std::endl; + deviceTemperatures.temp2P60dock.setValid(false); + deviceTemperatures.temp2P60dock = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.temp2P60dock.setValid(temp2P60dock.isValid()); + deviceTemperatures.temp2P60dock = temp2P60dock; + } } - lp_var_t tempStartracker = - lp_var_t(objects::STAR_TRACKER, startracker::MCU_TEMPERATURE); - result = tempStartracker.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read startracker temperature" << std::endl; - deviceTemperatures.startracker.setValid(false); - deviceTemperatures.startracker = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.startracker.setValid(tempStartracker.isValid()); - deviceTemperatures.startracker = tempStartracker; + + { + lp_var_t tempGyro0 = + lp_var_t(objects::GYRO_0_ADIS_HANDLER, ADIS1650X::TEMPERATURE); + PoolReadGuard pg(&tempGyro0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read gyro 0 temperature" << std::endl; + deviceTemperatures.gyro0SideA.setValid(false); + deviceTemperatures.gyro0SideA = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.gyro0SideA.setValid(tempGyro0.isValid()); + deviceTemperatures.gyro0SideA = tempGyro0; + } } - result = tempStartracker.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; + + { + lp_var_t tempGyro1 = lp_var_t(objects::GYRO_1_L3G_HANDLER, L3GD20H::TEMPERATURE); + PoolReadGuard pg(&tempGyro1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read gyro 1 temperature" << std::endl; + deviceTemperatures.gyro1SideA.setValid(false); + deviceTemperatures.gyro1SideA = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.gyro1SideA.setValid(tempGyro1.isValid()); + deviceTemperatures.gyro1SideA = tempGyro1; + } } - lp_var_t tempSyrlinksPowerAmplifier = - lp_var_t(objects::SYRLINKS_HK_HANDLER, syrlinks::TEMP_POWER_AMPLIFIER); - result = tempSyrlinksPowerAmplifier.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read syrlinks power amplifier temperature" - << std::endl; - deviceTemperatures.syrlinksPowerAmplifier.setValid(false); - deviceTemperatures.syrlinksPowerAmplifier = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.syrlinksPowerAmplifier.setValid(tempSyrlinksPowerAmplifier.isValid()); - deviceTemperatures.syrlinksPowerAmplifier = tempSyrlinksPowerAmplifier; + + { + lp_var_t tempGyro2 = + lp_var_t(objects::GYRO_2_ADIS_HANDLER, ADIS1650X::TEMPERATURE); + PoolReadGuard pg(&tempGyro2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read gyro 2 temperature" << std::endl; + deviceTemperatures.gyro2SideB.setValid(false); + deviceTemperatures.gyro2SideB = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.gyro2SideB.setValid(tempGyro2.isValid()); + deviceTemperatures.gyro2SideB = tempGyro2; + } } - result = tempSyrlinksPowerAmplifier.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; + + { + lp_var_t tempGyro3 = lp_var_t(objects::GYRO_3_L3G_HANDLER, L3GD20H::TEMPERATURE); + PoolReadGuard pg(&tempGyro3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read gyro 3 temperature" << std::endl; + deviceTemperatures.gyro3SideB.setValid(false); + deviceTemperatures.gyro3SideB = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.gyro3SideB.setValid(tempGyro3.isValid()); + deviceTemperatures.gyro3SideB = tempGyro3; + } } - lp_var_t tempSyrlinksBasebandBoard = - lp_var_t(objects::SYRLINKS_HK_HANDLER, syrlinks::TEMP_BASEBAND_BOARD); - result = tempSyrlinksBasebandBoard.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read syrlinks baseband board temperature" - << std::endl; - deviceTemperatures.syrlinksBasebandBoard.setValid(false); - deviceTemperatures.syrlinksBasebandBoard = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.syrlinksBasebandBoard.setValid(tempSyrlinksBasebandBoard.isValid()); - deviceTemperatures.syrlinksBasebandBoard = tempSyrlinksBasebandBoard; + + { + lp_var_t tempMgm0 = + lp_var_t(objects::MGM_0_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS); + PoolReadGuard pg(&tempMgm0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read MGM 0 temperature" << std::endl; + deviceTemperatures.mgm0SideA.setValid(false); + deviceTemperatures.mgm0SideA = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.mgm0SideA.setValid(tempMgm0.isValid()); + deviceTemperatures.mgm0SideA = tempMgm0; + } } - result = tempSyrlinksBasebandBoard.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; + + { + lp_var_t tempMgm2 = + lp_var_t(objects::MGM_2_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS); + PoolReadGuard pg(&tempMgm2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read MGM 2 temperature" << std::endl; + deviceTemperatures.mgm2SideB.setValid(false); + deviceTemperatures.mgm2SideB = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.mgm2SideB.setValid(tempMgm2.isValid()); + deviceTemperatures.mgm2SideB = tempMgm2; + } } - lp_var_t tempMgt = lp_var_t(objects::IMTQ_HANDLER, IMTQ::MCU_TEMPERATURE); - result = tempMgt.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read MGT temperature" << std::endl; - deviceTemperatures.mgt.setValid(false); - deviceTemperatures.mgt = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.mgt.setValid(tempMgt.isValid()); - deviceTemperatures.mgt = tempMgt; - } - result = tempMgt.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; - } - lp_vec_t tempAcu = - lp_vec_t(objects::ACU_HANDLER, ACU::pool::ACU_TEMPERATURES); - result = tempAcu.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read ACU temperatures" << std::endl; - deviceTemperatures.acu.setValid(false); - deviceTemperatures.acu[0] = static_cast(INVALID_TEMPERATURE); - deviceTemperatures.acu[1] = static_cast(INVALID_TEMPERATURE); - deviceTemperatures.acu[2] = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.acu.setValid(tempAcu.isValid()); - deviceTemperatures.acu = tempAcu; - } - result = tempAcu.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; - } - lp_var_t tempPdu1 = lp_var_t(objects::PDU1_HANDLER, PDU::pool::PDU_TEMPERATURE); - result = tempPdu1.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read PDU1 temperature" << std::endl; - deviceTemperatures.pdu1.setValid(false); - deviceTemperatures.pdu1 = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.pdu1.setValid(tempPdu1.isValid()); - deviceTemperatures.pdu1 = tempPdu1; - } - result = tempPdu1.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; - } - lp_var_t tempPdu2 = lp_var_t(objects::PDU2_HANDLER, PDU::pool::PDU_TEMPERATURE); - result = tempPdu2.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read PDU2 temperature" << std::endl; - deviceTemperatures.pdu2.setValid(false); - deviceTemperatures.pdu2 = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.pdu2.setValid(tempPdu1.isValid()); - deviceTemperatures.pdu2 = tempPdu1; - } - result = tempPdu2.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; - } - lp_var_t temp1P60dock = - lp_var_t(objects::P60DOCK_HANDLER, P60Dock::pool::P60DOCK_TEMPERATURE_1); - result = temp1P60dock.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read P60 dock temperature 1" << std::endl; - deviceTemperatures.temp1P60dock.setValid(false); - deviceTemperatures.temp1P60dock = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.temp1P60dock.setValid(temp1P60dock.isValid()); - deviceTemperatures.temp1P60dock = temp1P60dock; - } - result = temp1P60dock.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; - } - lp_var_t temp2P60dock = - lp_var_t(objects::P60DOCK_HANDLER, P60Dock::pool::P60DOCK_TEMPERATURE_2); - result = temp2P60dock.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read P60 dock temperature 2" << std::endl; - deviceTemperatures.temp2P60dock.setValid(false); - deviceTemperatures.temp2P60dock = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.temp2P60dock.setValid(temp2P60dock.isValid()); - deviceTemperatures.temp2P60dock = temp2P60dock; - } - result = temp2P60dock.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; - } - lp_var_t tempGyro0 = lp_var_t(objects::GYRO_0_ADIS_HANDLER, ADIS1650X::TEMPERATURE); - result = tempGyro0.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read gyro 0 temperature" << std::endl; - deviceTemperatures.gyro0SideA.setValid(false); - deviceTemperatures.gyro0SideA = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.gyro0SideA.setValid(tempGyro0.isValid()); - deviceTemperatures.gyro0SideA = tempGyro0; - } - result = tempGyro0.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; - } - lp_var_t tempGyro1 = lp_var_t(objects::GYRO_1_L3G_HANDLER, L3GD20H::TEMPERATURE); - result = tempGyro1.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read gyro 1 temperature" << std::endl; - deviceTemperatures.gyro1SideA.setValid(false); - deviceTemperatures.gyro1SideA = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.gyro1SideA.setValid(tempGyro1.isValid()); - deviceTemperatures.gyro1SideA = tempGyro1; - } - result = tempGyro1.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; - } - lp_var_t tempGyro2 = lp_var_t(objects::GYRO_2_ADIS_HANDLER, ADIS1650X::TEMPERATURE); - result = tempGyro2.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read gyro 2 temperature" << std::endl; - deviceTemperatures.gyro2SideB.setValid(false); - deviceTemperatures.gyro2SideB = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.gyro2SideB.setValid(tempGyro2.isValid()); - deviceTemperatures.gyro2SideB = tempGyro2; - } - result = tempGyro2.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; - } - lp_var_t tempGyro3 = lp_var_t(objects::GYRO_3_L3G_HANDLER, L3GD20H::TEMPERATURE); - result = tempGyro3.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read gyro 3 temperature" << std::endl; - deviceTemperatures.gyro3SideB.setValid(false); - deviceTemperatures.gyro3SideB = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.gyro3SideB.setValid(tempGyro3.isValid()); - deviceTemperatures.gyro3SideB = tempGyro3; - } - result = tempGyro3.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; - } - lp_var_t tempMgm0 = - lp_var_t(objects::MGM_0_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS); - result = tempMgm0.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read MGM 0 temperature" << std::endl; - deviceTemperatures.mgm0SideA.setValid(false); - deviceTemperatures.mgm0SideA = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.mgm0SideA.setValid(tempMgm0.isValid()); - deviceTemperatures.mgm0SideA = tempMgm0; - } - result = tempMgm0.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; - } - lp_var_t tempMgm2 = - lp_var_t(objects::MGM_2_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS); - result = tempMgm2.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read MGM 2 temperature" << std::endl; - deviceTemperatures.mgm2SideB.setValid(false); - deviceTemperatures.mgm2SideB = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.mgm2SideB.setValid(tempMgm2.isValid()); - deviceTemperatures.mgm2SideB = tempMgm2; - } - result = tempMgm2.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; - } - lp_var_t tempAdcPayloadPcdu = lp_var_t(objects::PLPCDU_HANDLER, plpcdu::TEMP); - result = tempAdcPayloadPcdu.read(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to read payload PCDU ADC temperature" << std::endl; - deviceTemperatures.adcPayloadPcdu.setValid(false); - deviceTemperatures.adcPayloadPcdu = static_cast(INVALID_TEMPERATURE); - } else { - deviceTemperatures.adcPayloadPcdu.setValid(tempAdcPayloadPcdu.isValid()); - deviceTemperatures.adcPayloadPcdu = tempAdcPayloadPcdu; - } - result = tempAdcPayloadPcdu.commit(); - if (result != returnvalue::OK) { - sif::warning << "ThermalController: Failed to commit" << std::endl; + + { + lp_var_t tempAdcPayloadPcdu = lp_var_t(objects::PLPCDU_HANDLER, plpcdu::TEMP); + PoolReadGuard pg(&tempAdcPayloadPcdu, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() != returnvalue::OK) { + sif::warning << "ThermalController: Failed to read payload PCDU ADC temperature" << std::endl; + deviceTemperatures.adcPayloadPcdu.setValid(false); + deviceTemperatures.adcPayloadPcdu = static_cast(INVALID_TEMPERATURE); + } else { + deviceTemperatures.adcPayloadPcdu.setValid(tempAdcPayloadPcdu.isValid()); + deviceTemperatures.adcPayloadPcdu = tempAdcPayloadPcdu; + } } } diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 7ca517cb..76366d0c 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -12,7 +12,7 @@ class ThermalController : public ExtendedControllerBase { public: static const uint16_t INVALID_TEMPERATURE = 999; - ThermalController(object_id_t objectId, object_id_t parentId); + ThermalController(object_id_t objectId); ReturnValue_t initialize() override; diff --git a/mission/controller/controllerdefinitions/AcsControllerDefinitions.h b/mission/controller/controllerdefinitions/AcsControllerDefinitions.h new file mode 100644 index 00000000..8122178d --- /dev/null +++ b/mission/controller/controllerdefinitions/AcsControllerDefinitions.h @@ -0,0 +1,14 @@ +#ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCONTROLLERDEFINITIONS_H_ +#define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCONTROLLERDEFINITIONS_H_ + +#include + +namespace acs { + +enum CtrlModes { OFF = HasModesIF::MODE_OFF, SAFE = 1, DETUMBLE = 2, IDLE = 3, TARGET_PT = 4 }; + +static constexpr Submode_t IDLE_CHARGE = 1; + +} // namespace acs + +#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCONTROLLERDEFINITIONS_H_ */ diff --git a/mission/core/GenericFactory.cpp b/mission/core/GenericFactory.cpp index e81818e9..0e1926f4 100644 --- a/mission/core/GenericFactory.cpp +++ b/mission/core/GenericFactory.cpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include "OBSWConfig.h" #include "eive/definitions.h" @@ -66,7 +66,8 @@ EiveFaultHandler EIVE_FAULT_HANDLER; } // namespace cfdp -void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_) { +void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFunnel** pusFunnel, + CfdpTmFunnel** cfdpFunnel) { // Framework objects new EventManager(objects::EVENT_MANAGER); auto healthTable = new HealthTable(objects::HEALTH_TABLE); @@ -79,14 +80,14 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_) { StorageManagerIF* tcStore; StorageManagerIF* tmStore; { - PoolManager::LocalPoolConfig poolCfg = {{200, 16}, {200, 32}, {150, 64}, - {100, 128}, {100, 1024}, {100, 2048}}; + PoolManager::LocalPoolConfig poolCfg = {{250, 16}, {250, 32}, {250, 64}, + {150, 128}, {120, 1024}, {120, 2048}}; tcStore = new PoolManager(objects::TC_STORE, poolCfg); } { - PoolManager::LocalPoolConfig poolCfg = {{300, 16}, {300, 32}, {100, 64}, - {100, 128}, {100, 1024}, {100, 2048}}; + PoolManager::LocalPoolConfig poolCfg = {{300, 16}, {350, 32}, {350, 64}, + {200, 128}, {150, 1024}, {150, 2048}}; tmStore = new PoolManager(objects::TM_STORE, poolCfg); } @@ -98,12 +99,12 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_) { #if OBSW_ADD_TCPIP_BRIDGE == 1 #if OBSW_USE_TMTC_TCP_BRIDGE == 0 - auto tmtcBridge = new UdpTmTcBridge(objects::TMTC_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR); + auto tcpIpTmtcBridge = new UdpTmTcBridge(objects::TMTC_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR); new UdpTcPollingTask(objects::TMTC_POLLING_TASK, objects::TMTC_BRIDGE); sif::info << "Created UDP server for TMTC commanding with listener port " << udpBridge->getUdpPort() << std::endl; #else - auto tmtcBridge = new TcpTmTcBridge(objects::TMTC_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR); + auto tcpIpTmtcBridge = new TcpTmTcBridge(objects::TMTC_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR); auto tcpServer = new TcpTmTcServer(objects::TMTC_POLLING_TASK, objects::TMTC_BRIDGE); // TCP is stream based. Use packet ID as start marker when parsing for space packets tcpServer->setSpacePacketParsingOptions({common::PUS_PACKET_ID, common::CFDP_PACKET_ID}); @@ -113,23 +114,21 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_) { tcpServer->enableWiretapping(true); #endif /* OBSW_TCP_SERVER_WIRETAPPING == 1 */ #endif /* OBSW_USE_TMTC_TCP_BRIDGE == 0 */ - tmtcBridge->setMaxNumberOfPacketsStored(300); + tcpIpTmtcBridge->setMaxNumberOfPacketsStored(150); #endif /* OBSW_ADD_TCPIP_BRIDGE == 1 */ auto* ccsdsDistrib = new CcsdsDistributor(config::EIVE_PUS_APID, objects::CCSDS_PACKET_DISTRIBUTOR); new PusDistributor(config::EIVE_PUS_APID, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib); - uint8_t vc = 0; -#if OBSW_TM_TO_PTME == 1 - vc = config::LIVE_TM; + *cfdpFunnel = new CfdpTmFunnel(objects::CFDP_TM_FUNNEL, config::EIVE_CFDP_APID, *tmStore, 50); + *pusFunnel = new PusTmFunnel(objects::PUS_TM_FUNNEL, *timeStamper, *tmStore, 80); +#if OBSW_ADD_TCPIP_BRIDGE == 1 + (*cfdpFunnel)->addDestination(*tcpIpTmtcBridge, 0); + (*pusFunnel)->addDestination(*tcpIpTmtcBridge, 0); #endif - auto* cfdpFunnel = - new CfdpTmFunnel(objects::CFDP_TM_FUNNEL, config::EIVE_CFDP_APID, *tmtcBridge, *tmStore, vc); - auto* pusFunnel = - new PusTmFunnel(objects::PUS_TM_FUNNEL, *tmtcBridge, *timeStamper, *tmStore, vc); // Every TM packet goes through this funnel - new TmFunnel(objects::TM_FUNNEL, *pusFunnel, *cfdpFunnel); + new TmFunnelHandler(objects::TM_FUNNEL, **pusFunnel, **cfdpFunnel); // PUS service stack new Service1TelecommandVerification(objects::PUS_SERVICE_1_VERIFICATION, config::EIVE_PUS_APID, @@ -165,7 +164,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_) { new CfdpDistributor(distribCfg); auto* msgQueue = QueueFactory::instance()->createMessageQueue(32); - FsfwHandlerParams params(objects::CFDP_HANDLER, HOST_FS, *cfdpFunnel, *tcStore, *tmStore, + FsfwHandlerParams params(objects::CFDP_HANDLER, HOST_FS, **cfdpFunnel, *tcStore, *tmStore, *msgQueue); cfdp::IndicationCfg indicationCfg; UnsignedByteField apid(config::EIVE_LOCAL_CFDP_ENTITY_ID); diff --git a/mission/core/GenericFactory.h b/mission/core/GenericFactory.h index 4bc6695d..2e2b0748 100644 --- a/mission/core/GenericFactory.h +++ b/mission/core/GenericFactory.h @@ -2,10 +2,13 @@ #define MISSION_CORE_GENERICFACTORY_H_ class HealthTableIF; +class PusTmFunnel; +class CfdpTmFunnel; namespace ObjectFactory { -void produceGenericObjects(HealthTableIF** healthTable = nullptr); +void produceGenericObjects(HealthTableIF** healthTable, PusTmFunnel** pusFunnel, + CfdpTmFunnel** cfdpFunnel); } diff --git a/mission/devices/BpxBatteryHandler.cpp b/mission/devices/BpxBatteryHandler.cpp index 02eea4fe..4e49bebe 100644 --- a/mission/devices/BpxBatteryHandler.cpp +++ b/mission/devices/BpxBatteryHandler.cpp @@ -216,7 +216,7 @@ ReturnValue_t BpxBatteryHandler::interpretDeviceReply(DeviceCommandId_t id, cons if (packet[2] != sentPingByte) { return DeviceHandlerIF::INVALID_DATA; } - if (mode == _MODE_START_UP) { + if (getMode() == _MODE_START_UP) { commandExecuted = true; } break; diff --git a/mission/devices/CMakeLists.txt b/mission/devices/CMakeLists.txt index 56184ea4..b8fb326c 100644 --- a/mission/devices/CMakeLists.txt +++ b/mission/devices/CMakeLists.txt @@ -11,7 +11,7 @@ target_sources( SyrlinksHkHandler.cpp Max31865PT1000Handler.cpp Max31865EiveHandler.cpp - IMTQHandler.cpp + ImtqHandler.cpp HeaterHandler.cpp RadiationSensorHandler.cpp GyroADIS1650XHandler.cpp @@ -20,6 +20,7 @@ target_sources( SusHandler.cpp PayloadPcduHandler.cpp SolarArrayDeploymentHandler.cpp - ScexDeviceHandler.cpp) + ScexDeviceHandler.cpp + torquer.cpp) add_subdirectory(devicedefinitions) diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index 254b6993..389b08c7 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -570,7 +570,7 @@ ReturnValue_t GomspaceDeviceHandler::generateRequestFullCfgTableCmd(DeviceType d uint32_t GomspaceDeviceHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 0; } -void GomspaceDeviceHandler::setModeNormal() { mode = MODE_NORMAL; } +void GomspaceDeviceHandler::setModeNormal() { setMode(_MODE_TO_NORMAL); } ReturnValue_t GomspaceDeviceHandler::printStatus(DeviceCommandId_t cmd) { sif::info << "No printHkTable implementation given.." << std::endl; diff --git a/mission/devices/GyroADIS1650XHandler.cpp b/mission/devices/GyroADIS1650XHandler.cpp index 5ff84a96..79e529b6 100644 --- a/mission/devices/GyroADIS1650XHandler.cpp +++ b/mission/devices/GyroADIS1650XHandler.cpp @@ -422,12 +422,12 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie * cookie->setTransferSize(2); gpioId_t gpioId = cookie->getChipSelectPin(); - GpioIF *gpioIF = comIf->getGpioInterface(); + GpioIF &gpioIF = comIf->getGpioInterface(); MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; uint32_t timeoutMs = 0; MutexIF *mutex = comIf->getCsMutex(); cookie->getMutexParams(timeoutType, timeoutMs); - if (mutex == nullptr or gpioIF == nullptr) { + if (mutex == nullptr) { #if OBSW_VERBOSE_LEVEL >= 1 sif::warning << "GyroADIS16507Handler::spiSendCallback: " "Mutex or GPIO interface invalid" @@ -453,7 +453,7 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie * while (idx < sendLen) { // Pull SPI CS low. For now, no support for active high given if (gpioId != gpio::NO_GPIO) { - gpioIF->pullLow(gpioId); + gpioIF.pullLow(gpioId); } // Execute transfer @@ -468,7 +468,7 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie * #endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ if (gpioId != gpio::NO_GPIO) { - gpioIF->pullHigh(gpioId); + gpioIF.pullHigh(gpioId); } idx += 2; diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/ImtqHandler.cpp similarity index 92% rename from mission/devices/IMTQHandler.cpp rename to mission/devices/ImtqHandler.cpp index 6ab5ed48..a0e717f8 100644 --- a/mission/devices/IMTQHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -1,18 +1,41 @@ -#include "IMTQHandler.h" - +#include +#include +#include #include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include +#include -#include "OBSWConfig.h" +#include "mission/devices/torquer.h" -IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, +static constexpr bool ACTUATION_WIRETAPPING = false; + +ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, power::Switch_t pwrSwitcher) : DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this), calMtmMeasurementSet(this), rawMtmMeasurementSet(this), + dipoleSet(*this), posXselfTestDataset(this), negXselfTestDataset(this), posYselfTestDataset(this), @@ -25,9 +48,9 @@ IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC } } -IMTQHandler::~IMTQHandler() {} +ImtqHandler::~ImtqHandler() = default; -void IMTQHandler::doStartUp() { +void ImtqHandler::doStartUp() { if (goToNormalMode) { setMode(MODE_NORMAL); } else { @@ -35,9 +58,11 @@ void IMTQHandler::doStartUp() { } } -void IMTQHandler::doShutDown() { setMode(_MODE_POWER_DOWN); } +void ImtqHandler::doShutDown() { setMode(_MODE_POWER_DOWN); } -ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { +ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { + bool buildCommand = true; + // Depending on the normal polling mode configuration, 3-4 communication steps are recommended switch (communicationStep) { case CommunicationStep::GET_ENG_HK_DATA: *id = IMTQ::GET_ENG_HK_DATA; @@ -45,29 +70,61 @@ ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { break; case CommunicationStep::START_MTM_MEASUREMENT: *id = IMTQ::START_MTM_MEASUREMENT; - communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT; - break; - case CommunicationStep::GET_CAL_MTM_MEASUREMENT: - *id = IMTQ::GET_CAL_MTM_MEASUREMENT; - communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT; + if (pollingMode == NormalPollingMode::BOTH or + pollingMode == NormalPollingMode::UNCALIBRATED) { + communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT; + } else { + communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT; + } break; case CommunicationStep::GET_RAW_MTM_MEASUREMENT: + if (integrationTimeCd.getRemainingMillis() > 0) { + TaskFactory::delayTask(integrationTimeCd.getRemainingMillis()); + } *id = IMTQ::GET_RAW_MTM_MEASUREMENT; + if (pollingMode == NormalPollingMode::BOTH) { + communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT; + } else { + communicationStep = CommunicationStep::DIPOLE_ACTUATION; + } + break; + case CommunicationStep::GET_CAL_MTM_MEASUREMENT: + if (integrationTimeCd.getRemainingMillis() > 0) { + TaskFactory::delayTask(integrationTimeCd.getRemainingMillis()); + } + *id = IMTQ::GET_CAL_MTM_MEASUREMENT; + communicationStep = CommunicationStep::DIPOLE_ACTUATION; + break; + case CommunicationStep::DIPOLE_ACTUATION: { + // If the dipole is not commanded but set by the ACS control algorithm, + // the dipoles will be set by the ACS controller directly using the dipole local pool set. + // This set has a flag to determine whether the ACS controller actually set any new input. + MutexGuard mg(torquer::lazyLock()); + if (torquer::NEW_ACTUATION_FLAG) { + *id = IMTQ::START_ACTUATION_DIPOLE; + torquer::NEW_ACTUATION_FLAG = false; + } else { + buildCommand = false; + } communicationStep = CommunicationStep::GET_ENG_HK_DATA; break; + } default: sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step" << std::endl; break; } - return buildCommandFromCommand(*id, NULL, 0); -} - -ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { + if (buildCommand) { + return buildCommandFromCommand(*id, nullptr, 0); + } return NOTHING_TO_SEND; } -ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, +ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { + return NOTHING_TO_SEND; +} + +ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, size_t commandDataLen) { switch (deviceCommand) { @@ -122,20 +179,36 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma case (IMTQ::START_ACTUATION_DIPOLE): { /* IMTQ expects low byte first */ commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; - if (commandData == nullptr) { + if (commandData != nullptr && commandDataLen < 8) { return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } - commandBuffer[1] = commandData[1]; - commandBuffer[2] = commandData[0]; - commandBuffer[3] = commandData[3]; - commandBuffer[4] = commandData[2]; - commandBuffer[5] = commandData[5]; - commandBuffer[6] = commandData[4]; - commandBuffer[7] = commandData[7]; - commandBuffer[8] = commandData[6]; - rawPacket = commandBuffer; - rawPacketLen = 9; - return returnvalue::OK; + ReturnValue_t result; + // Commands override anything which was set in the software + if (commandData != nullptr) { + dipoleSet.setValidityBufferGeneration(false); + result = + dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK); + dipoleSet.setValidityBufferGeneration(true); + if (result != returnvalue::OK) { + return result; + } + } else { + // Read set dipole values from local pool + PoolReadGuard pg(&dipoleSet); + } + if (ACTUATION_WIRETAPPING) { + sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value + << ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value + << ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl; + } + result = buildDipoleActuationCommand(); + if (result != returnvalue::OK) { + return result; + } + MutexGuard mg(torquer::lazyLock()); + torquer::TORQUEING = true; + torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value); + return result; } case (IMTQ::GET_ENG_HK_DATA): { commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA; @@ -151,6 +224,7 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma } case (IMTQ::START_MTM_MEASUREMENT): { commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT; + integrationTimeCd.resetTimer(); rawPacket = commandBuffer; rawPacketLen = 1; return returnvalue::OK; @@ -173,30 +247,42 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma return returnvalue::FAILED; } -void IMTQHandler::fillCommandAndReplyMap() { - this->insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr, - IMTQ::SIZE_SELF_TEST_RESULTS); - this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr, - IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset, - IMTQ::SIZE_ENG_HK_DATA_REPLY); - this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr, - IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY); - this->insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr, - IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet, - IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT); - this->insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet, - IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT); +ReturnValue_t ImtqHandler::buildDipoleActuationCommand() { + commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; + uint8_t* serPtr = commandBuffer + 1; + size_t serSize = 1; + dipoleSet.setValidityBufferGeneration(false); + ReturnValue_t result = dipoleSet.serialize(&serPtr, &serSize, sizeof(commandBuffer), + SerializeIF::Endianness::LITTLE); + dipoleSet.setValidityBufferGeneration(true); + if (result != returnvalue::OK) { + return result; + } + rawPacket = commandBuffer; + rawPacketLen = 9; + return result; } -ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSize, +void ImtqHandler::fillCommandAndReplyMap() { + insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr, IMTQ::SIZE_SELF_TEST_RESULTS); + insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset, IMTQ::SIZE_ENG_HK_DATA_REPLY); + insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr, + IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY); + insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet, + IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT); + insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet, + IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT); +} + +ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { ReturnValue_t result = returnvalue::OK; @@ -233,8 +319,16 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSi *foundLen = IMTQ::SIZE_SELF_TEST_RESULTS; *foundId = IMTQ::GET_SELF_TEST_RESULT; break; + case (IMTQ::CC::PAST_AVAILABLE_RESPONSE_BYTES): { + sif::warning << "IMTQHandler::scanForReply: Read 0xFF command byte, reading past available " + "bytes. Keep 1 ms delay between I2C send and read" + << std::endl; + result = IGNORE_REPLY_DATA; + break; + } default: - sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl; + sif::debug << "IMTQHandler::scanForReply: Reply with length " << remainingSize + << "contains invalid command code " << static_cast(*start) << std::endl; result = IGNORE_REPLY_DATA; break; } @@ -242,7 +336,7 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSi return result; } -ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { +ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { ReturnValue_t result = returnvalue::OK; result = parseStatusByte(packet); @@ -286,9 +380,9 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint return returnvalue::OK; } -void IMTQHandler::setNormalDatapoolEntriesInvalid() {} +void ImtqHandler::setNormalDatapoolEntriesInvalid() {} -LocalPoolDataSetBase* IMTQHandler::getDataSetHandle(sid_t sid) { +LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) { if (sid == engHkDataset.getSid()) { return &engHkDataset; } else if (sid == calMtmMeasurementSet.getSid()) { @@ -313,9 +407,9 @@ LocalPoolDataSetBase* IMTQHandler::getDataSetHandle(sid_t sid) { } } -uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } +uint32_t ImtqHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } -ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, +ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { /** Entries of engineering housekeeping dataset */ localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry({0})); @@ -330,6 +424,11 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(IMTQ::DIPOLES_X, &dipoleXEntry); + localDataPoolMap.emplace(IMTQ::DIPOLES_Y, &dipoleYEntry); + localDataPoolMap.emplace(IMTQ::DIPOLES_Z, &dipoleZEntry); + localDataPoolMap.emplace(IMTQ::CURRENT_TORQUE_DURATION, &torqueDurationEntry); + /** Entries of calibrated MTM measurement dataset */ localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, &mgmCalEntry); localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); @@ -611,7 +710,7 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat return returnvalue::OK; } -ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) { +ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) { DeviceCommandId_t commandId = getPendingCommand(); switch (commandId) { case IMTQ::POS_X_SELF_TEST: @@ -630,7 +729,7 @@ ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) { return returnvalue::OK; } -ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) { +ReturnValue_t ImtqHandler::parseStatusByte(const uint8_t* packet) { uint8_t cmdErrorField = *(packet + 1) & 0xF; switch (cmdErrorField) { case 0: @@ -661,7 +760,7 @@ ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) { } } -void IMTQHandler::fillEngHkDataset(const uint8_t* packet) { +void ImtqHandler::fillEngHkDataset(const uint8_t* packet) { PoolReadGuard rg(&engHkDataset); uint8_t offset = 2; engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset); @@ -687,7 +786,9 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) { offset += 2; engHkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; - engHkDataset.mcuTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); + size_t dummy = 2; + SerializeAdapter::deSerialize(&engHkDataset.mcuTemperature.value, packet + offset, &dummy, + SerializeIF::Endianness::LITTLE); engHkDataset.setValidity(true, true); @@ -708,9 +809,9 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) { } } -void IMTQHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; } +void ImtqHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; } -void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) { +void ImtqHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) { if (wiretappingMode == RAW) { /* Data already sent in doGetRead() */ return; @@ -734,7 +835,7 @@ void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCom } } -void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) { +void ImtqHandler::handleGetCommandedDipoleReply(const uint8_t* packet) { uint8_t tmData[6]; /* Switching endianess of received dipole values */ tmData[0] = *(packet + 3); @@ -746,7 +847,7 @@ void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) { handleDeviceTM(tmData, sizeof(tmData), IMTQ::GET_COMMANDED_DIPOLE); } -void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) { +void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) { PoolReadGuard rg(&calMtmMeasurementSet); calMtmMeasurementSet.setValidity(true, true); int8_t offset = 2; @@ -776,7 +877,7 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) { } } -void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { +void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) { PoolReadGuard rg(&rawMtmMeasurementSet); unsigned int offset = 2; size_t deSerLen = 16; @@ -824,7 +925,7 @@ void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { } } -void IMTQHandler::handleSelfTestReply(const uint8_t* packet) { +void ImtqHandler::handleSelfTestReply(const uint8_t* packet) { uint16_t offset = 2; checkErrorByte(*(packet + offset), *(packet + offset + 1)); @@ -858,7 +959,7 @@ void IMTQHandler::handleSelfTestReply(const uint8_t* packet) { } } -void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { +void ImtqHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; @@ -1070,7 +1171,7 @@ void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { } } -void IMTQHandler::handleNegativeXSelfTestReply(const uint8_t* packet) { +void ImtqHandler::handleNegativeXSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; @@ -1282,7 +1383,7 @@ void IMTQHandler::handleNegativeXSelfTestReply(const uint8_t* packet) { } } -void IMTQHandler::handlePositiveYSelfTestReply(const uint8_t* packet) { +void ImtqHandler::handlePositiveYSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; @@ -1494,7 +1595,7 @@ void IMTQHandler::handlePositiveYSelfTestReply(const uint8_t* packet) { } } -void IMTQHandler::handleNegativeYSelfTestReply(const uint8_t* packet) { +void ImtqHandler::handleNegativeYSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; @@ -1706,7 +1807,7 @@ void IMTQHandler::handleNegativeYSelfTestReply(const uint8_t* packet) { } } -void IMTQHandler::handlePositiveZSelfTestReply(const uint8_t* packet) { +void ImtqHandler::handlePositiveZSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; @@ -1918,7 +2019,7 @@ void IMTQHandler::handlePositiveZSelfTestReply(const uint8_t* packet) { } } -void IMTQHandler::handleNegativeZSelfTestReply(const uint8_t* packet) { +void ImtqHandler::handleNegativeZSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; @@ -2130,9 +2231,9 @@ void IMTQHandler::handleNegativeZSelfTestReply(const uint8_t* packet) { } } -void IMTQHandler::setDebugMode(bool enable) { this->debugMode = enable; } +void ImtqHandler::setDebugMode(bool enable) { this->debugMode = enable; } -void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) { +void ImtqHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) { std::string stepString(""); if (step < 8) { stepString = makeStepString(step); @@ -2190,7 +2291,12 @@ void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) { } } -std::string IMTQHandler::makeStepString(const uint8_t step) { +void ImtqHandler::doSendRead() { + TaskFactory::delayTask(1); + DeviceHandlerBase::doSendRead(); +} + +std::string ImtqHandler::makeStepString(const uint8_t step) { std::string stepString(""); switch (step) { case IMTQ::SELF_TEST_STEPS::INIT: @@ -2225,7 +2331,7 @@ std::string IMTQHandler::makeStepString(const uint8_t step) { return stepString; } -ReturnValue_t IMTQHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) { +ReturnValue_t ImtqHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) { if (switcher != power::NO_SWITCH) { *numberOfSwitches = 1; *switches = &switcher; diff --git a/mission/devices/IMTQHandler.h b/mission/devices/ImtqHandler.h similarity index 90% rename from mission/devices/IMTQHandler.h rename to mission/devices/ImtqHandler.h index 5102b2e0..cfad9d7f 100644 --- a/mission/devices/IMTQHandler.h +++ b/mission/devices/ImtqHandler.h @@ -2,7 +2,7 @@ #define MISSION_DEVICES_IMTQHANDLER_H_ #include -#include +#include #include #include "events/subsystemIdRanges.h" @@ -13,12 +13,17 @@ * * @author J. Meier */ -class IMTQHandler : public DeviceHandlerBase { +class ImtqHandler : public DeviceHandlerBase { public: - IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, - power::Switch_t pwrSwitcher); - virtual ~IMTQHandler(); + enum NormalPollingMode { UNCALIBRATED = 0, CALIBRATED = 1, BOTH = 2 }; + ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, + power::Switch_t pwrSwitcher); + virtual ~ImtqHandler(); + + void setPollingMode(NormalPollingMode pollMode); + + void doSendRead() override; /** * @brief Sets mode to MODE_NORMAL. Can be used for debugging. */ @@ -95,6 +100,7 @@ class IMTQHandler : public DeviceHandlerBase { IMTQ::EngHkDataset engHkDataset; IMTQ::CalibratedMtmMeasurementSet calMtmMeasurementSet; IMTQ::RawMtmMeasurementSet rawMtmMeasurementSet; + IMTQ::DipoleActuationSet dipoleSet; IMTQ::PosXSelfTestSet posXselfTestDataset; IMTQ::NegXSelfTestSet negXselfTestDataset; IMTQ::PosYSelfTestSet posYselfTestDataset; @@ -102,7 +108,16 @@ class IMTQHandler : public DeviceHandlerBase { IMTQ::PosZSelfTestSet posZselfTestDataset; IMTQ::NegZSelfTestSet negZselfTestDataset; + NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED; + PoolEntry mgmCalEntry = PoolEntry(3); + PoolEntry dipoleXEntry = PoolEntry(0, false); + PoolEntry dipoleYEntry = PoolEntry(0, false); + PoolEntry dipoleZEntry = PoolEntry(0, false); + PoolEntry torqueDurationEntry = PoolEntry(0, false); + // Hardcoded to default integration time of 10 ms. + // SHOULDDO: Support for other integration times + Countdown integrationTimeCd = Countdown(10); power::Switch_t switcher = power::NO_SWITCH; @@ -114,7 +129,8 @@ class IMTQHandler : public DeviceHandlerBase { GET_ENG_HK_DATA, START_MTM_MEASUREMENT, GET_CAL_MTM_MEASUREMENT, - GET_RAW_MTM_MEASUREMENT + GET_RAW_MTM_MEASUREMENT, + DIPOLE_ACTUATION }; CommunicationStep communicationStep = CommunicationStep::GET_ENG_HK_DATA; @@ -196,6 +212,7 @@ class IMTQHandler : public DeviceHandlerBase { void handlePositiveZSelfTestReply(const uint8_t* packet); void handleNegativeZSelfTestReply(const uint8_t* packet); + ReturnValue_t buildDipoleActuationCommand(); /** * @brief This function checks the error byte of a self test measurement. * diff --git a/mission/devices/Max31865EiveHandler.cpp b/mission/devices/Max31865EiveHandler.cpp index 8858cf38..db6f5a83 100644 --- a/mission/devices/Max31865EiveHandler.cpp +++ b/mission/devices/Max31865EiveHandler.cpp @@ -30,16 +30,13 @@ void Max31865EiveHandler::doStartUp() { } void Max31865EiveHandler::doShutDown() { - updatePeriodicReply(false, EiveMax31855::RtdCommands::EXCHANGE_SET_ID); if (state == InternalState::NONE or state == InternalState::ACTIVE or state == InternalState::ON) { state = InternalState::INACTIVE; transitionOk = false; - } else { - transitionOk = true; } if (state == InternalState::INACTIVE and transitionOk) { - setMode(_MODE_POWER_DOWN); + setMode(MODE_OFF); } } @@ -104,7 +101,7 @@ void Max31865EiveHandler::simpleCommand(EiveMax31855::RtdCommands cmd) { rawPacketLen = 1; } void Max31865EiveHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { - if (mode == _MODE_TO_NORMAL) { + if (getMode() == _MODE_TO_NORMAL) { if (state != InternalState::ACTIVE) { state = InternalState::ACTIVE; transitionOk = false; @@ -125,7 +122,7 @@ void Max31865EiveHandler::fillCommandAndReplyMap() { ReturnValue_t Max31865EiveHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { - if (mode == _MODE_POWER_ON or mode == _MODE_WAIT_ON) { + if (getMode() == _MODE_POWER_ON or getMode() == _MODE_WAIT_ON) { return IGNORE_FULL_PACKET; } if (remainingSize != structLen) { @@ -145,12 +142,17 @@ ReturnValue_t Max31865EiveHandler::interpretDeviceReply(DeviceCommandId_t id, if (result != returnvalue::OK) { return result; } - if (mode == _MODE_TO_NORMAL and exchangeStruct.active and state == InternalState::ACTIVE) { + if (getMode() == _MODE_TO_NORMAL and exchangeStruct.active and state == InternalState::ACTIVE) { transitionOk = true; } - if (mode == _MODE_START_UP and exchangeStruct.configured and state == InternalState::ON) { + if (getMode() == _MODE_START_UP and exchangeStruct.configured and state == InternalState::ON) { transitionOk = true; } + if (getMode() == _MODE_SHUT_DOWN and not exchangeStruct.active) { + transitionOk = true; + return returnvalue::OK; + } + // Calculate resistance float rtdValue = exchangeStruct.adcCode * EiveMax31855::RTD_RREF_PT1000 / INT16_MAX; // calculate approximation diff --git a/mission/devices/Max31865PT1000Handler.cpp b/mission/devices/Max31865PT1000Handler.cpp index 30e19d0e..75588cbd 100644 --- a/mission/devices/Max31865PT1000Handler.cpp +++ b/mission/devices/Max31865PT1000Handler.cpp @@ -302,7 +302,7 @@ ReturnValue_t Max31865PT1000Handler::scanForReply(const uint8_t *start, size_t r } else if (internalState == InternalState::CLEAR_FAULT_BYTE) { *foundId = MAX31865::CLEAR_FAULT_BYTE; *foundLen = 2; - if (mode == _MODE_START_UP) { + if (getMode() == _MODE_START_UP) { commandExecuted = true; } else { internalState = InternalState::RUNNING; @@ -524,7 +524,7 @@ void Max31865PT1000Handler::setInstantNormal(bool instantNormal) { } void Max31865PT1000Handler::modeChanged() { - if (mode == MODE_OFF) { + if (getMode() == MODE_OFF) { lastFaultStatus = 0; currentFaultStatus = 0; sameFaultStatusCounter = 0; diff --git a/mission/devices/PayloadPcduHandler.cpp b/mission/devices/PayloadPcduHandler.cpp index d9e41c7f..7b4353e4 100644 --- a/mission/devices/PayloadPcduHandler.cpp +++ b/mission/devices/PayloadPcduHandler.cpp @@ -66,7 +66,7 @@ void PayloadPcduHandler::doShutDown() { } void PayloadPcduHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { - if (mode == _MODE_TO_NORMAL) { + if (getMode() == _MODE_TO_NORMAL) { stateMachineToNormal(modeFrom, subModeFrom); return; } @@ -76,7 +76,7 @@ void PayloadPcduHandler::doTransition(Mode_t modeFrom, Submode_t 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 (((getSubmode() >> 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; @@ -129,7 +129,7 @@ ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_ auto switchHandler = [&](NormalSubmodeBits bit, gpioId_t id, std::string info) { if (((diffMask >> bit) & 1) == 1) { - if (((submode >> bit) & 1) == 1) { + if (((getSubmode() >> bit) & 1) == 1) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "Enabling PL PCDU " << info << " module" << std::endl; #endif @@ -150,7 +150,7 @@ ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_ switchHandler(MPA_ON, gpioIds::PLPCDU_ENB_MPA, "MPA"); switchHandler(HPA_ON, gpioIds::PLPCDU_ENB_HPA, "HPA"); if (doFinish) { - setMode(MODE_NORMAL, submode); + setMode(MODE_NORMAL); } return returnvalue::OK; } @@ -177,7 +177,7 @@ ReturnValue_t PayloadPcduHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id = plpcdu::SETUP_CMD; return buildCommandFromCommand(*id, nullptr, 0); } - if (mode == _MODE_TO_NORMAL) { + if (getMode() == _MODE_TO_NORMAL) { return buildNormalDeviceCommand(id); } return NOTHING_TO_SEND; @@ -248,7 +248,7 @@ ReturnValue_t PayloadPcduHandler::interpretDeviceReply(DeviceCommandId_t id, using namespace plpcdu; switch (id) { case (SETUP_CMD): { - if (mode == _MODE_TO_NORMAL) { + if (getMode() == _MODE_TO_NORMAL) { adcCmdExecuted = true; } break; @@ -540,33 +540,34 @@ bool PayloadPcduHandler::checkCurrent(float val, float upperBound, Event event) ReturnValue_t PayloadPcduHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) { using namespace plpcdu; if (mode == MODE_NORMAL) { - diffMask = submode ^ this->submode; + uint8_t dhbSubmode = getSubmode(); + diffMask = submode ^ dhbSubmode; // 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)) { + (getMode() == MODE_NORMAL and dhbSubmode != ALL_OFF_SUBMODE)) { return TRANS_NOT_ALLOWED; } if (((((submode >> DRO_ON) & 1) == 1) and - ((this->submode & 0b1) != (1 << SOLID_STATE_RELAYS_ADC_ON)))) { + ((dhbSubmode & 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)))) { + ((dhbSubmode & 0b11) != ((1 << SOLID_STATE_RELAYS_ADC_ON) | (1 << DRO_ON)))) { return TRANS_NOT_ALLOWED; } if (((((submode >> TX_ON) & 1) == 1) and - ((this->submode & 0b111) != + ((dhbSubmode & 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) != + ((dhbSubmode & 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))))) { + ((dhbSubmode & 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; @@ -719,11 +720,11 @@ ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cook cookie->setTransferSize(transferLen); gpioId_t gpioId = cookie->getChipSelectPin(); - GpioIF* gpioIF = comIf->getGpioInterface(); + 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 (mutex == nullptr) { #if OBSW_VERBOSE_LEVEL >= 1 sif::warning << "GyroADIS16507Handler::spiSendCallback: " "Mutex or GPIO interface invalid" @@ -753,7 +754,7 @@ ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cook transferStruct->len = transferLen; // Pull SPI CS low. For now, no support for active high given if (gpioId != gpio::NO_GPIO) { - gpioIF->pullLow(gpioId); + gpioIF.pullLow(gpioId); } // Execute transfer @@ -768,14 +769,14 @@ ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cook #endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ if (gpioId != gpio::NO_GPIO) { - gpioIF->pullHigh(gpioId); + 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); + gpioIF.pullLow(gpioId); } // Execute transfer @@ -790,7 +791,7 @@ ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cook #endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ if (gpioId != gpio::NO_GPIO) { - gpioIF->pullHigh(gpioId); + gpioIF.pullHigh(gpioId); } transferStruct->tx_buf = origTx; diff --git a/mission/devices/PayloadPcduHandler.h b/mission/devices/PayloadPcduHandler.h index 2f89c837..137a9a0b 100644 --- a/mission/devices/PayloadPcduHandler.h +++ b/mission/devices/PayloadPcduHandler.h @@ -10,8 +10,8 @@ #include "fsfw_hal/common/gpio/GpioIF.h" #include "mission/devices/devicedefinitions/payloadPcduDefinitions.h" #include "mission/memory/SdCardMountedIF.h" -#include "mission/system/DualLanePowerStateMachine.h" -#include "mission/system/definitions.h" +#include "mission/system/objects/DualLanePowerStateMachine.h" +#include "mission/system/objects/definitions.h" #ifdef FSFW_OSAL_LINUX class SpiComIF; diff --git a/mission/devices/ScexDeviceHandler.cpp b/mission/devices/ScexDeviceHandler.cpp index 1cc2cd39..8da6d229 100644 --- a/mission/devices/ScexDeviceHandler.cpp +++ b/mission/devices/ScexDeviceHandler.cpp @@ -1,5 +1,6 @@ #include "ScexDeviceHandler.h" +#include #include #include @@ -27,6 +28,7 @@ void ScexDeviceHandler::doStartUp() { setMode(MODE_ON); } void ScexDeviceHandler::doShutDown() { reader.reset(); commandActive = false; + multiFileFinishOutstanding = false; setMode(_MODE_POWER_DOWN); } @@ -95,6 +97,7 @@ ReturnValue_t ScexDeviceHandler::buildCommandFromCommand(DeviceCommandId_t devic << remainingMillis << std::endl; } + multiFileFinishOutstanding = true; prepareScexCmd(cmdTyped, {cmdBuf.data(), cmdBuf.size()}, rawPacketLen, {commandData + 1, commandDataLen - 1}, tempCheck); updatePeriodicReply(true, deviceCommand); @@ -104,6 +107,7 @@ ReturnValue_t ScexDeviceHandler::buildCommandFromCommand(DeviceCommandId_t devic finishCountdown.setTimeout(LONG_CD); // countdown starts finishCountdown.resetTimer(); + multiFileFinishOutstanding = true; prepareScexCmd(cmdTyped, {cmdBuf.data(), cmdBuf.size()}, rawPacketLen, {commandData + 1, commandDataLen - 1}, tempCheck); updatePeriodicReply(true, deviceCommand); @@ -113,6 +117,7 @@ ReturnValue_t ScexDeviceHandler::buildCommandFromCommand(DeviceCommandId_t devic finishCountdown.setTimeout(LONG_CD); // countdown starts finishCountdown.resetTimer(); + multiFileFinishOutstanding = true; prepareScexCmd(cmdTyped, {cmdBuf.data(), cmdBuf.size()}, rawPacketLen, {commandData + 1, commandDataLen - 1}, tempCheck); updatePeriodicReply(true, deviceCommand); @@ -172,17 +177,14 @@ ReturnValue_t ScexDeviceHandler::handleValidReply(size_t remSize, DeviceCommandI sif::info << "ScexDeviceHandler::handleValidReply: RemMillis: " << remainingMillis << std::endl; } - finishAction(true, helper.getCmd(), OK); result = APERIODIC_REPLY; break; } case (ONE_CELL): { - finishAction(true, helper.getCmd(), OK); result = APERIODIC_REPLY; break; } case (ALL_CELLS_CMD): { - finishAction(true, helper.getCmd(), OK); result = APERIODIC_REPLY; break; } @@ -190,6 +192,11 @@ ReturnValue_t ScexDeviceHandler::handleValidReply(size_t remSize, DeviceCommandI break; } } + if (result == APERIODIC_REPLY and multiFileFinishOutstanding) { + finishAction(true, helper.getCmd(), OK); + multiFileFinishOutstanding = false; + } + *foundId = helper.getCmd(); *foundLen = remSize; return result; @@ -200,6 +207,10 @@ ReturnValue_t ScexDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, cons ReturnValue_t status = OK; auto oneFileHandler = [&](std::string cmdName) { + auto activeSd = sdcMan.getActiveSdCard(); + if (not activeSd) { + return HasFileSystemIF::FILESYSTEM_INACTIVE; + } fileId = date_time_string(); std::ostringstream oss; auto prefix = sdcMan.getCurrentMountPrefix(); @@ -216,6 +227,10 @@ ReturnValue_t ScexDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, cons }; auto multiFileHandler = [&](std::string cmdName) { if ((helper.getPacketCounter() == 1) or (not fileNameSet)) { + auto activeSd = sdcMan.getActiveSdCard(); + if (not activeSd) { + return HasFileSystemIF::FILESYSTEM_INACTIVE; + } fileId = date_time_string(); std::ostringstream oss; auto prefix = sdcMan.getCurrentMountPrefix(); @@ -241,6 +256,7 @@ ReturnValue_t ScexDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, cons } return OK; }; + id = helper.getCmd(); switch (id) { case (PING): { status = oneFileHandler("ping_"); diff --git a/mission/devices/ScexDeviceHandler.h b/mission/devices/ScexDeviceHandler.h index 1c96618e..a164c5e2 100644 --- a/mission/devices/ScexDeviceHandler.h +++ b/mission/devices/ScexDeviceHandler.h @@ -28,6 +28,7 @@ class ScexDeviceHandler : public DeviceHandlerBase { std::string fileName = ""; bool fileNameSet = false; bool commandActive = false; + bool multiFileFinishOutstanding = false; bool debugMode = false; scex::Cmds currCmd = scex::Cmds::PING; diff --git a/mission/devices/SolarArrayDeploymentHandler.cpp b/mission/devices/SolarArrayDeploymentHandler.cpp index affc0e2e..5656692d 100644 --- a/mission/devices/SolarArrayDeploymentHandler.cpp +++ b/mission/devices/SolarArrayDeploymentHandler.cpp @@ -1,5 +1,6 @@ #include "SolarArrayDeploymentHandler.h" +#include #include #include @@ -150,7 +151,7 @@ bool SolarArrayDeploymentHandler::autonomousDeplForFile(sd::SdCard sdCard, const string line; string word; unsigned int lineNum = 0; - AutonomousDeplState deplState; + AutonomousDeplState deplState = AutonomousDeplState::INIT; bool stateSwitch = false; uint32_t secsSinceBoot = 0; while (std::getline(file, line)) { diff --git a/mission/devices/SusHandler.cpp b/mission/devices/SusHandler.cpp index 73a8344f..0b053e99 100644 --- a/mission/devices/SusHandler.cpp +++ b/mission/devices/SusHandler.cpp @@ -150,7 +150,7 @@ ReturnValue_t SusHandler::scanForReply(const uint8_t *start, size_t remainingSiz ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { switch (id) { case SUS::WRITE_SETUP: { - if (mode == _MODE_START_UP) { + if (getMode() == _MODE_START_UP) { commandExecuted = true; } return returnvalue::OK; diff --git a/mission/devices/SyrlinksHkHandler.cpp b/mission/devices/SyrlinksHkHandler.cpp index 1b4af19f..b07d665e 100644 --- a/mission/devices/SyrlinksHkHandler.cpp +++ b/mission/devices/SyrlinksHkHandler.cpp @@ -630,7 +630,7 @@ ReturnValue_t SyrlinksHkHandler::initializeLocalDataPool(localpool::DataPool& lo return returnvalue::OK; } -void SyrlinksHkHandler::setModeNormal() { mode = MODE_NORMAL; } +void SyrlinksHkHandler::setModeNormal() { setMode(MODE_NORMAL); } float SyrlinksHkHandler::calcTempVal(uint16_t raw) { return 0.126984 * raw - 67.87; } diff --git a/mission/devices/Tmp1075Handler.cpp b/mission/devices/Tmp1075Handler.cpp index 83a4643b..22d72999 100644 --- a/mission/devices/Tmp1075Handler.cpp +++ b/mission/devices/Tmp1075Handler.cpp @@ -12,7 +12,7 @@ Tmp1075Handler::Tmp1075Handler(object_id_t objectId, object_id_t comIF, CookieIF Tmp1075Handler::~Tmp1075Handler() {} void Tmp1075Handler::doStartUp() { - if (mode == _MODE_START_UP) { + if (getMode() == _MODE_START_UP) { setMode(MODE_ON); } } diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h similarity index 92% rename from mission/devices/devicedefinitions/IMTQHandlerDefinitions.h rename to mission/devices/devicedefinitions/imtqHandlerDefinitions.h index 485be5da..b3598970 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h @@ -1,8 +1,11 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ +#include #include +class ImtqHandler; + namespace IMTQ { static const DeviceCommandId_t NONE = 0x0; @@ -27,15 +30,18 @@ static const uint8_t GET_TEMP_REPLY_SIZE = 2; static const uint8_t CFGR_CMD_SIZE = 3; static const uint8_t POINTER_REG_SIZE = 1; -static const uint32_t ENG_HK_DATA_SET_ID = 1; -static const uint32_t CAL_MTM_SET = 2; -static const uint32_t RAW_MTM_SET = 3; -static const uint32_t POS_X_TEST_DATASET = 4; -static const uint32_t NEG_X_TEST_DATASET = 5; -static const uint32_t POS_Y_TEST_DATASET = 6; -static const uint32_t NEG_Y_TEST_DATASET = 7; -static const uint32_t POS_Z_TEST_DATASET = 8; -static const uint32_t NEG_Z_TEST_DATASET = 9; +enum SetIds : uint32_t { + ENG_HK = 1, + CAL_MGM = 2, + RAW_MGM = 3, + POS_X_TEST = 4, + NEG_X_TEST = 5, + POS_Y_TEST = 6, + NEG_Y_TEST = 7, + POS_Z_TEST = 8, + NEG_Z_TEST = 9, + DIPOLES = 10 +}; static const uint8_t SIZE_ENG_HK_COMMAND = 1; static const uint8_t SIZE_STATUS_REPLY = 2; @@ -80,6 +86,7 @@ static const uint8_t GET_COMMANDED_DIPOLE = 0x46; static const uint8_t GET_RAW_MTM_MEASUREMENT = 0x42; static const uint8_t GET_CAL_MTM_MEASUREMENT = 0x43; static const uint8_t GET_SELF_TEST_RESULT = 0x47; +static const uint8_t PAST_AVAILABLE_RESPONSE_BYTES = 0xff; }; // namespace CC namespace SELF_TEST_AXIS { @@ -103,7 +110,7 @@ static const uint8_t Z_NEGATIVE = 0x6; static const uint8_t FINA = 0x7; } // namespace SELF_TEST_STEPS -enum IMTQPoolIds : lp_id_t { +enum PoolIds : lp_id_t { DIGITAL_VOLTAGE_MV, ANALOG_VOLTAGE_MV, DIGITAL_CURRENT, @@ -119,6 +126,10 @@ enum IMTQPoolIds : lp_id_t { ACTUATION_CAL_STATUS, MTM_RAW, ACTUATION_RAW_STATUS, + DIPOLES_X, + DIPOLES_Y, + DIPOLES_Z, + CURRENT_TORQUE_DURATION, INIT_POS_X_ERR, INIT_POS_X_RAW_MAG_X, @@ -375,9 +386,9 @@ enum IMTQPoolIds : lp_id_t { class EngHkDataset : public StaticLocalDataSet { public: - EngHkDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, ENG_HK_DATA_SET_ID) {} + EngHkDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, IMTQ::SetIds::ENG_HK) {} - EngHkDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, ENG_HK_DATA_SET_ID)) {} + EngHkDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::ENG_HK)) {} lp_var_t digitalVoltageMv = lp_var_t(sid.objectId, DIGITAL_VOLTAGE_MV, this); lp_var_t analogVoltageMv = lp_var_t(sid.objectId, ANALOG_VOLTAGE_MV, this); @@ -398,13 +409,14 @@ class EngHkDataset : public StaticLocalDataSet { */ class CalibratedMtmMeasurementSet : public StaticLocalDataSet { public: - CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CAL_MTM_SET) {} + CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner) + : StaticLocalDataSet(owner, IMTQ::SetIds::CAL_MGM) {} CalibratedMtmMeasurementSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) {} + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::CAL_MGM)) {} /** The unit of all measurements is nT */ - lp_vec_t mgmXyz = lp_vec_t(sid.objectId, MGM_CAL_NT); + lp_vec_t mgmXyz = lp_vec_t(sid.objectId, MGM_CAL_NT, this); /** 1 if coils were actuating during measurement otherwise 0 */ lp_var_t coilActuationStatus = lp_var_t(sid.objectId, ACTUATION_CAL_STATUS, this); @@ -415,9 +427,11 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet { public: - RawMtmMeasurementSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, RAW_MTM_SET) {} + RawMtmMeasurementSet(HasLocalDataPoolIF* owner) + : StaticLocalDataSet(owner, IMTQ::SetIds::RAW_MGM) {} - RawMtmMeasurementSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, RAW_MTM_SET)) {} + RawMtmMeasurementSet(object_id_t objectId) + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::RAW_MGM)) {} /** The unit of all measurements is nT */ lp_vec_t mtmRawNt = lp_vec_t(sid.objectId, MTM_RAW, this); @@ -437,6 +451,11 @@ class CommandDipolePacket : public SerialLinkedListAdapter { public: CommandDipolePacket() { setLinks(); } + SerializeElement xDipole; + SerializeElement yDipole; + SerializeElement zDipole; + SerializeElement duration; + private: /** * @brief Constructor @@ -456,10 +475,46 @@ class CommandDipolePacket : public SerialLinkedListAdapter { yDipole.setNext(&zDipole); zDipole.setNext(&duration); } - SerializeElement xDipole; - SerializeElement yDipole; - SerializeElement zDipole; - SerializeElement duration; +}; + +class DipoleActuationSet : public StaticLocalDataSet<4> { + friend class ::ImtqHandler; + + public: + DipoleActuationSet(HasLocalDataPoolIF& owner) + : StaticLocalDataSet(&owner, IMTQ::SetIds::DIPOLES) {} + DipoleActuationSet(object_id_t objectId) + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::DIPOLES)) {} + + // Refresh torque command without changing any of the set dipoles. + void refreshTorqueing(uint16_t durationMs_) { currentTorqueDurationMs = durationMs_; } + + void setDipoles(uint16_t xDipole_, uint16_t yDipole_, uint16_t zDipole_, + uint16_t currentTorqueDurationMs_) { + if (xDipole.value != xDipole_) { + } + xDipole = xDipole_; + if (yDipole.value != yDipole_) { + } + yDipole = yDipole_; + if (zDipole.value != zDipole_) { + } + zDipole = zDipole_; + currentTorqueDurationMs = currentTorqueDurationMs_; + } + + void getDipoles(uint16_t& xDipole_, uint16_t& yDipole_, uint16_t& zDipole_) { + xDipole_ = xDipole.value; + yDipole_ = yDipole.value; + zDipole_ = zDipole.value; + } + + private: + lp_var_t xDipole = lp_var_t(sid.objectId, DIPOLES_X, this); + lp_var_t yDipole = lp_var_t(sid.objectId, DIPOLES_Y, this); + lp_var_t zDipole = lp_var_t(sid.objectId, DIPOLES_Z, this); + lp_var_t currentTorqueDurationMs = + lp_var_t(sid.objectId, CURRENT_TORQUE_DURATION, this); }; /** @@ -479,10 +534,10 @@ class CommandDipolePacket : public SerialLinkedListAdapter { class PosXSelfTestSet : public StaticLocalDataSet { public: PosXSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::POS_X_TEST_DATASET) {} + : StaticLocalDataSet(owner, IMTQ::SetIds::POS_X_TEST) {} PosXSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::POS_X_TEST_DATASET)) {} + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_X_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_X_ERR, this); @@ -556,10 +611,10 @@ class PosXSelfTestSet : public StaticLocalDataSet { class NegXSelfTestSet : public StaticLocalDataSet { public: NegXSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::NEG_X_TEST_DATASET) {} + : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_X_TEST) {} NegXSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_X_TEST_DATASET)) {} + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_X_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_X_ERR, this); @@ -633,10 +688,10 @@ class NegXSelfTestSet : public StaticLocalDataSet { class PosYSelfTestSet : public StaticLocalDataSet { public: PosYSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::POS_Y_TEST_DATASET) {} + : StaticLocalDataSet(owner, IMTQ::SetIds::POS_Y_TEST) {} PosYSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::POS_Y_TEST_DATASET)) {} + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Y_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_Y_ERR, this); @@ -710,10 +765,10 @@ class PosYSelfTestSet : public StaticLocalDataSet { class NegYSelfTestSet : public StaticLocalDataSet { public: NegYSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::NEG_Y_TEST_DATASET) {} + : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Y_TEST) {} NegYSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_Y_TEST_DATASET)) {} + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Y_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_Y_ERR, this); @@ -787,10 +842,10 @@ class NegYSelfTestSet : public StaticLocalDataSet { class PosZSelfTestSet : public StaticLocalDataSet { public: PosZSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::POS_Z_TEST_DATASET) {} + : StaticLocalDataSet(owner, IMTQ::SetIds::POS_Z_TEST) {} PosZSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::POS_Z_TEST_DATASET)) {} + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Z_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_Z_ERR, this); @@ -864,10 +919,10 @@ class PosZSelfTestSet : public StaticLocalDataSet { class NegZSelfTestSet : public StaticLocalDataSet { public: NegZSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::NEG_Z_TEST_DATASET) {} + : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Z_TEST) {} NegZSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_Z_TEST_DATASET)) {} + : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Z_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_Z_ERR, this); diff --git a/mission/devices/devicedefinitions/payloadPcduDefinitions.h b/mission/devices/devicedefinitions/payloadPcduDefinitions.h index 2730d1cb..b1adae5f 100644 --- a/mission/devices/devicedefinitions/payloadPcduDefinitions.h +++ b/mission/devices/devicedefinitions/payloadPcduDefinitions.h @@ -101,6 +101,9 @@ enum NormalSubmodeBits { }; static constexpr Submode_t ALL_OFF_SUBMODE = 0; +static constexpr Submode_t ALL_ON_SUBMODE = (1 << HPA_ON) | (1 << MPA_ON) | (1 << TX_ON) | + (1 << X8_ON) | (1 << DRO_ON) | + (1 << SOLID_STATE_RELAYS_ADC_ON); // 12 ADC values * 2 + trailing zero static constexpr size_t ADC_REPLY_SIZE = 25; diff --git a/mission/devices/torquer.cpp b/mission/devices/torquer.cpp new file mode 100644 index 00000000..27a32bef --- /dev/null +++ b/mission/devices/torquer.cpp @@ -0,0 +1,27 @@ +#include "torquer.h" + +#include + +MutexIF* TORQUE_LOCK = nullptr; + +namespace torquer { + +bool TORQUEING = false; +bool NEW_ACTUATION_FLAG = false; +Countdown TORQUE_COUNTDOWN = Countdown(); + +bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration) { + if (TORQUEING and remainingTorqueDuration != nullptr) { + *remainingTorqueDuration = TORQUE_COUNTDOWN.getRemainingMillis() + TORQUE_BUFFER_TIME_MS; + } + return TORQUEING; +} + +MutexIF* lazyLock() { + if (TORQUE_LOCK == nullptr) { + TORQUE_LOCK = MutexFactory::instance()->createMutex(); + } + return TORQUE_LOCK; +} + +} // namespace torquer diff --git a/mission/devices/torquer.h b/mission/devices/torquer.h new file mode 100644 index 00000000..10a27991 --- /dev/null +++ b/mission/devices/torquer.h @@ -0,0 +1,22 @@ +#ifndef MISSION_DEVICES_TOQUER_H_ +#define MISSION_DEVICES_TOQUER_H_ + +#include +#include + +namespace torquer { + +// Additional buffer time to accont for time until I2C command arrives and ramp up / ramp down +// time of the MGT +static constexpr dur_millis_t TORQUE_BUFFER_TIME_MS = 20; + +MutexIF* lazyLock(); +extern bool TORQUEING; +extern bool NEW_ACTUATION_FLAG; +extern Countdown TORQUE_COUNTDOWN; + +bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration); + +} // namespace torquer + +#endif /* MISSION_DEVICES_TOQUER_H_ */ diff --git a/mission/memory/SdCardMountedIF.h b/mission/memory/SdCardMountedIF.h index d88bb57a..ac705e8d 100644 --- a/mission/memory/SdCardMountedIF.h +++ b/mission/memory/SdCardMountedIF.h @@ -10,7 +10,7 @@ class SdCardMountedIF { public: virtual ~SdCardMountedIF(){}; virtual const std::string& getCurrentMountPrefix() const = 0; - virtual bool isSdCardUsable(sd::SdCard sdCard) = 0; + virtual bool isSdCardUsable(std::optional sdCard) = 0; virtual std::optional getPreferredSdCard() const = 0; virtual void setActiveSdCard(sd::SdCard sdCard) = 0; virtual std::optional getActiveSdCard() const = 0; diff --git a/mission/system/AcsSubsystem.cpp b/mission/system/AcsSubsystem.cpp deleted file mode 100644 index 37da0123..00000000 --- a/mission/system/AcsSubsystem.cpp +++ /dev/null @@ -1,5 +0,0 @@ -#include "AcsSubsystem.h" - -AcsSubsystem::AcsSubsystem(object_id_t setObjectId, object_id_t parent, - uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables) - : Subsystem(setObjectId, parent, maxNumberOfSequences, maxNumberOfTables) {} diff --git a/mission/system/CMakeLists.txt b/mission/system/CMakeLists.txt index 21a2fd72..617e2765 100644 --- a/mission/system/CMakeLists.txt +++ b/mission/system/CMakeLists.txt @@ -1,15 +1,3 @@ -target_sources( - ${LIB_EIVE_MISSION} - PRIVATE EiveSystem.cpp - AcsSubsystem.cpp - ComSubsystem.cpp - PayloadSubsystem.cpp - AcsBoardAssembly.cpp - RwAssembly.cpp - SusAssembly.cpp - DualLanePowerStateMachine.cpp - PowerStateMachineBase.cpp - DualLaneAssemblyBase.cpp - TcsBoardAssembly.cpp) - +add_subdirectory(objects) +add_subdirectory(tree) add_subdirectory(fdir) diff --git a/mission/system/ComSubsystem.cpp b/mission/system/ComSubsystem.cpp deleted file mode 100644 index 3308de8c..00000000 --- a/mission/system/ComSubsystem.cpp +++ /dev/null @@ -1,5 +0,0 @@ -#include "ComSubsystem.h" - -ComSubsystem::ComSubsystem(object_id_t setObjectId, object_id_t parent, - uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables) - : Subsystem(setObjectId, parent, maxNumberOfSequences, maxNumberOfTables) {} diff --git a/mission/system/EiveSystem.cpp b/mission/system/EiveSystem.cpp deleted file mode 100644 index 1be0152a..00000000 --- a/mission/system/EiveSystem.cpp +++ /dev/null @@ -1,5 +0,0 @@ -#include "EiveSystem.h" - -EiveSystem::EiveSystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences, - uint32_t maxNumberOfTables) - : Subsystem(setObjectId, parent, maxNumberOfSequences, maxNumberOfTables) {} diff --git a/mission/system/PayloadSubsystem.cpp b/mission/system/PayloadSubsystem.cpp deleted file mode 100644 index 97008c2f..00000000 --- a/mission/system/PayloadSubsystem.cpp +++ /dev/null @@ -1,5 +0,0 @@ -#include "PayloadSubsystem.h" - -PayloadSubsystem::PayloadSubsystem(object_id_t setObjectId, object_id_t parent, - uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables) - : Subsystem(setObjectId, parent, maxNumberOfSequences, maxNumberOfTables) {} diff --git a/mission/system/AcsBoardAssembly.cpp b/mission/system/objects/AcsBoardAssembly.cpp similarity index 89% rename from mission/system/AcsBoardAssembly.cpp rename to mission/system/objects/AcsBoardAssembly.cpp index 6a59183c..bcc2eb7a 100644 --- a/mission/system/AcsBoardAssembly.cpp +++ b/mission/system/objects/AcsBoardAssembly.cpp @@ -6,11 +6,10 @@ #include "OBSWConfig.h" -AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId, - PowerSwitchIF* switcher, AcsBoardHelper helper, GpioIF* gpioIF) - : DualLaneAssemblyBase(objectId, parentId, switcher, SWITCH_A, SWITCH_B, - POWER_STATE_MACHINE_TIMEOUT, SIDE_SWITCH_TRANSITION_NOT_ALLOWED, - TRANSITION_OTHER_SIDE_FAILED), +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) { @@ -275,42 +274,4 @@ void AcsBoardAssembly::refreshHelperModes() { } } -ReturnValue_t AcsBoardAssembly::initialize() { - ReturnValue_t result = registerChild(helper.gyro0AdisIdSideA); - if (result != returnvalue::OK) { - return result; - } - result = registerChild(helper.gyro1L3gIdSideA); - if (result != returnvalue::OK) { - return result; - } - result = registerChild(helper.gyro2AdisIdSideB); - if (result != returnvalue::OK) { - return result; - } - result = registerChild(helper.gyro3L3gIdSideB); - if (result != returnvalue::OK) { - return result; - } - result = registerChild(helper.mgm0Lis3IdSideA); - if (result != returnvalue::OK) { - return result; - } - result = registerChild(helper.mgm1Rm3100IdSideA); - if (result != returnvalue::OK) { - return result; - } - result = registerChild(helper.mgm2Lis3IdSideB); - if (result != returnvalue::OK) { - return result; - } - result = registerChild(helper.mgm3Rm3100IdSideB); - if (result != returnvalue::OK) { - return result; - } - result = registerChild(helper.gpsId); - if (result != returnvalue::OK) { - return result; - } - return AssemblyBase::initialize(); -} +ReturnValue_t AcsBoardAssembly::initialize() { return AssemblyBase::initialize(); } diff --git a/mission/system/AcsBoardAssembly.h b/mission/system/objects/AcsBoardAssembly.h similarity index 96% rename from mission/system/AcsBoardAssembly.h rename to mission/system/objects/AcsBoardAssembly.h index 820060ce..1fe3336d 100644 --- a/mission/system/AcsBoardAssembly.h +++ b/mission/system/objects/AcsBoardAssembly.h @@ -94,8 +94,8 @@ class AcsBoardAssembly : public DualLaneAssemblyBase { static constexpr uint8_t NUMBER_DEVICES_MODE_TABLE = 9; - AcsBoardAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, - AcsBoardHelper helper, GpioIF* gpioIF); + AcsBoardAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, AcsBoardHelper helper, + GpioIF* gpioIF); /** * In dual mode, the A side or the B side GPS device can be used, but not both. diff --git a/mission/system/objects/AcsSubsystem.cpp b/mission/system/objects/AcsSubsystem.cpp new file mode 100644 index 00000000..96735ae9 --- /dev/null +++ b/mission/system/objects/AcsSubsystem.cpp @@ -0,0 +1,5 @@ +#include "AcsSubsystem.h" + +AcsSubsystem::AcsSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, + uint32_t maxNumberOfTables) + : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {} diff --git a/mission/system/AcsSubsystem.h b/mission/system/objects/AcsSubsystem.h similarity index 62% rename from mission/system/AcsSubsystem.h rename to mission/system/objects/AcsSubsystem.h index 20fffdbe..8673395a 100644 --- a/mission/system/AcsSubsystem.h +++ b/mission/system/objects/AcsSubsystem.h @@ -5,8 +5,7 @@ class AcsSubsystem : public Subsystem { public: - AcsSubsystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences, - uint32_t maxNumberOfTables); + AcsSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables); private: }; diff --git a/mission/system/objects/CMakeLists.txt b/mission/system/objects/CMakeLists.txt new file mode 100644 index 00000000..23e97047 --- /dev/null +++ b/mission/system/objects/CMakeLists.txt @@ -0,0 +1,14 @@ +target_sources( + ${LIB_EIVE_MISSION} + PRIVATE EiveSystem.cpp + CamSwitcher.cpp + AcsSubsystem.cpp + ComSubsystem.cpp + PayloadSubsystem.cpp + AcsBoardAssembly.cpp + SusAssembly.cpp + RwAssembly.cpp + DualLanePowerStateMachine.cpp + PowerStateMachineBase.cpp + DualLaneAssemblyBase.cpp + TcsBoardAssembly.cpp) diff --git a/mission/system/objects/CamSwitcher.cpp b/mission/system/objects/CamSwitcher.cpp new file mode 100644 index 00000000..b995e211 --- /dev/null +++ b/mission/system/objects/CamSwitcher.cpp @@ -0,0 +1,5 @@ +#include "CamSwitcher.h" + +CamSwitcher::CamSwitcher(object_id_t objectId, PowerSwitchIF &pwrSwitcher, + power::Switch_t pwrSwitch) + : PowerSwitcherComponent(objectId, &pwrSwitcher, pwrSwitch) {} diff --git a/mission/system/objects/CamSwitcher.h b/mission/system/objects/CamSwitcher.h new file mode 100644 index 00000000..672b884b --- /dev/null +++ b/mission/system/objects/CamSwitcher.h @@ -0,0 +1,13 @@ +#ifndef MISSION_SYSTEM_OBJECTS_CAMSWITCHER_H_ +#define MISSION_SYSTEM_OBJECTS_CAMSWITCHER_H_ + +#include + +class CamSwitcher : public PowerSwitcherComponent { + public: + CamSwitcher(object_id_t objectId, PowerSwitchIF &pwrSwitcher, power::Switch_t pwrSwitch); + + private: +}; + +#endif /* MISSION_SYSTEM_OBJECTS_CAMSWITCHER_H_ */ diff --git a/mission/system/objects/ComSubsystem.cpp b/mission/system/objects/ComSubsystem.cpp new file mode 100644 index 00000000..794eede6 --- /dev/null +++ b/mission/system/objects/ComSubsystem.cpp @@ -0,0 +1,5 @@ +#include "ComSubsystem.h" + +ComSubsystem::ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, + uint32_t maxNumberOfTables) + : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {} diff --git a/mission/system/ComSubsystem.h b/mission/system/objects/ComSubsystem.h similarity index 62% rename from mission/system/ComSubsystem.h rename to mission/system/objects/ComSubsystem.h index fc35438c..0bdaeb15 100644 --- a/mission/system/ComSubsystem.h +++ b/mission/system/objects/ComSubsystem.h @@ -5,8 +5,7 @@ class ComSubsystem : public Subsystem { public: - ComSubsystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences, - uint32_t maxNumberOfTables); + ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables); private: }; diff --git a/mission/system/DualLaneAssemblyBase.cpp b/mission/system/objects/DualLaneAssemblyBase.cpp similarity index 96% rename from mission/system/DualLaneAssemblyBase.cpp rename to mission/system/objects/DualLaneAssemblyBase.cpp index f9ca44fe..a55553a0 100644 --- a/mission/system/DualLaneAssemblyBase.cpp +++ b/mission/system/objects/DualLaneAssemblyBase.cpp @@ -4,12 +4,11 @@ #include "OBSWConfig.h" -DualLaneAssemblyBase::DualLaneAssemblyBase(object_id_t objectId, object_id_t parentId, - PowerSwitchIF* pwrSwitcher, pcdu::Switches switch1, - pcdu::Switches switch2, Event pwrTimeoutEvent, - Event sideSwitchNotAllowedEvent, +DualLaneAssemblyBase::DualLaneAssemblyBase(object_id_t objectId, PowerSwitchIF* pwrSwitcher, + pcdu::Switches switch1, pcdu::Switches switch2, + Event pwrTimeoutEvent, Event sideSwitchNotAllowedEvent, Event transitionOtherSideFailedEvent) - : AssemblyBase(objectId, parentId, 20), + : AssemblyBase(objectId, 20), pwrStateMachine(switch1, switch2, pwrSwitcher), pwrTimeoutEvent(pwrTimeoutEvent), sideSwitchNotAllowedEvent(sideSwitchNotAllowedEvent), diff --git a/mission/system/DualLaneAssemblyBase.h b/mission/system/objects/DualLaneAssemblyBase.h similarity index 93% rename from mission/system/DualLaneAssemblyBase.h rename to mission/system/objects/DualLaneAssemblyBase.h index 4233d860..eadfb77f 100644 --- a/mission/system/DualLaneAssemblyBase.h +++ b/mission/system/objects/DualLaneAssemblyBase.h @@ -2,7 +2,7 @@ #define MISSION_SYSTEM_DUALLANEASSEMBLYBASE_H_ #include -#include +#include /** * @brief Encapsulates assemblies which are also responsible for dual lane power switching @@ -18,8 +18,8 @@ class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF { static constexpr UniqueEventId_t POWER_STATE_MACHINE_TIMEOUT_ID = 2; static constexpr UniqueEventId_t SIDE_SWITCH_TRANSITION_NOT_ALLOWED_ID = 3; - DualLaneAssemblyBase(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, - pcdu::Switches switch1, pcdu::Switches switch2, Event pwrSwitchTimeoutEvent, + DualLaneAssemblyBase(object_id_t objectId, PowerSwitchIF* pwrSwitcher, pcdu::Switches switch1, + pcdu::Switches switch2, Event pwrSwitchTimeoutEvent, Event sideSwitchNotAllowedEvent, Event transitionOtherSideFailedEvent); protected: diff --git a/mission/system/DualLanePowerStateMachine.cpp b/mission/system/objects/DualLanePowerStateMachine.cpp similarity index 100% rename from mission/system/DualLanePowerStateMachine.cpp rename to mission/system/objects/DualLanePowerStateMachine.cpp diff --git a/mission/system/DualLanePowerStateMachine.h b/mission/system/objects/DualLanePowerStateMachine.h similarity index 91% rename from mission/system/DualLanePowerStateMachine.h rename to mission/system/objects/DualLanePowerStateMachine.h index d2160c64..7a6f29b6 100644 --- a/mission/system/DualLanePowerStateMachine.h +++ b/mission/system/objects/DualLanePowerStateMachine.h @@ -3,7 +3,7 @@ #include #include -#include +#include #include "definitions.h" diff --git a/mission/system/objects/EiveSystem.cpp b/mission/system/objects/EiveSystem.cpp new file mode 100644 index 00000000..a870214d --- /dev/null +++ b/mission/system/objects/EiveSystem.cpp @@ -0,0 +1,5 @@ +#include "EiveSystem.h" + +EiveSystem::EiveSystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, + uint32_t maxNumberOfTables) + : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {} diff --git a/mission/system/EiveSystem.h b/mission/system/objects/EiveSystem.h similarity index 62% rename from mission/system/EiveSystem.h rename to mission/system/objects/EiveSystem.h index 671d47b6..e88d2402 100644 --- a/mission/system/EiveSystem.h +++ b/mission/system/objects/EiveSystem.h @@ -5,8 +5,7 @@ class EiveSystem : public Subsystem { public: - EiveSystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences, - uint32_t maxNumberOfTables); + EiveSystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables); private: }; diff --git a/mission/system/objects/PayloadSubsystem.cpp b/mission/system/objects/PayloadSubsystem.cpp new file mode 100644 index 00000000..8bc98f0b --- /dev/null +++ b/mission/system/objects/PayloadSubsystem.cpp @@ -0,0 +1,5 @@ +#include "PayloadSubsystem.h" + +PayloadSubsystem::PayloadSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, + uint32_t maxNumberOfTables) + : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {} diff --git a/mission/system/PayloadSubsystem.h b/mission/system/objects/PayloadSubsystem.h similarity index 75% rename from mission/system/PayloadSubsystem.h rename to mission/system/objects/PayloadSubsystem.h index b25454f7..af3c1efb 100644 --- a/mission/system/PayloadSubsystem.h +++ b/mission/system/objects/PayloadSubsystem.h @@ -5,7 +5,7 @@ class PayloadSubsystem : public Subsystem { public: - PayloadSubsystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences, + PayloadSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables); private: diff --git a/mission/system/PowerStateMachineBase.cpp b/mission/system/objects/PowerStateMachineBase.cpp similarity index 100% rename from mission/system/PowerStateMachineBase.cpp rename to mission/system/objects/PowerStateMachineBase.cpp diff --git a/mission/system/PowerStateMachineBase.h b/mission/system/objects/PowerStateMachineBase.h similarity index 100% rename from mission/system/PowerStateMachineBase.h rename to mission/system/objects/PowerStateMachineBase.h diff --git a/mission/system/RwAssembly.cpp b/mission/system/objects/RwAssembly.cpp similarity index 91% rename from mission/system/RwAssembly.cpp rename to mission/system/objects/RwAssembly.cpp index 6e393034..6e1c86ae 100644 --- a/mission/system/RwAssembly.cpp +++ b/mission/system/objects/RwAssembly.cpp @@ -1,8 +1,8 @@ #include "RwAssembly.h" -RwAssembly::RwAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, - power::Switch_t switcher, RwHelper helper) - : AssemblyBase(objectId, parentId), helper(helper), switcher(pwrSwitcher, switcher) { +RwAssembly::RwAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, power::Switch_t switcher, + RwHelper helper) + : AssemblyBase(objectId), helper(helper), switcher(pwrSwitcher, switcher) { ModeListEntry entry; for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) { entry.setObject(helper.rwIds[idx]); @@ -167,16 +167,7 @@ bool RwAssembly::isUseable(object_id_t object, Mode_t mode) { return false; } -ReturnValue_t RwAssembly::initialize() { - ReturnValue_t result = returnvalue::OK; - for (const auto& obj : helper.rwIds) { - result = registerChild(obj); - if (result != returnvalue::OK) { - return result; - } - } - return SubsystemBase::initialize(); -} +ReturnValue_t RwAssembly::initialize() { return SubsystemBase::initialize(); } void RwAssembly::handleModeTransitionFailed(ReturnValue_t result) { if (targetMode == MODE_OFF) { diff --git a/mission/system/RwAssembly.h b/mission/system/objects/RwAssembly.h similarity index 91% rename from mission/system/RwAssembly.h rename to mission/system/objects/RwAssembly.h index 482a18a3..0eb0b59f 100644 --- a/mission/system/RwAssembly.h +++ b/mission/system/objects/RwAssembly.h @@ -12,8 +12,8 @@ struct RwHelper { class RwAssembly : public AssemblyBase { public: - RwAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, - power::Switch_t switcher, RwHelper helper); + RwAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, power::Switch_t switcher, + RwHelper helper); private: static constexpr uint8_t NUMBER_RWS = 4; diff --git a/mission/system/SusAssembly.cpp b/mission/system/objects/SusAssembly.cpp similarity index 90% rename from mission/system/SusAssembly.cpp rename to mission/system/objects/SusAssembly.cpp index 472a6580..3f75a369 100644 --- a/mission/system/SusAssembly.cpp +++ b/mission/system/objects/SusAssembly.cpp @@ -4,9 +4,8 @@ #include #include -SusAssembly::SusAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, - SusAssHelper helper) - : DualLaneAssemblyBase(objectId, parentId, pwrSwitcher, SWITCH_NOM, SWITCH_RED, +SusAssembly::SusAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, SusAssHelper helper) + : DualLaneAssemblyBase(objectId, pwrSwitcher, SWITCH_NOM, SWITCH_RED, POWER_STATE_MACHINE_TIMEOUT, SIDE_SWITCH_TRANSITION_NOT_ALLOWED, TRANSITION_OTHER_SIDE_FAILED), helper(helper), @@ -121,16 +120,7 @@ ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wan return returnvalue::OK; } -ReturnValue_t SusAssembly::initialize() { - ReturnValue_t result = returnvalue::OK; - for (const auto& id : helper.susIds) { - result = registerChild(id); - if (result != returnvalue::OK) { - return result; - } - } - return AssemblyBase::initialize(); -} +ReturnValue_t SusAssembly::initialize() { return AssemblyBase::initialize(); } bool SusAssembly::isUseable(object_id_t object, Mode_t mode) { if (healthHelper.healthTable->isFaulty(object)) { diff --git a/mission/system/SusAssembly.h b/mission/system/objects/SusAssembly.h similarity index 95% rename from mission/system/SusAssembly.h rename to mission/system/objects/SusAssembly.h index 3cce9d86..673e91f5 100644 --- a/mission/system/SusAssembly.h +++ b/mission/system/objects/SusAssembly.h @@ -40,8 +40,7 @@ class SusAssembly : public DualLaneAssemblyBase { static constexpr Event SIDE_SWITCH_TRANSITION_NOT_ALLOWED = event::makeEvent(SUBSYSTEM_ID, 3, severity::LOW); - SusAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, - SusAssHelper helper); + SusAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, SusAssHelper helper); private: enum class States { IDLE, SWITCHING_POWER, MODE_COMMANDING } state = States::IDLE; diff --git a/mission/system/TcsBoardAssembly.cpp b/mission/system/objects/TcsBoardAssembly.cpp similarity index 91% rename from mission/system/TcsBoardAssembly.cpp rename to mission/system/objects/TcsBoardAssembly.cpp index c753504b..88075fdc 100644 --- a/mission/system/TcsBoardAssembly.cpp +++ b/mission/system/objects/TcsBoardAssembly.cpp @@ -3,10 +3,9 @@ #include #include -TcsBoardAssembly::TcsBoardAssembly(object_id_t objectId, object_id_t parentId, - PowerSwitchIF* pwrSwitcher, power::Switch_t theSwitch, - TcsBoardHelper helper) - : AssemblyBase(objectId, parentId, 24), switcher(pwrSwitcher, theSwitch), helper(helper) { +TcsBoardAssembly::TcsBoardAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, + power::Switch_t theSwitch, TcsBoardHelper helper) + : AssemblyBase(objectId, 24), switcher(pwrSwitcher, theSwitch), helper(helper) { eventQueue = QueueFactory::instance()->createMessageQueue(24); ModeListEntry entry; for (uint8_t idx = 0; idx < NUMBER_RTDS; idx++) { @@ -90,16 +89,7 @@ ReturnValue_t TcsBoardAssembly::isModeCombinationValid(Mode_t mode, Submode_t su return HasModesIF::INVALID_MODE; } -ReturnValue_t TcsBoardAssembly::initialize() { - ReturnValue_t result = returnvalue::OK; - for (const auto& obj : helper.rtdInfos) { - result = registerChild(obj.first); - if (result != returnvalue::OK) { - return result; - } - } - return SubsystemBase::initialize(); -} +ReturnValue_t TcsBoardAssembly::initialize() { return AssemblyBase::initialize(); } void TcsBoardAssembly::startTransition(Mode_t mode, Submode_t submode) { if (mode != MODE_OFF) { diff --git a/mission/system/TcsBoardAssembly.h b/mission/system/objects/TcsBoardAssembly.h similarity index 92% rename from mission/system/TcsBoardAssembly.h rename to mission/system/objects/TcsBoardAssembly.h index 6042abaa..fb5f7d38 100644 --- a/mission/system/TcsBoardAssembly.h +++ b/mission/system/objects/TcsBoardAssembly.h @@ -20,8 +20,8 @@ class TcsBoardAssembly : public AssemblyBase, public ConfirmsFailuresIF { static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::TCS_BOARD_ASS; static constexpr Event CHILDREN_LOST_MODE = event::makeEvent(SUBSYSTEM_ID, 0, severity::MEDIUM); - TcsBoardAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, - power::Switch_t switcher, TcsBoardHelper helper); + TcsBoardAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, power::Switch_t switcher, + TcsBoardHelper helper); ReturnValue_t initialize() override; diff --git a/mission/system/definitions.h b/mission/system/objects/definitions.h similarity index 68% rename from mission/system/definitions.h rename to mission/system/objects/definitions.h index aee127bb..99fc5eb6 100644 --- a/mission/system/definitions.h +++ b/mission/system/objects/definitions.h @@ -16,4 +16,16 @@ enum Submodes : Submode_t { A_SIDE = 0, B_SIDE = 1, DUAL_MODE = 2 }; } // namespace duallane +namespace payload { + +enum Modes { NONE = 0, SUPV_ONLY = 1, MPSOC_STREAM = 2, CAM_STREAM = 3, EARTH_OBSV = 4, SCEX = 5 }; + +namespace ploc { + +enum Modes { OFF = 0, SUPV_ONLY = 1, MPSOC_ON = 2 }; + +} + +} // namespace payload + #endif /* MISSION_SYSTEM_DEFINITIONS_H_ */ diff --git a/mission/system/tree/CMakeLists.txt b/mission/system/tree/CMakeLists.txt new file mode 100644 index 00000000..6e7f1619 --- /dev/null +++ b/mission/system/tree/CMakeLists.txt @@ -0,0 +1,2 @@ +target_sources(${LIB_EIVE_MISSION} PRIVATE acsModeTree.cpp payloadModeTree.cpp + system.cpp util.cpp) diff --git a/mission/system/tree/acsModeTree.cpp b/mission/system/tree/acsModeTree.cpp new file mode 100644 index 00000000..4e390985 --- /dev/null +++ b/mission/system/tree/acsModeTree.cpp @@ -0,0 +1,391 @@ +#include "acsModeTree.h" + +#include +#include +#include +#include +#include + +#include "eive/objects.h" +#include "mission/controller/controllerdefinitions/AcsControllerDefinitions.h" +#include "util.h" + +Subsystem satsystem::acs::ACS_SUBSYSTEM(objects::ACS_SUBSYSTEM, 12, 24); + +namespace { +// Alias for checker function +const auto check = subsystem::checkInsert; + +void buildOffSequence(Subsystem* ss, ModeListEntry& eh); +void buildDetumbleSequence(Subsystem* ss, ModeListEntry& entryHelper); +void buildSafeSequence(Subsystem* ss, ModeListEntry& entryHelper); +void buildIdleSequence(Subsystem* ss, ModeListEntry& entryHelper); +void buildIdleChargeSequence(Subsystem* ss, ModeListEntry& entryHelper); +void buildTargetPtSequence(Subsystem* ss, ModeListEntry& entryHelper); +} // namespace + +static const auto OFF = HasModesIF::MODE_OFF; +static const auto NML = DeviceHandlerIF::MODE_NORMAL; + +auto ACS_SEQUENCE_OFF = + std::make_pair(acs::CtrlModes::OFF << 24, FixedArrayList()); +auto ACS_TABLE_OFF_TGT = + std::make_pair((acs::CtrlModes::OFF << 24) | 1, FixedArrayList()); +auto ACS_TABLE_OFF_TRANS = + std::make_pair((acs::CtrlModes::OFF << 24) | 2, FixedArrayList()); + +auto ACS_SEQUENCE_DETUMBLE = + std::make_pair(acs::CtrlModes::DETUMBLE << 24, FixedArrayList()); +auto ACS_TABLE_DETUMBLE_TGT = + std::make_pair((acs::CtrlModes::DETUMBLE << 24) | 1, FixedArrayList()); +auto ACS_TABLE_DETUMBLE_TRANS_0 = + std::make_pair((acs::CtrlModes::DETUMBLE << 24) | 2, FixedArrayList()); +auto ACS_TABLE_DETUMBLE_TRANS_1 = + std::make_pair((acs::CtrlModes::DETUMBLE << 24) | 3, FixedArrayList()); + +auto ACS_SEQUENCE_SAFE = + std::make_pair(acs::CtrlModes::SAFE << 24, FixedArrayList()); +auto ACS_TABLE_SAFE_TGT = + std::make_pair((acs::CtrlModes::SAFE << 24) | 1, FixedArrayList()); +auto ACS_TABLE_SAFE_TRANS_0 = + std::make_pair((acs::CtrlModes::SAFE << 24) | 2, FixedArrayList()); +auto ACS_TABLE_SAFE_TRANS_1 = + std::make_pair((acs::CtrlModes::SAFE << 24) | 3, FixedArrayList()); + +auto ACS_SEQUENCE_IDLE = + std::make_pair(acs::CtrlModes::IDLE << 24, FixedArrayList()); +auto ACS_TABLE_IDLE_TGT = + std::make_pair((acs::CtrlModes::IDLE << 24) | 1, FixedArrayList()); +auto ACS_TABLE_IDLE_TRANS_0 = + std::make_pair((acs::CtrlModes::IDLE << 24) | 2, FixedArrayList()); +auto ACS_TABLE_IDLE_TRANS_1 = + std::make_pair((acs::CtrlModes::IDLE << 24) | 3, FixedArrayList()); + +auto ACS_SEQUENCE_IDLE_CHRG = std::make_pair(acs::CtrlModes::IDLE << 24 | (acs::IDLE_CHARGE << 8), + FixedArrayList()); +auto ACS_TABLE_IDLE_CHRG_TGT = std::make_pair( + (acs::CtrlModes::IDLE << 24) | (acs::IDLE_CHARGE << 8) | 1, FixedArrayList()); +auto ACS_TABLE_IDLE_CHRG_TRANS_0 = std::make_pair( + (acs::CtrlModes::IDLE << 24) | (acs::IDLE_CHARGE << 8) | 2, FixedArrayList()); +auto ACS_TABLE_IDLE_CHRG_TRANS_1 = std::make_pair( + (acs::CtrlModes::IDLE << 24) | (acs::IDLE_CHARGE << 8) | 3, FixedArrayList()); + +auto ACS_SEQUENCE_TARGET_PT = + std::make_pair(acs::CtrlModes::TARGET_PT, FixedArrayList()); +auto ACS_TABLE_TARGET_PT_TGT = + std::make_pair((acs::CtrlModes::TARGET_PT << 24) | 1, FixedArrayList()); +auto ACS_TABLE_TARGET_PT_TRANS_0 = + std::make_pair((acs::CtrlModes::TARGET_PT << 24) | 2, FixedArrayList()); +auto ACS_TABLE_TARGET_PT_TRANS_1 = + std::make_pair((acs::CtrlModes::TARGET_PT << 24) | 3, FixedArrayList()); + +void satsystem::acs::init() { + ModeListEntry entry; + buildOffSequence(&ACS_SUBSYSTEM, entry); + buildSafeSequence(&ACS_SUBSYSTEM, entry); + buildDetumbleSequence(&ACS_SUBSYSTEM, entry); + buildIdleSequence(&ACS_SUBSYSTEM, entry); + buildIdleChargeSequence(&ACS_SUBSYSTEM, entry); + buildTargetPtSequence(&ACS_SUBSYSTEM, entry); + ACS_SUBSYSTEM.setInitialMode(HasModesIF::MODE_OFF); +} + +namespace { + +void buildOffSequence(Subsystem* ss, ModeListEntry& eh) { + std::string context = "satsystem::acs::buildOffSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(table.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + // OFF Target table is empty + check(ss->addTable(&ACS_TABLE_OFF_TGT.second, ACS_TABLE_OFF_TGT.first, false, true), ctxc); + + // Build OFF transition + iht(objects::ACS_CONTROLLER, OFF, 0, ACS_TABLE_OFF_TRANS.second); + iht(objects::IMTQ_HANDLER, OFF, 0, ACS_TABLE_OFF_TRANS.second); + iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_OFF_TRANS.second); + iht(objects::ACS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS.second); + iht(objects::SUS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS.second); + iht(objects::RW_ASS, OFF, 0, ACS_TABLE_OFF_TRANS.second); + check(ss->addTable(&ACS_TABLE_OFF_TRANS.second, ACS_TABLE_OFF_TRANS.first, false, true), ctxc); + + // Build OFF sequence + ihs(ACS_SEQUENCE_OFF.second, ACS_TABLE_OFF_TGT.first, 0, false); + ihs(ACS_SEQUENCE_OFF.second, ACS_TABLE_OFF_TRANS.first, 0, false); + check(ss->addSequence(&ACS_SEQUENCE_OFF.second, ACS_SEQUENCE_OFF.first, ACS_SEQUENCE_OFF.first, + false, true), + ctxc); +} + +void buildSafeSequence(Subsystem* ss, ModeListEntry& eh) { + std::string context = "satsystem::acs::buildSafeSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, + ArrayList& sequence) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(sequence.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + // Build SAFE target + iht(objects::ACS_CONTROLLER, acs::CtrlModes::SAFE, 0, ACS_TABLE_SAFE_TGT.second); + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_SAFE_TGT.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second); + check(ss->addTable(&ACS_TABLE_SAFE_TGT.second, ACS_TABLE_SAFE_TGT.first, false, true), ctxc); + + // Build SAFE transition 0 + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_SAFE_TRANS_0.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TRANS_0.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TRANS_0.second); + iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second); + iht(objects::RW_ASS, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second); + check(ss->addTable(&ACS_TABLE_SAFE_TRANS_0.second, ACS_TABLE_SAFE_TRANS_0.first, false, true), + ctxc); + + // Build SAFE transition 1 + iht(objects::ACS_CONTROLLER, acs::CtrlModes::SAFE, 0, ACS_TABLE_SAFE_TRANS_1.second); + check(ss->addTable(&ACS_TABLE_SAFE_TRANS_1.second, ACS_TABLE_SAFE_TRANS_1.first, false, true), + ctxc); + + // Build SAFE sequence + ihs(ACS_SEQUENCE_SAFE.second, ACS_TABLE_SAFE_TGT.first, 0, true); + ihs(ACS_SEQUENCE_SAFE.second, ACS_TABLE_SAFE_TRANS_0.first, 0, false); + ihs(ACS_SEQUENCE_SAFE.second, ACS_TABLE_SAFE_TRANS_1.first, 0, false); + check(ss->addSequence(&ACS_SEQUENCE_SAFE.second, ACS_SEQUENCE_SAFE.first, ACS_SEQUENCE_SAFE.first, + false, true), + ctxc); +} + +void buildDetumbleSequence(Subsystem* ss, ModeListEntry& eh) { + std::string context = "satsystem::acs::buildDetumbleSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, + ArrayList& sequence) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(sequence.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + // Build DETUMBLE target + iht(objects::ACS_CONTROLLER, acs::CtrlModes::DETUMBLE, 0, ACS_TABLE_DETUMBLE_TGT.second); + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); + check(ss->addTable(&ACS_TABLE_DETUMBLE_TGT.second, ACS_TABLE_DETUMBLE_TGT.first, false, true), + ctxc); + + // Build DETUMBLE transition 0 + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); + iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); + iht(objects::RW_ASS, OFF, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); + check(ss->addTable(&ACS_TABLE_DETUMBLE_TRANS_0.second, ACS_TABLE_DETUMBLE_TRANS_0.first, false, + true), + ctxc); + + // Build DETUMBLE transition 1 + iht(objects::ACS_CONTROLLER, acs::CtrlModes::DETUMBLE, 0, ACS_TABLE_DETUMBLE_TRANS_1.second); + check(ss->addTable(&ACS_TABLE_DETUMBLE_TRANS_1.second, ACS_TABLE_DETUMBLE_TRANS_1.first, false, + true), + ctxc); + + // Build DETUMBLE sequence + ihs(ACS_SEQUENCE_DETUMBLE.second, ACS_TABLE_DETUMBLE_TGT.first, 0, true); + ihs(ACS_SEQUENCE_DETUMBLE.second, ACS_TABLE_DETUMBLE_TRANS_0.first, 0, false); + ihs(ACS_SEQUENCE_DETUMBLE.second, ACS_TABLE_DETUMBLE_TRANS_1.first, 0, false); + check(ss->addSequence(&ACS_SEQUENCE_DETUMBLE.second, ACS_SEQUENCE_DETUMBLE.first, + ACS_SEQUENCE_SAFE.first, false, true), + ctxc); +} + +void buildIdleSequence(Subsystem* ss, ModeListEntry& eh) { + std::string context = "satsystem::acs::buildIdleSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, + ArrayList& sequence) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(sequence.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + // Build IDLE target + iht(objects::ACS_CONTROLLER, acs::CtrlModes::IDLE, 0, ACS_TABLE_IDLE_TGT.second); + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TGT.second); + iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); + ss->addTable(&ACS_TABLE_IDLE_TGT.second, ACS_TABLE_IDLE_TGT.first, false, true); + + // Build IDLE transition 0 + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); + iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); + iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_IDLE_TRANS_0.second); + ss->addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true); + + // Build IDLE transition 1 + iht(objects::ACS_CONTROLLER, acs::CtrlModes::IDLE, 0, ACS_TABLE_IDLE_TRANS_1.second); + ss->addTable(&ACS_TABLE_IDLE_TRANS_1.second, ACS_TABLE_IDLE_TRANS_1.first, false, true); + + // Build IDLE sequence + ihs(ACS_SEQUENCE_IDLE.second, ACS_TABLE_IDLE_TGT.first, 0, true); + ihs(ACS_SEQUENCE_IDLE.second, ACS_TABLE_IDLE_TRANS_0.first, 0, true); + ihs(ACS_SEQUENCE_IDLE.second, ACS_TABLE_IDLE_TRANS_1.first, 0, false); + ss->addSequence(&ACS_SEQUENCE_IDLE.second, ACS_SEQUENCE_IDLE.first, ACS_SEQUENCE_SAFE.first, + false, true); +} + +void buildIdleChargeSequence(Subsystem* ss, ModeListEntry& eh) { + std::string context = "satsystem::acs::buildIdleChargeSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, + ArrayList& sequence) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(sequence.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + // Build IDLE target + iht(objects::ACS_CONTROLLER, acs::CtrlModes::IDLE, acs::IDLE_CHARGE, + ACS_TABLE_IDLE_CHRG_TGT.second); + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_CHRG_TGT.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_CHRG_TGT.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_CHRG_TGT.second); + check(ss->addTable(&ACS_TABLE_IDLE_CHRG_TGT.second, ACS_TABLE_IDLE_CHRG_TGT.first, false, true), + ctxc); + + // Build IDLE transition 0 + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_CHRG_TRANS_0.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_CHRG_TRANS_0.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_CHRG_TRANS_0.second); + iht(objects::RW_ASS, OFF, 0, ACS_TABLE_IDLE_CHRG_TRANS_0.second); + iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_IDLE_CHRG_TRANS_0.second); + check(ss->addTable(&ACS_TABLE_IDLE_CHRG_TRANS_0.second, ACS_TABLE_IDLE_CHRG_TRANS_0.first, false, + true), + ctxc); + + // Build IDLE transition 1 + iht(objects::ACS_CONTROLLER, acs::CtrlModes::IDLE, acs::IDLE_CHARGE, + ACS_TABLE_IDLE_CHRG_TRANS_1.second); + check(ss->addTable(&ACS_TABLE_IDLE_CHRG_TRANS_1.second, ACS_TABLE_IDLE_CHRG_TRANS_1.first, false, + true), + ctxc); + + // Build IDLE sequence + ihs(ACS_SEQUENCE_IDLE_CHRG.second, ACS_TABLE_IDLE_CHRG_TGT.first, 0, true); + ihs(ACS_SEQUENCE_IDLE_CHRG.second, ACS_TABLE_IDLE_CHRG_TRANS_0.first, 0, true); + ihs(ACS_SEQUENCE_IDLE_CHRG.second, ACS_TABLE_IDLE_CHRG_TRANS_1.first, 0, false); + check(ss->addSequence(&ACS_SEQUENCE_IDLE_CHRG.second, ACS_SEQUENCE_IDLE_CHRG.first, + ACS_SEQUENCE_SAFE.first, false, true), + ctxc); +} + +void buildTargetPtSequence(Subsystem* ss, ModeListEntry& eh) { + std::string context = "satsystem::acs::buildTargetPtSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, + ArrayList& sequence) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(sequence.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + // Build TARGET PT table + iht(objects::ACS_CONTROLLER, acs::CtrlModes::TARGET_PT, 0, ACS_TABLE_TARGET_PT_TGT.second); + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_TARGET_PT_TGT.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_TARGET_PT_TGT.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_TARGET_PT_TGT.second); + iht(objects::RW_ASS, NML, 0, ACS_TABLE_TARGET_PT_TGT.second); + iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_TARGET_PT_TGT.second); + check(ss->addTable(&ACS_TABLE_TARGET_PT_TGT.second, ACS_TABLE_TARGET_PT_TGT.first, false, true), + ctxc); + + // Build TARGET PT transition 0 + iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_TARGET_PT_TRANS_0.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_TARGET_PT_TRANS_0.second); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_TARGET_PT_TRANS_0.second); + iht(objects::RW_ASS, NML, 0, ACS_TABLE_TARGET_PT_TRANS_0.second); + iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_TARGET_PT_TRANS_0.second); + check(ss->addTable(&ACS_TABLE_TARGET_PT_TRANS_0.second, ACS_TABLE_TARGET_PT_TRANS_0.first, false, + true), + ctxc); + + // Build TARGET PT transition 1 + iht(objects::ACS_CONTROLLER, acs::CtrlModes::TARGET_PT, 0, ACS_TABLE_TARGET_PT_TRANS_1.second); + check(ss->addTable(&ACS_TABLE_TARGET_PT_TRANS_1.second, ACS_TABLE_TARGET_PT_TRANS_1.first, false, + true), + ctxc); + + // Build IDLE sequence + ihs(ACS_SEQUENCE_TARGET_PT.second, ACS_TABLE_TARGET_PT_TGT.first, 0, true); + ihs(ACS_SEQUENCE_TARGET_PT.second, ACS_TABLE_TARGET_PT_TRANS_0.first, 0, true); + ihs(ACS_SEQUENCE_TARGET_PT.second, ACS_TABLE_TARGET_PT_TRANS_1.first, 0, false); + check(ss->addSequence(&ACS_SEQUENCE_TARGET_PT.second, ACS_SEQUENCE_TARGET_PT.first, + ACS_SEQUENCE_IDLE.first, false, true), + ctxc); +} + +} // namespace diff --git a/mission/system/tree/acsModeTree.h b/mission/system/tree/acsModeTree.h new file mode 100644 index 00000000..210f8dcd --- /dev/null +++ b/mission/system/tree/acsModeTree.h @@ -0,0 +1,12 @@ +#include + +class Subsystem; + +namespace satsystem { +namespace acs { + +extern Subsystem ACS_SUBSYSTEM; +void init(); + +} // namespace acs +} // namespace satsystem diff --git a/mission/system/tree/payloadModeTree.cpp b/mission/system/tree/payloadModeTree.cpp new file mode 100644 index 00000000..6cbc4004 --- /dev/null +++ b/mission/system/tree/payloadModeTree.cpp @@ -0,0 +1,372 @@ +#include "payloadModeTree.h" + +#include +#include +#include +#include + +#include "eive/objects.h" +#include "mission/devices/devicedefinitions/payloadPcduDefinitions.h" +#include "mission/system/objects/PayloadSubsystem.h" +#include "mission/system/objects/definitions.h" +#include "util.h" + +namespace { +void initOffSequence(Subsystem& ss, ModeListEntry& eh); +void initPlMpsocStreamSequence(Subsystem& ss, ModeListEntry& eh); +void initPlCamStreamSequence(Subsystem& ss, ModeListEntry& eh); +void initPlSpvSequence(Subsystem& ss, ModeListEntry& eh); +void initEarthObsvSequence(Subsystem& ss, ModeListEntry& eh); +void initScexSequence(Subsystem& ss, ModeListEntry& eh); +} // namespace + +Subsystem satsystem::pl::SUBSYSTEM = Subsystem(objects::PL_SUBSYSTEM, 12, 24); + +const auto check = subsystem::checkInsert; +static const auto OFF = HasModesIF::MODE_OFF; +static const auto ON = HasModesIF::MODE_ON; +static const auto NML = DeviceHandlerIF::MODE_NORMAL; + +auto PL_SEQUENCE_OFF = std::make_pair(OFF << 24, FixedArrayList()); +auto PL_TABLE_OFF_TGT = std::make_pair((OFF << 24) | 1, FixedArrayList()); +auto PL_TABLE_OFF_TRANS_0 = std::make_pair((OFF << 24) | 2, FixedArrayList()); +auto PL_TABLE_OFF_TRANS_1 = std::make_pair((OFF << 24) | 3, FixedArrayList()); + +auto PL_SEQUENCE_MPSOC_STREAM = + std::make_pair(payload::Modes::MPSOC_STREAM << 24, FixedArrayList()); +auto PL_TABLE_MPSOC_STREAM_TGT = + std::make_pair((payload::Modes::MPSOC_STREAM << 24) | 1, FixedArrayList()); +auto PL_TABLE_MPSOC_STREAM_TRANS_0 = + std::make_pair((payload::Modes::MPSOC_STREAM << 24) | 2, FixedArrayList()); +auto PL_TABLE_MPSOC_STREAM_TRANS_1 = + std::make_pair((payload::Modes::MPSOC_STREAM << 24) | 3, FixedArrayList()); + +auto PL_SEQUENCE_CAM_STREAM = + std::make_pair(payload::Modes::CAM_STREAM << 24, FixedArrayList()); +auto PL_TABLE_CAM_STREAM_TGT = + std::make_pair((payload::Modes::CAM_STREAM << 24) | 1, FixedArrayList()); +auto PL_TABLE_CAM_STREAM_TRANS_0 = + std::make_pair((payload::Modes::CAM_STREAM << 24) | 2, FixedArrayList()); +auto PL_TABLE_CAM_STREAM_TRANS_1 = + std::make_pair((payload::Modes::CAM_STREAM << 24) | 3, FixedArrayList()); + +auto PL_SEQUENCE_SUPV_ONLY = + std::make_pair(payload::Modes::SUPV_ONLY << 24, FixedArrayList()); +auto PL_TABLE_SUPV_ONLY_TGT = + std::make_pair((payload::Modes::SUPV_ONLY << 24) | 1, FixedArrayList()); +auto PL_TABLE_SUPV_ONLY_TRANS_0 = + std::make_pair((payload::Modes::SUPV_ONLY << 24) | 2, FixedArrayList()); +auto PL_TABLE_SUPV_ONLY_TRANS_1 = + std::make_pair((payload::Modes::SUPV_ONLY << 24) | 3, FixedArrayList()); + +auto PL_SEQUENCE_EARTH_OBSV = + std::make_pair(payload::Modes::EARTH_OBSV << 24, FixedArrayList()); +auto PL_TABLE_EARTH_OBSV_TGT = + std::make_pair((payload::Modes::EARTH_OBSV << 24) | 1, FixedArrayList()); +auto PL_TABLE_EARTH_OBSV_TRANS_0 = + std::make_pair((payload::Modes::EARTH_OBSV << 24) | 2, FixedArrayList()); +auto PL_TABLE_EARTH_OBSV_TRANS_1 = + std::make_pair((payload::Modes::EARTH_OBSV << 24) | 3, FixedArrayList()); + +auto PL_SEQUENCE_SCEX = + std::make_pair(payload::Modes::SCEX << 24, FixedArrayList()); +auto PL_TABLE_SCEX_TGT = + std::make_pair((payload::Modes::SCEX << 24) | 1, FixedArrayList()); +auto PL_TABLE_SCEX_TRANS_0 = + std::make_pair((payload::Modes::SCEX << 24) | 2, FixedArrayList()); + +void satsystem::pl::init() { + ModeListEntry entry; + initOffSequence(SUBSYSTEM, entry); + initPlMpsocStreamSequence(SUBSYSTEM, entry); + initPlCamStreamSequence(SUBSYSTEM, entry); + initPlSpvSequence(SUBSYSTEM, entry); + initEarthObsvSequence(SUBSYSTEM, entry); + initScexSequence(SUBSYSTEM, entry); + SUBSYSTEM.setInitialMode(OFF); +} + +namespace { + +void initOffSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::payload::buildOffSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, + ArrayList& sequence) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(sequence.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + // Build OFF target. Is empty + check(ss.addTable(TableEntry(PL_TABLE_OFF_TGT.first, &PL_TABLE_OFF_TGT.second)), ctxc); + + // Build OFF transition 0 + iht(objects::CAM_SWITCHER, OFF, 0, PL_TABLE_OFF_TRANS_0.second); + iht(objects::SCEX, OFF, 0, PL_TABLE_OFF_TRANS_0.second); + iht(objects::PLPCDU_HANDLER, OFF, 0, PL_TABLE_OFF_TRANS_0.second); + iht(objects::PLOC_MPSOC_HANDLER, OFF, 0, PL_TABLE_OFF_TRANS_0.second); + check(ss.addTable(TableEntry(PL_TABLE_OFF_TRANS_0.first, &PL_TABLE_OFF_TRANS_0.second)), ctxc); + + // Build OFF transition 1 + iht(objects::PLOC_SUPERVISOR_HANDLER, OFF, 0, PL_TABLE_OFF_TRANS_1.second); + check(ss.addTable(TableEntry(PL_TABLE_OFF_TRANS_1.first, &PL_TABLE_OFF_TRANS_1.second)), ctxc); + + // Build OFF sequence + ihs(PL_SEQUENCE_OFF.second, PL_TABLE_OFF_TGT.first, 0, false); + ihs(PL_SEQUENCE_OFF.second, PL_TABLE_OFF_TRANS_0.first, 0, false); + ihs(PL_SEQUENCE_OFF.second, PL_TABLE_OFF_TRANS_1.first, 0, false); + check(ss.addSequence( + SequenceEntry(PL_SEQUENCE_OFF.first, &PL_SEQUENCE_OFF.second, PL_SEQUENCE_OFF.first)), + ctxc); +} + +void initPlMpsocStreamSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::payload::initPlMpsocStreamSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, + ArrayList& sequence) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(sequence.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + // Build MPSoC stream target + // Camera should always be off to prevent a conflict with the MPSoC streaming + // PL PCDU must be on and in normal mode, but this is commanded separately because of the + // number of commands invovled + iht(objects::PLPCDU_HANDLER, NML, plpcdu::ALL_ON_SUBMODE, PL_TABLE_MPSOC_STREAM_TGT.second); + iht(objects::CAM_SWITCHER, OFF, 0, PL_TABLE_MPSOC_STREAM_TGT.second); + iht(objects::PLOC_MPSOC_HANDLER, NML, 0, PL_TABLE_MPSOC_STREAM_TGT.second); + iht(objects::PLOC_SUPERVISOR_HANDLER, NML, 0, PL_TABLE_MPSOC_STREAM_TGT.second); + check(ss.addTable(TableEntry(PL_TABLE_MPSOC_STREAM_TGT.first, &PL_TABLE_MPSOC_STREAM_TGT.second)), + ctxc); + + // Build MPSoC stream transition 0 + iht(objects::CAM_SWITCHER, OFF, 0, PL_TABLE_MPSOC_STREAM_TRANS_0.second); + iht(objects::SCEX, OFF, 0, PL_TABLE_MPSOC_STREAM_TRANS_0.second); + iht(objects::PLOC_SUPERVISOR_HANDLER, NML, 0, PL_TABLE_MPSOC_STREAM_TRANS_0.second); + check(ss.addTable( + TableEntry(PL_TABLE_MPSOC_STREAM_TRANS_0.first, &PL_TABLE_MPSOC_STREAM_TRANS_0.second)), + ctxc); + + // Build MPSoC stream transition 1 + iht(objects::PLOC_MPSOC_HANDLER, NML, 0, PL_TABLE_MPSOC_STREAM_TRANS_1.second); + check(ss.addTable( + TableEntry(PL_TABLE_MPSOC_STREAM_TRANS_1.first, &PL_TABLE_MPSOC_STREAM_TRANS_1.second)), + ctxc); + + // Build MPSoC stream sequence + ihs(PL_SEQUENCE_MPSOC_STREAM.second, PL_TABLE_MPSOC_STREAM_TGT.first, 0, true); + ihs(PL_SEQUENCE_MPSOC_STREAM.second, PL_TABLE_MPSOC_STREAM_TRANS_0.first, 0, true); + ihs(PL_SEQUENCE_MPSOC_STREAM.second, PL_TABLE_MPSOC_STREAM_TRANS_1.first, 0, false); + check(ss.addSequence(SequenceEntry(PL_SEQUENCE_MPSOC_STREAM.first, + &PL_SEQUENCE_MPSOC_STREAM.second, PL_SEQUENCE_OFF.first)), + ctxc); +} + +void initPlCamStreamSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::payload::initPlCamStreamSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, + ArrayList& sequence) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(sequence.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + // Build CAM target + // Only check that the PL PCDU is on for now. It might later become necessary to switch on + // the PLOC, so we ignore its state. + iht(objects::PLPCDU_HANDLER, NML, plpcdu::ALL_ON_SUBMODE, PL_TABLE_CAM_STREAM_TGT.second); + check(ss.addTable(TableEntry(PL_TABLE_CAM_STREAM_TGT.first, &PL_TABLE_CAM_STREAM_TGT.second)), + ctxc); + + // Build CAM transition 0 + // PLOC is actively commanded off here + iht(objects::PLOC_MPSOC_HANDLER, OFF, 0, PL_TABLE_CAM_STREAM_TRANS_0.second); + iht(objects::CAM_SWITCHER, ON, 0, PL_TABLE_CAM_STREAM_TRANS_0.second); + iht(objects::SCEX, OFF, 0, PL_TABLE_CAM_STREAM_TRANS_0.second); + check(ss.addTable( + TableEntry(PL_TABLE_CAM_STREAM_TRANS_0.first, &PL_TABLE_CAM_STREAM_TRANS_0.second)), + ctxc); + + // Build CAM transition 1 + iht(objects::PLOC_SUPERVISOR_HANDLER, OFF, 0, PL_TABLE_CAM_STREAM_TRANS_1.second); + check(ss.addTable( + TableEntry(PL_TABLE_CAM_STREAM_TRANS_1.first, &PL_TABLE_CAM_STREAM_TRANS_1.second)), + ctxc); + + // Build CAM stream sequence + ihs(PL_SEQUENCE_CAM_STREAM.second, PL_TABLE_CAM_STREAM_TGT.first, 0, true); + ihs(PL_SEQUENCE_CAM_STREAM.second, PL_TABLE_CAM_STREAM_TRANS_0.first, 0, true); + ihs(PL_SEQUENCE_CAM_STREAM.second, PL_TABLE_CAM_STREAM_TRANS_1.first, 0, false); + check(ss.addSequence(SequenceEntry(PL_SEQUENCE_CAM_STREAM.first, &PL_SEQUENCE_CAM_STREAM.second, + PL_SEQUENCE_OFF.first)), + ctxc); +} + +void initPlSpvSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::payload::initPlSupvSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, + ArrayList& sequence) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(sequence.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + // Build Payload Supervisor Only target + iht(objects::PLOC_SUPERVISOR_HANDLER, NML, 0, PL_TABLE_SUPV_ONLY_TGT.second); + check(ss.addTable(TableEntry(PL_TABLE_SUPV_ONLY_TGT.first, &PL_TABLE_SUPV_ONLY_TGT.second)), + ctxc); + + // Build Payload Supervisor Only transition 0 + iht(objects::PLOC_SUPERVISOR_HANDLER, NML, 0, PL_TABLE_SUPV_ONLY_TRANS_0.second); + iht(objects::CAM_SWITCHER, OFF, 0, PL_TABLE_SUPV_ONLY_TRANS_0.second); + check( + ss.addTable(TableEntry(PL_TABLE_SUPV_ONLY_TRANS_0.first, &PL_TABLE_SUPV_ONLY_TRANS_0.second)), + ctxc); + + // Build Payload Supervisor Only transition 1 + iht(objects::PLOC_MPSOC_HANDLER, OFF, 0, PL_TABLE_SUPV_ONLY_TRANS_1.second); + check( + ss.addTable(TableEntry(PL_TABLE_SUPV_ONLY_TRANS_1.first, &PL_TABLE_SUPV_ONLY_TRANS_1.second)), + ctxc); + + // Build Payload Supervisor Only Sequence + ihs(PL_SEQUENCE_SUPV_ONLY.second, PL_TABLE_SUPV_ONLY_TGT.first, 0, true); + ihs(PL_SEQUENCE_SUPV_ONLY.second, PL_TABLE_SUPV_ONLY_TRANS_0.first, 0, true); + ihs(PL_SEQUENCE_SUPV_ONLY.second, PL_TABLE_SUPV_ONLY_TRANS_1.first, 0, false); + check(ss.addSequence(SequenceEntry(PL_SEQUENCE_SUPV_ONLY.first, &PL_SEQUENCE_SUPV_ONLY.second, + PL_SEQUENCE_OFF.first)), + ctxc); +} + +void initEarthObsvSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::payload::initEarthObsvSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, + ArrayList& sequence) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(sequence.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + // Build Earth Observation target + iht(objects::PLOC_MPSOC_HANDLER, NML, 0, PL_TABLE_EARTH_OBSV_TGT.second); + iht(objects::PLOC_SUPERVISOR_HANDLER, NML, 0, PL_TABLE_EARTH_OBSV_TGT.second); + iht(objects::CAM_SWITCHER, ON, 0, PL_TABLE_EARTH_OBSV_TGT.second); + iht(objects::PLPCDU_HANDLER, OFF, 0, PL_TABLE_EARTH_OBSV_TGT.second); + check(ss.addTable(TableEntry(PL_TABLE_EARTH_OBSV_TGT.first, &PL_TABLE_EARTH_OBSV_TGT.second)), + ctxc); + + // Build Earth Observation transition 0 + iht(objects::PLOC_SUPERVISOR_HANDLER, NML, 0, PL_TABLE_EARTH_OBSV_TRANS_0.second); + iht(objects::CAM_SWITCHER, ON, 0, PL_TABLE_EARTH_OBSV_TRANS_0.second); + iht(objects::PLPCDU_HANDLER, OFF, 0, PL_TABLE_EARTH_OBSV_TRANS_0.second); + check(ss.addTable( + TableEntry(PL_TABLE_EARTH_OBSV_TRANS_0.first, &PL_TABLE_EARTH_OBSV_TRANS_0.second)), + ctxc); + + // Build Earth Observation transition 1 + iht(objects::PLOC_MPSOC_HANDLER, NML, 0, PL_TABLE_CAM_STREAM_TRANS_1.second); + check(ss.addTable( + TableEntry(PL_TABLE_EARTH_OBSV_TRANS_1.first, &PL_TABLE_EARTH_OBSV_TRANS_1.second)), + ctxc); + + ihs(PL_SEQUENCE_EARTH_OBSV.second, PL_TABLE_EARTH_OBSV_TGT.first, 0, true); + ihs(PL_SEQUENCE_EARTH_OBSV.second, PL_TABLE_EARTH_OBSV_TRANS_0.first, 0, true); + ihs(PL_SEQUENCE_EARTH_OBSV.second, PL_TABLE_EARTH_OBSV_TRANS_1.first, 0, false); + check(ss.addSequence(SequenceEntry(PL_SEQUENCE_EARTH_OBSV.first, &PL_SEQUENCE_EARTH_OBSV.second, + PL_SEQUENCE_OFF.first)), + ctxc); +} + +void initScexSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::payload::initScexSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, + ArrayList& sequence) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(sequence.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + // Build SCEX target + iht(objects::SCEX, NML, 0, PL_TABLE_SCEX_TGT.second); + check(ss.addTable(TableEntry(PL_TABLE_SCEX_TGT.first, &PL_TABLE_SCEX_TGT.second)), ctxc); + + // Build SCEX transition 0 + iht(objects::SCEX, NML, 0, PL_TABLE_SCEX_TRANS_0.second); + check(ss.addTable(TableEntry(PL_TABLE_SCEX_TRANS_0.first, &PL_TABLE_SCEX_TRANS_0.second)), ctxc); + + // Build SCEX sequence + ihs(PL_SEQUENCE_SCEX.second, PL_TABLE_SCEX_TGT.first, 0, true); + ihs(PL_SEQUENCE_SCEX.second, PL_TABLE_SCEX_TRANS_0.first, 0, false); + check(ss.addSequence( + SequenceEntry(PL_SEQUENCE_SCEX.first, &PL_SEQUENCE_SCEX.second, PL_SEQUENCE_OFF.first)), + ctxc); +} + +} // namespace diff --git a/mission/system/tree/payloadModeTree.h b/mission/system/tree/payloadModeTree.h new file mode 100644 index 00000000..d1eae4d7 --- /dev/null +++ b/mission/system/tree/payloadModeTree.h @@ -0,0 +1,17 @@ +#ifndef MISSION_SYSTEM_TREE_PAYLOADMODETREE_H_ +#define MISSION_SYSTEM_TREE_PAYLOADMODETREE_H_ + +#include + +namespace satsystem { + +namespace pl { + +extern Subsystem SUBSYSTEM; + +void init(); +} // namespace pl + +} // namespace satsystem + +#endif /* MISSION_SYSTEM_TREE_PAYLOADMODETREE_H_ */ diff --git a/mission/system/tree/system.cpp b/mission/system/tree/system.cpp new file mode 100644 index 00000000..4455588f --- /dev/null +++ b/mission/system/tree/system.cpp @@ -0,0 +1,9 @@ +#include "system.h" + +#include "acsModeTree.h" +#include "payloadModeTree.h" + +void satsystem::init() { + acs::init(); + pl::init(); +} diff --git a/mission/system/tree/system.h b/mission/system/tree/system.h new file mode 100644 index 00000000..a8599121 --- /dev/null +++ b/mission/system/tree/system.h @@ -0,0 +1,10 @@ +#ifndef MISSION_SYSTEM_TREE_SYSTEM_H_ +#define MISSION_SYSTEM_TREE_SYSTEM_H_ + +namespace satsystem { + +void init(); + +} + +#endif /* MISSION_SYSTEM_TREE_SYSTEM_H_ */ diff --git a/mission/system/tree/util.cpp b/mission/system/tree/util.cpp new file mode 100644 index 00000000..935e9d79 --- /dev/null +++ b/mission/system/tree/util.cpp @@ -0,0 +1,19 @@ +#include "util.h" + +#include "fsfw/container/FixedMap.h" +#include "fsfw/serviceinterface.h" + +void subsystem::checkInsert(ReturnValue_t result, const char* ctx) { + if (result != returnvalue::OK) { + sif::warning << "satsystem::checkInsert: Insertion failed at " << ctx; + if (result == containers::KEY_ALREADY_EXISTS) { + sif::warning << ": Key already exists" << std::endl; + } else if (result == containers::MAP_FULL) { + sif::warning << ": Map full" << std::endl; + } else if (result == containers::LIST_FULL) { + sif::warning << ": List full" << std::endl; + } else { + sif::warning << std::endl; + } + } +} diff --git a/mission/system/tree/util.h b/mission/system/tree/util.h new file mode 100644 index 00000000..da73e3bf --- /dev/null +++ b/mission/system/tree/util.h @@ -0,0 +1,12 @@ +#ifndef MISSION_SYSTEM_TREE_UTIL_H_ +#define MISSION_SYSTEM_TREE_UTIL_H_ + +#include + +namespace subsystem { + +void checkInsert(ReturnValue_t result, const char* ctx); + +} + +#endif /* MISSION_SYSTEM_TREE_UTIL_H_ */ diff --git a/mission/tmtc/CMakeLists.txt b/mission/tmtc/CMakeLists.txt index 0f77efcd..3c631be7 100644 --- a/mission/tmtc/CMakeLists.txt +++ b/mission/tmtc/CMakeLists.txt @@ -1,9 +1,4 @@ target_sources( ${LIB_EIVE_MISSION} - PRIVATE CCSDSHandler.cpp - VirtualChannel.cpp - TmFunnel.cpp - CfdpTmFunnel.cpp - PusTmFunnel.cpp - Service15TmStorage.cpp - TmStore.cpp) + PRIVATE CcsdsIpCoreHandler.cpp VirtualChannel.cpp TmFunnelHandler.cpp + TmFunnelBase.cpp CfdpTmFunnel.cpp Service15TmStorage.cpp TmStore.cpp PusTmFunnel.cpp) diff --git a/mission/tmtc/CCSDSHandler.cpp b/mission/tmtc/CcsdsIpCoreHandler.cpp similarity index 73% rename from mission/tmtc/CCSDSHandler.cpp rename to mission/tmtc/CcsdsIpCoreHandler.cpp index 26abec25..c65a12a8 100644 --- a/mission/tmtc/CCSDSHandler.cpp +++ b/mission/tmtc/CcsdsIpCoreHandler.cpp @@ -1,7 +1,7 @@ -#include "CCSDSHandler.h" +#include "CcsdsIpCoreHandler.h" -#include -#include +#include +#include #include "fsfw/events/EventManagerIF.h" #include "fsfw/ipc/QueueFactory.h" @@ -11,9 +11,10 @@ #include "fsfw/serviceinterface/serviceInterfaceDefintions.h" #include "mission/devices/devicedefinitions/SyrlinksDefinitions.h" -CCSDSHandler::CCSDSHandler(object_id_t objectId, object_id_t ptmeId, object_id_t tcDestination, - PtmeConfig* ptmeConfig, GpioIF* gpioIF, gpioId_t enTxClock, - gpioId_t enTxData, uint32_t transmitterTimeout) +CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId, + object_id_t tcDestination, PtmeConfig* ptmeConfig, + GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData, + uint32_t transmitterTimeout) : SystemObject(objectId), ptmeId(ptmeId), tcDestination(tcDestination), @@ -30,9 +31,9 @@ CCSDSHandler::CCSDSHandler(object_id_t objectId, object_id_t ptmeId, object_id_t QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs); } -CCSDSHandler::~CCSDSHandler() {} +CcsdsIpCoreHandler::~CcsdsIpCoreHandler() {} -ReturnValue_t CCSDSHandler::performOperation(uint8_t operationCode) { +ReturnValue_t CcsdsIpCoreHandler::performOperation(uint8_t operationCode) { checkEvents(); readCommandQueue(); handleTelemetry(); @@ -41,16 +42,16 @@ ReturnValue_t CCSDSHandler::performOperation(uint8_t operationCode) { return returnvalue::OK; } -void CCSDSHandler::handleTelemetry() { +void CcsdsIpCoreHandler::handleTelemetry() { VirtualChannelMapIter iter; for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) { iter->second->performOperation(); } } -void CCSDSHandler::handleTelecommands() {} +void CcsdsIpCoreHandler::handleTelecommands() {} -ReturnValue_t CCSDSHandler::initialize() { +ReturnValue_t CcsdsIpCoreHandler::initialize() { ReturnValue_t result = returnvalue::OK; PtmeIF* ptme = ObjectManager::instance()->get(ptmeId); if (ptme == nullptr) { @@ -62,7 +63,7 @@ ReturnValue_t CCSDSHandler::initialize() { ObjectManager::instance()->get(tcDestination); if (tcDistributor == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "CCSDSHandler::initialize: Invalid TC Distributor object" << std::endl; + sif::error << "CcsdsHandler::initialize: Invalid TC Distributor object" << std::endl; #endif return ObjectManagerIF::CHILD_INIT_FAILED; } @@ -91,14 +92,14 @@ ReturnValue_t CCSDSHandler::initialize() { EventManagerIF* manager = ObjectManager::instance()->get(objects::EVENT_MANAGER); if (manager == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "CCSDSHandler::initialize: Invalid event manager" << std::endl; + sif::error << "CcsdsHandler::initialize: Invalid event manager" << std::endl; #endif return ObjectManagerIF::CHILD_INIT_FAILED; } result = manager->registerListener(eventQueue->getId()); if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "CCSDSHandler::initialize: Failed to register CCSDS handler as event " + sif::warning << "CcsdsHandler::initialize: Failed to register CCSDS handler as event " "listener" << std::endl; #endif @@ -110,7 +111,7 @@ ReturnValue_t CCSDSHandler::initialize() { event::getEventId(PdecHandler::BIT_LOCK_PDEC)); if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "CCSDSHandler::initialize: Failed to subscribe to events from PDEC " + sif::error << "CcsdsHandler::initialize: Failed to subscribe to events from PDEC " "handler" << std::endl; #endif @@ -136,7 +137,7 @@ ReturnValue_t CCSDSHandler::initialize() { return result; } -void CCSDSHandler::readCommandQueue(void) { +void CcsdsIpCoreHandler::readCommandQueue(void) { CommandMessage commandMessage; ReturnValue_t result = returnvalue::FAILED; @@ -157,61 +158,62 @@ void CCSDSHandler::readCommandQueue(void) { } } -MessageQueueId_t CCSDSHandler::getCommandQueue() const { return commandQueue->getId(); } +MessageQueueId_t CcsdsIpCoreHandler::getCommandQueue() const { return commandQueue->getId(); } -void CCSDSHandler::addVirtualChannel(VcId_t vcId, VirtualChannel* virtualChannel) { +void CcsdsIpCoreHandler::addVirtualChannel(VcId_t vcId, VirtualChannel* virtualChannel) { if (vcId > common::NUMBER_OF_VIRTUAL_CHANNELS) { - sif::warning << "CCSDSHandler::addVirtualChannel: Invalid virtual channel ID" << std::endl; + sif::warning << "CcsdsHandler::addVirtualChannel: Invalid virtual channel ID" << std::endl; return; } if (virtualChannel == nullptr) { - sif::warning << "CCSDSHandler::addVirtualChannel: Invalid virtual channel interface" + sif::warning << "CcsdsHandler::addVirtualChannel: Invalid virtual channel interface" << std::endl; return; } auto status = virtualChannelMap.emplace(vcId, virtualChannel); if (status.second == false) { - sif::warning << "CCSDSHandler::addVirtualChannel: Failed to add virtual channel to " + sif::warning << "CcsdsHandler::addVirtualChannel: Failed to add virtual channel to " "virtual channel map" << std::endl; return; } } -MessageQueueId_t CCSDSHandler::getReportReceptionQueue(uint8_t virtualChannel) const { +MessageQueueId_t CcsdsIpCoreHandler::getReportReceptionQueue(uint8_t virtualChannel) const { if (virtualChannel < common::NUMBER_OF_VIRTUAL_CHANNELS) { auto iter = virtualChannelMap.find(virtualChannel); if (iter != virtualChannelMap.end()) { return iter->second->getReportReceptionQueue(); } else { - sif::warning << "CCSDSHandler::getReportReceptionQueue: Virtual channel with ID " + sif::warning << "CcsdsHandler::getReportReceptionQueue: Virtual channel with ID " << static_cast(virtualChannel) << " not in virtual channel map" << std::endl; return MessageQueueIF::NO_QUEUE; } } else { - sif::debug << "CCSDSHandler::getReportReceptionQueue: Invalid virtual channel requested"; + sif::debug << "CcsdsHandler::getReportReceptionQueue: Invalid virtual channel requested"; } return MessageQueueIF::NO_QUEUE; } -ReturnValue_t CCSDSHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier, - ParameterWrapper* parameterWrapper, - const ParameterWrapper* newValues, uint16_t startAtIndex) { +ReturnValue_t CcsdsIpCoreHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier, + ParameterWrapper* parameterWrapper, + const ParameterWrapper* newValues, + uint16_t startAtIndex) { return returnvalue::OK; } -uint32_t CCSDSHandler::getIdentifier() const { return 0; } +uint32_t CcsdsIpCoreHandler::getIdentifier() const { return 0; } -MessageQueueId_t CCSDSHandler::getRequestQueue() const { +MessageQueueId_t CcsdsIpCoreHandler::getRequestQueue() const { // Forward packets directly to TC distributor return tcDistributorQueueId; } -ReturnValue_t CCSDSHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, - const uint8_t* data, size_t size) { +ReturnValue_t CcsdsIpCoreHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, + const uint8_t* data, size_t size) { ReturnValue_t result = returnvalue::OK; switch (actionId) { case SET_LOW_RATE: { @@ -261,7 +263,7 @@ ReturnValue_t CCSDSHandler::executeAction(ActionId_t actionId, MessageQueueId_t return EXECUTION_FINISHED; } -void CCSDSHandler::checkEvents() { +void CcsdsIpCoreHandler::checkEvents() { EventMessage event; for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK; result = eventQueue->receiveMessage(&event)) { @@ -270,14 +272,14 @@ void CCSDSHandler::checkEvents() { handleEvent(&event); break; default: - sif::debug << "CCSDSHandler::checkEvents: Did not subscribe to this event message" + sif::debug << "CcsdsHandler::checkEvents: Did not subscribe to this event message" << std::endl; break; } } } -void CCSDSHandler::handleEvent(EventMessage* eventMessage) { +void CcsdsIpCoreHandler::handleEvent(EventMessage* eventMessage) { Event event = eventMessage->getEvent(); switch (event) { case PdecHandler::BIT_LOCK_PDEC: { @@ -289,12 +291,12 @@ void CCSDSHandler::handleEvent(EventMessage* eventMessage) { break; } default: - sif::debug << "CCSDSHandler::handleEvent: Did not subscribe to this event" << std::endl; + sif::debug << "CcsdsHandler::handleEvent: Did not subscribe to this event" << std::endl; break; } } -void CCSDSHandler::handleBitLockEvent() { +void CcsdsIpCoreHandler::handleBitLockEvent() { if (transmitterCountdown.isBusy()) { // Transmitter already enabled return; @@ -302,21 +304,21 @@ void CCSDSHandler::handleBitLockEvent() { enableTransmit(); } -void CCSDSHandler::handleCarrierLockEvent() { +void CcsdsIpCoreHandler::handleCarrierLockEvent() { if (!enableTxWhenCarrierLock) { return; } enableTransmit(); } -void CCSDSHandler::forwardLinkstate() { +void CcsdsIpCoreHandler::forwardLinkstate() { VirtualChannelMapIter iter; for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) { iter->second->setLinkState(linkState); } } -void CCSDSHandler::enableTransmit() { +void CcsdsIpCoreHandler::enableTransmit() { if (transmitterCountdown.isBusy()) { // Transmitter already enabled return; @@ -327,7 +329,7 @@ void CCSDSHandler::enableTransmit() { #endif } -void CCSDSHandler::checkTxTimer() { +void CcsdsIpCoreHandler::checkTxTimer() { if (linkState == DOWN) { return; } @@ -336,7 +338,7 @@ void CCSDSHandler::checkTxTimer() { } } -void CCSDSHandler::disableTransmit() { +void CcsdsIpCoreHandler::disableTransmit() { #ifndef TE0720_1CFA gpioIF->pullLow(enTxClock); gpioIF->pullLow(enTxData); @@ -346,4 +348,4 @@ void CCSDSHandler::disableTransmit() { transmitterCountdown.setTimeout(0); } -const char* CCSDSHandler::getName() const { return "CCSDS Handler"; } +const char* CcsdsIpCoreHandler::getName() const { return "CCSDS Handler"; } diff --git a/mission/tmtc/CCSDSHandler.h b/mission/tmtc/CcsdsIpCoreHandler.h similarity index 90% rename from mission/tmtc/CCSDSHandler.h rename to mission/tmtc/CcsdsIpCoreHandler.h index d76dbdaf..942b73cb 100644 --- a/mission/tmtc/CCSDSHandler.h +++ b/mission/tmtc/CcsdsIpCoreHandler.h @@ -17,7 +17,7 @@ #include "fsfw/tmtcservices/AcceptsTelemetryIF.h" #include "fsfw_hal/common/gpio/GpioIF.h" #include "fsfw_hal/common/gpio/gpioDefinitions.h" -#include "linux/obc/PtmeConfig.h" +#include "linux/ipcore/PtmeConfig.h" /** * @brief This class handles the data exchange with the CCSDS IP cores implemented in the @@ -28,12 +28,12 @@ * * @author J. Meier */ -class CCSDSHandler : public SystemObject, - public ExecutableObjectIF, - public AcceptsTelemetryIF, - public AcceptsTelecommandsIF, - public ReceivesParameterMessagesIF, - public HasActionsIF { +class CcsdsIpCoreHandler : public SystemObject, + public ExecutableObjectIF, + public AcceptsTelemetryIF, + public AcceptsTelecommandsIF, + public ReceivesParameterMessagesIF, + public HasActionsIF { public: using VcId_t = uint8_t; @@ -49,11 +49,11 @@ class CCSDSHandler : public SystemObject, * @param enTxClock GPIO ID of RS485 tx clock enable * @param enTxData GPIO ID of RS485 tx data enable */ - CCSDSHandler(object_id_t objectId, object_id_t ptmeId, object_id_t tcDestination, - PtmeConfig* ptmeConfig, GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData, - uint32_t transmitterTimeout = 900000); + CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId, object_id_t tcDestination, + PtmeConfig* ptmeConfig, GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData, + uint32_t transmitterTimeout = 900000); - ~CCSDSHandler(); + ~CcsdsIpCoreHandler(); ReturnValue_t performOperation(uint8_t operationCode = 0) override; ReturnValue_t initialize(); diff --git a/mission/tmtc/CfdpTmFunnel.cpp b/mission/tmtc/CfdpTmFunnel.cpp index 7969961b..aff7c95d 100644 --- a/mission/tmtc/CfdpTmFunnel.cpp +++ b/mission/tmtc/CfdpTmFunnel.cpp @@ -5,29 +5,21 @@ #include "fsfw/tmtcservices/TmTcMessage.h" CfdpTmFunnel::CfdpTmFunnel(object_id_t objectId, uint16_t cfdpInCcsdsApid, - const AcceptsTelemetryIF& downlinkDestination, StorageManagerIF& tmStore, - uint8_t vc) - : SystemObject(objectId), cfdpInCcsdsApid(cfdpInCcsdsApid), tmStore(tmStore) { - msgQueue = QueueFactory::instance()->createMessageQueue(5); - msgQueue->setDefaultDestination(downlinkDestination.getReportReceptionQueue(vc)); -} + StorageManagerIF& tmStore, uint32_t messageDepth) + : TmFunnelBase(objectId, tmStore, messageDepth), cfdpInCcsdsApid(cfdpInCcsdsApid) {} const char* CfdpTmFunnel::getName() const { return "CFDP TM Funnel"; } -MessageQueueId_t CfdpTmFunnel::getReportReceptionQueue(uint8_t virtualChannel) const { - return msgQueue->getId(); -} - ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) { TmTcMessage currentMessage; - ReturnValue_t status = msgQueue->receiveMessage(¤tMessage); + ReturnValue_t status = tmQueue->receiveMessage(¤tMessage); while (status == returnvalue::OK) { status = handlePacket(currentMessage); if (status != returnvalue::OK) { sif::warning << "CfdpTmFunnel packet handling failed" << std::endl; break; } - status = msgQueue->receiveMessage(¤tMessage); + status = tmQueue->receiveMessage(¤tMessage); } if (status == MessageQueueIF::EMPTY) { @@ -74,12 +66,34 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) { // Delete old packet tmStore.deleteData(msg.getStorageId()); msg.setStorageId(newStoreId); - result = msgQueue->sendToDefault(&msg); - if (result != returnvalue::OK) { - tmStore.deleteData(msg.getStorageId()); + store_address_t origStoreId = newStoreId; + for (unsigned int idx = 0; idx < destinations.size(); idx++) { + const auto& destVcidPair = destinations[idx]; + if (destinations.size() > 1) { + if (idx < destinations.size() - 1) { + // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need + // to bother with send order and where the data is deleted. + store_address_t storeId; + result = tmStore.addData(&storeId, newPacketData, serSize); + if (result == returnvalue::OK) { + msg.setStorageId(storeId); + } else { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "CfdpTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl; + sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy" + << std::endl; #endif + } + } else { + msg.setStorageId(origStoreId); + } + } + result = tmQueue->sendMessage(destVcidPair.first, &msg); + if (result != returnvalue::OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl; +#endif + tmStore.deleteData(msg.getStorageId()); + } } return result; } diff --git a/mission/tmtc/CfdpTmFunnel.h b/mission/tmtc/CfdpTmFunnel.h index fe2d664d..e294956a 100644 --- a/mission/tmtc/CfdpTmFunnel.h +++ b/mission/tmtc/CfdpTmFunnel.h @@ -1,19 +1,20 @@ #ifndef FSFW_EXAMPLE_COMMON_CFDPTMFUNNEL_H #define FSFW_EXAMPLE_COMMON_CFDPTMFUNNEL_H +#include + +#include + #include "fsfw/objectmanager/SystemObject.h" #include "fsfw/storagemanager/StorageManagerIF.h" #include "fsfw/tmtcservices/AcceptsTelemetryIF.h" #include "fsfw/tmtcservices/TmTcMessage.h" -class CfdpTmFunnel : public AcceptsTelemetryIF, public SystemObject { +class CfdpTmFunnel : public TmFunnelBase { public: - CfdpTmFunnel(object_id_t objectId, uint16_t cfdpInCcsdsApid, - const AcceptsTelemetryIF& downlinkDestination, StorageManagerIF& tmStore, - uint8_t vc); + CfdpTmFunnel(object_id_t objectId, uint16_t cfdpInCcsdsApid, StorageManagerIF& tmStore, + uint32_t messageDepth); [[nodiscard]] const char* getName() const override; - [[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override; - ReturnValue_t performOperation(uint8_t opCode); ReturnValue_t initialize() override; @@ -22,7 +23,5 @@ class CfdpTmFunnel : public AcceptsTelemetryIF, public SystemObject { uint16_t sourceSequenceCount = 0; uint16_t cfdpInCcsdsApid; - MessageQueueIF* msgQueue; - StorageManagerIF& tmStore; }; #endif // FSFW_EXAMPLE_COMMON_CFDPTMFUNNEL_H diff --git a/mission/tmtc/PusTmFunnel.cpp b/mission/tmtc/PusTmFunnel.cpp index f4072f0f..5d6bbb4d 100644 --- a/mission/tmtc/PusTmFunnel.cpp +++ b/mission/tmtc/PusTmFunnel.cpp @@ -4,21 +4,12 @@ #include "fsfw/objectmanager.h" #include "fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h" -PusTmFunnel::PusTmFunnel(object_id_t objectId, const AcceptsTelemetryIF &downlinkDestination, - TimeReaderIF &timeReader, StorageManagerIF &tmStore, uint8_t vcId, +PusTmFunnel::PusTmFunnel(object_id_t objectId, TimeReaderIF &timeReader, StorageManagerIF &tmStore, uint32_t messageDepth) - : SystemObject(objectId), timeReader(timeReader), tmStore(tmStore) { - tmQueue = QueueFactory::instance()->createMessageQueue(messageDepth, - MessageQueueMessage::MAX_MESSAGE_SIZE); - tmQueue->setDefaultDestination(downlinkDestination.getReportReceptionQueue(vcId)); -} + : TmFunnelBase(objectId, tmStore, messageDepth), timeReader(timeReader) {} PusTmFunnel::~PusTmFunnel() = default; -MessageQueueId_t PusTmFunnel::getReportReceptionQueue(uint8_t virtualChannel) const { - return tmQueue->getId(); -} - ReturnValue_t PusTmFunnel::performOperation(uint8_t) { TmTcMessage currentMessage; ReturnValue_t status = tmQueue->receiveMessage(¤tMessage); @@ -40,7 +31,8 @@ ReturnValue_t PusTmFunnel::performOperation(uint8_t) { ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage &message) { uint8_t *packetData = nullptr; size_t size = 0; - ReturnValue_t result = tmStore.modifyData(message.getStorageId(), &packetData, &size); + store_address_t origStoreId = message.getStorageId(); + ReturnValue_t result = tmStore.modifyData(origStoreId, &packetData, &size); if (result != returnvalue::OK) { return result; } @@ -56,12 +48,33 @@ ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage &message) { sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT; packet.updateErrorControl(); - result = tmQueue->sendToDefault(&message); - if (result != returnvalue::OK) { - tmStore.deleteData(message.getStorageId()); + for (unsigned int idx = 0; idx < destinations.size(); idx++) { + const auto &destVcidPair = destinations[idx]; + if (destinations.size() > 1) { + if (idx < destinations.size() - 1) { + // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need + // to bother with send order and where the data is deleted. + store_address_t storeId; + result = tmStore.addData(&storeId, packetData, size); + if (result == returnvalue::OK) { + message.setStorageId(storeId); + } else { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl; + sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy" + << std::endl; #endif + } + } else { + message.setStorageId(origStoreId); + } + } + result = tmQueue->sendMessage(destVcidPair.first, &message); + if (result != returnvalue::OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl; +#endif + tmStore.deleteData(message.getStorageId()); + } } return result; } diff --git a/mission/tmtc/PusTmFunnel.h b/mission/tmtc/PusTmFunnel.h index ae0390b3..ca9a6016 100644 --- a/mission/tmtc/PusTmFunnel.h +++ b/mission/tmtc/PusTmFunnel.h @@ -6,6 +6,9 @@ #include #include #include +#include + +#include #include "fsfw/timemanager/TimeReaderIF.h" @@ -20,22 +23,19 @@ * @ingroup utility * @author J. Meier, R. Mueller */ -class PusTmFunnel : public AcceptsTelemetryIF, public SystemObject { +class PusTmFunnel : public TmFunnelBase { public: - explicit PusTmFunnel(object_id_t objectId, const AcceptsTelemetryIF &downlinkDestination, - TimeReaderIF &timeReader, StorageManagerIF &tmStore, uint8_t vdId, - uint32_t messageDepth = 20); + explicit PusTmFunnel(object_id_t objectId, TimeReaderIF &timeReader, StorageManagerIF &tmStore, + uint32_t messageDepth = 10); [[nodiscard]] const char *getName() const override; ~PusTmFunnel() override; - [[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override; ReturnValue_t performOperation(uint8_t operationCode); private: uint16_t sourceSequenceCount = 0; TimeReaderIF &timeReader; - StorageManagerIF &tmStore; - MessageQueueIF *tmQueue = nullptr; + ReturnValue_t handlePacket(TmTcMessage &message); }; diff --git a/mission/tmtc/TmFunnel.cpp b/mission/tmtc/TmFunnel.cpp deleted file mode 100644 index 01bf0437..00000000 --- a/mission/tmtc/TmFunnel.cpp +++ /dev/null @@ -1,16 +0,0 @@ -#include "TmFunnel.h" - -#include - -TmFunnel::TmFunnel(object_id_t objectId, PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel) - : SystemObject(objectId), pusFunnel(pusFunnel), cfdpFunnel(cfdpFunnel) {} - -TmFunnel::~TmFunnel() = default; - -ReturnValue_t TmFunnel::performOperation(uint8_t operationCode) { - pusFunnel.performOperation(operationCode); - cfdpFunnel.performOperation(operationCode); - return returnvalue::OK; -} - -ReturnValue_t TmFunnel::initialize() { return returnvalue::OK; } diff --git a/mission/tmtc/TmFunnelBase.cpp b/mission/tmtc/TmFunnelBase.cpp new file mode 100644 index 00000000..fa0062e6 --- /dev/null +++ b/mission/tmtc/TmFunnelBase.cpp @@ -0,0 +1,19 @@ +#include "TmFunnelBase.h" + +#include "fsfw/ipc/QueueFactory.h" + +TmFunnelBase::TmFunnelBase(object_id_t objectId, StorageManagerIF &tmStore, uint32_t tmMsgDepth) + : SystemObject(objectId), tmStore(tmStore) { + tmQueue = QueueFactory::instance()->createMessageQueue(tmMsgDepth); +} + +TmFunnelBase::~TmFunnelBase() { QueueFactory::instance()->deleteMessageQueue(tmQueue); } + +MessageQueueId_t TmFunnelBase::getReportReceptionQueue(uint8_t virtualChannel) const { + return tmQueue->getId(); +} + +void TmFunnelBase::addDestination(const AcceptsTelemetryIF &downlinkDestination, uint8_t vcid) { + auto queueId = downlinkDestination.getReportReceptionQueue(vcid); + destinations.emplace_back(queueId, vcid); +} diff --git a/mission/tmtc/TmFunnelBase.h b/mission/tmtc/TmFunnelBase.h new file mode 100644 index 00000000..c630fefd --- /dev/null +++ b/mission/tmtc/TmFunnelBase.h @@ -0,0 +1,24 @@ +#ifndef MISSION_TMTC_TMFUNNELBASE_H_ +#define MISSION_TMTC_TMFUNNELBASE_H_ + +#include +#include +#include + +#include + +class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject { + public: + TmFunnelBase(object_id_t objectId, StorageManagerIF& tmStore, uint32_t tmMsgDepth); + void addDestination(const AcceptsTelemetryIF& downlinkDestination, uint8_t vcid = 0); + [[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override; + + virtual ~TmFunnelBase(); + + protected: + StorageManagerIF& tmStore; + std::vector> destinations; + MessageQueueIF* tmQueue = nullptr; +}; + +#endif /* MISSION_TMTC_TMFUNNELBASE_H_ */ diff --git a/mission/tmtc/TmFunnelHandler.cpp b/mission/tmtc/TmFunnelHandler.cpp new file mode 100644 index 00000000..33f8addc --- /dev/null +++ b/mission/tmtc/TmFunnelHandler.cpp @@ -0,0 +1,17 @@ +#include "TmFunnelHandler.h" + +#include + +TmFunnelHandler::TmFunnelHandler(object_id_t objectId, PusTmFunnel& pusFunnel, + CfdpTmFunnel& cfdpFunnel) + : SystemObject(objectId), pusFunnel(pusFunnel), cfdpFunnel(cfdpFunnel) {} + +TmFunnelHandler::~TmFunnelHandler() = default; + +ReturnValue_t TmFunnelHandler::performOperation(uint8_t operationCode) { + pusFunnel.performOperation(operationCode); + cfdpFunnel.performOperation(operationCode); + return returnvalue::OK; +} + +ReturnValue_t TmFunnelHandler::initialize() { return returnvalue::OK; } diff --git a/mission/tmtc/TmFunnel.h b/mission/tmtc/TmFunnelHandler.h similarity index 80% rename from mission/tmtc/TmFunnel.h rename to mission/tmtc/TmFunnelHandler.h index 5441db2b..a101f361 100644 --- a/mission/tmtc/TmFunnel.h +++ b/mission/tmtc/TmFunnelHandler.h @@ -19,10 +19,10 @@ * @ingroup utility * @author J. Meier, R. Mueller */ -class TmFunnel : public ExecutableObjectIF, public SystemObject { +class TmFunnelHandler : public ExecutableObjectIF, public SystemObject { public: - TmFunnel(object_id_t objectId, PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel); - ~TmFunnel() override; + TmFunnelHandler(object_id_t objectId, PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel); + ~TmFunnelHandler() override; ReturnValue_t performOperation(uint8_t operationCode) override; ReturnValue_t initialize() override; diff --git a/mission/tmtc/VirtualChannel.cpp b/mission/tmtc/VirtualChannel.cpp index 96a37b3b..b0f9391d 100644 --- a/mission/tmtc/VirtualChannel.cpp +++ b/mission/tmtc/VirtualChannel.cpp @@ -1,6 +1,6 @@ #include "VirtualChannel.h" -#include "CCSDSHandler.h" +#include "CcsdsIpCoreHandler.h" #include "OBSWConfig.h" #include "fsfw/ipc/QueueFactory.h" #include "fsfw/objectmanager/ObjectManager.h" @@ -34,7 +34,7 @@ ReturnValue_t VirtualChannel::performOperation() { size_t size = 0; result = tmStore->getData(storeId, &data, &size); if (result != returnvalue::OK) { - sif::warning << "VirtualChannel::performOperation: Failed to read data from IPC store" + sif::warning << "VirtualChannel::performOperation: Failed to read data from TM store" << std::endl; tmStore->deleteData(storeId); return result; diff --git a/mission/tmtc/VirtualChannel.h b/mission/tmtc/VirtualChannel.h index 024ccee6..96d7ba9d 100644 --- a/mission/tmtc/VirtualChannel.h +++ b/mission/tmtc/VirtualChannel.h @@ -2,7 +2,7 @@ #define VIRTUALCHANNEL_H_ #include -#include +#include #include "OBSWConfig.h" #include "fsfw/returnvalues/returnvalue.h" diff --git a/scripts/install-obsw-yocto.sh b/scripts/install-obsw-yocto.sh index a7e3f20a..08fb78d3 100755 --- a/scripts/install-obsw-yocto.sh +++ b/scripts/install-obsw-yocto.sh @@ -2,7 +2,7 @@ # This is a helper script to install the compiles EIVE OBSW files # into the yocto repository to re-generate the mission root filesystem build_dir=cmake-build-release-q7s -if [ ! -z ${1} && "${1}" == "em" ] || [[ ${EIVE_Q7S_EM} == "1" ]]; then +if [[ ! -z ${1} && "${1}" == "em" ]] || [[ ${EIVE_Q7S_EM} == "1" ]]; then echo "-I- Installing EM binaries" em_install="1" build_dir=cmake-build-release-q7s-em @@ -17,7 +17,7 @@ q7s_package_path="q7s-package/${q7s_yocto_dir}" obsw_version_filename="obsw_version.txt" yocto_obsw_path="yocto/meta-eive/recipes-core/eive-obsw/files" variant_specific_path="" -if [ ${em_install} == "1" ]; then +if [[ ${em_install} == "1" ]]; then variant_specific_path="${yocto_obsw_path}/em" else variant_specific_path="${yocto_obsw_path}/fm" diff --git a/tmtc b/tmtc index ff1b4016..89285ad5 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit ff1b401622cf430f8c312f3c0ca0132918dbf346 +Subproject commit 89285ad511ef11582b67b307c3840a09efd0577a diff --git a/unittest/controller/testThermalController.cpp b/unittest/controller/testThermalController.cpp index d8cd2a1d..3fac0f4e 100644 --- a/unittest/controller/testThermalController.cpp +++ b/unittest/controller/testThermalController.cpp @@ -17,7 +17,7 @@ TEST_CASE("Thermal Controller", "[ThermalController]") { // testEnvironment::initialize(); - ThermalController controller(THERMAL_CONTROLLER_ID, objects::NO_OBJECT); + ThermalController controller(THERMAL_CONTROLLER_ID); ReturnValue_t result = controller.initialize(); REQUIRE(result == returnvalue::OK);