diff --git a/CHANGELOG.md b/CHANGELOG.md index b078c4d7..dd1f57a2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,10 +18,197 @@ will consitute of a breaking change warranting a new major release: ## Fixed +- Pointing control of the `AcsController` was still expecting submodes instead of modes. +- Limitation of RW speeds was done before converting them to the correct unit scale. + +# [v1.37.0] 2023-03-11 + +eive-tmtc: v2.18.1 + +## Added + +- `SensorProcessing` now includes an FDIR for GPS altitude. If the measured GPS altitude is out + of bounds of the range defined in the `AcsParameters`, the altitude defaults to an altitude + set in the `AcsParameters`. +- `AcsController` will now never command a RW speed larger than the maximum allowed speed. + +## Fixed + +- `PAPB_EMPTY_SIGNAL_VC1` GPIO was not set up properly. +- Fix for heater names: HPA heater (index 7) is now the Syrlinks heater. +- `AcsParameters` setter were previously all for scalar parameters. Now vector and matrix + parameters use their respective setters. +- Several `AcsController` components had their own implementation of `AcsParameters`. This resulted + in those parameters not being updated, while the actual ones were updated. All instances of + `AcsParameters` not belonging to `AcsController` are eiter removed or replaced by pointer + instances. +- Instead of updating the `gsTargetModeControllerParameters`, the `targetModeControllerParameters` + were updated. +- Instead of updating the `idleModeControllerParameters`, the `targetModeControllerParameters` + were updated. +- Fixed Idle Mode Controller never calling `ptgLaw` and therefore never calculating control + values. +- Fixed wrong check on wether file used for persistant boolean flag on successful still existed. +- Scaling of MTQ Cmds now scales the current values to command with the current values and not + the values of the last step, which would result in undefined behaviour. +- Solved naming collision between file used for solar array deployment and confirmation for + ACS for solar array deployment. +- Fixed that scaling of RW torque would result in a zero vector unless the maximum value was exceeded. +- Bias for the GYR data was substracted within the wrong rf (sensor rf vs body rf). + +## Changed + +- Refactored TM pipeline to optimize usage of the PTME and communication downlink bandwidth. + This was done by moving the dumping of TMs to the VCs into separate threads with permanent loops. + These threads are then able to process high TM loads on demand. The PUS TM funnel will route + PUS packets to the approrpiate persisten TM stores and then demultiplex the TM to all registered + TM destinations as before. +- Service 5 now handles 40 events per cycle instead of 15 +- Remove periodic SD card check. The file system is not mounted read-only anymore when using an + ext4 filesystem +- The `detumbleCounter` now does not get hard reset anymore, if the critical rate does not get + violated anymore. Instead it is incrementally reset. +- The RW antistiction now only takes the RW speeds in account. +- ACS CTRL transition to DETUBMLE is now done in CTRL internally. No + system level handling necessary anymore. +- More fixes and improvements for SD card handling. Extend SD card setup in core controller to + create full initial state for SD card manager are core controller as early as possible, turn + execution of setup file update blocking. This might solve the issue with the SD card manager + sometimes blocking for a long time. +- Request raw MTM measurement twice for IMTQ, might reduce number of times measurement could not + be retrieved. +- Event manager and event service have larger queues now: 45 -> 120 for Service 5, 80 -> 120 for + event manager +- ACS mode changes: The ACS CTRL submodes are now modes. DETUBMLE is now submode of SAFE mode. +- EIVE system now tracks the mode of the ACS subsyste in SAFE mode. + +# [v1.36.0] 2023-03-08 + +eive-tmtc: v2.17.2 + +## Added + +- Star Tracker Assembly +- New `REBOOT_COUNTER` and `INDIVIDUAL_BOOT_COUNTS` events. The first contains the total boot count + as a u64, the second one contains the individual boot counts as 4 u16. Add new core controller + action command `ANNOUNCE_BOOT_COUNTS` with action ID 3 which triggers both events. These events + will also be triggered on each reboot. + +## Changed + +- Persistent TM stores will now create new files on each reboot. +- Fast ACS subsystem commanding: Command SUS board consecutively with other devices now +- Star Tracker: Use ground confguration for EM and flight config for FM by default. + +## Fixed + +- Command TCS controller off first for TCS subsystem transition to off. +- Health handling for TCS board assembly +- Mode fallback from IDLE mode to SAFE mode due to ACS errors/events now works properly for + the ACS subsystem +- Bugfix in IDLE transition for system. +- `std::filesystem` API usages: Avoid exceptions by using variants which return an error code + instead of throwing exceptions. +- GPS fix loss was not reported if mode is unset. +- Star Tracker: OFF to NORMAL transition now posssible. Requires FSFW bump which sets + transition source modes properly for those transitions. + FSFW PR: https://egit.irs.uni-stuttgart.de/eive/fsfw/pulls/131 +- Star Tracker JSON initialization is now done during object initization instead of redoing it + when building a command. This avoids missed deadlines issues in the ACS PST. +- Allow arbitrary submodes for dual lane boards to avoid FDIR reactions of subsystem components. + Bump FSFW to allow this. +- PUS 15 was not scheduled +- Transmitter timeout set to 2 minutes instead of 15 minutes. This will prevent to discharge the + battery in case the syrlinks starts transmitting due to detection of unintentional bitlock. This + happened e.g. on ground when the uplink to the flying latop was established. +- ACS system components are now always scheduled (EM specific) + +# [v1.35.1] 2023-03-04 + +## Fixed + +- ACS Board Assembly FDIR: Prevent permanent SAFE mode fallbacks by introducing special health + handling. + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/418/files +- Watchdog fixes +- IMTQ timing fixes + +## Added + +- Add IMTQ assembly + +# [v1.35.0] 2023-03-04 + +eive-tmtc: v2.16.4 + +## Added + +- Improved the OBSW watchdog by adding a watch functionality. The watch functionality is optional + and has to be enabled specifically by the application being watched by the watchdog when + starting the watchdog. If the watch functionality is enabled and the OBSW has not pinged + the watchdog via the FIFO for 2 minutes, the watchdog will restart the OBSW service via systemd. + The primary OBSW will only activate the watch functionality if it is the OBSW inside the + `/usr/bin` directory. This allows debugging the system by leaving flashed or manually copied + debugging images 2 minutes to start the watchdog without the watch functionality. + +## Fixed + +- Bumped FSFW: `Countdown` and `Stopwatch` use new monotonic clock API now. +- IMTQ: Various fixes, most notably missing buffer time after starting MGM measurement + and corrections for actuator commanding. + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/430 + +# [v1.34.0] 2023-03-03 + +eive-tmtc: v2.16.3 + +This might include the fix for the race condition where CPU usage jumped to 200 %. The race +condition was traced to the `Countdown` class, more specifically to the `getUptime` function where +the `/proc/uptime` file is read. + +## Changed + +- The SD card prefix is now set earlier inside the `CoreController` constructor +- The watchdog handling was moved outside the `CoreController` into the main loop. +- Moved polling of all SPI parts to the same PST. +- Allow quicker transition for the EIVE system component by allowing consecutive TCS and ACS + component commanding again. +- Changed a lot of lock guards to use timeouts +- Queue sizes of TCP/UDP servers increased from 20 to 50 +- Significantly simplified and improved lock guard handling in TCS and ACS board polling + tasks. + +## Fixed + +- IMTQ: Sets were filled with wrong data, e.g. Raw MTM was filled with calibrated MTM measurements. +- Set RM3100 dataset to valid. +- Fixed units in calculation of ACS control laws safe and detumble. +- Bump FSFW for change in Countdown: Use system clock instead of reading uptime from file + to prevent possible race condition. +- GPS: No fix considered a fault now after 30 minutes instead of 5 hours. +- SUS Assembly FDIR: Prevent permanent SAFE mode fallbacks by introducing special health + handling. + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/418/files + +## Added + +- Added Syrlinks Assembly object to allow recovery handling and to fix faulty FDIR behaviour. + +# [v1.33.0] 2023-03-01 + +eive-tmtc: v2.16.2 + +## Changed + +- Move ACS board polling to separate worker thread. +- Move SUS board polling to separate worker thread. + +## Fixed + - Linux GPS handler now checks the individual `*_SET` flags when analysing the `gpsd` struct. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/400 -# [v1.32.0] +# [v1.32.0] 2023-02-24 eive-tmtc: v2.16.1 @@ -62,7 +249,7 @@ eive-tmtc: v2.16.1 - `RwDummy` now initializes with a non faulty state -# [v1.31.1] +# [v1.31.1] 2023-02-23 ## Fixed @@ -72,7 +259,7 @@ eive-tmtc: v2.16.1 for actuator control which lead to a crash. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/403 -# [v1.31.0] +# [v1.31.0] 2023-02-23 eive-tmtc: v2.16.0 @@ -113,7 +300,7 @@ COM PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/364 - `MekfData` now includes `mekfStatus` - `CtrlValData` now includes `tgtRotRate` -# [v1.30.0] +# [v1.30.0] 2023-02-22 eive-tmtc: v2.14.0 @@ -130,7 +317,7 @@ Event IDs for PDEC handler have changed in a breaking manner. an event is triggered and the task is delayed for 400 ms. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/393 -# [v1.29.1] +# [v1.29.1] 2023-02-21 ## Fixed @@ -142,7 +329,7 @@ Event IDs for PDEC handler have changed in a breaking manner. Issue: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/388 - Disable stopwatch in MAX31865 polling task -# [v1.29.0] +# [v1.29.0] 2023-02-21 eive-tmtc: v2.13.0 @@ -163,7 +350,7 @@ eive-tmtc: v2.13.0 will be part of the TCS tree. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/351 -# [v1.28.1] +# [v1.28.1] 2023-02-21 ## Fixed diff --git a/CMakeLists.txt b/CMakeLists.txt index a9eef222..cb5b5b28 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 1) -set(OBSW_VERSION_MINOR 32) +set(OBSW_VERSION_MINOR 37) set(OBSW_VERSION_REVISION 0) # set(CMAKE_VERBOSE TRUE) @@ -71,11 +71,13 @@ if(EIVE_Q7S_EM) 1 CACHE STRING "Q7S EM configuration") set(INIT_VAL 0) + set(OBSW_STAR_TRACKER_GROUND_CONFIG 1) else() set(OBSW_Q7S_EM 0 CACHE STRING "Q7S EM configuration") set(INIT_VAL 1) + set(OBSW_STAR_TRACKER_GROUND_CONFIG 0) endif() set(OBSW_ADD_MGT ${INIT_VAL} @@ -295,8 +297,10 @@ include(BuildType) set_build_type() set(FSFW_DEBUG_INFO 0) +set(Q7S_CHECK_FOR_ALREADY_RUNNING_IMG 0) if(RELEASE_BUILD MATCHES 0) set(FSFW_DEBUG_INFO 1) + set(Q7S_CHECK_FOR_ALREADY_RUNNING_IMG 1) endif() # Configuration files diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 5ef9e2c2..eb633339 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -58,8 +58,12 @@ void ObjectFactory::produce(void* args) { Factory::setStaticFrameworkObjectIds(); PusTmFunnel* pusFunnel; CfdpTmFunnel* cfdpFunnel; + StorageManagerIF* tmStore; + StorageManagerIF* ipcStore; + PersistentTmStores persistentStores; auto sdcMan = new DummySdCardManager("/tmp"); - ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan); + ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan, &ipcStore, + &tmStore, persistentStores); auto* dummyGpioIF = new DummyGpioIF(); auto* dummySwitcher = new DummyPowerSwitcher(objects::PCDU_HANDLER, 18, 0); diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index c979ff5e..94d829fe 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp +++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 263 translations. + * @brief Auto-generated event translation file. Contains 277 translations. * @details - * Generated on: 2023-02-24 16:57:00 + * Generated on: 2023-03-11 15:01:05 */ #include "translateEvents.h" @@ -158,6 +158,8 @@ const char *LOST_BIT_LOCK_PDEC_STRING = "LOST_BIT_LOCK_PDEC"; const char *TOO_MANY_IRQS_STRING = "TOO_MANY_IRQS"; const char *POLL_SYSCALL_ERROR_PDEC_STRING = "POLL_SYSCALL_ERROR_PDEC"; const char *WRITE_SYSCALL_ERROR_PDEC_STRING = "WRITE_SYSCALL_ERROR_PDEC"; +const char *PDEC_RESET_FAILED_STRING = "PDEC_RESET_FAILED"; +const char *OPEN_IRQ_FILE_FAILED_STRING = "OPEN_IRQ_FILE_FAILED"; 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"; @@ -255,7 +257,8 @@ const char *REBOOT_HW_STRING = "REBOOT_HW"; const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE"; const char *VERSION_INFO_STRING = "VERSION_INFO"; const char *CURRENT_IMAGE_INFO_STRING = "CURRENT_IMAGE_INFO"; -const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; +const char *REBOOT_COUNTER_STRING = "REBOOT_COUNTER"; +const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS"; const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE"; const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE"; const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING"; @@ -263,6 +266,17 @@ const char *PLOC_OVERHEATING_STRING = "PLOC_OVERHEATING"; const char *OBC_OVERHEATING_STRING = "OBC_OVERHEATING"; const char *HPA_OVERHEATING_STRING = "HPA_OVERHEATING"; const char *PLPCDU_OVERHEATING_STRING = "PLPCDU_OVERHEATING"; +const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED"; +const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON"; +const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; +const char *FILE_TOO_LARGE_STRING = "FILE_TOO_LARGE"; +const char *BUSY_DUMPING_EVENT_STRING = "BUSY_DUMPING_EVENT"; +const char *DUMP_WAS_CANCELLED_STRING = "DUMP_WAS_CANCELLED"; +const char *DUMP_OK_STORE_DONE_STRING = "DUMP_OK_STORE_DONE"; +const char *DUMP_NOK_STORE_DONE_STRING = "DUMP_NOK_STORE_DONE"; +const char *DUMP_MISC_STORE_DONE_STRING = "DUMP_MISC_STORE_DONE"; +const char *DUMP_HK_STORE_DONE_STRING = "DUMP_HK_STORE_DONE"; +const char *DUMP_CFDP_STORE_DONE_STRING = "DUMP_CFDP_STORE_DONE"; const char *translateEvents(Event event) { switch ((event & 0xFFFF)) { @@ -572,6 +586,10 @@ const char *translateEvents(Event event) { return POLL_SYSCALL_ERROR_PDEC_STRING; case (12409): return WRITE_SYSCALL_ERROR_PDEC_STRING; + case (12410): + return PDEC_RESET_FAILED_STRING; + case (12411): + return OPEN_IRQ_FILE_FAILED_STRING; case (12500): return IMAGE_UPLOAD_FAILED_STRING; case (12501): @@ -766,22 +784,46 @@ const char *translateEvents(Event event) { return VERSION_INFO_STRING; case (14006): return CURRENT_IMAGE_INFO_STRING; + case (14007): + return REBOOT_COUNTER_STRING; + case (14008): + return INDIVIDUAL_BOOT_COUNTS_STRING; case (14100): - return POSSIBLE_FILE_CORRUPTION_STRING; - case (14200): return NO_VALID_SENSOR_TEMPERATURE_STRING; - case (14201): + case (14101): return NO_HEALTHY_HEATER_AVAILABLE_STRING; - case (14202): + case (14102): return SYRLINKS_OVERHEATING_STRING; - case (14203): + case (14103): return PLOC_OVERHEATING_STRING; - case (14204): + case (14104): return OBC_OVERHEATING_STRING; - case (14205): + case (14105): return HPA_OVERHEATING_STRING; - case (14206): + case (14106): return PLPCDU_OVERHEATING_STRING; + case (14201): + return TX_TIMER_EXPIRED_STRING; + case (14202): + return BIT_LOCK_TX_ON_STRING; + case (14300): + return POSSIBLE_FILE_CORRUPTION_STRING; + case (14301): + return FILE_TOO_LARGE_STRING; + case (14302): + return BUSY_DUMPING_EVENT_STRING; + case (14303): + return DUMP_WAS_CANCELLED_STRING; + case (14305): + return DUMP_OK_STORE_DONE_STRING; + case (14306): + return DUMP_NOK_STORE_DONE_STRING; + case (14307): + return DUMP_MISC_STORE_DONE_STRING; + case (14308): + return DUMP_HK_STORE_DONE_STRING; + case (14309): + return DUMP_CFDP_STORE_DONE_STRING; default: return "UNKNOWN_EVENT"; } diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index be60d130..54802800 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -1,8 +1,8 @@ /** * @brief Auto-generated object translation file. * @details - * Contains 154 translations. - * Generated on: 2023-02-24 16:57:00 + * Contains 169 translations. + * Generated on: 2023-03-11 15:01:05 */ #include "translateObjects.h" @@ -54,6 +54,10 @@ const char *STR_HELPER_STRING = "STR_HELPER"; const char *PLOC_MPSOC_HELPER_STRING = "PLOC_MPSOC_HELPER"; const char *AXI_PTME_CONFIG_STRING = "AXI_PTME_CONFIG"; const char *PTME_CONFIG_STRING = "PTME_CONFIG"; +const char *PTME_VC0_LIVE_TM_STRING = "PTME_VC0_LIVE_TM"; +const char *PTME_VC1_LOG_TM_STRING = "PTME_VC1_LOG_TM"; +const char *PTME_VC2_HK_TM_STRING = "PTME_VC2_HK_TM"; +const char *PTME_VC3_CFDP_TM_STRING = "PTME_VC3_CFDP_TM"; const char *PLOC_MPSOC_HANDLER_STRING = "PLOC_MPSOC_HANDLER"; const char *PLOC_SUPERVISOR_HANDLER_STRING = "PLOC_SUPERVISOR_HANDLER"; const char *PLOC_SUPERVISOR_HELPER_STRING = "PLOC_SUPERVISOR_HELPER"; @@ -85,8 +89,11 @@ const char *SYRLINKS_HANDLER_STRING = "SYRLINKS_HANDLER"; const char *ARDUINO_COM_IF_STRING = "ARDUINO_COM_IF"; const char *DUMMY_COM_IF_STRING = "DUMMY_COM_IF"; const char *SCEX_UART_READER_STRING = "SCEX_UART_READER"; -const char *SPI_RTD_COM_IF_STRING = "SPI_RTD_COM_IF"; const char *UART_COM_IF_STRING = "UART_COM_IF"; +const char *ACS_BOARD_POLLING_TASK_STRING = "ACS_BOARD_POLLING_TASK"; +const char *RW_POLLING_TASK_STRING = "RW_POLLING_TASK"; +const char *SPI_RTD_COM_IF_STRING = "SPI_RTD_COM_IF"; +const char *SUS_POLLING_TASK_STRING = "SUS_POLLING_TASK"; const char *CCSDS_PACKET_DISTRIBUTOR_STRING = "CCSDS_PACKET_DISTRIBUTOR"; const char *PUS_PACKET_DISTRIBUTOR_STRING = "PUS_PACKET_DISTRIBUTOR"; const char *TCP_TMTC_SERVER_STRING = "TCP_TMTC_SERVER"; @@ -135,12 +142,15 @@ const char *HEATER_3_OBC_BRD_STRING = "HEATER_3_OBC_BRD"; const char *HEATER_4_CAMERA_STRING = "HEATER_4_CAMERA"; const char *HEATER_5_STR_STRING = "HEATER_5_STR"; const char *HEATER_6_DRO_STRING = "HEATER_6_DRO"; -const char *HEATER_7_HPA_STRING = "HEATER_7_HPA"; +const char *HEATER_7_SYRLINKS_STRING = "HEATER_7_SYRLINKS"; 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 *RW_ASSY_STRING = "RW_ASSY"; const char *CAM_SWITCHER_STRING = "CAM_SWITCHER"; +const char *SYRLINKS_ASSY_STRING = "SYRLINKS_ASSY"; +const char *IMTQ_ASSY_STRING = "IMTQ_ASSY"; +const char *STR_ASSY_STRING = "STR_ASSY"; 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"; @@ -156,6 +166,11 @@ const char *OK_TM_STORE_STRING = "OK_TM_STORE"; const char *NOT_OK_TM_STORE_STRING = "NOT_OK_TM_STORE"; const char *HK_TM_STORE_STRING = "HK_TM_STORE"; const char *CFDP_TM_STORE_STRING = "CFDP_TM_STORE"; +const char *LIVE_TM_TASK_STRING = "LIVE_TM_TASK"; +const char *LOG_STORE_AND_TM_TASK_STRING = "LOG_STORE_AND_TM_TASK"; +const char *HK_STORE_AND_TM_TASK_STRING = "HK_STORE_AND_TM_TASK"; +const char *CFDP_STORE_AND_TM_TASK_STRING = "CFDP_STORE_AND_TM_TASK"; +const char *DOWNLINK_RAM_STORE_STRING = "DOWNLINK_RAM_STORE"; const char *CCSDS_IP_CORE_BRIDGE_STRING = "CCSDS_IP_CORE_BRIDGE"; const char *THERMAL_TEMP_INSERTER_STRING = "THERMAL_TEMP_INSERTER"; const char *DUMMY_INTERFACE_STRING = "DUMMY_INTERFACE"; @@ -259,6 +274,14 @@ const char *translateObject(object_id_t object) { return AXI_PTME_CONFIG_STRING; case 0x44330005: return PTME_CONFIG_STRING; + case 0x44330006: + return PTME_VC0_LIVE_TM_STRING; + case 0x44330007: + return PTME_VC1_LOG_TM_STRING; + case 0x44330008: + return PTME_VC2_HK_TM_STRING; + case 0x44330009: + return PTME_VC3_CFDP_TM_STRING; case 0x44330015: return PLOC_MPSOC_HANDLER_STRING; case 0x44330016: @@ -321,10 +344,16 @@ const char *translateObject(object_id_t object) { return DUMMY_COM_IF_STRING; case 0x49010006: return SCEX_UART_READER_STRING; - case 0x49020006: - return SPI_RTD_COM_IF_STRING; case 0x49030003: return UART_COM_IF_STRING; + case 0x49060004: + return ACS_BOARD_POLLING_TASK_STRING; + case 0x49060005: + return RW_POLLING_TASK_STRING; + case 0x49060006: + return SPI_RTD_COM_IF_STRING; + case 0x49060007: + return SUS_POLLING_TASK_STRING; case 0x50000100: return CCSDS_PACKET_DISTRIBUTOR_STRING; case 0x50000200: @@ -422,7 +451,7 @@ const char *translateObject(object_id_t object) { case 0x60000006: return HEATER_6_DRO_STRING; case 0x60000007: - return HEATER_7_HPA_STRING; + return HEATER_7_SYRLINKS_STRING; case 0x73000001: return ACS_BOARD_ASS_STRING; case 0x73000002: @@ -430,9 +459,15 @@ const char *translateObject(object_id_t object) { case 0x73000003: return TCS_BOARD_ASS_STRING; case 0x73000004: - return RW_ASS_STRING; + return RW_ASSY_STRING; case 0x73000006: return CAM_SWITCHER_STRING; + case 0x73000007: + return SYRLINKS_ASSY_STRING; + case 0x73000008: + return IMTQ_ASSY_STRING; + case 0x73000009: + return STR_ASSY_STRING; case 0x73000100: return TM_FUNNEL_STRING; case 0x73000101: @@ -463,6 +498,16 @@ const char *translateObject(object_id_t object) { return HK_TM_STORE_STRING; case 0x73030000: return CFDP_TM_STORE_STRING; + case 0x73040000: + return LIVE_TM_TASK_STRING; + case 0x73040001: + return LOG_STORE_AND_TM_TASK_STRING; + case 0x73040002: + return HK_STORE_AND_TM_TASK_STRING; + case 0x73040003: + return CFDP_STORE_AND_TM_TASK_STRING; + case 0x73040004: + return DOWNLINK_RAM_STORE_STRING; case 0x73500000: return CCSDS_IP_CORE_BRIDGE_STRING; case 0x90000003: diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index 687d9363..1dff8f7f 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -67,7 +67,7 @@ #define OBSW_PRINT_MISSED_DEADLINES 1 #define OBSW_MPSOC_JTAG_BOOT 0 -#define OBSW_STAR_TRACKER_GROUND_CONFIG 1 +#define OBSW_STAR_TRACKER_GROUND_CONFIG @OBSW_STAR_TRACKER_GROUND_CONFIG@ #define OBSW_SYRLINKS_SIMULATED @OBSW_SYRLINKS_SIMULATED@ #define OBSW_ADD_TEST_CODE 0 #define OBSW_ADD_TEST_TASK 0 diff --git a/bsp_q7s/boardconfig/busConf.h b/bsp_q7s/boardconfig/busConf.h index a094c029..4c142644 100644 --- a/bsp_q7s/boardconfig/busConf.h +++ b/bsp_q7s/boardconfig/busConf.h @@ -28,9 +28,13 @@ static constexpr char UIO_PDEC_IRQ[] = "/dev/uio_pdec_irq"; static constexpr int MAP_ID_PTME_CONFIG = 3; namespace uiomapids { +// Live TM static const int PTME_VC0 = 0; +// OK/NOK/MISC Store static const int PTME_VC1 = 1; +// HK store static const int PTME_VC2 = 2; +// CFDP static const int PTME_VC3 = 3; static const int PTME_CONFIG = 4; } // namespace uiomapids diff --git a/bsp_q7s/boardconfig/q7sConfig.h.in b/bsp_q7s/boardconfig/q7sConfig.h.in index ee9cd863..8fe0f658 100644 --- a/bsp_q7s/boardconfig/q7sConfig.h.in +++ b/bsp_q7s/boardconfig/q7sConfig.h.in @@ -17,7 +17,7 @@ /*******************************************************************/ // Probably better if this is disabled for mission code. Convenient for development -#define Q7S_CHECK_FOR_ALREADY_RUNNING_IMG 1 +#define Q7S_CHECK_FOR_ALREADY_RUNNING_IMG @Q7S_CHECK_FOR_ALREADY_RUNNING_IMG@ #define Q7S_SIMPLE_ADD_FILE_SYSTEM_TEST 0 diff --git a/bsp_q7s/core/CMakeLists.txt b/bsp_q7s/core/CMakeLists.txt index 15d361fd..b726885b 100644 --- a/bsp_q7s/core/CMakeLists.txt +++ b/bsp_q7s/core/CMakeLists.txt @@ -1,4 +1,4 @@ target_sources(${OBSW_NAME} PRIVATE CoreController.cpp scheduling.cpp - ObjectFactory.cpp) + ObjectFactory.cpp WatchdogHandler.cpp) target_sources(${SIMPLE_OBSW_NAME} PRIVATE scheduling.cpp) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 7ee3191b..5c4710d4 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -33,12 +33,7 @@ xsc::Copy CoreController::CURRENT_COPY = xsc::Copy::NO_COPY; CoreController::CoreController(object_id_t objectId) : ExtendedControllerBase(objectId, 5), opDivider5(5), opDivider10(10), hkSet(this) { - ReturnValue_t result = returnvalue::OK; try { - result = initWatchdogFifo(); - if (result != returnvalue::OK) { - sif::warning << "CoreController::CoreController: Watchdog FIFO init failed" << std::endl; - } sdcMan = SdCardManager::instance(); if (sdcMan == nullptr) { sif::error << "CoreController::CoreController: SD card manager invalid!" << std::endl; @@ -47,6 +42,26 @@ CoreController::CoreController(object_id_t objectId) if (not BLOCKING_SD_INIT) { sdcMan->setBlocking(false); } + // Set up state of SD card manager and own initial state. + // Stopwatch watch; + sdcMan->updateSdCardStateFile(); + sdcMan->updateSdStatePair(); + SdCardManager::SdStatePair sdStates; + sdcMan->getSdCardsStatus(sdStates); + auto sdCard = sdcMan->getPreferredSdCard(); + if (not sdCard.has_value()) { + sif::error << "CoreController::initializeAfterTaskCreation: " + "Issues getting preferred SD card, setting to 0" + << std::endl; + sdCard = sd::SdCard::SLOT_0; + } + sdInfo.active = sdCard.value(); + if (sdStates.first == sd::SdState::MOUNTED) { + sdcMan->setActiveSdCard(sd::SdCard::SLOT_0); + } else if (sdStates.second == sd::SdState::MOUNTED) { + sdcMan->setActiveSdCard(sd::SdCard::SLOT_1); + } + currMntPrefix = sdcMan->getCurrentMountPrefix(); getCurrentBootCopy(CURRENT_CHIP, CURRENT_COPY); @@ -54,6 +69,10 @@ CoreController::CoreController(object_id_t objectId) } catch (const std::filesystem::filesystem_error &e) { sif::error << "CoreController::CoreController: Failed with exception " << e.what() << std::endl; } + // Add script folder to path + char *currentEnvPath = getenv("PATH"); + std::string updatedEnvPath = std::string(currentEnvPath) + ":/home/root/scripts:/usr/local/bin"; + setenv("PATH", updatedEnvPath.c_str(), true); sdCardCheckCd.timeOut(); eventQueue = QueueFactory::instance()->createMessageQueue(5, EventMessage::MAX_MESSAGE_SIZE); } @@ -78,19 +97,8 @@ void CoreController::performControlOperation() { } } } - performWatchdogControlOperation(); sdStateMachine(); performMountedSdCardOperations(); - if (sdCardCheckCd.hasTimedOut()) { - if (shortSdCardCdCounter < 2) { - shortSdCardCdCounter++; - } - if (shortSdCardCdCounter == 2) { - sdCardCheckCd.setTimeout(DEFAULT_SD_CARD_CHECK_TIMEOUT); - } - performSdCardCheck(); - sdCardCheckCd.resetTimer(); - } readHkData(); opDivider5.checkAndIncrement(); opDivider10.checkAndIncrement(); @@ -148,22 +156,6 @@ ReturnValue_t CoreController::initialize() { ReturnValue_t CoreController::initializeAfterTaskCreation() { ReturnValue_t result = returnvalue::OK; - auto sdCard = sdcMan->getPreferredSdCard(); - if (not sdCard) { - return returnvalue::FAILED; - } - sdInfo.active = sdCard.value(); - if (sdInfo.active == sd::SdCard::NONE) { - sif::error << "CoreController::initializeAfterTaskCreation: " - "Issues getting preferred SD card, setting to 0" - << std::endl; - sdInfo.active = sd::SdCard::SLOT_0; - } - sdcMan->setActiveSdCard(sdInfo.active); - currMntPrefix = sdcMan->getCurrentMountPrefix(); - if (currMntPrefix == "") { - return ObjectManagerIF::CHILD_INIT_FAILED; - } if (BLOCKING_SD_INIT) { result = initSdCardBlocking(); if (result != returnvalue::OK and result != SdCardManager::ALREADY_MOUNTED) { @@ -175,12 +167,7 @@ ReturnValue_t CoreController::initializeAfterTaskCreation() { if (result != returnvalue::OK) { sif::warning << "CoreController::initialize: Version initialization failed" << std::endl; } - // Add script folder to path - char *currentEnvPath = getenv("PATH"); - std::string updatedEnvPath = std::string(currentEnvPath) + ":/home/root/scripts:/usr/local/bin"; - setenv("PATH", updatedEnvPath.c_str(), true); updateProtInfo(); - initPrint(); return ExtendedControllerBase::initializeAfterTaskCreation(); } @@ -203,6 +190,10 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ triggerEvent(VERSION_INFO, p1, p2); return HasActionsIF::EXECUTION_FINISHED; } + case (ANNOUNCE_BOOT_COUNTS): { + announceBootCounts(); + return HasActionsIF::EXECUTION_FINISHED; + } case (ANNOUNCE_CURRENT_IMAGE): { triggerEvent(CURRENT_IMAGE_INFO, CURRENT_CHIP, CURRENT_COPY); return HasActionsIF::EXECUTION_FINISHED; @@ -409,7 +400,9 @@ ReturnValue_t CoreController::sdStateMachine() { sif::warning << "CoreController::sdStateMachine: Updating SD card state file failed" << std::endl; } - sdInfo.commandExecuted = true; + sdFsmState = SdStates::SET_STATE_SELF; + sdInfo.commandExecuted = false; + sdInfo.cycleCount = 0; } else { nonBlockingOpChecking(SdStates::SET_STATE_SELF, 4, "Updating SDC file"); } @@ -552,10 +545,6 @@ ReturnValue_t CoreController::sdStateMachine() { if (sdFsmState == SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE) { sdFsmState = SdStates::UPDATE_INFO; } else if (sdFsmState == SdStates::UPDATE_INFO) { - // It is assumed that all tasks are running by the point this section is reached. - // Therefore, perform this operation in blocking mode because it does not take long - // and the ready state of the SD card is available sooner - sdcMan->setBlocking(true); // Update status file result = sdcMan->updateSdCardStateFile(); if (result != returnvalue::OK) { @@ -614,7 +603,8 @@ ReturnValue_t CoreController::sdCardSetup(sd::SdCard sdCard, sd::SdState targetS sif::info << "Unmounting SD card " << sdChar << std::endl; return sdcMan->unmountSdCard(sdCard); } else { - if (std::filesystem::exists(mountString)) { + std::error_code e; + if (std::filesystem::exists(mountString, e)) { sif::info << "SD card " << sdChar << " already on and mounted at " << mountString << std::endl; return SdCardManager::ALREADY_MOUNTED; @@ -711,7 +701,8 @@ ReturnValue_t CoreController::initVersionFile() { std::string versionFilePath = currMntPrefix + VERSION_FILE; std::fstream versionFile; - if (not std::filesystem::exists(versionFilePath)) { + std::error_code e; + if (not std::filesystem::exists(versionFilePath, e)) { sif::info << "Writing version file " << versionFilePath << ".." << std::endl; versionFile.open(versionFilePath, std::ios_base::out); versionFile << fullObswVersionString << std::endl; @@ -823,7 +814,8 @@ ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId, } ReturnValue_t CoreController::initBootCopyFile() { - if (not std::filesystem::exists(CURR_COPY_FILE)) { + std::error_code e; + if (not std::filesystem::exists(CURR_COPY_FILE, e)) { // This file is created by the systemd service eive-early-config so this should // not happen normally std::string cmd = "xsc_boot_copy > " + std::string(CURR_COPY_FILE); @@ -844,36 +836,6 @@ void CoreController::getCurrentBootCopy(xsc::Chip &chip, xsc::Copy ©) { copy = static_cast(xscCopy); } -ReturnValue_t CoreController::initWatchdogFifo() { - if (not std::filesystem::exists(watchdog::FIFO_NAME)) { - // Still return returnvalue::OK for now - sif::info << "Watchdog FIFO " << watchdog::FIFO_NAME << " does not exist, can't initiate" - << " watchdog" << std::endl; - return returnvalue::OK; - } - // Open FIFO write only and non-blocking to prevent SW from killing itself. - watchdogFifoFd = open(watchdog::FIFO_NAME.c_str(), O_WRONLY | O_NONBLOCK); - if (watchdogFifoFd < 0) { - if (errno == ENXIO) { - watchdogFifoFd = RETRY_FIFO_OPEN; - sif::info << "eive-watchdog not running. FIFO can not be opened" << std::endl; - } else { - sif::error << "Opening pipe " << watchdog::FIFO_NAME << " write-only failed with " << errno - << ": " << strerror(errno) << std::endl; - return returnvalue::FAILED; - } - } - return returnvalue::OK; -} - -void CoreController::initPrint() { -#if OBSW_VERBOSE_LEVEL >= 1 - if (watchdogFifoFd > 0) { - sif::info << "Opened watchdog FIFO successfully.." << std::endl; - } -#endif -} - ReturnValue_t CoreController::actionXscReboot(const uint8_t *data, size_t size) { if (size < 1) { return HasActionsIF::INVALID_PARAMETERS; @@ -1157,7 +1119,8 @@ ReturnValue_t CoreController::updateProtInfo(bool regenerateChipStateFile) { return result; } } - if (not filesystem::exists(CHIP_STATE_FILE)) { + std::error_code e; + if (not filesystem::exists(CHIP_STATE_FILE, e)) { return returnvalue::FAILED; } ifstream chipStateFile(CHIP_STATE_FILE); @@ -1224,38 +1187,11 @@ ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) { } } wordIdx++; - } - return returnvalue::OK; -} - -void CoreController::performWatchdogControlOperation() { - // Only perform each fifth iteration - if (watchdogFifoFd != 0 and opDivider5.check()) { - if (watchdogFifoFd == RETRY_FIFO_OPEN) { - // Open FIFO write only and non-blocking - watchdogFifoFd = open(watchdog::FIFO_NAME.c_str(), O_WRONLY | O_NONBLOCK); - if (watchdogFifoFd < 0) { - if (errno == ENXIO) { - watchdogFifoFd = RETRY_FIFO_OPEN; - // No printout for now, would be spam - return; - } else { - sif::error << "Opening pipe " << watchdog::FIFO_NAME << " write-only failed with " - << errno << ": " << strerror(errno) << std::endl; - return; - } - } - sif::info << "Opened " << watchdog::FIFO_NAME << " successfully" << std::endl; - } else if (watchdogFifoFd > 0) { - // Write to OBSW watchdog FIFO here - const char writeChar = 'a'; - ssize_t writtenBytes = write(watchdogFifoFd, &writeChar, 1); - if (writtenBytes < 0) { - sif::error << "Errors writing to watchdog FIFO, code " << errno << ": " << strerror(errno) - << std::endl; - } + if (wordIdx >= 10) { + break; } } + return returnvalue::OK; } void CoreController::performMountedSdCardOperations() { @@ -1263,8 +1199,14 @@ void CoreController::performMountedSdCardOperations() { if (not performOneShotSdCardOpsSwitch) { std::ostringstream path; path << mntPoint << "/" << CONF_FOLDER; - if (not std::filesystem::exists(path.str())) { - std::filesystem::create_directory(path.str()); + std::error_code e; + if (not std::filesystem::exists(path.str()), e) { + bool created = std::filesystem::create_directory(path.str(), e); + if (not created) { + sif::error << "Could not create CONF folder at " << path.str() << ": " << e.message() + << std::endl; + return; + } } initVersionFile(); ReturnValue_t result = initBootCopyFile(); @@ -1349,7 +1291,8 @@ ReturnValue_t CoreController::performSdCardCheck() { void CoreController::performRebootFileHandling(bool recreateFile) { using namespace std; std::string path = currMntPrefix + REBOOT_FILE; - if (not std::filesystem::exists(path) or recreateFile) { + std::error_code e; + if (not std::filesystem::exists(path, e) or recreateFile) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "CoreController::performRebootFileHandling: Recreating reboot file" << std::endl; #endif @@ -1391,13 +1334,13 @@ void CoreController::performRebootFileHandling(bool recreateFile) { if (rebootFile.bootFlag) { // Trigger event to inform ground that a reboot was triggered uint32_t p1 = rebootFile.lastChip << 16 | rebootFile.lastCopy; - uint32_t p2 = rebootFile.img00Cnt << 24 | rebootFile.img01Cnt << 16 | rebootFile.img10Cnt << 8 | - rebootFile.img11Cnt; - triggerEvent(REBOOT_MECHANISM_TRIGGERED, p1, p2); + triggerEvent(REBOOT_MECHANISM_TRIGGERED, p1, 0); // Clear the boot flag rebootFile.bootFlag = false; } + announceBootCounts(); + if (rebootFile.mechanismNextChip != xsc::NO_CHIP and rebootFile.mechanismNextCopy != xsc::NO_COPY) { if (CURRENT_CHIP != rebootFile.mechanismNextChip or @@ -1816,7 +1759,8 @@ ReturnValue_t CoreController::initClockFromTimeFile() { using namespace GpsHyperion; using namespace std; std::string fileName = currMntPrefix + BACKUP_TIME_FILE; - if (sdcMan->isSdCardUsable(std::nullopt) and std::filesystem::exists(fileName) and + std::error_code e; + if (sdcMan->isSdCardUsable(std::nullopt) and std::filesystem::exists(fileName, e) and ((gpsFix == FixMode::UNKNOWN or gpsFix == FixMode::NOT_SEEN) or not utility::timeSanityCheck())) { ifstream timeFile(fileName); @@ -1941,7 +1885,8 @@ ReturnValue_t CoreController::executeSwUpdate(SwUpdateSources sourceDir, const u prefixPath = path("/tmp"); } path archivePath = prefixPath / path(config::OBSW_UPDATE_ARCHIVE_FILE_NAME); - if (not exists(archivePath)) { + std::error_code e; + if (not exists(archivePath, e)) { return HasFileSystemIF::FILE_DOES_NOT_EXIST; } ostringstream cmd("tar -xJf", ios::app); @@ -1951,12 +1896,12 @@ ReturnValue_t CoreController::executeSwUpdate(SwUpdateSources sourceDir, const u utility::handleSystemError(result, "CoreController::executeAction: SW Update Decompression"); } path strippedImagePath = prefixPath / path(config::STRIPPED_OBSW_BINARY_FILE_NAME); - if (!exists(strippedImagePath)) { + if (!exists(strippedImagePath, e)) { // TODO: Custom returnvalue? return returnvalue::FAILED; } path obswVersionFilePath = prefixPath / path(config::OBSW_VERSION_FILE_NAME); - if (!exists(obswVersionFilePath)) { + if (!exists(obswVersionFilePath, e)) { // TODO: Custom returnvalue? return returnvalue::FAILED; } @@ -2053,6 +1998,15 @@ bool CoreController::startSdStateMachine(sd::SdCard targetActiveSd, SdCfgMode mo return true; } +void CoreController::announceBootCounts() { + uint64_t totalBootCount = + rebootFile.img00Cnt + rebootFile.img01Cnt + rebootFile.img10Cnt + rebootFile.img11Cnt; + uint32_t individualBootCountsP1 = (rebootFile.img00Cnt << 16) | rebootFile.img01Cnt; + uint32_t individualBootCountsP2 = (rebootFile.img10Cnt << 16) | rebootFile.img11Cnt; + triggerEvent(INDIVIDUAL_BOOT_COUNTS, individualBootCountsP1, individualBootCountsP2); + triggerEvent(REBOOT_COUNTER, (totalBootCount >> 32) & 0xffffffff, totalBootCount & 0xffffffff); +} + bool CoreController::isNumber(const std::string &s) { return !s.empty() && std::find_if(s.begin(), s.end(), [](unsigned char c) { return !std::isdigit(c); }) == s.end(); diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index 1376c0a8..c5e23d48 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -78,6 +78,7 @@ class CoreController : public ExtendedControllerBase { static constexpr ActionId_t LIST_DIRECTORY_INTO_FILE = 0; static constexpr ActionId_t ANNOUNCE_VERSION = 1; static constexpr ActionId_t ANNOUNCE_CURRENT_IMAGE = 2; + static constexpr ActionId_t ANNOUNCE_BOOT_COUNTS = 3; static constexpr ActionId_t SWITCH_REBOOT_FILE_HANDLING = 5; static constexpr ActionId_t RESET_REBOOT_COUNTERS = 6; static constexpr ActionId_t SWITCH_IMG_LOCK = 7; @@ -102,7 +103,7 @@ class CoreController : public ExtendedControllerBase { static constexpr Event ALLOC_FAILURE = event::makeEvent(SUBSYSTEM_ID, 0, severity::MEDIUM); //! [EXPORT] : [COMMENT] Software reboot occurred. Can also be a systemd reboot. //! P1: Current Chip, P2: Current Copy - static constexpr Event REBOOT_SW = event::makeEvent(SUBSYSTEM_ID, 1, severity::MEDIUM); + static constexpr Event REBOOT_SW = event::makeEvent(SUBSYSTEM_ID, 1, severity::LOW); //! [EXPORT] : [COMMENT] The reboot mechanism was triggered. //! P1: First 16 bits: Last Chip, Last 16 bits: Last Copy, //! P2: Each byte is the respective reboot count for the slots @@ -119,6 +120,13 @@ class CoreController : public ExtendedControllerBase { static constexpr Event VERSION_INFO = event::makeEvent(SUBSYSTEM_ID, 5, severity::INFO); //! [EXPORT] : [COMMENT] P1: Current Chip, P2: Current Copy static constexpr Event CURRENT_IMAGE_INFO = event::makeEvent(SUBSYSTEM_ID, 6, severity::INFO); + //! [EXPORT] : [COMMENT] Total reboot counter, which is the sum of the boot count of all + //! individual images. + static constexpr Event REBOOT_COUNTER = event::makeEvent(SUBSYSTEM_ID, 7, severity::INFO); + //! [EXPORT] : [COMMENT] Get the boot count of the individual images. + //! P1: First 16 bits boot count of image 0 0, last 16 bits boot count of image 0 1. + //! P2: First 16 bits boot count of image 1 0, last 16 bits boot count of image 1 1. + static constexpr Event INDIVIDUAL_BOOT_COUNTS = event::makeEvent(SUBSYSTEM_ID, 8, severity::INFO); CoreController(object_id_t objectId); virtual ~CoreController(); @@ -164,9 +172,6 @@ class CoreController : public ExtendedControllerBase { static constexpr uint32_t BOOT_OFFSET_SECONDS = 15; static constexpr MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING; static constexpr uint32_t MUTEX_TIMEOUT = 20; - // Designated value for rechecking FIFO open - static constexpr int RETRY_FIFO_OPEN = -2; - int watchdogFifoFd = 0; GpsHyperion::FixMode gpsFix = GpsHyperion::FixMode::UNKNOWN; // States for SD state machine, which is used in non-blocking mode @@ -263,7 +268,6 @@ class CoreController : public ExtendedControllerBase { ReturnValue_t performSdCardCheck(); ReturnValue_t backupTimeFileHandler(); ReturnValue_t initBootCopyFile(); - ReturnValue_t initWatchdogFifo(); ReturnValue_t initSdCardBlocking(); bool startSdStateMachine(sd::SdCard targetActiveSd, SdCfgMode mode, MessageQueueId_t commander, DeviceCommandId_t actionId); @@ -288,8 +292,6 @@ class CoreController : public ExtendedControllerBase { ReturnValue_t gracefulShutdownTasks(xsc::Chip chip, xsc::Copy copy, bool& protOpPerformed); - void performWatchdogControlOperation(); - ReturnValue_t handleProtInfoUpdateLine(std::string nextLine); int handleBootCopyProtAtIndex(xsc::Chip targetChip, xsc::Copy targetCopy, bool protect, bool& protOperationPerformed, bool selfChip, bool selfCopy, @@ -300,6 +302,7 @@ class CoreController : public ExtendedControllerBase { void setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy); bool parseRebootFile(std::string path, RebootFile& file); void rewriteRebootFile(RebootFile file); + void announceBootCounts(); void readHkData(); bool isNumber(const std::string& s); }; diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 41098175..3092e69d 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -1,9 +1,20 @@ #include "ObjectFactory.h" #include +#include #include #include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include +#include #include "OBSWConfig.h" #include "bsp_q7s/boardtest/Q7STestTask.h" @@ -54,15 +65,18 @@ #include "mission/system/tree/comModeTree.h" #include "mission/system/tree/payloadModeTree.h" #include "mission/system/tree/tcsModeTree.h" +#include "mission/tmtc/tmFilters.h" #include "mission/utility/GlobalConfigHandler.h" #include "tmtc/pusIds.h" #if OBSW_TEST_LIBGPIOD == 1 #include "linux/boardtest/LibgpiodTest.h" #endif +#include #include #include #include #include +#include #include @@ -85,7 +99,6 @@ #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/Max31865PT1000Handler.h" #include "mission/devices/P60DockHandler.h" @@ -104,9 +117,9 @@ #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; +std::atomic_bool LINK_STATE = CcsdsIpCoreHandler::LINK_DOWN; void Factory::setStaticFrameworkObjectIds() { PusServiceBase::PUS_DISTRIBUTOR = objects::PUS_PACKET_DISTRIBUTOR; @@ -241,8 +254,8 @@ ReturnValue_t ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF, return returnvalue::OK; } -void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, - PowerSwitchIF& pwrSwitcher) { +void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, + SerialComIF* uartComIF, PowerSwitchIF& pwrSwitcher) { using namespace gpio; GpioCookie* gpioCookieAcsBoard = new GpioCookie(); @@ -345,14 +358,16 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo static_cast(fdir); #if OBSW_ADD_ACS_BOARD == 1 + new AcsBoardPolling(objects::ACS_BOARD_POLLING_TASK, spiComIF, *gpioComIF); std::string spiDev = q7s::SPI_DEFAULT_DEV; std::array assemblyChildren; SpiCookie* spiCookie = - new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, MGMLIS3MDL::MAX_BUFFER_SIZE, + new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, mgmLis3::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto mgmLis3Handler0 = new MgmLIS3MDLHandler( - objects::MGM_0_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY); + auto mgmLis3Handler0 = + new MgmLis3CustomHandler(objects::MGM_0_LIS3_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, spi::LIS3_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_0_LIS3_HANDLER); mgmLis3Handler0->setCustomFdir(fdir); assemblyChildren[0] = mgmLis3Handler0; @@ -364,12 +379,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo mgmLis3Handler->enablePeriodicPrintouts(true, 10); #endif spiCookie = - new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, RM3100::MAX_BUFFER_SIZE, + new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, mgmRm3100::MAX_BUFFER_SIZE, spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); auto mgmRm3100Handler1 = - new MgmRM3100Handler(objects::MGM_1_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, - spi::RM3100_TRANSITION_DELAY); + new MgmRm3100CustomHandler(objects::MGM_1_RM3100_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, spi::RM3100_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_1_RM3100_HANDLER); mgmRm3100Handler1->setCustomFdir(fdir); assemblyChildren[1] = mgmRm3100Handler1; @@ -380,12 +395,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo #if OBSW_DEBUG_ACS == 1 mgmRm3100Handler->enablePeriodicPrintouts(true, 10); #endif - spiCookie = - new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, MGMLIS3MDL::MAX_BUFFER_SIZE, - spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); + spiCookie = new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, mgmLis3::MAX_BUFFER_SIZE, + spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto* mgmLis3Handler2 = new MgmLIS3MDLHandler( - objects::MGM_2_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY); + auto* mgmLis3Handler2 = + new MgmLis3CustomHandler(objects::MGM_2_LIS3_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, spi::LIS3_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_2_LIS3_HANDLER); mgmLis3Handler2->setCustomFdir(fdir); assemblyChildren[2] = mgmLis3Handler2; @@ -397,12 +412,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo mgmLis3Handler->enablePeriodicPrintouts(true, 10); #endif spiCookie = - new SpiCookie(addresses::MGM_3_RM3100, gpioIds::MGM_3_RM3100_CS, RM3100::MAX_BUFFER_SIZE, + new SpiCookie(addresses::MGM_3_RM3100, gpioIds::MGM_3_RM3100_CS, mgmRm3100::MAX_BUFFER_SIZE, spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); auto* mgmRm3100Handler3 = - new MgmRM3100Handler(objects::MGM_3_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, - spi::RM3100_TRANSITION_DELAY); + new MgmRm3100CustomHandler(objects::MGM_3_RM3100_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, spi::RM3100_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_3_RM3100_HANDLER); mgmRm3100Handler3->setCustomFdir(fdir); assemblyChildren[3] = mgmRm3100Handler3; @@ -416,12 +431,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo // Commented until ACS board V2 in in clean room again // Gyro 0 Side A spiCookie = - new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, ADIS1650X::MAXIMUM_REPLY_SIZE, + new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, adis1650x::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); auto adisHandler = - new GyroADIS1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, - ADIS1650X::Type::ADIS16505); + new GyrAdis1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, adis1650x::Type::ADIS16505); fdir = new AcsBoardFdir(objects::GYRO_0_ADIS_HANDLER); adisHandler->setCustomFdir(fdir); assemblyChildren[4] = adisHandler; @@ -433,11 +448,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo adisHandler->enablePeriodicPrintouts(true, 10); #endif // Gyro 1 Side A - spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, L3GD20H::MAX_BUFFER_SIZE, + spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, l3gd20h::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto gyroL3gHandler1 = new GyroHandlerL3GD20H( - objects::GYRO_1_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY); + auto gyroL3gHandler1 = + new GyrL3gCustomHandler(objects::GYRO_1_L3G_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, spi::L3G_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::GYRO_1_L3G_HANDLER); gyroL3gHandler1->setCustomFdir(fdir); assemblyChildren[5] = gyroL3gHandler1; @@ -450,11 +466,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo #endif // Gyro 2 Side B spiCookie = - new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, ADIS1650X::MAXIMUM_REPLY_SIZE, + new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, adis1650x::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - adisHandler = new GyroADIS1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_MAIN_COM_IF, - spiCookie, ADIS1650X::Type::ADIS16505); + adisHandler = + new GyrAdis1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, adis1650x::Type::ADIS16505); fdir = new AcsBoardFdir(objects::GYRO_2_ADIS_HANDLER); adisHandler->setCustomFdir(fdir); assemblyChildren[6] = adisHandler; @@ -463,11 +480,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo adisHandler->setToGoToNormalModeImmediately(); #endif // Gyro 3 Side B - spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, L3GD20H::MAX_BUFFER_SIZE, + spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, l3gd20h::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto gyroL3gHandler3 = new GyroHandlerL3GD20H( - objects::GYRO_3_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY); + auto gyroL3gHandler3 = + new GyrL3gCustomHandler(objects::GYRO_3_L3G_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, spi::L3G_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::GYRO_3_L3G_HANDLER); gyroL3gHandler3->setCustomFdir(fdir); assemblyChildren[7] = gyroL3gHandler3; @@ -570,12 +588,14 @@ void ObjectFactory::createSyrlinksComponents(PowerSwitchIF* pwrSwitcher) { syrlinks::MAX_REPLY_SIZE, UartModes::NON_CANONICAL); syrlinksUartCookie->setParityEven(); + auto* syrlinksAssy = new SyrlinksAssembly(objects::SYRLINKS_ASSY); + syrlinksAssy->connectModeTreeParent(satsystem::com::SUBSYSTEM); auto syrlinksFdir = new SyrlinksFdir(objects::SYRLINKS_HANDLER); auto syrlinksHandler = new SyrlinksHandler(objects::SYRLINKS_HANDLER, objects::UART_COM_IF, syrlinksUartCookie, pcdu::PDU1_CH1_SYRLINKS_12V, syrlinksFdir); syrlinksHandler->setPowerSwitcher(pwrSwitcher); - syrlinksHandler->connectModeTreeParent(satsystem::com::SUBSYSTEM); + syrlinksHandler->connectModeTreeParent(*syrlinksAssy); #if OBSW_DEBUG_SYRLINKS == 1 syrlinksHandler->setDebugMode(true); #endif @@ -699,55 +719,41 @@ void ObjectFactory::createReactionWheelComponents(LinuxLibgpioIF* gpioComIF, #endif /* OBSW_ADD_RW == 1 */ } -ReturnValue_t ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF, - CcsdsIpCoreHandler** ipCoreHandler) { +ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) { using namespace gpio; // GPIO definitions of signals connected to the virtual channel interfaces of the PTME IP Core GpioCookie* gpioCookiePtmeIp = new GpioCookie; GpiodRegularByLineName* gpio = nullptr; - std::stringstream consumer; - consumer.str("PAPB VC0"); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC0, consumer.str()); + gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC0, "PAPB VC0"); gpioCookiePtmeIp->addGpio(gpioIds::VC0_PAPB_BUSY, gpio); - consumer.str("PAPB VC0"); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC0, consumer.str()); + gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC0, "PAPB VC0"); gpioCookiePtmeIp->addGpio(gpioIds::VC0_PAPB_EMPTY, gpio); - consumer.str("PAPB VC 1"); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC1, consumer.str()); + gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC1, "PAPB VC1"); gpioCookiePtmeIp->addGpio(gpioIds::VC1_PAPB_BUSY, gpio); - consumer.str(""); - consumer.str("PAPB VC 1"); + gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC1, "PAPB VC1"); gpioCookiePtmeIp->addGpio(gpioIds::VC1_PAPB_EMPTY, gpio); - consumer.str(""); - consumer.str("PAPB VC 2"); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC2, consumer.str()); + gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC2, "PAPB VC2"); gpioCookiePtmeIp->addGpio(gpioIds::VC2_PAPB_BUSY, gpio); - consumer.str(""); - consumer.str("PAPB VC 2"); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC2, consumer.str()); + gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC2, "PAPB VC2"); gpioCookiePtmeIp->addGpio(gpioIds::VC2_PAPB_EMPTY, gpio); - consumer.str(""); - consumer.str("PAPB VC 3"); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC3, consumer.str()); + gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC3, "PAPB VC3"); gpioCookiePtmeIp->addGpio(gpioIds::VC3_PAPB_BUSY, gpio); - consumer.str(""); - consumer.str("PAPB VC 3"); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC3, consumer.str()); + gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC3, "PAPB VC3"); gpioCookiePtmeIp->addGpio(gpioIds::VC3_PAPB_EMPTY, gpio); - gpioChecker(gpioComIF->addGpios(gpioCookiePtmeIp), "PTME PAPB VCs"); + gpioChecker(args.gpioComIF.addGpios(gpioCookiePtmeIp), "PTME PAPB VCs"); // Creating virtual channel interfaces - VcInterfaceIF* vc0 = - new PapbVcInterface(gpioComIF, gpioIds::VC0_PAPB_BUSY, gpioIds::VC0_PAPB_EMPTY, q7s::UIO_PTME, - q7s::uiomapids::PTME_VC0); - VcInterfaceIF* vc1 = - new PapbVcInterface(gpioComIF, gpioIds::VC1_PAPB_BUSY, gpioIds::VC1_PAPB_EMPTY, q7s::UIO_PTME, - q7s::uiomapids::PTME_VC1); - VcInterfaceIF* vc2 = - new PapbVcInterface(gpioComIF, gpioIds::VC2_PAPB_BUSY, gpioIds::VC2_PAPB_EMPTY, q7s::UIO_PTME, - q7s::uiomapids::PTME_VC2); - VcInterfaceIF* vc3 = - new PapbVcInterface(gpioComIF, gpioIds::VC3_PAPB_BUSY, gpioIds::VC3_PAPB_EMPTY, q7s::UIO_PTME, - q7s::uiomapids::PTME_VC3); + VirtualChannelIF* vc0 = + new PapbVcInterface(&args.gpioComIF, gpioIds::VC0_PAPB_BUSY, gpioIds::VC0_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC0); + VirtualChannelIF* vc1 = + new PapbVcInterface(&args.gpioComIF, gpioIds::VC1_PAPB_BUSY, gpioIds::VC1_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC1); + VirtualChannelIF* vc2 = + new PapbVcInterface(&args.gpioComIF, gpioIds::VC2_PAPB_BUSY, gpioIds::VC2_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC2); + VirtualChannelIF* vc3 = + new PapbVcInterface(&args.gpioComIF, gpioIds::VC3_PAPB_BUSY, gpioIds::VC3_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC3); // Creating ptme object and adding virtual channel interfaces Ptme* ptme = new Ptme(objects::PTME); ptme->addVcInterface(ccsds::VC0, vc0); @@ -758,20 +764,38 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF, new AxiPtmeConfig(objects::AXI_PTME_CONFIG, q7s::UIO_PTME, q7s::uiomapids::PTME_CONFIG); PtmeConfig* ptmeConfig = new PtmeConfig(objects::PTME_CONFIG, axiPtmeConfig); - *ipCoreHandler = new CcsdsIpCoreHandler(objects::CCSDS_HANDLER, objects::PTME, - objects::CCSDS_PACKET_DISTRIBUTOR, ptmeConfig, gpioComIF, - gpioIds::RS485_EN_TX_CLOCK, gpioIds::RS485_EN_TX_DATA); - VirtualChannel* vc = nullptr; - vc = new VirtualChannel(ccsds::VC0, config::VC0_QUEUE_SIZE, objects::CCSDS_HANDLER); - (*ipCoreHandler)->addVirtualChannel(ccsds::VC0, vc); - vc = new VirtualChannel(ccsds::VC1, config::VC1_QUEUE_SIZE, objects::CCSDS_HANDLER); - (*ipCoreHandler)->addVirtualChannel(ccsds::VC1, vc); - vc = new VirtualChannel(ccsds::VC2, config::VC2_QUEUE_SIZE, objects::CCSDS_HANDLER); - (*ipCoreHandler)->addVirtualChannel(ccsds::VC2, vc); - vc = new VirtualChannel(ccsds::VC3, config::VC3_QUEUE_SIZE, objects::CCSDS_HANDLER); - (*ipCoreHandler)->addVirtualChannel(ccsds::VC3, vc); + *args.ipCoreHandler = new CcsdsIpCoreHandler( + objects::CCSDS_HANDLER, objects::CCSDS_PACKET_DISTRIBUTOR, *ptmeConfig, LINK_STATE, + &args.gpioComIF, gpioIds::RS485_EN_TX_CLOCK, gpioIds::RS485_EN_TX_DATA); + // This VC will receive all live TM + auto* vcWithQueue = + new VirtualChannelWithQueue(objects::PTME_VC0_LIVE_TM, ccsds::VC0, "PTME VC0 LIVE TM", *ptme, + LINK_STATE, args.tmStore, 500); + args.liveDestination = vcWithQueue; + new LiveTmTask(objects::LIVE_TM_TASK, args.pusFunnel, args.cfdpFunnel, *vcWithQueue); - ReturnValue_t result = (*ipCoreHandler)->connectModeTreeParent(satsystem::com::SUBSYSTEM); + // Set up log store. + auto* vc = new VirtualChannel(objects::PTME_VC1_LOG_TM, ccsds::VC1, "PTME VC1 LOG TM", *ptme, + LINK_STATE); + LogStores logStores(args.stores); + // Core task which handles the LOG store and takes care of dumping it as TM using a VC directly + new PersistentLogTmStoreTask(objects::LOG_STORE_AND_TM_TASK, args.ipcStore, logStores, *vc, + *SdCardManager::instance()); + + vc = new VirtualChannel(objects::PTME_VC2_HK_TM, ccsds::VC2, "PTME VC2 HK TM", *ptme, LINK_STATE); + // Core task which handles the HK store and takes care of dumping it as TM using a VC directly + new PersistentSingleTmStoreTask(objects::HK_STORE_AND_TM_TASK, args.ipcStore, + *args.stores.hkStore, *vc, persTmStore::DUMP_HK_STORE_DONE, + *SdCardManager::instance()); + + vc = new VirtualChannel(objects::PTME_VC3_CFDP_TM, ccsds::VC3, "PTME VC3 CFDP TM", *ptme, + LINK_STATE); + // Core task which handles the CFDP store and takes care of dumping it as TM using a VC directly + new PersistentSingleTmStoreTask(objects::CFDP_STORE_AND_TM_TASK, args.ipcStore, + *args.stores.cfdpStore, *vc, persTmStore::DUMP_CFDP_STORE_DONE, + *SdCardManager::instance()); + + ReturnValue_t result = (*args.ipCoreHandler)->connectModeTreeParent(satsystem::com::SUBSYSTEM); if (result != returnvalue::OK) { sif::error << "ObjectFactory::createCcsdsComponents: Connecting COM subsystem to CCSDS handler failed" @@ -779,20 +803,18 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF, } GpioCookie* gpioCookiePdec = new GpioCookie; - consumer.str(""); - consumer << "0x" << std::hex << objects::PDEC_HANDLER; // GPIO also low after linux boot (specified by device-tree) - gpio = new GpiodRegularByLineName(q7s::gpioNames::PDEC_RESET, consumer.str(), Direction::OUT, + gpio = new GpiodRegularByLineName(q7s::gpioNames::PDEC_RESET, "PDEC Handler", Direction::OUT, Levels::LOW); gpioCookiePdec->addGpio(gpioIds::PDEC_RESET, gpio); - gpioChecker(gpioComIF->addGpios(gpioCookiePdec), "PDEC"); + gpioChecker(args.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, - uioNames); + new PdecHandler(objects::PDEC_HANDLER, objects::CCSDS_HANDLER, &args.gpioComIF, + gpioIds::PDEC_RESET, uioNames); GpioCookie* gpioRS485Chip = new GpioCookie; gpio = new GpiodRegularByLineName(q7s::gpioNames::RS485_EN_TX_CLOCK, "RS485 Transceiver", Direction::OUT, Levels::LOW); @@ -807,7 +829,7 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF, gpio = new GpiodRegularByLineName(q7s::gpioNames::RS485_EN_RX_DATA, "RS485 Transceiver", Direction::OUT, Levels::LOW); gpioRS485Chip->addGpio(gpioIds::RS485_EN_RX_DATA, gpio); - gpioChecker(gpioComIF->addGpios(gpioRS485Chip), "RS485 Transceiver"); + gpioChecker(args.gpioComIF.addGpios(gpioRS485Chip), "RS485 Transceiver"); return returnvalue::OK; } @@ -890,26 +912,47 @@ void ObjectFactory::createTestComponents(LinuxLibgpioIF* gpioComIF) { } void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher) { + auto* strAssy = new StrAssembly(objects::STR_ASSY); + strAssy->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); auto* starTrackerCookie = new SerialCookie(objects::STAR_TRACKER, q7s::UART_STAR_TRACKER_DEV, uart::STAR_TRACKER_BAUD, startracker::MAX_FRAME_SIZE * 2 + 2, UartModes::NON_CANONICAL); starTrackerCookie->setNoFixedSizeReply(); StrHelper* strHelper = new StrHelper(objects::STR_HELPER); + + const char* paramJsonFile = nullptr; +#ifdef EGSE + paramJsonFile = "/home/pi/arcsec/json/flight-config.json"; +#else +#if OBSW_STAR_TRACKER_GROUND_CONFIG == 1 + paramJsonFile = "/mnt/sd0/startracker/ground-config.json"; +#else + paramJsonFile = "/mnt/sd0/startracker/flight-config.json"; +#endif +#endif + if (paramJsonFile == nullptr) { + sif::error << "No valid Star Tracker parameter JSON file" << std::endl; + } + auto strFdir = new StrFdir(objects::STAR_TRACKER); auto starTracker = new StarTrackerHandler(objects::STAR_TRACKER, objects::UART_COM_IF, starTrackerCookie, - strHelper, pcdu::PDU1_CH2_STAR_TRACKER_5V); + paramJsonFile, strHelper, pcdu::PDU1_CH2_STAR_TRACKER_5V); starTracker->setPowerSwitcher(pwrSwitcher); - starTracker->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); + starTracker->connectModeTreeParent(*strAssy); + starTracker->setCustomFdir(strFdir); } void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher) { + auto* imtqAssy = new ImtqAssembly(objects::IMTQ_ASSY); + imtqAssy->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); + new ImtqPollingTask(objects::IMTQ_POLLING); I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, imtq::MAX_REPLY_SIZE, q7s::I2C_PL_EIVE); auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::IMTQ_POLLING, imtqI2cCookie, pcdu::Switches::PDU1_CH3_MGT_5V); imtqHandler->enableThermalModule(ThermalStateCfg()); imtqHandler->setPowerSwitcher(pwrSwitcher); - imtqHandler->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); + imtqHandler->connectModeTreeParent(*imtqAssy); 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 2118e3d7..7b713005 100644 --- a/bsp_q7s/core/ObjectFactory.h +++ b/bsp_q7s/core/ObjectFactory.h @@ -2,10 +2,13 @@ #define BSP_Q7S_OBJECTFACTORY_H_ #include +#include +#include #include #include #include #include +#include #include #include @@ -21,6 +24,27 @@ class GpioIF; namespace ObjectFactory { +struct CcsdsComponentArgs { + CcsdsComponentArgs(LinuxLibgpioIF& gpioIF, StorageManagerIF& ipcStore, StorageManagerIF& tmStore, + PersistentTmStores& stores, PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel, + CcsdsIpCoreHandler** ipCoreHandler) + : gpioComIF(gpioIF), + ipcStore(ipcStore), + tmStore(tmStore), + stores(stores), + pusFunnel(pusFunnel), + cfdpFunnel(cfdpFunnel), + ipCoreHandler(ipCoreHandler) {} + LinuxLibgpioIF& gpioComIF; + StorageManagerIF& ipcStore; + StorageManagerIF& tmStore; + PersistentTmStores& stores; + PusTmFunnel& pusFunnel; + CfdpTmFunnel& cfdpFunnel; + CcsdsIpCoreHandler** ipCoreHandler; + AcceptsTelemetryIF* liveDestination = nullptr; +}; + void setStatics(); void produce(void* args); @@ -31,7 +55,7 @@ void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF, PowerSwitchIF* pwrSwitcher, Stack5VHandler& stackHandler); void createTmpComponents(); ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler); -void createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, +void createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, PowerSwitchIF& pwrSwitcher); void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTableIF* healthTable, HeaterHandler*& heaterHandler); @@ -42,7 +66,7 @@ void createSolarArrayDeploymentComponents(PowerSwitchIF& pwrSwitcher, GpioIF& gp void createSyrlinksComponents(PowerSwitchIF* pwrSwitcher); void createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF& pwrSwitcher); void createReactionWheelComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF* pwrSwitcher); -ReturnValue_t createCcsdsComponents(LinuxLibgpioIF* gpioComIF, CcsdsIpCoreHandler** ipCoreHandler); +ReturnValue_t createCcsdsComponents(CcsdsComponentArgs& args); void createMiscComponents(); void createTestComponents(LinuxLibgpioIF* gpioComIF); diff --git a/bsp_q7s/core/WatchdogHandler.cpp b/bsp_q7s/core/WatchdogHandler.cpp new file mode 100644 index 00000000..9e3a1020 --- /dev/null +++ b/bsp_q7s/core/WatchdogHandler.cpp @@ -0,0 +1,87 @@ +#include "WatchdogHandler.h" + +#include +#include + +#include +#include +#include + +#include "fsfw/serviceinterface.h" +#include "watchdog/definitions.h" + +WatchdogHandler::WatchdogHandler() {} + +void WatchdogHandler::periodicOperation() { + if (watchdogFifoFd != 0) { + if (watchdogFifoFd == RETRY_FIFO_OPEN) { + // Open FIFO write only and non-blocking + watchdogFifoFd = open(watchdog::FIFO_NAME.c_str(), O_WRONLY | O_NONBLOCK); + if (watchdogFifoFd < 0) { + if (errno == ENXIO) { + watchdogFifoFd = RETRY_FIFO_OPEN; + // No printout for now, would be spam + return; + } else { + sif::error << "Opening pipe " << watchdog::FIFO_NAME << " write-only failed with " + << errno << ": " << strerror(errno) << std::endl; + return; + } + } + sif::info << "Opened " << watchdog::FIFO_NAME << " successfully" << std::endl; + performStartHandling(); + } else if (watchdogFifoFd > 0) { + // Write to OBSW watchdog FIFO here + const char writeChar = watchdog::first::IDLE_CHAR; + ssize_t writtenBytes = write(watchdogFifoFd, &writeChar, 1); + if (writtenBytes < 0) { + sif::error << "Errors writing to watchdog FIFO, code " << errno << ": " << strerror(errno) + << std::endl; + } + } + } +} + +ReturnValue_t WatchdogHandler::initialize(bool enableWatchdogFunction) { + using namespace std::filesystem; + this->enableWatchFunction = enableWatchdogFunction; + std::error_code e; + if (not std::filesystem::exists(watchdog::FIFO_NAME, e)) { + // Still return returnvalue::OK for now + sif::info << "Watchdog FIFO " << watchdog::FIFO_NAME << " does not exist, can't initiate" + << " watchdog" << std::endl; + return returnvalue::OK; + } + // Open FIFO write only and non-blocking to prevent SW from killing itself. + watchdogFifoFd = open(watchdog::FIFO_NAME.c_str(), O_WRONLY | O_NONBLOCK); + if (watchdogFifoFd < 0) { + if (errno == ENXIO) { + watchdogFifoFd = RETRY_FIFO_OPEN; + sif::info << "eive-watchdog not running. FIFO can not be opened" << std::endl; + } else { + sif::error << "Opening pipe " << watchdog::FIFO_NAME << " write-only failed with " << errno + << ": " << strerror(errno) << std::endl; + return returnvalue::FAILED; + } + } + return performStartHandling(); +} + +ReturnValue_t WatchdogHandler::performStartHandling() { + char startBuf[2]; + ssize_t writeLen = 1; + startBuf[0] = watchdog::first::START_CHAR; + if (enableWatchFunction) { + writeLen += 1; + startBuf[1] = watchdog::second::WATCH_FLAG; + } + ssize_t writtenBytes = write(watchdogFifoFd, &startBuf, writeLen); + if (writtenBytes < 0) { + sif::error << "WatchdogHandler: Errors writing to watchdog FIFO, code " << errno << ": " + << strerror(errno) << std::endl; + return returnvalue::FAILED; + } else if (writtenBytes != writeLen) { + sif::warning << "WatchdogHandler: Not all bytes were written, possible error" << std::endl; + } + return returnvalue::OK; +} diff --git a/bsp_q7s/core/WatchdogHandler.h b/bsp_q7s/core/WatchdogHandler.h new file mode 100644 index 00000000..5db42286 --- /dev/null +++ b/bsp_q7s/core/WatchdogHandler.h @@ -0,0 +1,23 @@ +#ifndef BSP_Q7S_CORE_WATCHDOGHANDLER_H_ +#define BSP_Q7S_CORE_WATCHDOGHANDLER_H_ + +#include "fsfw/returnvalues/returnvalue.h" + +class WatchdogHandler { + public: + WatchdogHandler(); + + ReturnValue_t initialize(bool enableWatchFunction); + void periodicOperation(); + + private: + // Designated value for rechecking FIFO open + static constexpr int RETRY_FIFO_OPEN = -2; + + int watchdogFifoFd = 0; + bool enableWatchFunction = false; + + ReturnValue_t performStartHandling(); +}; + +#endif /* BSP_Q7S_CORE_WATCHDOGHANDLER_H_ */ diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index 7647af8b..99bb90d2 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -116,10 +116,6 @@ void scheduling::initTasks() { if (result != returnvalue::OK) { scheduling::printAddObjectError("CFDP_DISTRIBUTOR", objects::CFDP_DISTRIBUTOR); } - result = tmTcDistributor->addComponent(objects::TM_FUNNEL); - if (result != returnvalue::OK) { - scheduling::printAddObjectError("TM_FUNNEL", objects::TM_FUNNEL); - } #if OBSW_ADD_TCPIP_SERVERS == 1 #if OBSW_ADD_TMTC_UDP_SERVER == 1 @@ -150,6 +146,10 @@ void scheduling::initTasks() { if (result != returnvalue::OK) { scheduling::printAddObjectError("COM_SUBSYSTEM", objects::COM_SUBSYSTEM); } + result = genericSysTask->addComponent(objects::SYRLINKS_ASSY); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("SYRLINKS_ASSY", objects::SYRLINKS_ASSY); + } result = genericSysTask->addComponent(objects::PL_SUBSYSTEM); if (result != returnvalue::OK) { scheduling::printAddObjectError("PL_SUBSYSTEM", objects::PL_SUBSYSTEM); @@ -176,7 +176,33 @@ void scheduling::initTasks() { if (result != returnvalue::OK) { scheduling::printAddObjectError("PDEC Handler", objects::PDEC_HANDLER); } + #endif /* OBSW_ADD_CCSDS_IP_CORE == 1 */ + // All the TM store tasks run in permanent loops, frequency does not matter + PeriodicTaskIF* liveTmTask = + factory->createPeriodicTask("LIVE_TM", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr); + result = liveTmTask->addComponent(objects::LIVE_TM_TASK); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("LIVE_TM", objects::LIVE_TM_TASK); + } + PeriodicTaskIF* logTmTask = factory->createPeriodicTask( + "LOG_STORE_AND_TM", 45, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr); + result = logTmTask->addComponent(objects::LOG_STORE_AND_TM_TASK); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("LOG_STORE_AND_TM", objects::LOG_STORE_AND_TM_TASK); + } + PeriodicTaskIF* hkTmTask = factory->createPeriodicTask( + "HK_STORE_AND_TM", 45, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr); + result = hkTmTask->addComponent(objects::HK_STORE_AND_TM_TASK); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("HK_STORE_AND_TM", objects::HK_STORE_AND_TM_TASK); + } + PeriodicTaskIF* cfdpTmTask = factory->createPeriodicTask( + "CFDP_STORE_AND_TM", 45, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr); + result = cfdpTmTask->addComponent(objects::CFDP_STORE_AND_TM_TASK); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("CFDP_STORE_AND_TM", objects::CFDP_STORE_AND_TM_TASK); + } #if OBSW_ADD_CFDP_COMPONENTS == 1 PeriodicTaskIF* cfdpTask = factory->createPeriodicTask( @@ -194,6 +220,15 @@ void scheduling::initTasks() { scheduling::printAddObjectError("GPS_CTRL", objects::GPS_CONTROLLER); } +#if OBSW_ADD_ACS_BOARD == 1 + PeriodicTaskIF* acsBrdPolling = factory->createPeriodicTask( + "ACS_BOARD_POLLING", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); + result = acsBrdPolling->addComponent(objects::ACS_BOARD_POLLING_TASK); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("ACS_BOARD_POLLING", objects::ACS_BOARD_POLLING_TASK); + } +#endif + #if OBSW_ADD_RW == 1 PeriodicTaskIF* rwPolling = factory->createPeriodicTask( "RW_POLLING_TASK", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); @@ -211,31 +246,41 @@ void scheduling::initTasks() { } #endif +#if OBSW_ADD_SUN_SENSORS == 1 + PeriodicTaskIF* susPolling = factory->createPeriodicTask( + "SUS_POLLING_TASK", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); + result = susPolling->addComponent(objects::SUS_POLLING_TASK); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("SUS_POLLING_TASK", objects::SUS_POLLING_TASK); + } +#endif + PeriodicTaskIF* acsSysTask = factory->createPeriodicTask( "ACS_SYS_TASK", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); - static_cast(acsSysTask); result = acsSysTask->addComponent(objects::ACS_SUBSYSTEM); if (result != returnvalue::OK) { scheduling::printAddObjectError("ACS_SUBSYSTEM", objects::ACS_SUBSYSTEM); } -#if OBSW_ADD_ACS_BOARD == 1 + result = acsSysTask->addComponent(objects::IMTQ_ASSY); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("IMTQ_ASSY", objects::IMTQ_ASSY); + } result = acsSysTask->addComponent(objects::ACS_BOARD_ASS); if (result != returnvalue::OK) { scheduling::printAddObjectError("ACS_BOARD_ASS", objects::ACS_BOARD_ASS); } -#endif /* OBSW_ADD_ACS_HANDLERS */ -#if OBSW_ADD_RW == 1 - result = acsSysTask->addComponent(objects::RW_ASS); + result = acsSysTask->addComponent(objects::RW_ASSY); if (result != returnvalue::OK) { - scheduling::printAddObjectError("RW_ASS", objects::RW_ASS); + scheduling::printAddObjectError("RW_ASS", objects::RW_ASSY); } -#endif -#if OBSW_ADD_SUS_BOARD_ASS == 1 result = acsSysTask->addComponent(objects::SUS_BOARD_ASS); if (result != returnvalue::OK) { scheduling::printAddObjectError("SUS_BOARD_ASS", objects::SUS_BOARD_ASS); } -#endif + result = acsSysTask->addComponent(objects::STR_ASSY); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("STR_ASSY", objects::STR_ASSY); + } PeriodicTaskIF* tcsSystemTask = factory->createPeriodicTask( "TCS_TASK", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.5, missedDeadlineFunc); @@ -346,14 +391,24 @@ void scheduling::initTasks() { #if OBSW_ADD_CCSDS_IP_CORES == 1 pdecHandlerTask->startTask(); #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ + liveTmTask->startTask(); + logTmTask->startTask(); + hkTmTask->startTask(); + cfdpTmTask->startTask(); coreCtrlTask->startTask(); #if OBSW_ADD_SA_DEPL == 1 solarArrayDeplTask->startTask(); #endif +#if OBSW_ADD_ACS_BOARD == 1 + acsBrdPolling->startTask(); +#endif #if OBSW_ADD_MGT == 1 imtqPolling->startTask(); #endif +#if OBSW_ADD_SUN_SENSORS == 1 + susPolling->startTask(); +#endif taskStarter(pstTasks, "PST task vector"); taskStarter(pusTasks, "PUS task vector"); @@ -493,6 +548,10 @@ void scheduling::createPusTasks(TaskFactory& factory, TaskDeadlineMissedFunction if (result != returnvalue::OK) { scheduling::printAddObjectError("PUS_8", objects::PUS_SERVICE_8_FUNCTION_MGMT); } + result = pusMedPrio->addComponent(objects::PUS_SERVICE_15_TM_STORAGE); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("PUS_15", objects::PUS_SERVICE_15_TM_STORAGE); + } result = pusMedPrio->addComponent(objects::PUS_SERVICE_11_TC_SCHEDULER); if (result != returnvalue::OK) { scheduling::printAddObjectError("PUS_11", objects::PUS_SERVICE_11_TC_SCHEDULER); diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 36e9df64..34a7c9dc 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -1,4 +1,6 @@ #include +#include +#include #include #include "OBSWConfig.h" @@ -12,14 +14,19 @@ #include "linux/callbacks/gpioCallbacks.h" #include "mission/core/GenericFactory.h" #include "mission/system/tree/system.h" +#include "mission/tmtc/tmFilters.h" void ObjectFactory::produce(void* args) { ObjectFactory::setStatics(); HealthTableIF* healthTable = nullptr; PusTmFunnel* pusFunnel = nullptr; CfdpTmFunnel* cfdpFunnel = nullptr; + StorageManagerIF* ipcStore = nullptr; + StorageManagerIF* tmStore = nullptr; + + PersistentTmStores stores; ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel, - *SdCardManager::instance()); + *SdCardManager::instance(), &ipcStore, &tmStore, stores); LinuxLibgpioIF* gpioComIF = nullptr; SerialComIF* uartComIF = nullptr; @@ -43,7 +50,7 @@ void ObjectFactory::produce(void* args) { #endif #if OBSW_ADD_ACS_BOARD == 1 - createAcsBoardComponents(gpioComIF, uartComIF, *pwrSwitcher); + createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher); #endif HeaterHandler* heaterHandler; createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler); @@ -72,9 +79,13 @@ void ObjectFactory::produce(void* args) { #endif /* OBSW_ADD_STAR_TRACKER == 1 */ #if OBSW_ADD_CCSDS_IP_CORES == 1 CcsdsIpCoreHandler* ipCoreHandler = nullptr; - createCcsdsComponents(gpioComIF, &ipCoreHandler); + CcsdsComponentArgs ccsdsArgs(*gpioComIF, *ipcStore, *tmStore, stores, *pusFunnel, *cfdpFunnel, + &ipCoreHandler); + createCcsdsComponents(ccsdsArgs); #if OBSW_TM_TO_PTME == 1 - addTmtcIpCoresToFunnels(*ipCoreHandler, *pusFunnel, *cfdpFunnel); + if (ccsdsArgs.liveDestination != nullptr) { + pusFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); + } #endif #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ diff --git a/bsp_q7s/fs/SdCardManager.cpp b/bsp_q7s/fs/SdCardManager.cpp index 9e5d05bc..e954e7bb 100644 --- a/bsp_q7s/fs/SdCardManager.cpp +++ b/bsp_q7s/fs/SdCardManager.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -20,16 +21,16 @@ SdCardManager* SdCardManager::INSTANCE = nullptr; SdCardManager::SdCardManager() : SystemObject(objects::SDC_MANAGER), cmdExecutor(256) { - mutex = MutexFactory::instance()->createMutex(); - ReturnValue_t result = mutex->lockMutex(); - if (result != returnvalue::OK) { + sdLock = MutexFactory::instance()->createMutex(); + prefLock = MutexFactory::instance()->createMutex(); + defaultLock = MutexFactory::instance()->createMutex(); + + MutexGuard mg(prefLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX); + if (mg.getLockResult() != returnvalue::OK) { sif::error << "SdCardManager::SdCardManager: Mutex lock failed" << std::endl; } uint8_t prefSdRaw = 0; - result = scratch::readNumber(scratch::PREFERED_SDC_KEY, prefSdRaw); - if (mutex->unlockMutex() != returnvalue::OK) { - sif::error << "SdCardManager::SdCardManager: Mutex unlock failed" << std::endl; - } + ReturnValue_t result = scratch::readNumber(scratch::PREFERED_SDC_KEY, prefSdRaw); if (result != returnvalue::OK) { if (result == scratch::KEY_NOT_FOUND) { @@ -37,14 +38,16 @@ SdCardManager::SdCardManager() : SystemObject(objects::SDC_MANAGER), cmdExecutor "Preferred SD card not set. Setting to 0" << std::endl; setPreferredSdCard(sd::SdCard::SLOT_0); - sdInfo.pref = sd::SdCard::SLOT_0; + scratch::writeNumber(scratch::PREFERED_SDC_KEY, static_cast(sd::SdCard::SLOT_0)); + prefSdRaw = sd::SdCard::SLOT_0; + } else { // Should not happen. // TODO: Maybe trigger event? sif::error << "SdCardManager::SdCardManager: Reading preferred SD card from scratch" "buffer failed" << std::endl; - sdInfo.pref = sd::SdCard::SLOT_0; + prefSdRaw = sd::SdCard::SLOT_0; } } sdInfo.pref = static_cast(prefSdRaw); @@ -193,28 +196,9 @@ ReturnValue_t SdCardManager::setSdCardState(sd::SdCard sdCard, bool on) { return result; } -ReturnValue_t SdCardManager::getSdCardsStatus(SdStatePair& active) { - using namespace std; - MutexGuard mg(mutex); - if (not filesystem::exists(SD_STATE_FILE)) { - return STATUS_FILE_NEXISTS; - } - - // Now the file should exist in any case. Still check whether it exists. - fstream sdStatus(SD_STATE_FILE); - if (not sdStatus.good()) { - return STATUS_FILE_NEXISTS; - } - string line; - uint8_t idx = 0; - sd::SdCard currentSd = sd::SdCard::SLOT_0; - // Process status file line by line - while (std::getline(sdStatus, line)) { - processSdStatusLine(active, line, idx, currentSd); - } - if (active.first != sd::SdState::MOUNTED && active.second != sd::SdState::MOUNTED) { - sdCardActive = false; - } +ReturnValue_t SdCardManager::getSdCardsStatus(SdStatePair& sdStates) { + MutexGuard mg(sdLock, LOCK_TYPE, SD_LOCK_TIMEOUT, LOCK_CTX); + sdStates = this->sdStates; return returnvalue::OK; } @@ -237,7 +221,8 @@ ReturnValue_t SdCardManager::mountSdCard(sd::SdCard sdCard) { mountDev = SD_1_DEV_NAME; mountPoint = config::SD_1_MOUNT_POINT; } - if (not filesystem::exists(mountDev)) { + std::error_code e; + if (not filesystem::exists(mountDev, e)) { sif::warning << "SdCardManager::mountSdCard: Device file does not exists. Make sure to" " turn on the SD card" << std::endl; @@ -272,7 +257,8 @@ ReturnValue_t SdCardManager::unmountSdCard(sd::SdCard sdCard) { } else if (sdCard == sd::SdCard::SLOT_1) { mountPoint = config::SD_1_MOUNT_POINT; } - if (not filesystem::exists(mountPoint)) { + std::error_code e; + if (not filesystem::exists(mountPoint, e)) { sif::error << "SdCardManager::unmountSdCard: Default mount point " << mountPoint << "does not exist" << std::endl; return UNMOUNT_ERROR; @@ -304,10 +290,9 @@ ReturnValue_t SdCardManager::sanitizeState(SdStatePair* statusPair, sd::SdCard p resetNonBlockingState = true; } if (statusPair == nullptr) { - sdStatusPtr = std::make_unique(); - statusPair = sdStatusPtr.get(); - getSdCardsStatus(*statusPair); + return returnvalue::FAILED; } + getSdCardsStatus(*statusPair); if (statusPair->first == sd::SdState::ON) { result = mountSdCard(prefSdCard); @@ -325,8 +310,34 @@ void SdCardManager::resetState() { currentOp = Operations::IDLE; } -void SdCardManager::processSdStatusLine(std::pair& active, - std::string& line, uint8_t& idx, sd::SdCard& currentSd) { +ReturnValue_t SdCardManager::updateSdStatePair() { + using namespace std; + + MutexGuard mg(sdLock, LOCK_TYPE, SD_LOCK_TIMEOUT, LOCK_CTX); + std::error_code e; + if (not filesystem::exists(SD_STATE_FILE, e)) { + return STATUS_FILE_NEXISTS; + } + + // Now the file should exist in any case. Still check whether it exists. + fstream sdStatus(SD_STATE_FILE); + if (not sdStatus.good()) { + return STATUS_FILE_NEXISTS; + } + string line; + uint8_t idx = 0; + sd::SdCard currentSd = sd::SdCard::SLOT_0; + // Process status file line by line + while (std::getline(sdStatus, line)) { + processSdStatusLine(line, idx, currentSd); + } + if (sdStates.first != sd::SdState::MOUNTED && sdStates.second != sd::SdState::MOUNTED) { + sdCardActive = false; + } + return returnvalue::OK; +} + +void SdCardManager::processSdStatusLine(std::string& line, uint8_t& idx, sd::SdCard& currentSd) { using namespace std; istringstream iss(line); string word; @@ -347,24 +358,24 @@ void SdCardManager::processSdStatusLine(std::pair& act if (word == "on") { if (currentSd == sd::SdCard::SLOT_0) { - active.first = sd::SdState::ON; + sdStates.first = sd::SdState::ON; } else { - active.second = sd::SdState::ON; + sdStates.second = sd::SdState::ON; } } else if (word == "off") { if (currentSd == sd::SdCard::SLOT_0) { - active.first = sd::SdState::OFF; + sdStates.first = sd::SdState::OFF; } else { - active.second = sd::SdState::OFF; + sdStates.second = sd::SdState::OFF; } } } if (mountLine) { if (currentSd == sd::SdCard::SLOT_0) { - active.first = sd::SdState::MOUNTED; + sdStates.first = sd::SdState::MOUNTED; } else { - active.second = sd::SdState::MOUNTED; + sdStates.second = sd::SdState::MOUNTED; } } @@ -378,7 +389,7 @@ void SdCardManager::processSdStatusLine(std::pair& act } std::optional SdCardManager::getPreferredSdCard() const { - MutexGuard mg(mutex); + MutexGuard mg(prefLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX); auto res = mg.getLockResult(); if (res != returnvalue::OK) { sif::error << "SdCardManager::getPreferredSdCard: Lock error" << std::endl; @@ -387,7 +398,7 @@ std::optional SdCardManager::getPreferredSdCard() const { } ReturnValue_t SdCardManager::setPreferredSdCard(sd::SdCard sdCard) { - MutexGuard mg(mutex); + MutexGuard mg(prefLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX); if (sdCard == sd::SdCard::BOTH) { return returnvalue::FAILED; } @@ -399,10 +410,10 @@ ReturnValue_t SdCardManager::updateSdCardStateFile() { if (cmdExecutor.getCurrentState() == CommandExecutor::States::PENDING) { return CommandExecutor::COMMAND_PENDING; } - MutexGuard mg(mutex); + MutexGuard mg(sdLock, LOCK_TYPE, SD_LOCK_TIMEOUT, LOCK_CTX); // Use q7hw utility and pipe the command output into the state file std::string updateCmd = "q7hw sd info all > " + std::string(SD_STATE_FILE); - cmdExecutor.load(updateCmd, blocking, printCmdOutput); + cmdExecutor.load(updateCmd, true, printCmdOutput); ReturnValue_t result = cmdExecutor.execute(); if (blocking and result != returnvalue::OK) { utility::handleSystemError(cmdExecutor.getLastError(), "SdCardManager::mountSdCard"); @@ -411,7 +422,7 @@ ReturnValue_t SdCardManager::updateSdCardStateFile() { } const char* SdCardManager::getCurrentMountPrefix() const { - MutexGuard mg(mutex); + MutexGuard mg(defaultLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX); if (currentPrefix.has_value()) { return currentPrefix.value().c_str(); } @@ -464,41 +475,35 @@ void SdCardManager::setPrintCommandOutput(bool print) { this->printCmdOutput = p bool SdCardManager::isSdCardUsable(std::optional sdCard) { { - MutexGuard mg(mutex); + MutexGuard mg(defaultLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX); if (markedUnusable) { return false; } } - SdCardManager::SdStatePair active; - ReturnValue_t result = this->getSdCardsStatus(active); - - if (result != returnvalue::OK) { - sif::debug << "SdCardManager::isSdCardMounted: Failed to get SD card active state"; - return false; - } + MutexGuard mg(sdLock, LOCK_TYPE, SD_LOCK_TIMEOUT, LOCK_CTX); if (not sdCard) { - if (active.first == sd::MOUNTED or active.second == sd::MOUNTED) { + if (sdStates.first == sd::MOUNTED or sdStates.second == sd::MOUNTED) { return true; } return false; } if (sdCard == sd::SLOT_0) { - if (active.first == sd::MOUNTED) { + if (sdStates.first == sd::MOUNTED) { return true; } else { return false; } } if (sdCard == sd::SLOT_1) { - if (active.second == sd::MOUNTED) { + if (sdStates.second == sd::MOUNTED) { return true; } else { return false; } } if (sdCard == sd::BOTH) { - if (active.first == sd::MOUNTED && active.second == sd::MOUNTED) { + if (sdStates.first == sd::MOUNTED && sdStates.second == sd::MOUNTED) { return true; } } @@ -508,9 +513,9 @@ bool SdCardManager::isSdCardUsable(std::optional sdCard) { ReturnValue_t SdCardManager::isSdCardMountedReadOnly(sd::SdCard sdcard, bool& readOnly) { std::ostringstream command; if (sdcard == sd::SdCard::SLOT_0) { - command << "grep -q '" << config::SD_0_MOUNT_POINT << " ext4 ro,' /proc/mounts"; + command << "grep -q '" << config::SD_0_MOUNT_POINT << " ext4 rw,' /proc/mounts"; } else if (sdcard == sd::SdCard::SLOT_1) { - command << "grep -q '" << config::SD_1_MOUNT_POINT << " ext4 ro,' /proc/mounts"; + command << "grep -q '" << config::SD_1_MOUNT_POINT << " ext4 rw,' /proc/mounts"; } else { return returnvalue::FAILED; } @@ -519,18 +524,9 @@ ReturnValue_t SdCardManager::isSdCardMountedReadOnly(sd::SdCard sdcard, bool& re return result; } result = cmdExecutor.execute(); - if (result != returnvalue::OK) { - int exitStatus = cmdExecutor.getLastError(); - if (exitStatus == 1) { - readOnly = false; - return returnvalue::OK; - } - return result; - } - auto& readVec = cmdExecutor.getReadVector(); - size_t readLen = strnlen(readVec.data(), readVec.size()); - if (readLen == 0) { + if (result == returnvalue::OK) { readOnly = false; + return result; } readOnly = true; return returnvalue::OK; @@ -569,7 +565,7 @@ ReturnValue_t SdCardManager::performFsck(sd::SdCard sdcard, bool printOutput, in } void SdCardManager::setActiveSdCard(sd::SdCard sdCard) { - MutexGuard mg(mutex); + MutexGuard mg(defaultLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX); sdInfo.active = sdCard; if (sdInfo.active == sd::SdCard::SLOT_0) { currentPrefix = config::SD_0_MOUNT_POINT; @@ -579,7 +575,7 @@ void SdCardManager::setActiveSdCard(sd::SdCard sdCard) { } std::optional SdCardManager::getActiveSdCard() const { - MutexGuard mg(mutex); + MutexGuard mg(defaultLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX); if (markedUnusable) { return std::nullopt; } @@ -587,6 +583,6 @@ std::optional SdCardManager::getActiveSdCard() const { } void SdCardManager::markUnusable() { - MutexGuard mg(mutex); + MutexGuard mg(defaultLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX); markedUnusable = true; } diff --git a/bsp_q7s/fs/SdCardManager.h b/bsp_q7s/fs/SdCardManager.h index 77055589..cee06894 100644 --- a/bsp_q7s/fs/SdCardManager.h +++ b/bsp_q7s/fs/SdCardManager.h @@ -25,7 +25,7 @@ class MutexIF; * state */ class SdCardManager : public SystemObject, public SdCardMountedIF { - friend class SdCardAccess; + friend class CoreController; public: using mountInitCb = ReturnValue_t (*)(void* args); @@ -218,19 +218,27 @@ class SdCardManager : public SystemObject, public SdCardMountedIF { private: CommandExecutor cmdExecutor; + SdStatePair sdStates; Operations currentOp = Operations::IDLE; bool blocking = false; bool sdCardActive = true; bool printCmdOutput = true; bool markedUnusable = false; - MutexIF* mutex = nullptr; + MutexIF* sdLock = nullptr; + MutexIF* prefLock = nullptr; + MutexIF* defaultLock = nullptr; + static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING; + static constexpr uint32_t SD_LOCK_TIMEOUT = 250; + static constexpr uint32_t OTHER_TIMEOUT = 20; + static constexpr char LOCK_CTX[] = "SdCardManager"; SdCardManager(); + ReturnValue_t updateSdStatePair(); + ReturnValue_t setSdCardState(sd::SdCard sdCard, bool on); - void processSdStatusLine(SdStatePair& active, std::string& line, uint8_t& idx, - sd::SdCard& currentSd); + void processSdStatusLine(std::string& line, uint8_t& idx, sd::SdCard& currentSd); std::optional currentPrefix; diff --git a/bsp_q7s/main.cpp b/bsp_q7s/main.cpp index 56327005..d557cdb9 100644 --- a/bsp_q7s/main.cpp +++ b/bsp_q7s/main.cpp @@ -12,10 +12,10 @@ * @brief This is the main program for the target hardware. * @return */ -int main(void) { +int main(int argc, char* argv[]) { using namespace std; #if Q7S_SIMPLE_MODE == 0 - return obsw::obsw(); + return obsw::obsw(argc, argv); #else return simple::simple(); #endif diff --git a/bsp_q7s/memory/LocalParameterHandler.h b/bsp_q7s/memory/LocalParameterHandler.h index a8b96950..74626202 100644 --- a/bsp_q7s/memory/LocalParameterHandler.h +++ b/bsp_q7s/memory/LocalParameterHandler.h @@ -1,7 +1,7 @@ #ifndef BSP_Q7S_MEMORY_LOCALPARAMETERHANDLER_H_ #define BSP_Q7S_MEMORY_LOCALPARAMETERHANDLER_H_ -#include +#include #include #include @@ -19,8 +19,8 @@ class LocalParameterHandler : public NVMParameterBase { * @brief Constructor * * @param sdRelativeName Absolute name of json file relative to mount - * directory of SD card. - * E.g. conf/example.json + * directory + * of SD card. E.g. conf/example.json * @param sdcMan Pointer to SD card manager */ LocalParameterHandler(std::string sdRelativeName, SdCardMountedIF* sdcMan); diff --git a/bsp_q7s/obsw.cpp b/bsp_q7s/obsw.cpp index cfd28565..e264283f 100644 --- a/bsp_q7s/obsw.cpp +++ b/bsp_q7s/obsw.cpp @@ -9,6 +9,7 @@ #include #include "OBSWConfig.h" +#include "bsp_q7s/core/WatchdogHandler.h" #include "commonConfig.h" #include "core/scheduling.h" #include "fsfw/tasks/TaskFactory.h" @@ -18,13 +19,16 @@ #include "q7sConfig.h" #include "watchdog/definitions.h" -static int OBSW_ALREADY_RUNNING = -2; +static constexpr int OBSW_ALREADY_RUNNING = -2; #if OBSW_Q7S_EM == 0 static const char* DEV_STRING = "Xiphos Q7S FM"; #else static const char* DEV_STRING = "Xiphos Q7S EM"; #endif -int obsw::obsw() { + +WatchdogHandler WATCHDOG_HANDLER; + +int obsw::obsw(int argc, char* argv[]) { using namespace fsfw; std::cout << "-- EIVE OBSW --" << std::endl; std::cout << "-- Compiled for Linux (" << DEV_STRING << ") --" << std::endl; @@ -33,9 +37,10 @@ int obsw::obsw() { std::cout << "-- " << __DATE__ << " " << __TIME__ << " --" << std::endl; #if Q7S_CHECK_FOR_ALREADY_RUNNING_IMG == 1 + std::error_code e; // Check special file here. This file is created or deleted by the eive-watchdog application // or systemd service! - if (std::filesystem::exists(watchdog::RUNNING_FILE_NAME)) { + if (std::filesystem::exists(watchdog::RUNNING_FILE_NAME, e)) { sif::warning << "File " << watchdog::RUNNING_FILE_NAME << " exists so the software might " "already be running. Check if obsw systemd service has been stopped." @@ -44,14 +49,45 @@ int obsw::obsw() { } #endif + // Delay the boot if applicable. + bootDelayHandling(); + + bool initWatchFunction = false; + std::string fullExecPath = argv[0]; + if (fullExecPath.find("/usr/bin") != std::string::npos) { + initWatchFunction = true; + } + ReturnValue_t result = WATCHDOG_HANDLER.initialize(initWatchFunction); + if (result != returnvalue::OK) { + std::cerr << "Initiating EIVE watchdog handler failed" << std::endl; + } + + scheduling::initMission(); + + // Command the EIVE system to safe mode +#if OBSW_COMMAND_SAFE_MODE_AT_STARTUP == 1 + commandEiveSystemToSafe(); +#else + announceAllModes(); +#endif + + for (;;) { + WATCHDOG_HANDLER.periodicOperation(); + TaskFactory::delayTask(2000); + } + return 0; +} + +void obsw::bootDelayHandling() { const char* homedir = nullptr; homedir = getenv("HOME"); if (homedir == nullptr) { homedir = getpwuid(getuid())->pw_dir; } std::filesystem::path bootDelayFile = std::filesystem::path(homedir) / "boot_delay_secs.txt"; + std::error_code e; // Init delay handling. - if (std::filesystem::exists(bootDelayFile)) { + if (std::filesystem::exists(bootDelayFile, e)) { std::ifstream ifile(bootDelayFile); std::string lineStr; unsigned int bootDelaySecs = 0; @@ -71,31 +107,26 @@ int obsw::obsw() { std::cout << "Delaying OBSW start for " << bootDelaySecs << " seconds" << std::endl; TaskFactory::delayTask(bootDelaySecs * 1000); } +} - scheduling::initMission(); - - // Command the EIVE system to safe mode +void obsw::commandEiveSystemToSafe() { auto sysQueueId = satsystem::EIVE_SYSTEM.getCommandQueue(); CommandMessage msg; -#if OBSW_COMMAND_SAFE_MODE_AT_STARTUP == 1 ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0); ReturnValue_t result = MessageQueueSenderIF::sendMessage(sysQueueId, &msg, MessageQueueIF::NO_QUEUE, false); if (result != returnvalue::OK) { sif::error << "Sending safe mode command to EIVE system failed" << std::endl; } -#else +} + +void obsw::announceAllModes() { + auto sysQueueId = satsystem::EIVE_SYSTEM.getCommandQueue(); + CommandMessage msg; ModeMessage::setModeAnnounceMessage(msg, true); ReturnValue_t result = MessageQueueSenderIF::sendMessage(sysQueueId, &msg, MessageQueueIF::NO_QUEUE, false); if (result != returnvalue::OK) { sif::error << "Sending safe mode command to EIVE system failed" << std::endl; } -#endif - - for (;;) { - /* Suspend main thread by sleeping it. */ - TaskFactory::delayTask(5000); - } - return 0; } diff --git a/bsp_q7s/obsw.h b/bsp_q7s/obsw.h index c2d974ae..1a6e4e05 100644 --- a/bsp_q7s/obsw.h +++ b/bsp_q7s/obsw.h @@ -3,8 +3,12 @@ namespace obsw { -int obsw(); +int obsw(int argc, char* argv[]); -}; +void bootDelayHandling(); +void commandEiveSystemToSafe(); +void announceAllModes(); + +}; // namespace obsw #endif /* BSP_Q7S_CORE_OBSW_H_ */ diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h index 4f2c948e..d6a1e757 100644 --- a/common/config/eive/definitions.h +++ b/common/config/eive/definitions.h @@ -45,26 +45,32 @@ static constexpr uint32_t SA_DEPL_MAX_BURN_TIME = 180; static constexpr uint32_t CCSDS_HANDLER_QUEUE_SIZE = 50; static constexpr uint8_t NUMBER_OF_VIRTUAL_CHANNELS = 4; -static constexpr uint8_t VC0_QUEUE_SIZE = 80; -static constexpr uint8_t VC1_QUEUE_SIZE = 80; -static constexpr uint8_t VC2_QUEUE_SIZE = 50; -static constexpr uint8_t VC3_QUEUE_SIZE = 50; +static constexpr uint32_t VC0_LIVE_TM_QUEUE_SIZE = 300; +// There are three individual log stores! +static constexpr uint32_t MISC_STORE_QUEUE_SIZE = 200; +static constexpr uint32_t OK_STORE_QUEUE_SIZE = 350; +static constexpr uint32_t NOK_STORE_QUEUE_SIZE = 350; +static constexpr uint32_t HK_STORE_QUEUE_SIZE = 300; +static constexpr uint32_t CFDP_STORE_QUEUE_SIZE = 300; static constexpr uint32_t MAX_PUS_FUNNEL_QUEUE_DEPTH = 100; +static constexpr uint32_t MAX_CFDP_FUNNEL_QUEUE_DEPTH = 80; static constexpr uint32_t MAX_STORED_CMDS_UDP = 120; static constexpr uint32_t MAX_STORED_CMDS_TCP = 150; -namespace acs { +namespace spiSched { static constexpr uint32_t SCHED_BLOCK_1_SUS_READ_MS = 15; static constexpr uint32_t SCHED_BLOCK_2_SENSOR_READ_MS = 30; -static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42; +static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 43; static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45; static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 55; -static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 95; +static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 105; static constexpr uint32_t SCHED_BLOCK_RTD = 150; static constexpr uint32_t SCHED_BLOCK_7_RW_READ_MS = 300; +static constexpr uint32_t SCHED_BLOCK_8_PLPCDU_MS = 320; +static constexpr uint32_t SCHED_BLOCK_9_RAD_SENS_MS = 340; // 15 ms for FM static constexpr float SCHED_BLOCK_1_PERIOD = static_cast(SCHED_BLOCK_1_SUS_READ_MS) / 400.0; @@ -76,10 +82,12 @@ static constexpr float SCHED_BLOCK_4_PERIOD = static_cast(SCHED_BLOCK_4_A static constexpr float SCHED_BLOCK_5_PERIOD = static_cast(SCHED_BLOCK_5_ACTUATOR_MS) / 400.0; static constexpr float SCHED_BLOCK_6_PERIOD = static_cast(SCHED_BLOCK_6_IMTQ_BLOCK_2_MS) / 400.0; -static constexpr float SCHED_BLOCK_7_PERIOD = static_cast(SCHED_BLOCK_7_RW_READ_MS) / 400.0; static constexpr float SCHED_BLOCK_RTD_PERIOD = static_cast(SCHED_BLOCK_RTD) / 400.0; +static constexpr float SCHED_BLOCK_7_PERIOD = static_cast(SCHED_BLOCK_7_RW_READ_MS) / 400.0; +static constexpr float SCHED_BLOCK_8_PERIOD = static_cast(SCHED_BLOCK_8_PLPCDU_MS) / 400.0; +static constexpr float SCHED_BLOCK_9_PERIOD = static_cast(SCHED_BLOCK_9_RAD_SENS_MS) / 400.0; -} // namespace acs +} // namespace spiSched } // namespace config diff --git a/common/config/eive/objects.h b/common/config/eive/objects.h index 08f07d3d..e22133e2 100644 --- a/common/config/eive/objects.h +++ b/common/config/eive/objects.h @@ -66,6 +66,10 @@ enum commonObjects : uint32_t { PLOC_MPSOC_HELPER = 0x44330003, AXI_PTME_CONFIG = 0x44330004, PTME_CONFIG = 0x44330005, + PTME_VC0_LIVE_TM = 0x44330006, + PTME_VC1_LOG_TM = 0x44330007, + PTME_VC2_HK_TM = 0x44330008, + PTME_VC3_CFDP_TM = 0x44330009, PLOC_MPSOC_HANDLER = 0x44330015, PLOC_SUPERVISOR_HANDLER = 0x44330016, PLOC_SUPERVISOR_HELPER = 0x44330017, @@ -123,7 +127,10 @@ enum commonObjects : uint32_t { // CCSDS_IP_CORE_BRIDGE = 0x73500000, /* 0x49 ('I') for Communication Interfaces */ - SPI_RTD_COM_IF = 0x49020006, + ACS_BOARD_POLLING_TASK = 0x49060004, + RW_POLLING_TASK = 0x49060005, + SPI_RTD_COM_IF = 0x49060006, + SUS_POLLING_TASK = 0x49060007, // 0x60 for other stuff HEATER_0_PLOC_PROC_BRD = 0x60000000, @@ -133,14 +140,17 @@ enum commonObjects : uint32_t { HEATER_4_CAMERA = 0x60000004, HEATER_5_STR = 0x60000005, HEATER_6_DRO = 0x60000006, - HEATER_7_HPA = 0x60000007, + HEATER_7_SYRLINKS = 0x60000007, // 0x73 ('s') for assemblies and system/subsystem components ACS_BOARD_ASS = 0x73000001, SUS_BOARD_ASS = 0x73000002, TCS_BOARD_ASS = 0x73000003, - RW_ASS = 0x73000004, + RW_ASSY = 0x73000004, CAM_SWITCHER = 0x73000006, + SYRLINKS_ASSY = 0x73000007, + IMTQ_ASSY = 0x73000008, + STR_ASSY = 0x73000009, EIVE_SYSTEM = 0x73010000, ACS_SUBSYSTEM = 0x73010001, PL_SUBSYSTEM = 0x73010002, @@ -158,6 +168,12 @@ enum commonObjects : uint32_t { HK_TM_STORE = 0x73020004, CFDP_TM_STORE = 0x73030000, + LIVE_TM_TASK = 0x73040000, + LOG_STORE_AND_TM_TASK = 0x73040001, + HK_STORE_AND_TM_TASK = 0x73040002, + CFDP_STORE_AND_TM_TASK = 0x73040003, + DOWNLINK_RAM_STORE = 0x73040004, + // Other stuff THERMAL_TEMP_INSERTER = 0x90000003, }; diff --git a/common/config/eive/resultClassIds.h b/common/config/eive/resultClassIds.h index f7bf26c1..9bad9ae3 100644 --- a/common/config/eive/resultClassIds.h +++ b/common/config/eive/resultClassIds.h @@ -40,8 +40,10 @@ enum commonClassIds : uint8_t { ACS_SAFE, // ACSSAF ACS_PTG, // ACSPTG ACS_DETUMBLE, // ACSDTB - SD_CARD_MANAGER, // SDMA - LOCAL_PARAM_HANDLER, // LPH + SD_CARD_MANAGER, // SDMA + LOCAL_PARAM_HANDLER, // LPH + PERSISTENT_TM_STORE, // PTM + TM_SINK, // TMS COMMON_CLASS_ID_END // [EXPORT] : [END] }; } diff --git a/dummies/CoreControllerDummy.h b/dummies/CoreControllerDummy.h index 1b4eeaf8..37d88081 100644 --- a/dummies/CoreControllerDummy.h +++ b/dummies/CoreControllerDummy.h @@ -1,7 +1,7 @@ #pragma once #include -#include +#include class CoreControllerDummy : public ExtendedControllerBase { public: diff --git a/dummies/GyroAdisDummy.cpp b/dummies/GyroAdisDummy.cpp index 050c838d..ed581138 100644 --- a/dummies/GyroAdisDummy.cpp +++ b/dummies/GyroAdisDummy.cpp @@ -1,6 +1,6 @@ #include "GyroAdisDummy.h" -#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" +#include GyroAdisDummy::GyroAdisDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie), dataset(this) {} @@ -40,13 +40,13 @@ uint32_t GyroAdisDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { r ReturnValue_t GyroAdisDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry({-0.5}, true)); - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry({0.2}, true)); - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry({-1.2}, true)); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_X, new PoolEntry({-0.5}, true)); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_Y, new PoolEntry({0.2}, true)); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_Z, new PoolEntry({-1.2}, true)); + localDataPoolMap.emplace(adis1650x::ACCELERATION_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::TEMPERATURE, new PoolEntry({0.0})); return returnvalue::OK; } diff --git a/dummies/GyroAdisDummy.h b/dummies/GyroAdisDummy.h index b3aad620..21de1eeb 100644 --- a/dummies/GyroAdisDummy.h +++ b/dummies/GyroAdisDummy.h @@ -2,8 +2,7 @@ #define DUMMIES_GYROADISDUMMY_H_ #include - -#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" +#include class GyroAdisDummy : public DeviceHandlerBase { public: diff --git a/dummies/GyroL3GD20Dummy.cpp b/dummies/GyroL3GD20Dummy.cpp index 20309c63..8c89f59d 100644 --- a/dummies/GyroL3GD20Dummy.cpp +++ b/dummies/GyroL3GD20Dummy.cpp @@ -1,6 +1,6 @@ #include "GyroL3GD20Dummy.h" -#include "fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h" +#include GyroL3GD20Dummy::GyroL3GD20Dummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie) {} @@ -40,9 +40,9 @@ uint32_t GyroL3GD20Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t GyroL3GD20Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry({1.2}, true)); - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry({-0.1}, true)); - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry({0.7}, true)); - localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry({1.2}, true)); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry({-0.1}, true)); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry({0.7}, true)); + localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry({0.0})); return returnvalue::OK; } diff --git a/dummies/MgmLIS3MDLDummy.cpp b/dummies/MgmLIS3MDLDummy.cpp index 31228260..9038d963 100644 --- a/dummies/MgmLIS3MDLDummy.cpp +++ b/dummies/MgmLIS3MDLDummy.cpp @@ -1,6 +1,6 @@ #include "MgmLIS3MDLDummy.h" -#include "fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h" +#include MgmLIS3MDLDummy::MgmLIS3MDLDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie), dataset(this) {} @@ -40,8 +40,8 @@ uint32_t MgmLIS3MDLDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t MgmLIS3MDLDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, new PoolEntry({0.0})); - localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, + localDataPoolMap.emplace(mgmLis3::TEMPERATURE_CELCIUS, new PoolEntry({0.0})); + localDataPoolMap.emplace(mgmLis3::FIELD_STRENGTHS, new PoolEntry({1.02, 0.56, -0.78}, true)); return returnvalue::OK; } diff --git a/dummies/MgmLIS3MDLDummy.h b/dummies/MgmLIS3MDLDummy.h index f63ed578..214f2f92 100644 --- a/dummies/MgmLIS3MDLDummy.h +++ b/dummies/MgmLIS3MDLDummy.h @@ -2,8 +2,7 @@ #define DUMMIES_MGMLIS3MDLDUMMY_H_ #include - -#include "fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h" +#include class MgmLIS3MDLDummy : public DeviceHandlerBase { public: @@ -17,7 +16,7 @@ class MgmLIS3MDLDummy : public DeviceHandlerBase { virtual ~MgmLIS3MDLDummy(); protected: - MGMLIS3MDL::MgmPrimaryDataset dataset; + mgmLis3::MgmPrimaryDataset dataset; void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; diff --git a/dummies/MgmRm3100Dummy.cpp b/dummies/MgmRm3100Dummy.cpp index 1bd4f513..fa5582c1 100644 --- a/dummies/MgmRm3100Dummy.cpp +++ b/dummies/MgmRm3100Dummy.cpp @@ -36,7 +36,7 @@ uint32_t MgmRm3100Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t MgmRm3100Dummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { - localDataPoolMap.emplace(RM3100::FIELD_STRENGTHS, + localDataPoolMap.emplace(mgmRm3100::FIELD_STRENGTHS, new PoolEntry({0.87, -0.95, 0.11}, true)); return returnvalue::OK; } diff --git a/dummies/MgmRm3100Dummy.h b/dummies/MgmRm3100Dummy.h index 857e6f69..0fa1004e 100644 --- a/dummies/MgmRm3100Dummy.h +++ b/dummies/MgmRm3100Dummy.h @@ -1,8 +1,9 @@ #ifndef DUMMIES_MGMRM3100DUMMY_H_ #define DUMMIES_MGMRM3100DUMMY_H_ +#include + #include "fsfw/devicehandlers/DeviceHandlerBase.h" -#include "fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h" class MgmRm3100Dummy : public DeviceHandlerBase { public: @@ -10,7 +11,7 @@ class MgmRm3100Dummy : public DeviceHandlerBase { virtual ~MgmRm3100Dummy(); protected: - RM3100::Rm3100PrimaryDataset dataset; + mgmRm3100::Rm3100PrimaryDataset dataset; void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index 7c271b21..d1129a49 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -35,8 +35,9 @@ uint32_t SusDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry({0}, 1, true)); - localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, + localDataPoolMap.emplace(susMax1227::SusPoolIds::TEMPERATURE_C, + new PoolEntry({0}, 1, true)); + localDataPoolMap.emplace(susMax1227::SusPoolIds::CHANNEL_VEC, new PoolEntry({2603, 781, 2760, 2048, 4056, 0}, true)); return returnvalue::OK; diff --git a/dummies/SusDummy.h b/dummies/SusDummy.h index a229bd87..9a49a591 100644 --- a/dummies/SusDummy.h +++ b/dummies/SusDummy.h @@ -2,8 +2,7 @@ #define DUMMIES_SUSDUMMY_H_ #include - -#include "mission/devices/devicedefinitions/SusDefinitions.h" +#include class SusDummy : public DeviceHandlerBase { public: @@ -17,7 +16,7 @@ class SusDummy : public DeviceHandlerBase { virtual ~SusDummy(); protected: - SUS::SusDataset susSet; + susMax1227::SusDataset susSet; void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; diff --git a/fsfw b/fsfw index bdfe31db..9a8d775e 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit bdfe31dba48039b60fe700e7d03bfb95e9549688 +Subproject commit 9a8d775eb1a8788ad844215bf2a42d9f707767c0 diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index 6d26ed51..b2c6c28f 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -151,7 +151,9 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 12406;0x3076;LOST_BIT_LOCK_PDEC;INFO;Lost bit lock;linux/ipcore/PdecHandler.h 12407;0x3077;TOO_MANY_IRQS;MEDIUM;Too many IRQs over the time window of one second. P1: Allowed TCs;linux/ipcore/PdecHandler.h 12408;0x3078;POLL_SYSCALL_ERROR_PDEC;MEDIUM;No description;linux/ipcore/PdecHandler.h -12409;0x3079;WRITE_SYSCALL_ERROR_PDEC;MEDIUM;No description;linux/ipcore/PdecHandler.h +12409;0x3079;WRITE_SYSCALL_ERROR_PDEC;HIGH;No description;linux/ipcore/PdecHandler.h +12410;0x307a;PDEC_RESET_FAILED;HIGH;Failed to pull PDEC reset to low;linux/ipcore/PdecHandler.h +12411;0x307b;OPEN_IRQ_FILE_FAILED;HIGH;Failed to open the IRQ uio file;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 @@ -248,17 +250,29 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 13904;0x3650;WRITE_CONFIGFILE_FAILED;MEDIUM;No description;mission/utility/GlobalConfigHandler.h 13905;0x3651;READ_CONFIGFILE_FAILED;MEDIUM;No description;mission/utility/GlobalConfigHandler.h 14000;0x36b0;ALLOC_FAILURE;MEDIUM;No description;bsp_q7s/core/CoreController.h -14001;0x36b1;REBOOT_SW;MEDIUM; Software reboot occurred. Can also be a systemd reboot. P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h +14001;0x36b1;REBOOT_SW;LOW; Software reboot occurred. Can also be a systemd reboot. P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h 14002;0x36b2;REBOOT_MECHANISM_TRIGGERED;MEDIUM;The reboot mechanism was triggered. P1: First 16 bits: Last Chip, Last 16 bits: Last Copy, P2: Each byte is the respective reboot count for the slots;bsp_q7s/core/CoreController.h 14003;0x36b3;REBOOT_HW;MEDIUM;No description;bsp_q7s/core/CoreController.h 14004;0x36b4;NO_SD_CARD_ACTIVE;HIGH;No SD card was active. Core controller will attempt to re-initialize a SD card.;bsp_q7s/core/CoreController.h 14005;0x36b5;VERSION_INFO;INFO;P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash P2: First four letters of Git SHA is the last byte of P1 is set.;bsp_q7s/core/CoreController.h 14006;0x36b6;CURRENT_IMAGE_INFO;INFO;P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h -14100;0x3714;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/tmtc/PersistentTmStore.h -14200;0x3778;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/ThermalController.h -14201;0x3779;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/ThermalController.h -14202;0x377a;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h -14203;0x377b;PLOC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h -14204;0x377c;OBC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h -14205;0x377d;HPA_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h -14206;0x377e;PLPCDU_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h +14007;0x36b7;REBOOT_COUNTER;INFO;Total reboot counter, which is the sum of the boot count of all individual images.;bsp_q7s/core/CoreController.h +14008;0x36b8;INDIVIDUAL_BOOT_COUNTS;INFO;Get the boot count of the individual images. P1: First 16 bits boot count of image 0 0, last 16 bits boot count of image 0 1. P2: First 16 bits boot count of image 1 0, last 16 bits boot count of image 1 1.;bsp_q7s/core/CoreController.h +14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/ThermalController.h +14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/ThermalController.h +14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h +14103;0x3717;PLOC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h +14104;0x3718;OBC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h +14105;0x3719;HPA_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h +14106;0x371a;PLPCDU_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h +14201;0x3779;TX_TIMER_EXPIRED;INFO;The transmit timer to protect the Syrlinks expired P1: The current timer value;mission/system/objects/ComSubsystem.h +14202;0x377a;BIT_LOCK_TX_ON;INFO;Transmitter will be turned on due to detection of bitlock;mission/system/objects/ComSubsystem.h +14300;0x37dc;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/persistentTmStoreDefs.h +14301;0x37dd;FILE_TOO_LARGE;LOW;File in store too large. P1: Detected file size P2: Allowed file size;mission/persistentTmStoreDefs.h +14302;0x37de;BUSY_DUMPING_EVENT;INFO;No description;mission/persistentTmStoreDefs.h +14303;0x37df;DUMP_WAS_CANCELLED;LOW;Dump was cancelled. P1: Object ID of store.;mission/persistentTmStoreDefs.h +14305;0x37e1;DUMP_OK_STORE_DONE;INFO;P1: Number of dumped packets. P2: Total dumped bytes.;mission/persistentTmStoreDefs.h +14306;0x37e2;DUMP_NOK_STORE_DONE;INFO;P1: Number of dumped packets. P2: Total dumped bytes.;mission/persistentTmStoreDefs.h +14307;0x37e3;DUMP_MISC_STORE_DONE;INFO;P1: Number of dumped packets. P2: Total dumped bytes.;mission/persistentTmStoreDefs.h +14308;0x37e4;DUMP_HK_STORE_DONE;INFO;P1: Number of dumped packets. P2: Total dumped bytes.;mission/persistentTmStoreDefs.h +14309;0x37e5;DUMP_CFDP_STORE_DONE;INFO;P1: Number of dumped packets. P2: Total dumped bytes.;mission/persistentTmStoreDefs.h diff --git a/generators/bsp_hosted_objects.csv b/generators/bsp_hosted_objects.csv index f6c11782..12a9ffb1 100644 --- a/generators/bsp_hosted_objects.csv +++ b/generators/bsp_hosted_objects.csv @@ -46,6 +46,10 @@ 0x44330003;PLOC_MPSOC_HELPER 0x44330004;AXI_PTME_CONFIG 0x44330005;PTME_CONFIG +0x44330006;PTME_VC0_LIVE_TM +0x44330007;PTME_VC1_LOG_TM +0x44330008;PTME_VC2_HK_TM +0x44330009;PTME_VC3_CFDP_TM 0x44330015;PLOC_MPSOC_HANDLER 0x44330016;PLOC_SUPERVISOR_HANDLER 0x44330017;PLOC_SUPERVISOR_HELPER @@ -77,8 +81,11 @@ 0x49000001;ARDUINO_COM_IF 0x49000002;DUMMY_COM_IF 0x49010006;SCEX_UART_READER -0x49020006;SPI_RTD_COM_IF 0x49030003;UART_COM_IF +0x49060004;ACS_BOARD_POLLING_TASK +0x49060005;RW_POLLING_TASK +0x49060006;SPI_RTD_COM_IF +0x49060007;SUS_POLLING_TASK 0x50000100;CCSDS_PACKET_DISTRIBUTOR 0x50000200;PUS_PACKET_DISTRIBUTOR 0x50000300;TCP_TMTC_SERVER @@ -127,12 +134,15 @@ 0x60000004;HEATER_4_CAMERA 0x60000005;HEATER_5_STR 0x60000006;HEATER_6_DRO -0x60000007;HEATER_7_HPA +0x60000007;HEATER_7_SYRLINKS 0x73000001;ACS_BOARD_ASS 0x73000002;SUS_BOARD_ASS 0x73000003;TCS_BOARD_ASS -0x73000004;RW_ASS +0x73000004;RW_ASSY 0x73000006;CAM_SWITCHER +0x73000007;SYRLINKS_ASSY +0x73000008;IMTQ_ASSY +0x73000009;STR_ASSY 0x73000100;TM_FUNNEL 0x73000101;PUS_TM_FUNNEL 0x73000102;CFDP_TM_FUNNEL @@ -148,6 +158,11 @@ 0x73020003;NOT_OK_TM_STORE 0x73020004;HK_TM_STORE 0x73030000;CFDP_TM_STORE +0x73040000;LIVE_TM_TASK +0x73040001;LOG_STORE_AND_TM_TASK +0x73040002;HK_STORE_AND_TM_TASK +0x73040003;CFDP_STORE_AND_TM_TASK +0x73040004;DOWNLINK_RAM_STORE 0x73500000;CCSDS_IP_CORE_BRIDGE 0x90000003;THERMAL_TEMP_INSERTER 0xCAFECAFE;DUMMY_INTERFACE diff --git a/generators/bsp_hosted_returnvalues.csv b/generators/bsp_hosted_returnvalues.csv index 86c3f699..50041e42 100644 --- a/generators/bsp_hosted_returnvalues.csv +++ b/generators/bsp_hosted_returnvalues.csv @@ -1,7 +1,10 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x0000;OK;System-wide code for ok.;0;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h 0x0001;Failed;Unspecified system-wide code for failed.;1;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h -0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h +0x7000;PTM_DumpDone;No description;0;PERSISTENT_TM_STORE;mission/tmtc/PersistentTmStore.h +0x7001;PTM_BusyDumping;No description;1;PERSISTENT_TM_STORE;mission/tmtc/PersistentTmStore.h +0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h +0x7100;TMS_IsBusy;No description;0;TM_SINK;mission/tmtc/DirectTmSinkIF.h 0x5100;IMTQ_InvalidCommandCode;No description;0;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h 0x5101;IMTQ_MgmMeasurementLowLevelError;No description;1;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h 0x5102;IMTQ_ActuateCmdLowLevelError;No description;2;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h @@ -20,14 +23,25 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x52b5;RWHA_NoReply;Reaction wheel only responds with empty frames.;181;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h 0x52b6;RWHA_NoStartMarker;Expected a start marker as first byte;182;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h 0x52b7;RWHA_SpiReadTimeout;Timeout when reading reply;183;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h -0x58a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/devices/SusHandler.h -0x58a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/devices/SusHandler.h -0x66a0;SADPL_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;SA_DEPL_HANDLER;mission/devices/RwHandler.h -0x66a1;SADPL_InvalidRampTime;Action Message with invalid ramp time was received.;161;SA_DEPL_HANDLER;mission/devices/RwHandler.h -0x66a2;SADPL_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;SA_DEPL_HANDLER;mission/devices/RwHandler.h -0x66a3;SADPL_ExecutionFailed;Command execution failed;163;SA_DEPL_HANDLER;mission/devices/RwHandler.h -0x66a4;SADPL_CrcError;Reaction wheel reply has invalid crc;164;SA_DEPL_HANDLER;mission/devices/RwHandler.h -0x66a5;SADPL_ValueNotRead;No description;165;SA_DEPL_HANDLER;mission/devices/RwHandler.h +0x58a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/devices/LegacySusHandler.h +0x58a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/devices/LegacySusHandler.h +0x5d00;GOMS_PacketTooLong;No description;0;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d01;GOMS_InvalidTableId;No description;1;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d02;GOMS_InvalidAddress;No description;2;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d03;GOMS_InvalidParamSize;No description;3;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d04;GOMS_InvalidPayloadSize;No description;4;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d05;GOMS_UnknownReplyId;No description;5;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5da0;GOMS_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;GOM_SPACE_HANDLER;mission/devices/RwHandler.h +0x5da1;GOMS_InvalidRampTime;Action Message with invalid ramp time was received.;161;GOM_SPACE_HANDLER;mission/devices/RwHandler.h +0x5da2;GOMS_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;GOM_SPACE_HANDLER;mission/devices/RwHandler.h +0x5da3;GOMS_ExecutionFailed;Command execution failed;163;GOM_SPACE_HANDLER;mission/devices/RwHandler.h +0x5da4;GOMS_CrcError;Reaction wheel reply has invalid crc;164;GOM_SPACE_HANDLER;mission/devices/RwHandler.h +0x5da5;GOMS_ValueNotRead;No description;165;GOM_SPACE_HANDLER;mission/devices/RwHandler.h +0x4fa1;HEATER_CommandNotSupported;No description;161;HEATER_HANDLER;mission/devices/HeaterHandler.h +0x4fa2;HEATER_InitFailed;No description;162;HEATER_HANDLER;mission/devices/HeaterHandler.h +0x4fa3;HEATER_InvalidSwitchNr;No description;163;HEATER_HANDLER;mission/devices/HeaterHandler.h +0x4fa4;HEATER_MainSwitchSetTimeout;No description;164;HEATER_HANDLER;mission/devices/HeaterHandler.h +0x4fa5;HEATER_CommandAlreadyWaiting;No description;165;HEATER_HANDLER;mission/devices/HeaterHandler.h 0x50a0;SYRLINKS_CrcFailure;No description;160;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h 0x50a1;SYRLINKS_UartFraminOrParityErrorAck;No description;161;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h 0x50a2;SYRLINKS_BadCharacterAck;No description;162;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h @@ -37,20 +51,13 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x50a6;SYRLINKS_BadCrcAck;No description;166;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h 0x50a7;SYRLINKS_ReplyWrongSize;No description;167;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h 0x50a8;SYRLINKS_MissingStartFrameCharacter;No description;168;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h -0x5d00;GOMS_PacketTooLong;No description;0;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d01;GOMS_InvalidTableId;No description;1;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d02;GOMS_InvalidAddress;No description;2;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d03;GOMS_InvalidParamSize;No description;3;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d04;GOMS_InvalidPayloadSize;No description;4;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d05;GOMS_UnknownReplyId;No description;5;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x4fa1;HEATER_CommandNotSupported;No description;161;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x4fa2;HEATER_InitFailed;No description;162;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x4fa3;HEATER_InvalidSwitchNr;No description;163;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x4fa4;HEATER_MainSwitchSetTimeout;No description;164;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x4fa5;HEATER_CommandAlreadyWaiting;No description;165;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h -0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h +0x66a0;SADPL_CommandNotSupported;No description;160;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a1;SADPL_DeploymentAlreadyExecuting;No description;161;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a2;SADPL_MainSwitchTimeoutFailure;No description;162;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a3;SADPL_SwitchingDeplSa1Failed;No description;163;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a4;SADPL_SwitchingDeplSa2Failed;No description;164;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h 0x6c01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h +0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h 0x6d01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h 0x6a02;ACSMEKF_MekfUninitialized;No description;2;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a03;ACSMEKF_MekfNoGyrData;No description;3;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h @@ -60,105 +67,125 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x6a07;ACSMEKF_MekfInitialized;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a08;ACSMEKF_MekfRunning;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6900;ACSCTRL_FileDeletionFailed;No description;0;ACS_CTRL;mission/controller/AcsController.h -0x4500;HSPI_OpeningFileFailed;No description;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h -0x4501;HSPI_FullDuplexTransferFailed;No description;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h -0x4502;HSPI_HalfDuplexTransferFailed;No description;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h -0x4801;HGIO_UnknownGpioId;No description;1;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4802;HGIO_DriveGpioFailure;No description;2;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4803;HGIO_GpioTypeFailure;No description;3;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4804;HGIO_GpioInvalidInstance;No description;4;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4805;HGIO_GpioDuplicateDetected;No description;5;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4806;HGIO_GpioInitFailed;No description;6;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4807;HGIO_GpioGetValueFailed;No description;7;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4601;HURT_UartReadFailure;No description;1;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h -0x4602;HURT_UartReadSizeMissmatch;No description;2;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h -0x4603;HURT_UartRxBufferTooSmall;No description;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 -0x4403;UXOS_CommandError;Command execution failed;3;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h -0x4404;UXOS_NoCommandLoadedOrPending;;4;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h -0x4406;UXOS_PcloseCallError;No description;6;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h -0x2801;SM_DataTooLarge;No description;1;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2802;SM_DataStorageFull;No description;2;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2803;SM_IllegalStorageId;No description;3;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2804;SM_DataDoesNotExist;No description;4;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2805;SM_IllegalAddress;No description;5;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2806;SM_PoolTooLarge;No description;6;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x0601;PP_DoItMyself;No description;1;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0602;PP_PointsToVariable;No description;2;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0603;PP_PointsToMemory;No description;3;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0604;PP_ActivityCompleted;No description;4;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0605;PP_PointsToVectorUint8;No description;5;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0606;PP_PointsToVectorUint16;No description;6;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0607;PP_PointsToVectorUint32;No description;7;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0608;PP_PointsToVectorFloat;No description;8;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06a0;PP_DumpNotSupported;No description;160;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e0;PP_InvalidSize;No description;224;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e1;PP_InvalidAddress;No description;225;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e2;PP_InvalidContent;No description;226;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e3;PP_UnalignedAccess;No description;227;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e4;PP_WriteProtected;No description;228;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x13e0;MH_UnknownCmd;No description;224;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x13e1;MH_InvalidAddress;No description;225;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x13e2;MH_InvalidSize;No description;226;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x13e3;MH_StateMismatch;No description;227;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x38a1;SGP4_InvalidEccentricity;No description;161;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a2;SGP4_InvalidMeanMotion;No description;162;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a3;SGP4_InvalidPerturbationElements;No description;163;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a4;SGP4_InvalidSemiLatusRectum;No description;164;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a5;SGP4_InvalidEpochElements;No description;165;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a6;SGP4_SatelliteHasDecayed;No description;166;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38b1;SGP4_TleTooOld;No description;177;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38b2;SGP4_TleNotInitialized;No description;178;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x1801;FF_Full;No description;1;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h -0x1802;FF_Empty;No description;2;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h -0x1601;FMM_MapFull;No description;1;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h -0x1602;FMM_KeyDoesNotExist;No description;2;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h -0x3901;MUX_NotEnoughResources;No description;1;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3902;MUX_InsufficientMemory;No description;2;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3903;MUX_NoPrivilege;No description;3;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3904;MUX_WrongAttributeSetting;No description;4;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3905;MUX_MutexAlreadyLocked;No description;5;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3906;MUX_MutexNotFound;No description;6;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3907;MUX_MutexMaxLocks;No description;7;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3908;MUX_CurrThreadAlreadyOwnsMutex;No description;8;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3909;MUX_CurrThreadDoesNotOwnMutex;No description;9;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x390a;MUX_MutexTimeout;No description;10;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x390b;MUX_MutexInvalidId;No description;11;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x390c;MUX_MutexDestroyedWhileWaiting;No description;12;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3a01;MQI_Empty;No description;1;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x3a02;MQI_Full;No space left for more messages;2;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x3a03;MQI_NoReplyPartner;Returned if a reply method was called without partner;3;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x3a04;MQI_DestinationInvalid;Returned if the target destination is invalid.;4;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x0f01;CM_UnknownCommand;No description;1;COMMAND_MESSAGE;fsfw/src/fsfw/ipc/CommandMessageIF.h +0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NvmParameterBase.h +0x2c01;CCS_BcIsSetVrCommand;No description;1;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2c02;CCS_BcIsUnlockCommand;No description;2;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cb0;CCS_BcIllegalCommand;No description;176;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cb1;CCS_BoardReadingNotFinished;No description;177;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf0;CCS_NsPositiveW;No description;240;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf1;CCS_NsNegativeW;No description;241;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf2;CCS_NsLockout;No description;242;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf3;CCS_FarmInLockout;No description;243;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf4;CCS_FarmInWait;No description;244;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce0;CCS_WrongSymbol;No description;224;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce1;CCS_DoubleStart;No description;225;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce2;CCS_StartSymbolMissed;No description;226;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce3;CCS_EndWithoutStart;No description;227;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce4;CCS_TooLarge;No description;228;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce5;CCS_TooShort;No description;229;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce6;CCS_WrongTfVersion;No description;230;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce7;CCS_WrongSpacecraftId;No description;231;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce8;CCS_NoValidFrameType;No description;232;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce9;CCS_CrcFailed;No description;233;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cea;CCS_VcNotFound;No description;234;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ceb;CCS_ForwardingFailed;No description;235;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cec;CCS_ContentTooLarge;No description;236;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ced;CCS_ResidualData;No description;237;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cee;CCS_DataCorrupted;No description;238;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cef;CCS_IllegalSegmentationFlag;No description;239;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd0;CCS_IllegalFlagCombination;No description;208;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd1;CCS_ShorterThanHeader;No description;209;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd2;CCS_TooShortBlockedPacket;No description;210;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd3;CCS_TooShortMapExtraction;No description;211;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x3b00;SPH_ConnBroken;No description;0;SEMAPHORE_IF;fsfw/src/fsfw/osal/common/TcpTmTcServer.h +0x2a01;IEC_NoConfigurationTable;No description;1;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a02;IEC_NoCpuTable;No description;2;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a03;IEC_InvalidWorkspaceAddress;No description;3;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a04;IEC_TooLittleWorkspace;No description;4;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a05;IEC_WorkspaceAllocation;No description;5;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a06;IEC_InterruptStackTooSmall;No description;6;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a07;IEC_ThreadExitted;No description;7;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a08;IEC_InconsistentMpInformation;No description;8;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a09;IEC_InvalidNode;No description;9;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0a;IEC_NoMpci;No description;10;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0b;IEC_BadPacket;No description;11;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0c;IEC_OutOfPackets;No description;12;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0d;IEC_OutOfGlobalObjects;No description;13;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0e;IEC_OutOfProxies;No description;14;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0f;IEC_InvalidGlobalId;No description;15;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a10;IEC_BadStackHook;No description;16;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a11;IEC_BadAttributes;No description;17;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a12;IEC_ImplementationKeyCreateInconsistency;No description;18;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a13;IEC_ImplementationBlockingOperationCancel;No description;19;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a14;IEC_MutexObtainFromBadState;No description;20;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a15;IEC_UnlimitedAndMaximumIs0;No description;21;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h 0x0e01;HM_InvalidMode;No description;1;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h 0x0e02;HM_TransNotAllowed;No description;2;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h 0x0e03;HM_InTransition;No description;3;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h 0x0e04;HM_InvalidSubmode;No description;4;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h -0x0c02;MS_InvalidEntry;No description;2;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h -0x0c03;MS_TooManyElements;No description;3;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h -0x0c04;MS_CantStoreEmpty;No description;4;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h -0x0b01;SB_ChildNotFound;No description;1;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b02;SB_ChildInfoUpdated;No description;2;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b03;SB_ChildDoesntHaveModes;No description;3;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b04;SB_CouldNotInsertChild;No description;4;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b05;SB_TableContainsInvalidObjectId;No description;5;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0d01;SS_SequenceAlreadyExists;No description;1;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d02;SS_TableAlreadyExists;No description;2;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d03;SS_TableDoesNotExist;No description;3;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d04;SS_TableOrSequenceLengthInvalid;No description;4;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d05;SS_SequenceDoesNotExist;No description;5;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d06;SS_TableContainsInvalidObjectId;No description;6;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d07;SS_FallbackSequenceDoesNotExist;No description;7;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d08;SS_NoTargetTable;No description;8;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d09;SS_SequenceOrTableTooLong;No description;9;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d0b;SS_IsFallbackSequence;No description;11;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d0c;SS_AccessDenied;No description;12;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d0e;SS_TableInUse;No description;14;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0da1;SS_TargetTableNotReached;No description;161;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0da2;SS_TableCheckFailed;No description;162;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x2501;EV_ListenerNotFound;No description;1;EVENT_MANAGER_IF;fsfw/src/fsfw/events/EventManagerIF.h +0x2e01;HPA_InvalidIdentifierId;No description;1;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2e02;HPA_InvalidDomainId;No description;2;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2e03;HPA_InvalidValue;No description;3;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2e05;HPA_ReadOnly;No description;5;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2d01;PAW_UnknownDatatype;No description;1;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d02;PAW_DatatypeMissmatch;No description;2;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d03;PAW_Readonly;No description;3;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d04;PAW_TooBig;No description;4;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d05;PAW_SourceNotSet;No description;5;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d06;PAW_OutOfBounds;No description;6;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d07;PAW_NotSet;No description;7;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d08;PAW_ColumnOrRowsZero;No description;8;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x3201;CF_ObjectHasNoFunctions;No description;1;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h +0x3202;CF_AlreadyCommanding;No description;2;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h +0x3301;HF_IsBusy;No description;1;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x3302;HF_InvalidParameters;No description;2;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x3303;HF_ExecutionFinished;No description;3;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x3304;HF_InvalidActionId;No description;4;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x0201;OM_InsertionFailed;No description;1;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x0202;OM_NotFound;No description;2;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x0203;OM_ChildInitFailed;No description;3;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x0204;OM_InternalErrReporterUninit;No description;4;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x2600;FDI_YourFault;No description;0;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h +0x2601;FDI_MyFault;No description;1;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h +0x2602;FDI_ConfirmLater;No description;2;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h +0x2201;TMF_Busy;No description;1;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2202;TMF_LastPacketFound;No description;2;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2203;TMF_StopFetch;No description;3;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2204;TMF_Timeout;No description;4;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2205;TMF_TmChannelFull;No description;5;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2206;TMF_NotStored;No description;6;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2207;TMF_AllDeleted;No description;7;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2208;TMF_InvalidData;No description;8;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2209;TMF_NotReady;No description;9;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2101;TMB_Busy;No description;1;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2102;TMB_Full;No description;2;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2103;TMB_Empty;No description;3;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2104;TMB_NullRequested;No description;4;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2105;TMB_TooLarge;No description;5;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2106;TMB_NotReady;No description;6;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2107;TMB_DumpError;No description;7;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2108;TMB_CrcError;No description;8;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2109;TMB_Timeout;No description;9;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210a;TMB_IdlePacketFound;No description;10;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210b;TMB_TelecommandFound;No description;11;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210c;TMB_NoPusATm;No description;12;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210d;TMB_TooSmall;No description;13;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210e;TMB_BlockNotFound;No description;14;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210f;TMB_InvalidRequest;No description;15;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x1c01;TCD_PacketLost;No description;1;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h +0x1c02;TCD_DestinationNotFound;No description;2;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h +0x1c03;TCD_ServiceIdAlreadyExists;No description;3;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h +0x1b00;TCC_NoDestinationFound;No description;0;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b01;TCC_InvalidCcsdsVersion;No description;1;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b02;TCC_InvalidApid;No description;2;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b03;TCC_InvalidPacketType;No description;3;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b04;TCC_InvalidSecHeaderField;No description;4;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b05;TCC_IncorrectPrimaryHeader;No description;5;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b07;TCC_IncompletePacket;No description;7;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b08;TCC_InvalidPusVersion;No description;8;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b09;TCC_IncorrectChecksum;No description;9;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b0a;TCC_IllegalPacketSubtype;No description;10;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b0b;TCC_IncorrectSecondaryHeader;No description;11;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h 0x04e1;RMP_CommandNoDescriptorsAvailable;No description;225;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x04e2;RMP_CommandBufferFull;No description;226;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x04e3;RMP_CommandChannelOutOfRange;No description;227;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h @@ -199,9 +226,95 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x040a;RMP_ReplyCommandNotImplementedOrNotAuthorised;No description;10;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x040b;RMP_ReplyRmwDataLengthError;No description;11;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x040c;RMP_ReplyInvalidTargetLogicalAddress;No description;12;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h -0x1401;SE_BufferTooShort;No description;1;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h -0x1402;SE_StreamTooShort;No description;2;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h -0x1403;SE_TooManyElements;No description;3;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x2801;SM_DataTooLarge;No description;1;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2802;SM_DataStorageFull;No description;2;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2803;SM_IllegalStorageId;No description;3;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2804;SM_DataDoesNotExist;No description;4;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2805;SM_IllegalAddress;No description;5;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2806;SM_PoolTooLarge;No description;6;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x38a1;SGP4_InvalidEccentricity;No description;161;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a2;SGP4_InvalidMeanMotion;No description;162;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a3;SGP4_InvalidPerturbationElements;No description;163;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a4;SGP4_InvalidSemiLatusRectum;No description;164;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a5;SGP4_InvalidEpochElements;No description;165;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a6;SGP4_SatelliteHasDecayed;No description;166;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38b1;SGP4_TleTooOld;No description;177;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38b2;SGP4_TleNotInitialized;No description;178;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x2401;MT_NoPacketFound;No description;1;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/DleParser.h +0x2402;MT_PossiblePacketLoss;No description;2;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/DleParser.h +0x2403;MT_NoMatch;No description;3;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h +0x2404;MT_Full;No description;4;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h +0x2405;MT_NewNodeCreated;No description;5;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h +0x3f01;DLEE_StreamTooShort;No description;1;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleEncoder.h +0x3f02;DLEE_DecodingError;No description;2;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleEncoder.h +0x2f01;ASC_TooLongForTargetType;No description;1;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h +0x2f02;ASC_InvalidCharacters;No description;2;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h +0x2f03;ASC_BufferTooSmall;No description;3;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h +0x0f01;CM_UnknownCommand;No description;1;COMMAND_MESSAGE;fsfw/src/fsfw/ipc/CommandMessageIF.h +0x3a01;MQI_Empty;No description;1;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3a02;MQI_Full;No space left for more messages;2;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3a03;MQI_NoReplyPartner;Returned if a reply method was called without partner;3;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3a04;MQI_DestinationInvalid;Returned if the target destination is invalid.;4;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3901;MUX_NotEnoughResources;No description;1;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3902;MUX_InsufficientMemory;No description;2;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3903;MUX_NoPrivilege;No description;3;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3904;MUX_WrongAttributeSetting;No description;4;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3905;MUX_MutexAlreadyLocked;No description;5;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3906;MUX_MutexNotFound;No description;6;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3907;MUX_MutexMaxLocks;No description;7;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3908;MUX_CurrThreadAlreadyOwnsMutex;No description;8;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3909;MUX_CurrThreadDoesNotOwnMutex;No description;9;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x390a;MUX_MutexTimeout;No description;10;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x390b;MUX_MutexInvalidId;No description;11;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x390c;MUX_MutexDestroyedWhileWaiting;No description;12;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3b01;SPH_SemaphoreTimeout;No description;1;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x3b02;SPH_SemaphoreNotOwned;No description;2;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x3b03;SPH_SemaphoreInvalid;No description;3;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x1e00;PUS_InvalidPusVersion;No description;0;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h +0x1e01;PUS_InvalidCrc16;No description;1;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h +0x3601;CFDP_InvalidTlvType;No description;1;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3602;CFDP_InvalidDirectiveField;No description;2;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3603;CFDP_InvalidPduDatafieldLen;No description;3;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3604;CFDP_InvalidAckDirectiveFields;No description;4;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3605;CFDP_MetadataCantParseOptions;No description;5;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3606;CFDP_NakCantParseOptions;No description;6;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3607;CFDP_FinishedCantParseFsResponses;No description;7;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3608;CFDP_FilestoreRequiresSecondFile;No description;8;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3609;CFDP_FilestoreResponseCantParseFsMessage;No description;9;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x360a;CFDP_InvalidPduFormat;No description;10;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x2901;TC_InvalidTargetState;No description;1;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h +0x29f1;TC_AboveOperationalLimit;No description;241;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h +0x29f2;TC_BelowOperationalLimit;No description;242;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h +0x0c02;MS_InvalidEntry;No description;2;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h +0x0c03;MS_TooManyElements;No description;3;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h +0x0c04;MS_CantStoreEmpty;No description;4;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h +0x0d01;SS_SequenceAlreadyExists;No description;1;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d02;SS_TableAlreadyExists;No description;2;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d03;SS_TableDoesNotExist;No description;3;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d04;SS_TableOrSequenceLengthInvalid;No description;4;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d05;SS_SequenceDoesNotExist;No description;5;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d06;SS_TableContainsInvalidObjectId;No description;6;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d07;SS_FallbackSequenceDoesNotExist;No description;7;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d08;SS_NoTargetTable;No description;8;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d09;SS_SequenceOrTableTooLong;No description;9;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d0b;SS_IsFallbackSequence;No description;11;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d0c;SS_AccessDenied;No description;12;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d0e;SS_TableInUse;No description;14;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0da1;SS_TargetTableNotReached;No description;161;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0da2;SS_TableCheckFailed;No description;162;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0b01;SB_ChildNotFound;No description;1;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b02;SB_ChildInfoUpdated;No description;2;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b03;SB_ChildDoesntHaveModes;No description;3;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b04;SB_CouldNotInsertChild;No description;4;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b05;SB_TableContainsInvalidObjectId;No description;5;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x3e00;HKM_QueueOrDestinationInvalid;No description;0;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e01;HKM_WrongHkPacketType;No description;1;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e02;HKM_ReportingStatusUnchanged;No description;2;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e03;HKM_PeriodicHelperInvalid;No description;3;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e04;HKM_PoolobjectNotFound;No description;4;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e05;HKM_DatasetNotFound;No description;5;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3c00;LPIF_PoolEntryNotFound;No description;0;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h +0x3c01;LPIF_PoolEntryTypeConflict;No description;1;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h 0x3da0;PVA_InvalidReadWriteMode;No description;160;POOL_VARIABLE_IF;fsfw/src/fsfw/datapool/PoolVariableIF.h 0x3da1;PVA_InvalidPoolEntry;No description;161;POOL_VARIABLE_IF;fsfw/src/fsfw/datapool/PoolVariableIF.h 0x0801;DPS_InvalidParameterDefinition;No description;1;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h @@ -210,20 +323,35 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x0804;DPS_DataSetUninitialised;No description;4;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h 0x0805;DPS_DataSetFull;No description;5;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h 0x0806;DPS_PoolVarNull;No description;6;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h -0x1c01;TCD_PacketLost;No description;1;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h -0x1c02;TCD_DestinationNotFound;No description;2;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h -0x1c03;TCD_ServiceIdAlreadyExists;No description;3;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h -0x1b00;TCC_NoDestinationFound;No description;0;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b01;TCC_InvalidCcsdsVersion;No description;1;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b02;TCC_InvalidApid;No description;2;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b03;TCC_InvalidPacketType;No description;3;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b04;TCC_InvalidSecHeaderField;No description;4;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b05;TCC_IncorrectPrimaryHeader;No description;5;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b07;TCC_IncompletePacket;No description;7;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b08;TCC_InvalidPusVersion;No description;8;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b09;TCC_IncorrectChecksum;No description;9;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b0a;TCC_IllegalPacketSubtype;No description;10;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b0b;TCC_IncorrectSecondaryHeader;No description;11;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1000;TIM_UnsupportedTimeFormat;No description;0;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1001;TIM_NotEnoughInformationForTargetFormat;No description;1;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1002;TIM_LengthMismatch;No description;2;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1003;TIM_InvalidTimeFormat;No description;3;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1004;TIM_InvalidDayOfYear;No description;4;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1005;TIM_TimeDoesNotFitFormat;No description;5;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x3701;TSI_BadTimestamp;No description;1;TIME_STAMPER_IF;fsfw/src/fsfw/timemanager/TimeStampIF.h +0x1d01;ATC_ActivityStarted;No description;1;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d02;ATC_InvalidSubservice;No description;2;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d03;ATC_IllegalApplicationData;No description;3;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d04;ATC_SendTmFailed;No description;4;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d05;ATC_Timeout;No description;5;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x4c00;SPPA_NoPacketFound;No description;0;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h +0x4c01;SPPA_SplitPacket;No description;1;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h +0x2001;CSB_ExecutionComplete;No description;1;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2002;CSB_NoStepMessage;No description;2;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2003;CSB_ObjectBusy;No description;3;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2004;CSB_Busy;No description;4;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2005;CSB_InvalidTc;No description;5;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2006;CSB_InvalidObject;No description;6;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2007;CSB_InvalidReply;No description;7;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x1801;FF_Full;No description;1;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h +0x1802;FF_Empty;No description;2;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h +0x1601;FMM_MapFull;No description;1;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h +0x1602;FMM_KeyDoesNotExist;No description;2;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h +0x2501;EV_ListenerNotFound;No description;1;EVENT_MANAGER_IF;fsfw/src/fsfw/events/EventManagerIF.h +0x1701;HHI_ObjectNotHealthy;No description;1;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h +0x1702;HHI_InvalidHealthState;No description;2;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h +0x1703;HHI_IsExternallyControlled;No description;3;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h 0x3001;POS_InPowerTransition;No description;1;POWER_SWITCHER;fsfw/src/fsfw/power/PowerSwitcher.h 0x3002;POS_SwitchStateMismatch;No description;2;POWER_SWITCHER;fsfw/src/fsfw/power/PowerSwitcher.h 0x0501;PS_SwitchOn;No description;1;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h @@ -231,76 +359,23 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x0502;PS_SwitchTimeout;No description;2;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h 0x0503;PS_FuseOn;No description;3;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h 0x0504;PS_FuseOff;No description;4;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h -0x3b00;SPH_ConnBroken;No description;0;SEMAPHORE_IF;fsfw/src/fsfw/osal/common/TcpTmTcServer.h -0x2a01;IEC_NoConfigurationTable;No description;1;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a02;IEC_NoCpuTable;No description;2;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a03;IEC_InvalidWorkspaceAddress;No description;3;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a04;IEC_TooLittleWorkspace;No description;4;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a05;IEC_WorkspaceAllocation;No description;5;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a06;IEC_InterruptStackTooSmall;No description;6;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a07;IEC_ThreadExitted;No description;7;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a08;IEC_InconsistentMpInformation;No description;8;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a09;IEC_InvalidNode;No description;9;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0a;IEC_NoMpci;No description;10;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0b;IEC_BadPacket;No description;11;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0c;IEC_OutOfPackets;No description;12;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0d;IEC_OutOfGlobalObjects;No description;13;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0e;IEC_OutOfProxies;No description;14;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0f;IEC_InvalidGlobalId;No description;15;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a10;IEC_BadStackHook;No description;16;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a11;IEC_BadAttributes;No description;17;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a12;IEC_ImplementationKeyCreateInconsistency;No description;18;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a13;IEC_ImplementationBlockingOperationCancel;No description;19;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a14;IEC_MutexObtainFromBadState;No description;20;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a15;IEC_UnlimitedAndMaximumIs0;No description;21;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2600;FDI_YourFault;No description;0;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h -0x2601;FDI_MyFault;No description;1;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h -0x2602;FDI_ConfirmLater;No description;2;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h -0x1e00;PUS_InvalidPusVersion;No description;0;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h -0x1e01;PUS_InvalidCrc16;No description;1;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h -0x0201;OM_InsertionFailed;No description;1;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x0202;OM_NotFound;No description;2;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x0203;OM_ChildInitFailed;No description;3;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x0204;OM_InternalErrReporterUninit;No description;4;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x2201;TMF_Busy;No description;1;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2202;TMF_LastPacketFound;No description;2;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2203;TMF_StopFetch;No description;3;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2204;TMF_Timeout;No description;4;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2205;TMF_TmChannelFull;No description;5;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2206;TMF_NotStored;No description;6;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2207;TMF_AllDeleted;No description;7;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2208;TMF_InvalidData;No description;8;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2209;TMF_NotReady;No description;9;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2101;TMB_Busy;No description;1;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2102;TMB_Full;No description;2;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2103;TMB_Empty;No description;3;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2104;TMB_NullRequested;No description;4;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2105;TMB_TooLarge;No description;5;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2106;TMB_NotReady;No description;6;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2107;TMB_DumpError;No description;7;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2108;TMB_CrcError;No description;8;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2109;TMB_Timeout;No description;9;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210a;TMB_IdlePacketFound;No description;10;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210b;TMB_TelecommandFound;No description;11;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210c;TMB_NoPusATm;No description;12;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210d;TMB_TooSmall;No description;13;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210e;TMB_BlockNotFound;No description;14;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210f;TMB_InvalidRequest;No description;15;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2d01;PAW_UnknownDatatype;No description;1;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d02;PAW_DatatypeMissmatch;No description;2;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d03;PAW_Readonly;No description;3;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d04;PAW_TooBig;No description;4;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d05;PAW_SourceNotSet;No description;5;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d06;PAW_OutOfBounds;No description;6;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d07;PAW_NotSet;No description;7;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d08;PAW_ColumnOrRowsZero;No description;8;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2e01;HPA_InvalidIdentifierId;No description;1;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x2e02;HPA_InvalidDomainId;No description;2;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x2e03;HPA_InvalidValue;No description;3;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x2e05;HPA_ReadOnly;No description;5;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x3b01;SPH_SemaphoreTimeout;No description;1;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h -0x3b02;SPH_SemaphoreNotOwned;No description;2;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h -0x3b03;SPH_SemaphoreInvalid;No description;3;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x4300;FILS_GenericFileError;No description;0;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4301;FILS_GenericDirError;No description;1;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4302;FILS_FilesystemInactive;No description;2;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4303;FILS_GenericRenameError;No description;3;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4304;FILS_IsBusy;No description;4;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4305;FILS_InvalidParameters;No description;5;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430a;FILS_FileDoesNotExist;No description;10;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430b;FILS_FileAlreadyExists;No description;11;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430c;FILS_NotAFile;No description;12;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430d;FILS_FileLocked;No description;13;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430e;FILS_PermissionDenied;No description;14;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4315;FILS_DirectoryDoesNotExist;No description;21;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4316;FILS_DirectoryAlreadyExists;No description;22;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4317;FILS_NotADirectory;No description;23;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4318;FILS_DirectoryNotEmpty;No description;24;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x431e;FILS_SequencePacketMissingWrite;No description;30;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x431f;FILS_SequencePacketMissingRead;No description;31;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h 0x1a01;TRC_NotEnoughSensors;No description;1;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h 0x1a02;TRC_LowestValueOol;No description;2;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h 0x1a03;TRC_HighestValueOol;No description;3;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h @@ -319,74 +394,36 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x31e2;LIM_WrongPid;No description;226;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h 0x31e3;LIM_WrongLimitId;No description;227;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h 0x31ee;LIM_MonitorNotFound;No description;238;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x3601;CFDP_InvalidTlvType;No description;1;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3602;CFDP_InvalidDirectiveField;No description;2;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3603;CFDP_InvalidPduDatafieldLen;No description;3;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3604;CFDP_InvalidAckDirectiveFields;No description;4;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3605;CFDP_MetadataCantParseOptions;No description;5;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3606;CFDP_NakCantParseOptions;No description;6;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3607;CFDP_FinishedCantParseFsResponses;No description;7;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3608;CFDP_FilestoreRequiresSecondFile;No description;8;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3609;CFDP_FilestoreResponseCantParseFsMessage;No description;9;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x360a;CFDP_InvalidPduFormat;No description;10;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x4300;FILS_GenericFileError;No description;0;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4301;FILS_GenericDirError;No description;1;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4302;FILS_FilesystemInactive;No description;2;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4303;FILS_GenericRenameError;No description;3;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4304;FILS_IsBusy;No description;4;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4305;FILS_InvalidParameters;No description;5;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430a;FILS_FileDoesNotExist;No description;10;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430b;FILS_FileAlreadyExists;No description;11;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430c;FILS_NotAFile;No description;12;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430d;FILS_FileLocked;No description;13;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430e;FILS_PermissionDenied;No description;14;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4315;FILS_DirectoryDoesNotExist;No description;21;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4316;FILS_DirectoryAlreadyExists;No description;22;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4317;FILS_NotADirectory;No description;23;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4318;FILS_DirectoryNotEmpty;No description;24;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x431e;FILS_SequencePacketMissingWrite;No description;30;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x431f;FILS_SequencePacketMissingRead;No description;31;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x2c01;CCS_BcIsSetVrCommand;No description;1;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2c02;CCS_BcIsUnlockCommand;No description;2;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cb0;CCS_BcIllegalCommand;No description;176;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cb1;CCS_BoardReadingNotFinished;No description;177;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf0;CCS_NsPositiveW;No description;240;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf1;CCS_NsNegativeW;No description;241;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf2;CCS_NsLockout;No description;242;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf3;CCS_FarmInLockout;No description;243;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf4;CCS_FarmInWait;No description;244;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce0;CCS_WrongSymbol;No description;224;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce1;CCS_DoubleStart;No description;225;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce2;CCS_StartSymbolMissed;No description;226;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce3;CCS_EndWithoutStart;No description;227;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce4;CCS_TooLarge;No description;228;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce5;CCS_TooShort;No description;229;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce6;CCS_WrongTfVersion;No description;230;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce7;CCS_WrongSpacecraftId;No description;231;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce8;CCS_NoValidFrameType;No description;232;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce9;CCS_CrcFailed;No description;233;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cea;CCS_VcNotFound;No description;234;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ceb;CCS_ForwardingFailed;No description;235;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cec;CCS_ContentTooLarge;No description;236;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ced;CCS_ResidualData;No description;237;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cee;CCS_DataCorrupted;No description;238;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cef;CCS_IllegalSegmentationFlag;No description;239;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd0;CCS_IllegalFlagCombination;No description;208;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd1;CCS_ShorterThanHeader;No description;209;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd2;CCS_TooShortBlockedPacket;No description;210;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd3;CCS_TooShortMapExtraction;No description;211;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h 0x4201;PUS11_InvalidTypeTimeWindow;No description;1;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h 0x4202;PUS11_InvalidTimeWindow;No description;2;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h 0x4203;PUS11_TimeshiftingNotPossible;No description;3;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h 0x4204;PUS11_InvalidRelativeTime;No description;4;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h 0x4205;PUS11_ContainedTcTooSmall;No description;5;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h 0x4206;PUS11_ContainedTcCrcMissmatch;No description;6;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h -0x3401;DC_NoReplyReceived;No description;1;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3402;DC_ProtocolError;No description;2;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3403;DC_Nullpointer;No description;3;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3404;DC_InvalidCookieType;No description;4;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3405;DC_NotActive;No description;5;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3406;DC_TooMuchData;No description;6;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x0601;PP_DoItMyself;No description;1;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0602;PP_PointsToVariable;No description;2;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0603;PP_PointsToMemory;No description;3;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0604;PP_ActivityCompleted;No description;4;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0605;PP_PointsToVectorUint8;No description;5;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0606;PP_PointsToVectorUint16;No description;6;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0607;PP_PointsToVectorUint32;No description;7;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0608;PP_PointsToVectorFloat;No description;8;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06a0;PP_DumpNotSupported;No description;160;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e0;PP_InvalidSize;No description;224;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e1;PP_InvalidAddress;No description;225;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e2;PP_InvalidContent;No description;226;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e3;PP_UnalignedAccess;No description;227;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e4;PP_WriteProtected;No description;228;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x13e0;MH_UnknownCmd;No description;224;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x13e1;MH_InvalidAddress;No description;225;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x13e2;MH_InvalidSize;No description;226;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x13e3;MH_StateMismatch;No description;227;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x1201;AB_NeedSecondStep;No description;1;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1202;AB_NeedToReconfigure;No description;2;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1203;AB_ModeFallback;No description;3;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1204;AB_ChildNotCommandable;No description;4;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1205;AB_NeedToChangeHealth;No description;5;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x12a1;AB_NotEnoughChildrenInCorrectState;No description;161;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h 0x03a0;DHB_InvalidChannel;No description;160;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03b0;DHB_AperiodicReply;No description;176;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03b1;DHB_IgnoreReplyData;No description;177;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -396,12 +433,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x03d0;DHB_NoSwitch;No description;208;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03e0;DHB_ChildTimeout;No description;224;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03e1;DHB_SwitchFailed;No description;225;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h -0x1201;AB_NeedSecondStep;No description;1;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1202;AB_NeedToReconfigure;No description;2;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1203;AB_ModeFallback;No description;3;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1204;AB_ChildNotCommandable;No description;4;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1205;AB_NeedToChangeHealth;No description;5;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x12a1;AB_NotEnoughChildrenInCorrectState;No description;161;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x3401;DC_NoReplyReceived;No description;1;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3402;DC_ProtocolError;No description;2;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3403;DC_Nullpointer;No description;3;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3404;DC_InvalidCookieType;No description;4;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3405;DC_NotActive;No description;5;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3406;DC_TooMuchData;No description;6;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h 0x27a0;DHI_NoCommandData;No description;160;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27a1;DHI_CommandNotSupported;No description;161;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27a2;DHI_CommandAlreadySent;No description;162;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h @@ -423,54 +460,25 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x27c3;DHI_DeviceReplyInvalid;No description;195;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27d0;DHI_InvalidCommandParameter;No description;208;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27d1;DHI_InvalidNumberOrLengthOfParameters;No description;209;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h -0x2401;MT_TooDetailedRequest;No description;1;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x2402;MT_TooGeneralRequest;No description;2;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x2403;MT_NoMatch;No description;3;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x2404;MT_Full;No description;4;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x2405;MT_NewNodeCreated;No description;5;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x3f01;DLEE_NoPacketFound;No description;1;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleParser.h -0x3f02;DLEE_PossiblePacketLoss;No description;2;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleParser.h -0x2f01;ASC_TooLongForTargetType;No description;1;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h -0x2f02;ASC_InvalidCharacters;No description;2;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h -0x2f03;ASC_BufferTooSmall;No description;3;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h -0x1701;HHI_ObjectNotHealthy;No description;1;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h -0x1702;HHI_InvalidHealthState;No description;2;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h -0x1703;HHI_IsExternallyControlled;No description;3;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h -0x3201;CF_ObjectHasNoFunctions;No description;1;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h -0x3202;CF_AlreadyCommanding;No description;2;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h -0x3301;HF_IsBusy;No description;1;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x3302;HF_InvalidParameters;No description;2;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x3303;HF_ExecutionFinished;No description;3;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x3304;HF_InvalidActionId;No description;4;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x1000;TIM_UnsupportedTimeFormat;No description;0;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1001;TIM_NotEnoughInformationForTargetFormat;No description;1;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1002;TIM_LengthMismatch;No description;2;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1003;TIM_InvalidTimeFormat;No description;3;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1004;TIM_InvalidDayOfYear;No description;4;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1005;TIM_TimeDoesNotFitFormat;No description;5;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x3701;TSI_BadTimestamp;No description;1;TIME_STAMPER_IF;fsfw/src/fsfw/timemanager/TimeStampIF.h -0x3c00;LPIF_PoolEntryNotFound;No description;0;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h -0x3c01;LPIF_PoolEntryTypeConflict;No description;1;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h -0x3e00;HKM_QueueOrDestinationInvalid;No description;0;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e01;HKM_WrongHkPacketType;No description;1;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e02;HKM_ReportingStatusUnchanged;No description;2;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e03;HKM_PeriodicHelperInvalid;No description;3;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e04;HKM_PoolobjectNotFound;No description;4;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e05;HKM_DatasetNotFound;No description;5;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x2901;TC_InvalidTargetState;No description;1;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h -0x29f1;TC_AboveOperationalLimit;No description;241;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h -0x29f2;TC_BelowOperationalLimit;No description;242;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h -0x2001;CSB_ExecutionComplete;No description;1;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2002;CSB_NoStepMessage;No description;2;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2003;CSB_ObjectBusy;No description;3;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2004;CSB_Busy;No description;4;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2005;CSB_InvalidTc;No description;5;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2006;CSB_InvalidObject;No description;6;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2007;CSB_InvalidReply;No description;7;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x4c00;SPPA_NoPacketFound;No description;0;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h -0x4c01;SPPA_SplitPacket;No description;1;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h -0x1d01;ATC_ActivityStarted;No description;1;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d02;ATC_InvalidSubservice;No description;2;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d03;ATC_IllegalApplicationData;No description;3;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d04;ATC_SendTmFailed;No description;4;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d05;ATC_Timeout;No description;5;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1401;SE_BufferTooShort;No description;1;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x1402;SE_StreamTooShort;No description;2;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x1403;SE_TooManyElements;No description;3;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x4500;HSPI_HalTimeoutRetval;No description;0;HAL_SPI;fsfw/src/fsfw_hal/stm32h7/spi/spiDefinitions.h +0x4501;HSPI_HalBusyRetval;No description;1;HAL_SPI;fsfw/src/fsfw_hal/stm32h7/spi/spiDefinitions.h +0x4502;HSPI_HalErrorRetval;No description;2;HAL_SPI;fsfw/src/fsfw_hal/stm32h7/spi/spiDefinitions.h +0x4601;HURT_UartReadFailure;No description;1;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h +0x4602;HURT_UartReadSizeMissmatch;No description;2;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h +0x4603;HURT_UartRxBufferTooSmall;No description;3;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h +0x4801;HGIO_UnknownGpioId;No description;1;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4802;HGIO_DriveGpioFailure;No description;2;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4803;HGIO_GpioTypeFailure;No description;3;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4804;HGIO_GpioInvalidInstance;No description;4;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4805;HGIO_GpioDuplicateDetected;No description;5;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4806;HGIO_GpioInitFailed;No description;6;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4807;HGIO_GpioGetValueFailed;No description;7;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.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 +0x4403;UXOS_CommandError;Command execution failed;3;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h +0x4404;UXOS_NoCommandLoadedOrPending;;4;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h +0x4406;UXOS_PcloseCallError;No description;6;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 6d26ed51..b2c6c28f 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -151,7 +151,9 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 12406;0x3076;LOST_BIT_LOCK_PDEC;INFO;Lost bit lock;linux/ipcore/PdecHandler.h 12407;0x3077;TOO_MANY_IRQS;MEDIUM;Too many IRQs over the time window of one second. P1: Allowed TCs;linux/ipcore/PdecHandler.h 12408;0x3078;POLL_SYSCALL_ERROR_PDEC;MEDIUM;No description;linux/ipcore/PdecHandler.h -12409;0x3079;WRITE_SYSCALL_ERROR_PDEC;MEDIUM;No description;linux/ipcore/PdecHandler.h +12409;0x3079;WRITE_SYSCALL_ERROR_PDEC;HIGH;No description;linux/ipcore/PdecHandler.h +12410;0x307a;PDEC_RESET_FAILED;HIGH;Failed to pull PDEC reset to low;linux/ipcore/PdecHandler.h +12411;0x307b;OPEN_IRQ_FILE_FAILED;HIGH;Failed to open the IRQ uio file;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 @@ -248,17 +250,29 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 13904;0x3650;WRITE_CONFIGFILE_FAILED;MEDIUM;No description;mission/utility/GlobalConfigHandler.h 13905;0x3651;READ_CONFIGFILE_FAILED;MEDIUM;No description;mission/utility/GlobalConfigHandler.h 14000;0x36b0;ALLOC_FAILURE;MEDIUM;No description;bsp_q7s/core/CoreController.h -14001;0x36b1;REBOOT_SW;MEDIUM; Software reboot occurred. Can also be a systemd reboot. P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h +14001;0x36b1;REBOOT_SW;LOW; Software reboot occurred. Can also be a systemd reboot. P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h 14002;0x36b2;REBOOT_MECHANISM_TRIGGERED;MEDIUM;The reboot mechanism was triggered. P1: First 16 bits: Last Chip, Last 16 bits: Last Copy, P2: Each byte is the respective reboot count for the slots;bsp_q7s/core/CoreController.h 14003;0x36b3;REBOOT_HW;MEDIUM;No description;bsp_q7s/core/CoreController.h 14004;0x36b4;NO_SD_CARD_ACTIVE;HIGH;No SD card was active. Core controller will attempt to re-initialize a SD card.;bsp_q7s/core/CoreController.h 14005;0x36b5;VERSION_INFO;INFO;P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash P2: First four letters of Git SHA is the last byte of P1 is set.;bsp_q7s/core/CoreController.h 14006;0x36b6;CURRENT_IMAGE_INFO;INFO;P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h -14100;0x3714;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/tmtc/PersistentTmStore.h -14200;0x3778;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/ThermalController.h -14201;0x3779;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/ThermalController.h -14202;0x377a;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h -14203;0x377b;PLOC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h -14204;0x377c;OBC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h -14205;0x377d;HPA_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h -14206;0x377e;PLPCDU_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h +14007;0x36b7;REBOOT_COUNTER;INFO;Total reboot counter, which is the sum of the boot count of all individual images.;bsp_q7s/core/CoreController.h +14008;0x36b8;INDIVIDUAL_BOOT_COUNTS;INFO;Get the boot count of the individual images. P1: First 16 bits boot count of image 0 0, last 16 bits boot count of image 0 1. P2: First 16 bits boot count of image 1 0, last 16 bits boot count of image 1 1.;bsp_q7s/core/CoreController.h +14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/ThermalController.h +14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/ThermalController.h +14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h +14103;0x3717;PLOC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h +14104;0x3718;OBC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h +14105;0x3719;HPA_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h +14106;0x371a;PLPCDU_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h +14201;0x3779;TX_TIMER_EXPIRED;INFO;The transmit timer to protect the Syrlinks expired P1: The current timer value;mission/system/objects/ComSubsystem.h +14202;0x377a;BIT_LOCK_TX_ON;INFO;Transmitter will be turned on due to detection of bitlock;mission/system/objects/ComSubsystem.h +14300;0x37dc;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/persistentTmStoreDefs.h +14301;0x37dd;FILE_TOO_LARGE;LOW;File in store too large. P1: Detected file size P2: Allowed file size;mission/persistentTmStoreDefs.h +14302;0x37de;BUSY_DUMPING_EVENT;INFO;No description;mission/persistentTmStoreDefs.h +14303;0x37df;DUMP_WAS_CANCELLED;LOW;Dump was cancelled. P1: Object ID of store.;mission/persistentTmStoreDefs.h +14305;0x37e1;DUMP_OK_STORE_DONE;INFO;P1: Number of dumped packets. P2: Total dumped bytes.;mission/persistentTmStoreDefs.h +14306;0x37e2;DUMP_NOK_STORE_DONE;INFO;P1: Number of dumped packets. P2: Total dumped bytes.;mission/persistentTmStoreDefs.h +14307;0x37e3;DUMP_MISC_STORE_DONE;INFO;P1: Number of dumped packets. P2: Total dumped bytes.;mission/persistentTmStoreDefs.h +14308;0x37e4;DUMP_HK_STORE_DONE;INFO;P1: Number of dumped packets. P2: Total dumped bytes.;mission/persistentTmStoreDefs.h +14309;0x37e5;DUMP_CFDP_STORE_DONE;INFO;P1: Number of dumped packets. P2: Total dumped bytes.;mission/persistentTmStoreDefs.h diff --git a/generators/bsp_q7s_objects.csv b/generators/bsp_q7s_objects.csv index 2bc03e8f..c90f83ee 100644 --- a/generators/bsp_q7s_objects.csv +++ b/generators/bsp_q7s_objects.csv @@ -45,6 +45,10 @@ 0x44330003;PLOC_MPSOC_HELPER 0x44330004;AXI_PTME_CONFIG 0x44330005;PTME_CONFIG +0x44330006;PTME_VC0_LIVE_TM +0x44330007;PTME_VC1_LOG_TM +0x44330008;PTME_VC2_HK_TM +0x44330009;PTME_VC3_CFDP_TM 0x44330015;PLOC_MPSOC_HANDLER 0x44330016;PLOC_SUPERVISOR_HANDLER 0x44330017;PLOC_SUPERVISOR_HELPER @@ -77,11 +81,13 @@ 0x49010005;GPIO_IF 0x49010006;SCEX_UART_READER 0x49020004;SPI_MAIN_COM_IF -0x49020005;RW_POLLING_TASK -0x49020006;SPI_RTD_COM_IF 0x49030003;UART_COM_IF 0x49040002;I2C_COM_IF 0x49050001;CSP_COM_IF +0x49060004;ACS_BOARD_POLLING_TASK +0x49060005;RW_POLLING_TASK +0x49060006;SPI_RTD_COM_IF +0x49060007;SUS_POLLING_TASK 0x50000100;CCSDS_PACKET_DISTRIBUTOR 0x50000200;PUS_PACKET_DISTRIBUTOR 0x50000300;TCP_TMTC_SERVER @@ -133,12 +139,15 @@ 0x60000004;HEATER_4_CAMERA 0x60000005;HEATER_5_STR 0x60000006;HEATER_6_DRO -0x60000007;HEATER_7_HPA +0x60000007;HEATER_7_SYRLINKS 0x73000001;ACS_BOARD_ASS 0x73000002;SUS_BOARD_ASS 0x73000003;TCS_BOARD_ASS -0x73000004;RW_ASS +0x73000004;RW_ASSY 0x73000006;CAM_SWITCHER +0x73000007;SYRLINKS_ASSY +0x73000008;IMTQ_ASSY +0x73000009;STR_ASSY 0x73000100;TM_FUNNEL 0x73000101;PUS_TM_FUNNEL 0x73000102;CFDP_TM_FUNNEL @@ -154,6 +163,11 @@ 0x73020003;NOT_OK_TM_STORE 0x73020004;HK_TM_STORE 0x73030000;CFDP_TM_STORE +0x73040000;LIVE_TM_TASK +0x73040001;LOG_STORE_AND_TM_TASK +0x73040002;HK_STORE_AND_TM_TASK +0x73040003;CFDP_STORE_AND_TM_TASK +0x73040004;DOWNLINK_RAM_STORE 0x73500000;CCSDS_IP_CORE_BRIDGE 0x90000003;THERMAL_TEMP_INSERTER 0xFFFFFFFF;NO_OBJECT diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index 4abc2ff7..fde6c972 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -1,7 +1,10 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x0000;OK;System-wide code for ok.;0;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h 0x0001;Failed;Unspecified system-wide code for failed.;1;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h -0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h +0x7000;PTM_DumpDone;No description;0;PERSISTENT_TM_STORE;mission/tmtc/PersistentTmStore.h +0x7001;PTM_BusyDumping;No description;1;PERSISTENT_TM_STORE;mission/tmtc/PersistentTmStore.h +0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h +0x7100;TMS_IsBusy;No description;0;TM_SINK;mission/tmtc/DirectTmSinkIF.h 0x5100;IMTQ_InvalidCommandCode;No description;0;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h 0x5101;IMTQ_MgmMeasurementLowLevelError;No description;1;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h 0x5102;IMTQ_ActuateCmdLowLevelError;No description;2;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h @@ -20,14 +23,25 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x52b5;RWHA_NoReply;Reaction wheel only responds with empty frames.;181;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h 0x52b6;RWHA_NoStartMarker;Expected a start marker as first byte;182;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h 0x52b7;RWHA_SpiReadTimeout;Timeout when reading reply;183;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h -0x58a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/devices/SusHandler.h -0x58a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/devices/SusHandler.h -0x66a0;SADPL_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;SA_DEPL_HANDLER;mission/devices/RwHandler.h -0x66a1;SADPL_InvalidRampTime;Action Message with invalid ramp time was received.;161;SA_DEPL_HANDLER;mission/devices/RwHandler.h -0x66a2;SADPL_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;SA_DEPL_HANDLER;mission/devices/RwHandler.h -0x66a3;SADPL_ExecutionFailed;Command execution failed;163;SA_DEPL_HANDLER;mission/devices/RwHandler.h -0x66a4;SADPL_CrcError;Reaction wheel reply has invalid crc;164;SA_DEPL_HANDLER;mission/devices/RwHandler.h -0x66a5;SADPL_ValueNotRead;No description;165;SA_DEPL_HANDLER;mission/devices/RwHandler.h +0x58a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/devices/LegacySusHandler.h +0x58a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/devices/LegacySusHandler.h +0x5d00;GOMS_PacketTooLong;No description;0;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d01;GOMS_InvalidTableId;No description;1;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d02;GOMS_InvalidAddress;No description;2;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d03;GOMS_InvalidParamSize;No description;3;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d04;GOMS_InvalidPayloadSize;No description;4;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d05;GOMS_UnknownReplyId;No description;5;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5da0;GOMS_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;GOM_SPACE_HANDLER;mission/devices/RwHandler.h +0x5da1;GOMS_InvalidRampTime;Action Message with invalid ramp time was received.;161;GOM_SPACE_HANDLER;mission/devices/RwHandler.h +0x5da2;GOMS_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;GOM_SPACE_HANDLER;mission/devices/RwHandler.h +0x5da3;GOMS_ExecutionFailed;Command execution failed;163;GOM_SPACE_HANDLER;mission/devices/RwHandler.h +0x5da4;GOMS_CrcError;Reaction wheel reply has invalid crc;164;GOM_SPACE_HANDLER;mission/devices/RwHandler.h +0x5da5;GOMS_ValueNotRead;No description;165;GOM_SPACE_HANDLER;mission/devices/RwHandler.h +0x4fa1;HEATER_CommandNotSupported;No description;161;HEATER_HANDLER;mission/devices/HeaterHandler.h +0x4fa2;HEATER_InitFailed;No description;162;HEATER_HANDLER;mission/devices/HeaterHandler.h +0x4fa3;HEATER_InvalidSwitchNr;No description;163;HEATER_HANDLER;mission/devices/HeaterHandler.h +0x4fa4;HEATER_MainSwitchSetTimeout;No description;164;HEATER_HANDLER;mission/devices/HeaterHandler.h +0x4fa5;HEATER_CommandAlreadyWaiting;No description;165;HEATER_HANDLER;mission/devices/HeaterHandler.h 0x50a0;SYRLINKS_CrcFailure;No description;160;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h 0x50a1;SYRLINKS_UartFraminOrParityErrorAck;No description;161;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h 0x50a2;SYRLINKS_BadCharacterAck;No description;162;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h @@ -37,20 +51,13 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x50a6;SYRLINKS_BadCrcAck;No description;166;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h 0x50a7;SYRLINKS_ReplyWrongSize;No description;167;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h 0x50a8;SYRLINKS_MissingStartFrameCharacter;No description;168;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h -0x5d00;GOMS_PacketTooLong;No description;0;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d01;GOMS_InvalidTableId;No description;1;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d02;GOMS_InvalidAddress;No description;2;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d03;GOMS_InvalidParamSize;No description;3;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d04;GOMS_InvalidPayloadSize;No description;4;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d05;GOMS_UnknownReplyId;No description;5;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x4fa1;HEATER_CommandNotSupported;No description;161;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x4fa2;HEATER_InitFailed;No description;162;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x4fa3;HEATER_InvalidSwitchNr;No description;163;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x4fa4;HEATER_MainSwitchSetTimeout;No description;164;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x4fa5;HEATER_CommandAlreadyWaiting;No description;165;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h -0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h +0x66a0;SADPL_CommandNotSupported;No description;160;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a1;SADPL_DeploymentAlreadyExecuting;No description;161;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a2;SADPL_MainSwitchTimeoutFailure;No description;162;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a3;SADPL_SwitchingDeplSa1Failed;No description;163;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a4;SADPL_SwitchingDeplSa2Failed;No description;164;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h 0x6c01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h +0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h 0x6d01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h 0x6a02;ACSMEKF_MekfUninitialized;No description;2;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a03;ACSMEKF_MekfNoGyrData;No description;3;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h @@ -60,105 +67,125 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x6a07;ACSMEKF_MekfInitialized;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a08;ACSMEKF_MekfRunning;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6900;ACSCTRL_FileDeletionFailed;No description;0;ACS_CTRL;mission/controller/AcsController.h -0x4500;HSPI_OpeningFileFailed;No description;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h -0x4501;HSPI_FullDuplexTransferFailed;No description;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h -0x4502;HSPI_HalfDuplexTransferFailed;No description;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h -0x4801;HGIO_UnknownGpioId;No description;1;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4802;HGIO_DriveGpioFailure;No description;2;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4803;HGIO_GpioTypeFailure;No description;3;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4804;HGIO_GpioInvalidInstance;No description;4;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4805;HGIO_GpioDuplicateDetected;No description;5;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4806;HGIO_GpioInitFailed;No description;6;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4807;HGIO_GpioGetValueFailed;No description;7;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4601;HURT_UartReadFailure;No description;1;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h -0x4602;HURT_UartReadSizeMissmatch;No description;2;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h -0x4603;HURT_UartRxBufferTooSmall;No description;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 -0x4403;UXOS_CommandError;Command execution failed;3;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h -0x4404;UXOS_NoCommandLoadedOrPending;;4;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h -0x4406;UXOS_PcloseCallError;No description;6;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h -0x2801;SM_DataTooLarge;No description;1;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2802;SM_DataStorageFull;No description;2;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2803;SM_IllegalStorageId;No description;3;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2804;SM_DataDoesNotExist;No description;4;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2805;SM_IllegalAddress;No description;5;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2806;SM_PoolTooLarge;No description;6;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x0601;PP_DoItMyself;No description;1;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0602;PP_PointsToVariable;No description;2;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0603;PP_PointsToMemory;No description;3;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0604;PP_ActivityCompleted;No description;4;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0605;PP_PointsToVectorUint8;No description;5;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0606;PP_PointsToVectorUint16;No description;6;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0607;PP_PointsToVectorUint32;No description;7;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0608;PP_PointsToVectorFloat;No description;8;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06a0;PP_DumpNotSupported;No description;160;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e0;PP_InvalidSize;No description;224;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e1;PP_InvalidAddress;No description;225;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e2;PP_InvalidContent;No description;226;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e3;PP_UnalignedAccess;No description;227;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e4;PP_WriteProtected;No description;228;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x13e0;MH_UnknownCmd;No description;224;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x13e1;MH_InvalidAddress;No description;225;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x13e2;MH_InvalidSize;No description;226;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x13e3;MH_StateMismatch;No description;227;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x38a1;SGP4_InvalidEccentricity;No description;161;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a2;SGP4_InvalidMeanMotion;No description;162;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a3;SGP4_InvalidPerturbationElements;No description;163;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a4;SGP4_InvalidSemiLatusRectum;No description;164;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a5;SGP4_InvalidEpochElements;No description;165;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a6;SGP4_SatelliteHasDecayed;No description;166;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38b1;SGP4_TleTooOld;No description;177;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38b2;SGP4_TleNotInitialized;No description;178;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x1801;FF_Full;No description;1;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h -0x1802;FF_Empty;No description;2;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h -0x1601;FMM_MapFull;No description;1;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h -0x1602;FMM_KeyDoesNotExist;No description;2;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h -0x3901;MUX_NotEnoughResources;No description;1;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3902;MUX_InsufficientMemory;No description;2;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3903;MUX_NoPrivilege;No description;3;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3904;MUX_WrongAttributeSetting;No description;4;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3905;MUX_MutexAlreadyLocked;No description;5;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3906;MUX_MutexNotFound;No description;6;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3907;MUX_MutexMaxLocks;No description;7;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3908;MUX_CurrThreadAlreadyOwnsMutex;No description;8;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3909;MUX_CurrThreadDoesNotOwnMutex;No description;9;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x390a;MUX_MutexTimeout;No description;10;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x390b;MUX_MutexInvalidId;No description;11;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x390c;MUX_MutexDestroyedWhileWaiting;No description;12;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3a01;MQI_Empty;No description;1;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x3a02;MQI_Full;No space left for more messages;2;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x3a03;MQI_NoReplyPartner;Returned if a reply method was called without partner;3;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x3a04;MQI_DestinationInvalid;Returned if the target destination is invalid.;4;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x0f01;CM_UnknownCommand;No description;1;COMMAND_MESSAGE;fsfw/src/fsfw/ipc/CommandMessageIF.h +0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NvmParameterBase.h +0x2c01;CCS_BcIsSetVrCommand;No description;1;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2c02;CCS_BcIsUnlockCommand;No description;2;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cb0;CCS_BcIllegalCommand;No description;176;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cb1;CCS_BoardReadingNotFinished;No description;177;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf0;CCS_NsPositiveW;No description;240;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf1;CCS_NsNegativeW;No description;241;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf2;CCS_NsLockout;No description;242;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf3;CCS_FarmInLockout;No description;243;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf4;CCS_FarmInWait;No description;244;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce0;CCS_WrongSymbol;No description;224;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce1;CCS_DoubleStart;No description;225;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce2;CCS_StartSymbolMissed;No description;226;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce3;CCS_EndWithoutStart;No description;227;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce4;CCS_TooLarge;No description;228;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce5;CCS_TooShort;No description;229;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce6;CCS_WrongTfVersion;No description;230;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce7;CCS_WrongSpacecraftId;No description;231;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce8;CCS_NoValidFrameType;No description;232;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce9;CCS_CrcFailed;No description;233;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cea;CCS_VcNotFound;No description;234;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ceb;CCS_ForwardingFailed;No description;235;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cec;CCS_ContentTooLarge;No description;236;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ced;CCS_ResidualData;No description;237;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cee;CCS_DataCorrupted;No description;238;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cef;CCS_IllegalSegmentationFlag;No description;239;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd0;CCS_IllegalFlagCombination;No description;208;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd1;CCS_ShorterThanHeader;No description;209;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd2;CCS_TooShortBlockedPacket;No description;210;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd3;CCS_TooShortMapExtraction;No description;211;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x3b00;SPH_ConnBroken;No description;0;SEMAPHORE_IF;fsfw/src/fsfw/osal/common/TcpTmTcServer.h +0x2a01;IEC_NoConfigurationTable;No description;1;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a02;IEC_NoCpuTable;No description;2;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a03;IEC_InvalidWorkspaceAddress;No description;3;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a04;IEC_TooLittleWorkspace;No description;4;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a05;IEC_WorkspaceAllocation;No description;5;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a06;IEC_InterruptStackTooSmall;No description;6;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a07;IEC_ThreadExitted;No description;7;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a08;IEC_InconsistentMpInformation;No description;8;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a09;IEC_InvalidNode;No description;9;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0a;IEC_NoMpci;No description;10;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0b;IEC_BadPacket;No description;11;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0c;IEC_OutOfPackets;No description;12;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0d;IEC_OutOfGlobalObjects;No description;13;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0e;IEC_OutOfProxies;No description;14;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0f;IEC_InvalidGlobalId;No description;15;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a10;IEC_BadStackHook;No description;16;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a11;IEC_BadAttributes;No description;17;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a12;IEC_ImplementationKeyCreateInconsistency;No description;18;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a13;IEC_ImplementationBlockingOperationCancel;No description;19;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a14;IEC_MutexObtainFromBadState;No description;20;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a15;IEC_UnlimitedAndMaximumIs0;No description;21;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h 0x0e01;HM_InvalidMode;No description;1;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h 0x0e02;HM_TransNotAllowed;No description;2;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h 0x0e03;HM_InTransition;No description;3;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h 0x0e04;HM_InvalidSubmode;No description;4;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h -0x0c02;MS_InvalidEntry;No description;2;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h -0x0c03;MS_TooManyElements;No description;3;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h -0x0c04;MS_CantStoreEmpty;No description;4;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h -0x0b01;SB_ChildNotFound;No description;1;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b02;SB_ChildInfoUpdated;No description;2;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b03;SB_ChildDoesntHaveModes;No description;3;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b04;SB_CouldNotInsertChild;No description;4;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b05;SB_TableContainsInvalidObjectId;No description;5;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0d01;SS_SequenceAlreadyExists;No description;1;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d02;SS_TableAlreadyExists;No description;2;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d03;SS_TableDoesNotExist;No description;3;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d04;SS_TableOrSequenceLengthInvalid;No description;4;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d05;SS_SequenceDoesNotExist;No description;5;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d06;SS_TableContainsInvalidObjectId;No description;6;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d07;SS_FallbackSequenceDoesNotExist;No description;7;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d08;SS_NoTargetTable;No description;8;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d09;SS_SequenceOrTableTooLong;No description;9;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d0b;SS_IsFallbackSequence;No description;11;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d0c;SS_AccessDenied;No description;12;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d0e;SS_TableInUse;No description;14;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0da1;SS_TargetTableNotReached;No description;161;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0da2;SS_TableCheckFailed;No description;162;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x2501;EV_ListenerNotFound;No description;1;EVENT_MANAGER_IF;fsfw/src/fsfw/events/EventManagerIF.h +0x2e01;HPA_InvalidIdentifierId;No description;1;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2e02;HPA_InvalidDomainId;No description;2;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2e03;HPA_InvalidValue;No description;3;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2e05;HPA_ReadOnly;No description;5;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2d01;PAW_UnknownDatatype;No description;1;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d02;PAW_DatatypeMissmatch;No description;2;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d03;PAW_Readonly;No description;3;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d04;PAW_TooBig;No description;4;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d05;PAW_SourceNotSet;No description;5;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d06;PAW_OutOfBounds;No description;6;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d07;PAW_NotSet;No description;7;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d08;PAW_ColumnOrRowsZero;No description;8;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x3201;CF_ObjectHasNoFunctions;No description;1;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h +0x3202;CF_AlreadyCommanding;No description;2;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h +0x3301;HF_IsBusy;No description;1;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x3302;HF_InvalidParameters;No description;2;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x3303;HF_ExecutionFinished;No description;3;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x3304;HF_InvalidActionId;No description;4;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x0201;OM_InsertionFailed;No description;1;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x0202;OM_NotFound;No description;2;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x0203;OM_ChildInitFailed;No description;3;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x0204;OM_InternalErrReporterUninit;No description;4;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x2600;FDI_YourFault;No description;0;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h +0x2601;FDI_MyFault;No description;1;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h +0x2602;FDI_ConfirmLater;No description;2;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h +0x2201;TMF_Busy;No description;1;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2202;TMF_LastPacketFound;No description;2;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2203;TMF_StopFetch;No description;3;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2204;TMF_Timeout;No description;4;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2205;TMF_TmChannelFull;No description;5;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2206;TMF_NotStored;No description;6;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2207;TMF_AllDeleted;No description;7;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2208;TMF_InvalidData;No description;8;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2209;TMF_NotReady;No description;9;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2101;TMB_Busy;No description;1;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2102;TMB_Full;No description;2;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2103;TMB_Empty;No description;3;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2104;TMB_NullRequested;No description;4;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2105;TMB_TooLarge;No description;5;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2106;TMB_NotReady;No description;6;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2107;TMB_DumpError;No description;7;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2108;TMB_CrcError;No description;8;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2109;TMB_Timeout;No description;9;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210a;TMB_IdlePacketFound;No description;10;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210b;TMB_TelecommandFound;No description;11;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210c;TMB_NoPusATm;No description;12;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210d;TMB_TooSmall;No description;13;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210e;TMB_BlockNotFound;No description;14;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210f;TMB_InvalidRequest;No description;15;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x1c01;TCD_PacketLost;No description;1;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h +0x1c02;TCD_DestinationNotFound;No description;2;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h +0x1c03;TCD_ServiceIdAlreadyExists;No description;3;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h +0x1b00;TCC_NoDestinationFound;No description;0;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b01;TCC_InvalidCcsdsVersion;No description;1;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b02;TCC_InvalidApid;No description;2;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b03;TCC_InvalidPacketType;No description;3;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b04;TCC_InvalidSecHeaderField;No description;4;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b05;TCC_IncorrectPrimaryHeader;No description;5;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b07;TCC_IncompletePacket;No description;7;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b08;TCC_InvalidPusVersion;No description;8;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b09;TCC_IncorrectChecksum;No description;9;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b0a;TCC_IllegalPacketSubtype;No description;10;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b0b;TCC_IncorrectSecondaryHeader;No description;11;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h 0x04e1;RMP_CommandNoDescriptorsAvailable;No description;225;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x04e2;RMP_CommandBufferFull;No description;226;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x04e3;RMP_CommandChannelOutOfRange;No description;227;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h @@ -199,9 +226,95 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x040a;RMP_ReplyCommandNotImplementedOrNotAuthorised;No description;10;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x040b;RMP_ReplyRmwDataLengthError;No description;11;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x040c;RMP_ReplyInvalidTargetLogicalAddress;No description;12;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h -0x1401;SE_BufferTooShort;No description;1;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h -0x1402;SE_StreamTooShort;No description;2;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h -0x1403;SE_TooManyElements;No description;3;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x2801;SM_DataTooLarge;No description;1;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2802;SM_DataStorageFull;No description;2;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2803;SM_IllegalStorageId;No description;3;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2804;SM_DataDoesNotExist;No description;4;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2805;SM_IllegalAddress;No description;5;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2806;SM_PoolTooLarge;No description;6;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x38a1;SGP4_InvalidEccentricity;No description;161;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a2;SGP4_InvalidMeanMotion;No description;162;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a3;SGP4_InvalidPerturbationElements;No description;163;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a4;SGP4_InvalidSemiLatusRectum;No description;164;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a5;SGP4_InvalidEpochElements;No description;165;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a6;SGP4_SatelliteHasDecayed;No description;166;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38b1;SGP4_TleTooOld;No description;177;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38b2;SGP4_TleNotInitialized;No description;178;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x2401;MT_NoPacketFound;No description;1;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/DleParser.h +0x2402;MT_PossiblePacketLoss;No description;2;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/DleParser.h +0x2403;MT_NoMatch;No description;3;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h +0x2404;MT_Full;No description;4;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h +0x2405;MT_NewNodeCreated;No description;5;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h +0x3f01;DLEE_StreamTooShort;No description;1;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleEncoder.h +0x3f02;DLEE_DecodingError;No description;2;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleEncoder.h +0x2f01;ASC_TooLongForTargetType;No description;1;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h +0x2f02;ASC_InvalidCharacters;No description;2;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h +0x2f03;ASC_BufferTooSmall;No description;3;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h +0x0f01;CM_UnknownCommand;No description;1;COMMAND_MESSAGE;fsfw/src/fsfw/ipc/CommandMessageIF.h +0x3a01;MQI_Empty;No description;1;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3a02;MQI_Full;No space left for more messages;2;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3a03;MQI_NoReplyPartner;Returned if a reply method was called without partner;3;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3a04;MQI_DestinationInvalid;Returned if the target destination is invalid.;4;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3901;MUX_NotEnoughResources;No description;1;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3902;MUX_InsufficientMemory;No description;2;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3903;MUX_NoPrivilege;No description;3;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3904;MUX_WrongAttributeSetting;No description;4;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3905;MUX_MutexAlreadyLocked;No description;5;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3906;MUX_MutexNotFound;No description;6;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3907;MUX_MutexMaxLocks;No description;7;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3908;MUX_CurrThreadAlreadyOwnsMutex;No description;8;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3909;MUX_CurrThreadDoesNotOwnMutex;No description;9;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x390a;MUX_MutexTimeout;No description;10;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x390b;MUX_MutexInvalidId;No description;11;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x390c;MUX_MutexDestroyedWhileWaiting;No description;12;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3b01;SPH_SemaphoreTimeout;No description;1;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x3b02;SPH_SemaphoreNotOwned;No description;2;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x3b03;SPH_SemaphoreInvalid;No description;3;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x1e00;PUS_InvalidPusVersion;No description;0;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h +0x1e01;PUS_InvalidCrc16;No description;1;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h +0x3601;CFDP_InvalidTlvType;No description;1;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3602;CFDP_InvalidDirectiveField;No description;2;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3603;CFDP_InvalidPduDatafieldLen;No description;3;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3604;CFDP_InvalidAckDirectiveFields;No description;4;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3605;CFDP_MetadataCantParseOptions;No description;5;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3606;CFDP_NakCantParseOptions;No description;6;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3607;CFDP_FinishedCantParseFsResponses;No description;7;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3608;CFDP_FilestoreRequiresSecondFile;No description;8;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3609;CFDP_FilestoreResponseCantParseFsMessage;No description;9;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x360a;CFDP_InvalidPduFormat;No description;10;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x2901;TC_InvalidTargetState;No description;1;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h +0x29f1;TC_AboveOperationalLimit;No description;241;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h +0x29f2;TC_BelowOperationalLimit;No description;242;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h +0x0c02;MS_InvalidEntry;No description;2;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h +0x0c03;MS_TooManyElements;No description;3;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h +0x0c04;MS_CantStoreEmpty;No description;4;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h +0x0d01;SS_SequenceAlreadyExists;No description;1;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d02;SS_TableAlreadyExists;No description;2;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d03;SS_TableDoesNotExist;No description;3;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d04;SS_TableOrSequenceLengthInvalid;No description;4;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d05;SS_SequenceDoesNotExist;No description;5;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d06;SS_TableContainsInvalidObjectId;No description;6;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d07;SS_FallbackSequenceDoesNotExist;No description;7;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d08;SS_NoTargetTable;No description;8;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d09;SS_SequenceOrTableTooLong;No description;9;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d0b;SS_IsFallbackSequence;No description;11;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d0c;SS_AccessDenied;No description;12;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d0e;SS_TableInUse;No description;14;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0da1;SS_TargetTableNotReached;No description;161;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0da2;SS_TableCheckFailed;No description;162;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0b01;SB_ChildNotFound;No description;1;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b02;SB_ChildInfoUpdated;No description;2;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b03;SB_ChildDoesntHaveModes;No description;3;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b04;SB_CouldNotInsertChild;No description;4;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b05;SB_TableContainsInvalidObjectId;No description;5;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x3e00;HKM_QueueOrDestinationInvalid;No description;0;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e01;HKM_WrongHkPacketType;No description;1;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e02;HKM_ReportingStatusUnchanged;No description;2;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e03;HKM_PeriodicHelperInvalid;No description;3;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e04;HKM_PoolobjectNotFound;No description;4;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e05;HKM_DatasetNotFound;No description;5;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3c00;LPIF_PoolEntryNotFound;No description;0;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h +0x3c01;LPIF_PoolEntryTypeConflict;No description;1;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h 0x3da0;PVA_InvalidReadWriteMode;No description;160;POOL_VARIABLE_IF;fsfw/src/fsfw/datapool/PoolVariableIF.h 0x3da1;PVA_InvalidPoolEntry;No description;161;POOL_VARIABLE_IF;fsfw/src/fsfw/datapool/PoolVariableIF.h 0x0801;DPS_InvalidParameterDefinition;No description;1;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h @@ -210,20 +323,35 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x0804;DPS_DataSetUninitialised;No description;4;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h 0x0805;DPS_DataSetFull;No description;5;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h 0x0806;DPS_PoolVarNull;No description;6;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h -0x1c01;TCD_PacketLost;No description;1;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h -0x1c02;TCD_DestinationNotFound;No description;2;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h -0x1c03;TCD_ServiceIdAlreadyExists;No description;3;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h -0x1b00;TCC_NoDestinationFound;No description;0;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b01;TCC_InvalidCcsdsVersion;No description;1;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b02;TCC_InvalidApid;No description;2;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b03;TCC_InvalidPacketType;No description;3;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b04;TCC_InvalidSecHeaderField;No description;4;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b05;TCC_IncorrectPrimaryHeader;No description;5;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b07;TCC_IncompletePacket;No description;7;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b08;TCC_InvalidPusVersion;No description;8;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b09;TCC_IncorrectChecksum;No description;9;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b0a;TCC_IllegalPacketSubtype;No description;10;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b0b;TCC_IncorrectSecondaryHeader;No description;11;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1000;TIM_UnsupportedTimeFormat;No description;0;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1001;TIM_NotEnoughInformationForTargetFormat;No description;1;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1002;TIM_LengthMismatch;No description;2;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1003;TIM_InvalidTimeFormat;No description;3;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1004;TIM_InvalidDayOfYear;No description;4;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1005;TIM_TimeDoesNotFitFormat;No description;5;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x3701;TSI_BadTimestamp;No description;1;TIME_STAMPER_IF;fsfw/src/fsfw/timemanager/TimeStampIF.h +0x1d01;ATC_ActivityStarted;No description;1;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d02;ATC_InvalidSubservice;No description;2;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d03;ATC_IllegalApplicationData;No description;3;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d04;ATC_SendTmFailed;No description;4;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d05;ATC_Timeout;No description;5;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x4c00;SPPA_NoPacketFound;No description;0;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h +0x4c01;SPPA_SplitPacket;No description;1;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h +0x2001;CSB_ExecutionComplete;No description;1;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2002;CSB_NoStepMessage;No description;2;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2003;CSB_ObjectBusy;No description;3;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2004;CSB_Busy;No description;4;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2005;CSB_InvalidTc;No description;5;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2006;CSB_InvalidObject;No description;6;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2007;CSB_InvalidReply;No description;7;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x1801;FF_Full;No description;1;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h +0x1802;FF_Empty;No description;2;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h +0x1601;FMM_MapFull;No description;1;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h +0x1602;FMM_KeyDoesNotExist;No description;2;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h +0x2501;EV_ListenerNotFound;No description;1;EVENT_MANAGER_IF;fsfw/src/fsfw/events/EventManagerIF.h +0x1701;HHI_ObjectNotHealthy;No description;1;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h +0x1702;HHI_InvalidHealthState;No description;2;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h +0x1703;HHI_IsExternallyControlled;No description;3;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h 0x3001;POS_InPowerTransition;No description;1;POWER_SWITCHER;fsfw/src/fsfw/power/PowerSwitcher.h 0x3002;POS_SwitchStateMismatch;No description;2;POWER_SWITCHER;fsfw/src/fsfw/power/PowerSwitcher.h 0x0501;PS_SwitchOn;No description;1;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h @@ -231,76 +359,23 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x0502;PS_SwitchTimeout;No description;2;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h 0x0503;PS_FuseOn;No description;3;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h 0x0504;PS_FuseOff;No description;4;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h -0x3b00;SPH_ConnBroken;No description;0;SEMAPHORE_IF;fsfw/src/fsfw/osal/common/TcpTmTcServer.h -0x2a01;IEC_NoConfigurationTable;No description;1;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a02;IEC_NoCpuTable;No description;2;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a03;IEC_InvalidWorkspaceAddress;No description;3;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a04;IEC_TooLittleWorkspace;No description;4;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a05;IEC_WorkspaceAllocation;No description;5;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a06;IEC_InterruptStackTooSmall;No description;6;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a07;IEC_ThreadExitted;No description;7;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a08;IEC_InconsistentMpInformation;No description;8;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a09;IEC_InvalidNode;No description;9;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0a;IEC_NoMpci;No description;10;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0b;IEC_BadPacket;No description;11;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0c;IEC_OutOfPackets;No description;12;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0d;IEC_OutOfGlobalObjects;No description;13;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0e;IEC_OutOfProxies;No description;14;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0f;IEC_InvalidGlobalId;No description;15;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a10;IEC_BadStackHook;No description;16;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a11;IEC_BadAttributes;No description;17;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a12;IEC_ImplementationKeyCreateInconsistency;No description;18;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a13;IEC_ImplementationBlockingOperationCancel;No description;19;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a14;IEC_MutexObtainFromBadState;No description;20;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a15;IEC_UnlimitedAndMaximumIs0;No description;21;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2600;FDI_YourFault;No description;0;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h -0x2601;FDI_MyFault;No description;1;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h -0x2602;FDI_ConfirmLater;No description;2;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h -0x1e00;PUS_InvalidPusVersion;No description;0;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h -0x1e01;PUS_InvalidCrc16;No description;1;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h -0x0201;OM_InsertionFailed;No description;1;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x0202;OM_NotFound;No description;2;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x0203;OM_ChildInitFailed;No description;3;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x0204;OM_InternalErrReporterUninit;No description;4;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x2201;TMF_Busy;No description;1;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2202;TMF_LastPacketFound;No description;2;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2203;TMF_StopFetch;No description;3;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2204;TMF_Timeout;No description;4;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2205;TMF_TmChannelFull;No description;5;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2206;TMF_NotStored;No description;6;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2207;TMF_AllDeleted;No description;7;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2208;TMF_InvalidData;No description;8;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2209;TMF_NotReady;No description;9;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2101;TMB_Busy;No description;1;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2102;TMB_Full;No description;2;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2103;TMB_Empty;No description;3;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2104;TMB_NullRequested;No description;4;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2105;TMB_TooLarge;No description;5;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2106;TMB_NotReady;No description;6;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2107;TMB_DumpError;No description;7;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2108;TMB_CrcError;No description;8;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2109;TMB_Timeout;No description;9;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210a;TMB_IdlePacketFound;No description;10;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210b;TMB_TelecommandFound;No description;11;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210c;TMB_NoPusATm;No description;12;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210d;TMB_TooSmall;No description;13;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210e;TMB_BlockNotFound;No description;14;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210f;TMB_InvalidRequest;No description;15;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2d01;PAW_UnknownDatatype;No description;1;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d02;PAW_DatatypeMissmatch;No description;2;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d03;PAW_Readonly;No description;3;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d04;PAW_TooBig;No description;4;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d05;PAW_SourceNotSet;No description;5;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d06;PAW_OutOfBounds;No description;6;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d07;PAW_NotSet;No description;7;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d08;PAW_ColumnOrRowsZero;No description;8;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2e01;HPA_InvalidIdentifierId;No description;1;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x2e02;HPA_InvalidDomainId;No description;2;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x2e03;HPA_InvalidValue;No description;3;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x2e05;HPA_ReadOnly;No description;5;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x3b01;SPH_SemaphoreTimeout;No description;1;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h -0x3b02;SPH_SemaphoreNotOwned;No description;2;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h -0x3b03;SPH_SemaphoreInvalid;No description;3;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x4300;FILS_GenericFileError;No description;0;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4301;FILS_GenericDirError;No description;1;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4302;FILS_FilesystemInactive;No description;2;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4303;FILS_GenericRenameError;No description;3;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4304;FILS_IsBusy;No description;4;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4305;FILS_InvalidParameters;No description;5;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430a;FILS_FileDoesNotExist;No description;10;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430b;FILS_FileAlreadyExists;No description;11;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430c;FILS_NotAFile;No description;12;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430d;FILS_FileLocked;No description;13;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430e;FILS_PermissionDenied;No description;14;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4315;FILS_DirectoryDoesNotExist;No description;21;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4316;FILS_DirectoryAlreadyExists;No description;22;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4317;FILS_NotADirectory;No description;23;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4318;FILS_DirectoryNotEmpty;No description;24;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x431e;FILS_SequencePacketMissingWrite;No description;30;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x431f;FILS_SequencePacketMissingRead;No description;31;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h 0x1a01;TRC_NotEnoughSensors;No description;1;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h 0x1a02;TRC_LowestValueOol;No description;2;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h 0x1a03;TRC_HighestValueOol;No description;3;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h @@ -319,74 +394,36 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x31e2;LIM_WrongPid;No description;226;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h 0x31e3;LIM_WrongLimitId;No description;227;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h 0x31ee;LIM_MonitorNotFound;No description;238;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x3601;CFDP_InvalidTlvType;No description;1;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3602;CFDP_InvalidDirectiveField;No description;2;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3603;CFDP_InvalidPduDatafieldLen;No description;3;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3604;CFDP_InvalidAckDirectiveFields;No description;4;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3605;CFDP_MetadataCantParseOptions;No description;5;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3606;CFDP_NakCantParseOptions;No description;6;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3607;CFDP_FinishedCantParseFsResponses;No description;7;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3608;CFDP_FilestoreRequiresSecondFile;No description;8;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3609;CFDP_FilestoreResponseCantParseFsMessage;No description;9;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x360a;CFDP_InvalidPduFormat;No description;10;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x4300;FILS_GenericFileError;No description;0;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4301;FILS_GenericDirError;No description;1;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4302;FILS_FilesystemInactive;No description;2;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4303;FILS_GenericRenameError;No description;3;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4304;FILS_IsBusy;No description;4;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4305;FILS_InvalidParameters;No description;5;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430a;FILS_FileDoesNotExist;No description;10;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430b;FILS_FileAlreadyExists;No description;11;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430c;FILS_NotAFile;No description;12;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430d;FILS_FileLocked;No description;13;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430e;FILS_PermissionDenied;No description;14;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4315;FILS_DirectoryDoesNotExist;No description;21;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4316;FILS_DirectoryAlreadyExists;No description;22;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4317;FILS_NotADirectory;No description;23;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4318;FILS_DirectoryNotEmpty;No description;24;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x431e;FILS_SequencePacketMissingWrite;No description;30;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x431f;FILS_SequencePacketMissingRead;No description;31;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x2c01;CCS_BcIsSetVrCommand;No description;1;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2c02;CCS_BcIsUnlockCommand;No description;2;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cb0;CCS_BcIllegalCommand;No description;176;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cb1;CCS_BoardReadingNotFinished;No description;177;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf0;CCS_NsPositiveW;No description;240;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf1;CCS_NsNegativeW;No description;241;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf2;CCS_NsLockout;No description;242;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf3;CCS_FarmInLockout;No description;243;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf4;CCS_FarmInWait;No description;244;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce0;CCS_WrongSymbol;No description;224;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce1;CCS_DoubleStart;No description;225;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce2;CCS_StartSymbolMissed;No description;226;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce3;CCS_EndWithoutStart;No description;227;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce4;CCS_TooLarge;No description;228;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce5;CCS_TooShort;No description;229;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce6;CCS_WrongTfVersion;No description;230;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce7;CCS_WrongSpacecraftId;No description;231;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce8;CCS_NoValidFrameType;No description;232;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce9;CCS_CrcFailed;No description;233;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cea;CCS_VcNotFound;No description;234;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ceb;CCS_ForwardingFailed;No description;235;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cec;CCS_ContentTooLarge;No description;236;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ced;CCS_ResidualData;No description;237;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cee;CCS_DataCorrupted;No description;238;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cef;CCS_IllegalSegmentationFlag;No description;239;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd0;CCS_IllegalFlagCombination;No description;208;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd1;CCS_ShorterThanHeader;No description;209;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd2;CCS_TooShortBlockedPacket;No description;210;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd3;CCS_TooShortMapExtraction;No description;211;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h 0x4201;PUS11_InvalidTypeTimeWindow;No description;1;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h 0x4202;PUS11_InvalidTimeWindow;No description;2;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h 0x4203;PUS11_TimeshiftingNotPossible;No description;3;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h 0x4204;PUS11_InvalidRelativeTime;No description;4;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h 0x4205;PUS11_ContainedTcTooSmall;No description;5;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h 0x4206;PUS11_ContainedTcCrcMissmatch;No description;6;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h -0x3401;DC_NoReplyReceived;No description;1;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3402;DC_ProtocolError;No description;2;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3403;DC_Nullpointer;No description;3;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3404;DC_InvalidCookieType;No description;4;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3405;DC_NotActive;No description;5;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3406;DC_TooMuchData;No description;6;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x0601;PP_DoItMyself;No description;1;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0602;PP_PointsToVariable;No description;2;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0603;PP_PointsToMemory;No description;3;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0604;PP_ActivityCompleted;No description;4;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0605;PP_PointsToVectorUint8;No description;5;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0606;PP_PointsToVectorUint16;No description;6;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0607;PP_PointsToVectorUint32;No description;7;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0608;PP_PointsToVectorFloat;No description;8;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06a0;PP_DumpNotSupported;No description;160;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e0;PP_InvalidSize;No description;224;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e1;PP_InvalidAddress;No description;225;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e2;PP_InvalidContent;No description;226;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e3;PP_UnalignedAccess;No description;227;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e4;PP_WriteProtected;No description;228;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x13e0;MH_UnknownCmd;No description;224;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x13e1;MH_InvalidAddress;No description;225;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x13e2;MH_InvalidSize;No description;226;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x13e3;MH_StateMismatch;No description;227;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x1201;AB_NeedSecondStep;No description;1;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1202;AB_NeedToReconfigure;No description;2;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1203;AB_ModeFallback;No description;3;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1204;AB_ChildNotCommandable;No description;4;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1205;AB_NeedToChangeHealth;No description;5;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x12a1;AB_NotEnoughChildrenInCorrectState;No description;161;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h 0x03a0;DHB_InvalidChannel;No description;160;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03b0;DHB_AperiodicReply;No description;176;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03b1;DHB_IgnoreReplyData;No description;177;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -396,12 +433,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x03d0;DHB_NoSwitch;No description;208;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03e0;DHB_ChildTimeout;No description;224;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03e1;DHB_SwitchFailed;No description;225;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h -0x1201;AB_NeedSecondStep;No description;1;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1202;AB_NeedToReconfigure;No description;2;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1203;AB_ModeFallback;No description;3;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1204;AB_ChildNotCommandable;No description;4;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1205;AB_NeedToChangeHealth;No description;5;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x12a1;AB_NotEnoughChildrenInCorrectState;No description;161;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x3401;DC_NoReplyReceived;No description;1;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3402;DC_ProtocolError;No description;2;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3403;DC_Nullpointer;No description;3;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3404;DC_InvalidCookieType;No description;4;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3405;DC_NotActive;No description;5;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3406;DC_TooMuchData;No description;6;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h 0x27a0;DHI_NoCommandData;No description;160;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27a1;DHI_CommandNotSupported;No description;161;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27a2;DHI_CommandAlreadySent;No description;162;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h @@ -423,73 +460,42 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x27c3;DHI_DeviceReplyInvalid;No description;195;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27d0;DHI_InvalidCommandParameter;No description;208;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27d1;DHI_InvalidNumberOrLengthOfParameters;No description;209;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h -0x2401;MT_TooDetailedRequest;No description;1;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x2402;MT_TooGeneralRequest;No description;2;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x2403;MT_NoMatch;No description;3;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x2404;MT_Full;No description;4;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x2405;MT_NewNodeCreated;No description;5;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x3f01;DLEE_NoPacketFound;No description;1;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleParser.h -0x3f02;DLEE_PossiblePacketLoss;No description;2;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleParser.h -0x2f01;ASC_TooLongForTargetType;No description;1;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h -0x2f02;ASC_InvalidCharacters;No description;2;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h -0x2f03;ASC_BufferTooSmall;No description;3;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h -0x1701;HHI_ObjectNotHealthy;No description;1;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h -0x1702;HHI_InvalidHealthState;No description;2;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h -0x1703;HHI_IsExternallyControlled;No description;3;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h -0x3201;CF_ObjectHasNoFunctions;No description;1;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h -0x3202;CF_AlreadyCommanding;No description;2;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h -0x3301;HF_IsBusy;No description;1;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x3302;HF_InvalidParameters;No description;2;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x3303;HF_ExecutionFinished;No description;3;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x3304;HF_InvalidActionId;No description;4;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x1000;TIM_UnsupportedTimeFormat;No description;0;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1001;TIM_NotEnoughInformationForTargetFormat;No description;1;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1002;TIM_LengthMismatch;No description;2;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1003;TIM_InvalidTimeFormat;No description;3;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1004;TIM_InvalidDayOfYear;No description;4;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1005;TIM_TimeDoesNotFitFormat;No description;5;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x3701;TSI_BadTimestamp;No description;1;TIME_STAMPER_IF;fsfw/src/fsfw/timemanager/TimeStampIF.h -0x3c00;LPIF_PoolEntryNotFound;No description;0;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h -0x3c01;LPIF_PoolEntryTypeConflict;No description;1;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h -0x3e00;HKM_QueueOrDestinationInvalid;No description;0;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e01;HKM_WrongHkPacketType;No description;1;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e02;HKM_ReportingStatusUnchanged;No description;2;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e03;HKM_PeriodicHelperInvalid;No description;3;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e04;HKM_PoolobjectNotFound;No description;4;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e05;HKM_DatasetNotFound;No description;5;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x2901;TC_InvalidTargetState;No description;1;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h -0x29f1;TC_AboveOperationalLimit;No description;241;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h -0x29f2;TC_BelowOperationalLimit;No description;242;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h -0x2001;CSB_ExecutionComplete;No description;1;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2002;CSB_NoStepMessage;No description;2;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2003;CSB_ObjectBusy;No description;3;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2004;CSB_Busy;No description;4;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2005;CSB_InvalidTc;No description;5;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2006;CSB_InvalidObject;No description;6;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2007;CSB_InvalidReply;No description;7;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x4c00;SPPA_NoPacketFound;No description;0;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h -0x4c01;SPPA_SplitPacket;No description;1;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h -0x1d01;ATC_ActivityStarted;No description;1;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d02;ATC_InvalidSubservice;No description;2;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d03;ATC_IllegalApplicationData;No description;3;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d04;ATC_SendTmFailed;No description;4;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d05;ATC_Timeout;No description;5;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x7000;SCBU_KeyNotFound;No description;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h +0x1401;SE_BufferTooShort;No description;1;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x1402;SE_StreamTooShort;No description;2;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x1403;SE_TooManyElements;No description;3;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x4500;HSPI_HalTimeoutRetval;No description;0;HAL_SPI;fsfw/src/fsfw_hal/stm32h7/spi/spiDefinitions.h +0x4501;HSPI_HalBusyRetval;No description;1;HAL_SPI;fsfw/src/fsfw_hal/stm32h7/spi/spiDefinitions.h +0x4502;HSPI_HalErrorRetval;No description;2;HAL_SPI;fsfw/src/fsfw_hal/stm32h7/spi/spiDefinitions.h +0x4601;HURT_UartReadFailure;No description;1;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h +0x4602;HURT_UartReadSizeMissmatch;No description;2;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h +0x4603;HURT_UartRxBufferTooSmall;No description;3;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h +0x4801;HGIO_UnknownGpioId;No description;1;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4802;HGIO_DriveGpioFailure;No description;2;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4803;HGIO_GpioTypeFailure;No description;3;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4804;HGIO_GpioInvalidInstance;No description;4;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4805;HGIO_GpioDuplicateDetected;No description;5;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4806;HGIO_GpioInitFailed;No description;6;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4807;HGIO_GpioGetValueFailed;No description;7;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.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 +0x4403;UXOS_CommandError;Command execution failed;3;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h +0x4404;UXOS_NoCommandLoadedOrPending;;4;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h +0x4406;UXOS_PcloseCallError;No description;6;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h 0x64a0;FSHLP_SdNotMounted;SD card specified with path string not mounted;160;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h 0x64a1;FSHLP_FileNotExists;Specified file does not exist on filesystem;161;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h -0x6f00;SDMA_OpOngoing;No description;0;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6f01;SDMA_AlreadyOn;No description;1;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6f02;SDMA_AlreadyMounted;No description;2;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6f03;SDMA_AlreadyOff;No description;3;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6f0a;SDMA_StatusFileNexists;No description;10;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6f0b;SDMA_StatusFileFormatInvalid;No description;11;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6f0c;SDMA_MountError;No description;12;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6f0d;SDMA_UnmountError;No description;13;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6f0e;SDMA_SystemCallError;No description;14;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6f0f;SDMA_PopenCallError;No description;15;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x65a0;PLMPHLP_FileClosedAccidentally;File accidentally close;160;PLOC_MPSOC_HELPER;linux/devices/ploc/PlocMPSoCHelper.h -0x5ea0;PLMEMDUMP_MramAddressTooHigh;The capacity of the MRAM amounts to 512 kB. Thus the maximum address must not be higher than 0x7d000.;160;PLOC_MEMORY_DUMPER;linux/devices/ploc/PlocMemoryDumper.h -0x5ea1;PLMEMDUMP_MramInvalidAddressCombination;The specified end address is lower than the start address;161;PLOC_MEMORY_DUMPER;linux/devices/ploc/PlocMemoryDumper.h +0x6e00;SDMA_OpOngoing;No description;0;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6e01;SDMA_AlreadyOn;No description;1;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6e02;SDMA_AlreadyMounted;No description;2;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6e03;SDMA_AlreadyOff;No description;3;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6e0a;SDMA_StatusFileNexists;No description;10;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6e0b;SDMA_StatusFileFormatInvalid;No description;11;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6e0c;SDMA_MountError;No description;12;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6e0d;SDMA_UnmountError;No description;13;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6e0e;SDMA_SystemCallError;No description;14;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6e0f;SDMA_PopenCallError;No description;15;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6f00;LPH_SdNotReady;No description;0;LOCAL_PARAM_HANDLER;bsp_q7s/memory/LocalParameterHandler.h +0x7300;SCBU_KeyNotFound;No description;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h 0x57a0;PLSPVhLP_FileClosedAccidentally;File accidentally close;160;PLOC_SUPV_HELPER;linux/devices/ploc/PlocSupvUartMan.h 0x57a1;PLSPVhLP_ProcessTerminated;Process has been terminated by command;161;PLOC_SUPV_HELPER;linux/devices/ploc/PlocSupvUartMan.h 0x57a2;PLSPVhLP_PathNotExists;Received command with invalid pathname;162;PLOC_SUPV_HELPER;linux/devices/ploc/PlocSupvUartMan.h @@ -500,16 +506,46 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x5703;PLSPVhLP_PossiblePacketLossConsecutiveStart;No description;3;PLOC_SUPV_HELPER;linux/devices/ploc/PlocSupvUartMan.h 0x5704;PLSPVhLP_PossiblePacketLossConsecutiveEnd;No description;4;PLOC_SUPV_HELPER;linux/devices/ploc/PlocSupvUartMan.h 0x5705;PLSPVhLP_HdlcError;No description;5;PLOC_SUPV_HELPER;linux/devices/ploc/PlocSupvUartMan.h -0x67a0;MPSOCRTVIF_CrcFailure;Space Packet received from PLOC has invalid CRC;160;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h -0x67a1;MPSOCRTVIF_ReceivedAckFailure;Received ACK failure reply from PLOC;161;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h -0x67a2;MPSOCRTVIF_ReceivedExeFailure;Received execution failure reply from PLOC;162;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h -0x67a3;MPSOCRTVIF_InvalidApid;Received space packet with invalid APID from PLOC;163;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h -0x67a4;MPSOCRTVIF_InvalidLength;Received command with invalid length;164;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h -0x67a5;MPSOCRTVIF_FilenameTooLong;Filename of file in OBC filesystem is too long;165;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h -0x67a6;MPSOCRTVIF_MpsocHelperExecuting;MPSoC helper is currently executing a command;166;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h -0x67a7;MPSOCRTVIF_MpsocFilenameTooLong;Filename of MPSoC file is to long (max. 256 bytes);167;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h -0x67a8;MPSOCRTVIF_InvalidParameter;Command has invalid parameter;168;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h -0x67a9;MPSOCRTVIF_NameTooLong;Received command has file string with invalid length;169;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h +0x65a0;PLMPHLP_FileClosedAccidentally;File accidentally close;160;PLOC_MPSOC_HELPER;linux/devices/ploc/PlocMPSoCHelper.h +0x5ea0;PLMEMDUMP_MramAddressTooHigh;The capacity of the MRAM amounts to 512 kB. Thus the maximum address must not be higher than 0x7d000.;160;PLOC_MEMORY_DUMPER;linux/devices/ploc/PlocMemoryDumper.h +0x5ea1;PLMEMDUMP_MramInvalidAddressCombination;The specified end address is lower than the start address;161;PLOC_MEMORY_DUMPER;linux/devices/ploc/PlocMemoryDumper.h +0x53a0;STRH_TemperatureReqFailed;Status in temperature reply signals error;160;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53a1;STRH_PingFailed;Ping command failed;161;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53a2;STRH_VersionReqFailed;Status in version reply signals error;162;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x6201;JSONBASE_JsonFileNotExists;Specified json file does not exist;1;ARCSEC_JSON_BASE;linux/devices/startracker/ArcsecJsonParamBase.h +0x6202;JSONBASE_SetNotExists;Requested set does not exist in json file;2;ARCSEC_JSON_BASE;linux/devices/startracker/ArcsecJsonParamBase.h +0x6203;JSONBASE_ParamNotExists;Requested parameter does not exist in json file;3;ARCSEC_JSON_BASE;linux/devices/startracker/ArcsecJsonParamBase.h +0x53a3;STRH_InterfaceReqFailed;Status in interface reply signals error;163;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53a4;STRH_PowerReqFailed;Status in power reply signals error;164;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53a5;STRH_SetParamFailed;Status of reply to parameter set command signals error;165;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53a6;STRH_ActionFailed;Status of reply to action command signals error;166;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53a7;STRH_FilePathTooLong;Received invalid path string. Exceeds allowed length;167;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53a8;STRH_FilenameTooLong;Name of file received with command is too long;168;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53a9;STRH_InvalidProgram;Received version reply with invalid program ID;169;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53aa;STRH_ReplyError;Status field reply signals error;170;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53ab;STRH_CommandTooShort;Received command which is too short (some data is missing for proper execution);171;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53ac;STRH_InvalidLength;Received command with invalid length (too few or too many parameters);172;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53ad;STRH_RegionMismatch;Region mismatch between send and received data;173;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53ae;STRH_AddressMismatch;Address mismatch between send and received data;174;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53af;STRH_LengthMismatch;Length field mismatch between send and received data;175;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53b0;STRH_FileNotExists;Specified file does not exist;176;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53b1;STRH_InvalidType;Download blob pixel command has invalid type field;177;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53b2;STRH_InvalidId;Received FPGA action command with invalid ID;178;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53b3;STRH_ReplyTooShort;Received reply is too short;179;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53b4;STRH_CrcFailure;Received reply with invalid CRC;180;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53b5;STRH_StrHelperExecuting;Star tracker handler currently executing a command and using the communication interface;181;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53b6;STRH_StartrackerAlreadyBooted;Star tracker is already in firmware mode;182;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53b7;STRH_StartrackerRunningFirmware;Star tracker is in firmware mode but must be in bootloader mode to execute this command;183;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x53b8;STRH_StartrackerRunningBootloader;Star tracker is in bootloader mode but must be in firmware mode to execute this command;184;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x5ca0;STRHLP_SdNotMounted;SD card specified in path string not mounted;160;STR_HELPER;linux/devices/startracker/StrHelper.h +0x5ca1;STRHLP_FileNotExists;Specified file does not exist on filesystem;161;STR_HELPER;linux/devices/startracker/StrHelper.h +0x5ca2;STRHLP_PathNotExists;Specified path does not exist;162;STR_HELPER;linux/devices/startracker/StrHelper.h +0x5ca3;STRHLP_FileCreationFailed;Failed to create download image or read flash file;163;STR_HELPER;linux/devices/startracker/StrHelper.h +0x5ca4;STRHLP_RegionMismatch;Region in flash write/read reply does not match expected region;164;STR_HELPER;linux/devices/startracker/StrHelper.h +0x5ca5;STRHLP_AddressMismatch;Address in flash write/read reply does not match expected address;165;STR_HELPER;linux/devices/startracker/StrHelper.h +0x5ca6;STRHLP_LengthMismatch;Length in flash write/read reply does not match expected length;166;STR_HELPER;linux/devices/startracker/StrHelper.h +0x5ca7;STRHLP_StatusError;Status field in reply signals error;167;STR_HELPER;linux/devices/startracker/StrHelper.h +0x5ca8;STRHLP_InvalidTypeId;Reply has invalid type ID (should be of action reply type);168;STR_HELPER;linux/devices/startracker/StrHelper.h 0x68a0;SPVRTVIF_CrcFailure;Space Packet received from PLOC supervisor has invalid CRC;160;SUPV_RETURN_VALUES_IF;linux/devices/devicedefinitions/PlocSupervisorDefinitions.h 0x68a1;SPVRTVIF_InvalidServiceId;No description;161;SUPV_RETURN_VALUES_IF;linux/devices/devicedefinitions/PlocSupervisorDefinitions.h 0x68a2;SPVRTVIF_ReceivedAckFailure;Received ACK failure reply from PLOC supervisor;162;SUPV_RETURN_VALUES_IF;linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -534,47 +570,21 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x68b5;SPVRTVIF_SupvHelperExecuting;Supervisor helper task ist currently executing a command (wait until helper tas has finished or interrupt by sending the terminate command);181;SUPV_RETURN_VALUES_IF;linux/devices/devicedefinitions/PlocSupervisorDefinitions.h 0x68c0;SPVRTVIF_BufTooSmall;No description;192;SUPV_RETURN_VALUES_IF;linux/devices/devicedefinitions/PlocSupervisorDefinitions.h 0x68c1;SPVRTVIF_NoReplyTimeout;No description;193;SUPV_RETURN_VALUES_IF;linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +0x67a0;MPSOCRTVIF_CrcFailure;Space Packet received from PLOC has invalid CRC;160;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h +0x67a1;MPSOCRTVIF_ReceivedAckFailure;Received ACK failure reply from PLOC;161;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h +0x67a2;MPSOCRTVIF_ReceivedExeFailure;Received execution failure reply from PLOC;162;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h +0x67a3;MPSOCRTVIF_InvalidApid;Received space packet with invalid APID from PLOC;163;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h +0x67a4;MPSOCRTVIF_InvalidLength;Received command with invalid length;164;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h +0x67a5;MPSOCRTVIF_FilenameTooLong;Filename of file in OBC filesystem is too long;165;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h +0x67a6;MPSOCRTVIF_MpsocHelperExecuting;MPSoC helper is currently executing a command;166;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h +0x67a7;MPSOCRTVIF_MpsocFilenameTooLong;Filename of MPSoC file is to long (max. 256 bytes);167;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h +0x67a8;MPSOCRTVIF_InvalidParameter;Command has invalid parameter;168;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h +0x67a9;MPSOCRTVIF_NameTooLong;Received command has file string with invalid length;169;MPSOC_RETURN_VALUES_IF;linux/devices/devicedefinitions/MPSoCReturnValuesIF.h 0x54e0;DWLPWRON_InvalidMode;Received command has invalid JESD mode (valid modes are 0 - 5);224;DWLPWRON_CMD;linux/devices/devicedefinitions/PlocMPSoCDefinitions.h 0x54e1;DWLPWRON_InvalidLaneRate;Received command has invalid lane rate (valid lane rate are 0 - 9);225;DWLPWRON_CMD;linux/devices/devicedefinitions/PlocMPSoCDefinitions.h -0x6201;JSONBASE_JsonFileNotExists;Specified json file does not exist;1;ARCSEC_JSON_BASE;linux/devices/startracker/ArcsecJsonParamBase.h -0x6202;JSONBASE_SetNotExists;Requested set does not exist in json file;2;ARCSEC_JSON_BASE;linux/devices/startracker/ArcsecJsonParamBase.h -0x6203;JSONBASE_ParamNotExists;Requested parameter does not exist in json file;3;ARCSEC_JSON_BASE;linux/devices/startracker/ArcsecJsonParamBase.h -0x53a0;STRH_TemperatureReqFailed;Status in temperature reply signals error;160;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53a1;STRH_PingFailed;Ping command failed;161;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53a2;STRH_VersionReqFailed;Status in version reply signals error;162;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x5ca0;STRHLP_SdNotMounted;SD card specified in path string not mounted;160;STR_HELPER;linux/devices/startracker/StrHelper.h -0x5ca1;STRHLP_FileNotExists;Specified file does not exist on filesystem;161;STR_HELPER;linux/devices/startracker/StrHelper.h -0x5ca2;STRHLP_PathNotExists;Specified path does not exist;162;STR_HELPER;linux/devices/startracker/StrHelper.h -0x5ca3;STRHLP_FileCreationFailed;Failed to create download image or read flash file;163;STR_HELPER;linux/devices/startracker/StrHelper.h -0x5ca4;STRHLP_RegionMismatch;Region in flash write/read reply does not match expected region;164;STR_HELPER;linux/devices/startracker/StrHelper.h -0x5ca5;STRHLP_AddressMismatch;Address in flash write/read reply does not match expected address;165;STR_HELPER;linux/devices/startracker/StrHelper.h -0x5ca6;STRHLP_LengthMismatch;Length in flash write/read reply does not match expected length;166;STR_HELPER;linux/devices/startracker/StrHelper.h -0x5ca7;STRHLP_StatusError;Status field in reply signals error;167;STR_HELPER;linux/devices/startracker/StrHelper.h -0x5ca8;STRHLP_InvalidTypeId;Reply has invalid type ID (should be of action reply type);168;STR_HELPER;linux/devices/startracker/StrHelper.h -0x53a3;STRH_InterfaceReqFailed;Status in interface reply signals error;163;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53a4;STRH_PowerReqFailed;Status in power reply signals error;164;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53a5;STRH_SetParamFailed;Status of reply to parameter set command signals error;165;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53a6;STRH_ActionFailed;Status of reply to action command signals error;166;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53a7;STRH_FilePathTooLong;Received invalid path string. Exceeds allowed length;167;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53a8;STRH_FilenameTooLong;Name of file received with command is too long;168;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53a9;STRH_InvalidProgram;Received version reply with invalid program ID;169;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53aa;STRH_ReplyError;Status field reply signals error;170;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53ab;STRH_CommandTooShort;Received command which is too short (some data is missing for proper execution);171;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53ac;STRH_InvalidLength;Received command with invalid length (too few or too many parameters);172;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53ad;STRH_RegionMismatch;Region mismatch between send and received data;173;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53ae;STRH_AddressMismatch;Address mismatch between send and received data;174;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53af;STRH_LengthMismatch;Length field mismatch between send and received data;175;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53b0;STRH_FileNotExists;Specified file does not exist;176;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53b1;STRH_InvalidType;Download blob pixel command has invalid type field;177;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53b2;STRH_InvalidId;Received FPGA action command with invalid ID;178;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53b3;STRH_ReplyTooShort;Received reply is too short;179;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53b4;STRH_CrcFailure;Received reply with invalid CRC;180;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53b5;STRH_StrHelperExecuting;Star tracker handler currently executing a command and using the communication interface;181;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53b6;STRH_StartrackerAlreadyBooted;Star tracker is already in firmware mode;182;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53b7;STRH_StartrackerRunningFirmware;Star tracker is in firmware mode but must be in bootloader mode to execute this command;183;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x53b8;STRH_StartrackerRunningBootloader;Star tracker is in bootloader mode but must be in firmware mode to execute this command;184;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h -0x5300;STRH_NoReplyAvailable;No description;0;STR_HANDLER;linux/devices/ImtqPollingTask.h -0x5302;STRH_InvalidCrc;No description;2;STR_HANDLER;linux/devices/ScexHelper.h +0x5400;DWLPWRON_NoReplyAvailable;No description;0;DWLPWRON_CMD;linux/devices/ImtqPollingTask.h +0x5402;DWLPWRON_InvalidCrc;No description;2;DWLPWRON_CMD;linux/devices/ScexHelper.h +0x59a0;IPCI_PapbBusy;No description;160;CCSDS_IP_CORE_BRIDGE;linux/ipcore/PapbVcInterface.h 0x5aa0;PTME_UnknownVcId;No description;160;PTME;linux/ipcore/Ptme.h 0x5fa0;PDEC_AbandonedCltuRetval;No description;160;PDEC_HANDLER;linux/ipcore/PdecHandler.h 0x5fa1;PDEC_FrameDirtyRetval;No description;161;PDEC_HANDLER;linux/ipcore/PdecHandler.h @@ -595,4 +605,3 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x61a1;RS_BadBitRate;Bad bitrate has been commanded (e.g. 0);161;RATE_SETTER;linux/ipcore/PtmeConfig.h 0x61a2;RS_ClkInversionFailed;Failed to invert clock and thus change the time the data is updated with respect to the tx clock;162;RATE_SETTER;linux/ipcore/PtmeConfig.h 0x61a3;RS_TxManipulatorConfigFailed;Failed to change configuration bit of tx clock manipulator;163;RATE_SETTER;linux/ipcore/PtmeConfig.h -0x59a0;IPCI_PapbBusy;No description;160;CCSDS_IP_CORE_BRIDGE;linux/ipcore/PapbVcInterface.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index c979ff5e..94d829fe 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 263 translations. + * @brief Auto-generated event translation file. Contains 277 translations. * @details - * Generated on: 2023-02-24 16:57:00 + * Generated on: 2023-03-11 15:01:05 */ #include "translateEvents.h" @@ -158,6 +158,8 @@ const char *LOST_BIT_LOCK_PDEC_STRING = "LOST_BIT_LOCK_PDEC"; const char *TOO_MANY_IRQS_STRING = "TOO_MANY_IRQS"; const char *POLL_SYSCALL_ERROR_PDEC_STRING = "POLL_SYSCALL_ERROR_PDEC"; const char *WRITE_SYSCALL_ERROR_PDEC_STRING = "WRITE_SYSCALL_ERROR_PDEC"; +const char *PDEC_RESET_FAILED_STRING = "PDEC_RESET_FAILED"; +const char *OPEN_IRQ_FILE_FAILED_STRING = "OPEN_IRQ_FILE_FAILED"; 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"; @@ -255,7 +257,8 @@ const char *REBOOT_HW_STRING = "REBOOT_HW"; const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE"; const char *VERSION_INFO_STRING = "VERSION_INFO"; const char *CURRENT_IMAGE_INFO_STRING = "CURRENT_IMAGE_INFO"; -const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; +const char *REBOOT_COUNTER_STRING = "REBOOT_COUNTER"; +const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS"; const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE"; const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE"; const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING"; @@ -263,6 +266,17 @@ const char *PLOC_OVERHEATING_STRING = "PLOC_OVERHEATING"; const char *OBC_OVERHEATING_STRING = "OBC_OVERHEATING"; const char *HPA_OVERHEATING_STRING = "HPA_OVERHEATING"; const char *PLPCDU_OVERHEATING_STRING = "PLPCDU_OVERHEATING"; +const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED"; +const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON"; +const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; +const char *FILE_TOO_LARGE_STRING = "FILE_TOO_LARGE"; +const char *BUSY_DUMPING_EVENT_STRING = "BUSY_DUMPING_EVENT"; +const char *DUMP_WAS_CANCELLED_STRING = "DUMP_WAS_CANCELLED"; +const char *DUMP_OK_STORE_DONE_STRING = "DUMP_OK_STORE_DONE"; +const char *DUMP_NOK_STORE_DONE_STRING = "DUMP_NOK_STORE_DONE"; +const char *DUMP_MISC_STORE_DONE_STRING = "DUMP_MISC_STORE_DONE"; +const char *DUMP_HK_STORE_DONE_STRING = "DUMP_HK_STORE_DONE"; +const char *DUMP_CFDP_STORE_DONE_STRING = "DUMP_CFDP_STORE_DONE"; const char *translateEvents(Event event) { switch ((event & 0xFFFF)) { @@ -572,6 +586,10 @@ const char *translateEvents(Event event) { return POLL_SYSCALL_ERROR_PDEC_STRING; case (12409): return WRITE_SYSCALL_ERROR_PDEC_STRING; + case (12410): + return PDEC_RESET_FAILED_STRING; + case (12411): + return OPEN_IRQ_FILE_FAILED_STRING; case (12500): return IMAGE_UPLOAD_FAILED_STRING; case (12501): @@ -766,22 +784,46 @@ const char *translateEvents(Event event) { return VERSION_INFO_STRING; case (14006): return CURRENT_IMAGE_INFO_STRING; + case (14007): + return REBOOT_COUNTER_STRING; + case (14008): + return INDIVIDUAL_BOOT_COUNTS_STRING; case (14100): - return POSSIBLE_FILE_CORRUPTION_STRING; - case (14200): return NO_VALID_SENSOR_TEMPERATURE_STRING; - case (14201): + case (14101): return NO_HEALTHY_HEATER_AVAILABLE_STRING; - case (14202): + case (14102): return SYRLINKS_OVERHEATING_STRING; - case (14203): + case (14103): return PLOC_OVERHEATING_STRING; - case (14204): + case (14104): return OBC_OVERHEATING_STRING; - case (14205): + case (14105): return HPA_OVERHEATING_STRING; - case (14206): + case (14106): return PLPCDU_OVERHEATING_STRING; + case (14201): + return TX_TIMER_EXPIRED_STRING; + case (14202): + return BIT_LOCK_TX_ON_STRING; + case (14300): + return POSSIBLE_FILE_CORRUPTION_STRING; + case (14301): + return FILE_TOO_LARGE_STRING; + case (14302): + return BUSY_DUMPING_EVENT_STRING; + case (14303): + return DUMP_WAS_CANCELLED_STRING; + case (14305): + return DUMP_OK_STORE_DONE_STRING; + case (14306): + return DUMP_NOK_STORE_DONE_STRING; + case (14307): + return DUMP_MISC_STORE_DONE_STRING; + case (14308): + return DUMP_HK_STORE_DONE_STRING; + case (14309): + return DUMP_CFDP_STORE_DONE_STRING; default: return "UNKNOWN_EVENT"; } diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index 93c4fb0f..212322bc 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -1,8 +1,8 @@ /** * @brief Auto-generated object translation file. * @details - * Contains 159 translations. - * Generated on: 2023-02-24 16:57:00 + * Contains 173 translations. + * Generated on: 2023-03-11 15:01:05 */ #include "translateObjects.h" @@ -53,6 +53,10 @@ const char *STR_HELPER_STRING = "STR_HELPER"; const char *PLOC_MPSOC_HELPER_STRING = "PLOC_MPSOC_HELPER"; const char *AXI_PTME_CONFIG_STRING = "AXI_PTME_CONFIG"; const char *PTME_CONFIG_STRING = "PTME_CONFIG"; +const char *PTME_VC0_LIVE_TM_STRING = "PTME_VC0_LIVE_TM"; +const char *PTME_VC1_LOG_TM_STRING = "PTME_VC1_LOG_TM"; +const char *PTME_VC2_HK_TM_STRING = "PTME_VC2_HK_TM"; +const char *PTME_VC3_CFDP_TM_STRING = "PTME_VC3_CFDP_TM"; const char *PLOC_MPSOC_HANDLER_STRING = "PLOC_MPSOC_HANDLER"; const char *PLOC_SUPERVISOR_HANDLER_STRING = "PLOC_SUPERVISOR_HANDLER"; const char *PLOC_SUPERVISOR_HELPER_STRING = "PLOC_SUPERVISOR_HELPER"; @@ -85,11 +89,13 @@ const char *ARDUINO_COM_IF_STRING = "ARDUINO_COM_IF"; const char *GPIO_IF_STRING = "GPIO_IF"; const char *SCEX_UART_READER_STRING = "SCEX_UART_READER"; const char *SPI_MAIN_COM_IF_STRING = "SPI_MAIN_COM_IF"; -const char *RW_POLLING_TASK_STRING = "RW_POLLING_TASK"; -const char *SPI_RTD_COM_IF_STRING = "SPI_RTD_COM_IF"; const char *UART_COM_IF_STRING = "UART_COM_IF"; const char *I2C_COM_IF_STRING = "I2C_COM_IF"; const char *CSP_COM_IF_STRING = "CSP_COM_IF"; +const char *ACS_BOARD_POLLING_TASK_STRING = "ACS_BOARD_POLLING_TASK"; +const char *RW_POLLING_TASK_STRING = "RW_POLLING_TASK"; +const char *SPI_RTD_COM_IF_STRING = "SPI_RTD_COM_IF"; +const char *SUS_POLLING_TASK_STRING = "SUS_POLLING_TASK"; const char *CCSDS_PACKET_DISTRIBUTOR_STRING = "CCSDS_PACKET_DISTRIBUTOR"; const char *PUS_PACKET_DISTRIBUTOR_STRING = "PUS_PACKET_DISTRIBUTOR"; const char *TCP_TMTC_SERVER_STRING = "TCP_TMTC_SERVER"; @@ -141,12 +147,15 @@ const char *HEATER_3_OBC_BRD_STRING = "HEATER_3_OBC_BRD"; const char *HEATER_4_CAMERA_STRING = "HEATER_4_CAMERA"; const char *HEATER_5_STR_STRING = "HEATER_5_STR"; const char *HEATER_6_DRO_STRING = "HEATER_6_DRO"; -const char *HEATER_7_HPA_STRING = "HEATER_7_HPA"; +const char *HEATER_7_SYRLINKS_STRING = "HEATER_7_SYRLINKS"; 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 *RW_ASSY_STRING = "RW_ASSY"; const char *CAM_SWITCHER_STRING = "CAM_SWITCHER"; +const char *SYRLINKS_ASSY_STRING = "SYRLINKS_ASSY"; +const char *IMTQ_ASSY_STRING = "IMTQ_ASSY"; +const char *STR_ASSY_STRING = "STR_ASSY"; 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"; @@ -162,6 +171,11 @@ const char *OK_TM_STORE_STRING = "OK_TM_STORE"; const char *NOT_OK_TM_STORE_STRING = "NOT_OK_TM_STORE"; const char *HK_TM_STORE_STRING = "HK_TM_STORE"; const char *CFDP_TM_STORE_STRING = "CFDP_TM_STORE"; +const char *LIVE_TM_TASK_STRING = "LIVE_TM_TASK"; +const char *LOG_STORE_AND_TM_TASK_STRING = "LOG_STORE_AND_TM_TASK"; +const char *HK_STORE_AND_TM_TASK_STRING = "HK_STORE_AND_TM_TASK"; +const char *CFDP_STORE_AND_TM_TASK_STRING = "CFDP_STORE_AND_TM_TASK"; +const char *DOWNLINK_RAM_STORE_STRING = "DOWNLINK_RAM_STORE"; const char *CCSDS_IP_CORE_BRIDGE_STRING = "CCSDS_IP_CORE_BRIDGE"; const char *THERMAL_TEMP_INSERTER_STRING = "THERMAL_TEMP_INSERTER"; const char *NO_OBJECT_STRING = "NO_OBJECT"; @@ -262,6 +276,14 @@ const char *translateObject(object_id_t object) { return AXI_PTME_CONFIG_STRING; case 0x44330005: return PTME_CONFIG_STRING; + case 0x44330006: + return PTME_VC0_LIVE_TM_STRING; + case 0x44330007: + return PTME_VC1_LOG_TM_STRING; + case 0x44330008: + return PTME_VC2_HK_TM_STRING; + case 0x44330009: + return PTME_VC3_CFDP_TM_STRING; case 0x44330015: return PLOC_MPSOC_HANDLER_STRING; case 0x44330016: @@ -326,16 +348,20 @@ const char *translateObject(object_id_t object) { return SCEX_UART_READER_STRING; case 0x49020004: return SPI_MAIN_COM_IF_STRING; - case 0x49020005: - return RW_POLLING_TASK_STRING; - case 0x49020006: - return SPI_RTD_COM_IF_STRING; case 0x49030003: return UART_COM_IF_STRING; case 0x49040002: return I2C_COM_IF_STRING; case 0x49050001: return CSP_COM_IF_STRING; + case 0x49060004: + return ACS_BOARD_POLLING_TASK_STRING; + case 0x49060005: + return RW_POLLING_TASK_STRING; + case 0x49060006: + return SPI_RTD_COM_IF_STRING; + case 0x49060007: + return SUS_POLLING_TASK_STRING; case 0x50000100: return CCSDS_PACKET_DISTRIBUTOR_STRING; case 0x50000200: @@ -439,7 +465,7 @@ const char *translateObject(object_id_t object) { case 0x60000006: return HEATER_6_DRO_STRING; case 0x60000007: - return HEATER_7_HPA_STRING; + return HEATER_7_SYRLINKS_STRING; case 0x73000001: return ACS_BOARD_ASS_STRING; case 0x73000002: @@ -447,9 +473,15 @@ const char *translateObject(object_id_t object) { case 0x73000003: return TCS_BOARD_ASS_STRING; case 0x73000004: - return RW_ASS_STRING; + return RW_ASSY_STRING; case 0x73000006: return CAM_SWITCHER_STRING; + case 0x73000007: + return SYRLINKS_ASSY_STRING; + case 0x73000008: + return IMTQ_ASSY_STRING; + case 0x73000009: + return STR_ASSY_STRING; case 0x73000100: return TM_FUNNEL_STRING; case 0x73000101: @@ -480,6 +512,16 @@ const char *translateObject(object_id_t object) { return HK_TM_STORE_STRING; case 0x73030000: return CFDP_TM_STORE_STRING; + case 0x73040000: + return LIVE_TM_TASK_STRING; + case 0x73040001: + return LOG_STORE_AND_TM_TASK_STRING; + case 0x73040002: + return HK_STORE_AND_TM_TASK_STRING; + case 0x73040003: + return CFDP_STORE_AND_TM_TASK_STRING; + case 0x73040004: + return DOWNLINK_RAM_STORE_STRING; case 0x73500000: return CCSDS_IP_CORE_BRIDGE_STRING; case 0x90000003: diff --git a/linux/ObjectFactory.cpp b/linux/ObjectFactory.cpp index e3162a07..1ac86886 100644 --- a/linux/ObjectFactory.cpp +++ b/linux/ObjectFactory.cpp @@ -10,8 +10,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -33,6 +35,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo PowerSwitchIF& pwrSwitcher, std::string spiDev, bool swap0And6) { using namespace gpio; + new SusPolling(objects::SUS_POLLING_TASK, *spiComIF, *gpioComIF); GpioCookie* gpioCookieSus = new GpioCookie(); GpioCallback* susgpio = nullptr; @@ -78,99 +81,100 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo #if OBSW_ADD_SUN_SENSORS == 1 SusFdir* fdir = nullptr; std::array susHandlers = {}; - SpiCookie* spiCookie = new SpiCookie(addresses::SUS_0, gpioIds::CS_SUS_0, SUS::MAX_CMD_SIZE, - spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); + SpiCookie* spiCookie = + new SpiCookie(addresses::SUS_0, gpioIds::CS_SUS_0, susMax1227::MAX_CMD_SIZE, + spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT); susHandlers[0] = - new SusHandler(objects::SUS_0_N_LOC_XFYFZM_PT_XF, 0, objects::SPI_MAIN_COM_IF, spiCookie); + new SusHandler(objects::SUS_0_N_LOC_XFYFZM_PT_XF, 0, objects::SUS_POLLING_TASK, spiCookie); fdir = new SusFdir(objects::SUS_0_N_LOC_XFYFZM_PT_XF); susHandlers[0]->setCustomFdir(fdir); - spiCookie = new SpiCookie(addresses::SUS_1, gpioIds::CS_SUS_1, SUS::MAX_CMD_SIZE, + spiCookie = new SpiCookie(addresses::SUS_1, gpioIds::CS_SUS_1, susMax1227::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT); susHandlers[1] = - new SusHandler(objects::SUS_1_N_LOC_XBYFZM_PT_XB, 1, objects::SPI_MAIN_COM_IF, spiCookie); + new SusHandler(objects::SUS_1_N_LOC_XBYFZM_PT_XB, 1, objects::SUS_POLLING_TASK, spiCookie); fdir = new SusFdir(objects::SUS_1_N_LOC_XBYFZM_PT_XB); susHandlers[1]->setCustomFdir(fdir); - spiCookie = new SpiCookie(addresses::SUS_2, gpioIds::CS_SUS_2, SUS::MAX_CMD_SIZE, + spiCookie = new SpiCookie(addresses::SUS_2, gpioIds::CS_SUS_2, susMax1227::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT); susHandlers[2] = - new SusHandler(objects::SUS_2_N_LOC_XFYBZB_PT_YB, 2, objects::SPI_MAIN_COM_IF, spiCookie); + new SusHandler(objects::SUS_2_N_LOC_XFYBZB_PT_YB, 2, objects::SUS_POLLING_TASK, spiCookie); fdir = new SusFdir(objects::SUS_2_N_LOC_XFYBZB_PT_YB); susHandlers[2]->setCustomFdir(fdir); - spiCookie = new SpiCookie(addresses::SUS_3, gpioIds::CS_SUS_3, SUS::MAX_CMD_SIZE, + spiCookie = new SpiCookie(addresses::SUS_3, gpioIds::CS_SUS_3, susMax1227::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT); susHandlers[3] = - new SusHandler(objects::SUS_3_N_LOC_XFYBZF_PT_YF, 3, objects::SPI_MAIN_COM_IF, spiCookie); + new SusHandler(objects::SUS_3_N_LOC_XFYBZF_PT_YF, 3, objects::SUS_POLLING_TASK, spiCookie); fdir = new SusFdir(objects::SUS_3_N_LOC_XFYBZF_PT_YF); susHandlers[3]->setCustomFdir(fdir); - spiCookie = new SpiCookie(addresses::SUS_4, gpioIds::CS_SUS_4, SUS::MAX_CMD_SIZE, + spiCookie = new SpiCookie(addresses::SUS_4, gpioIds::CS_SUS_4, susMax1227::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT); susHandlers[4] = - new SusHandler(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, 4, objects::SPI_MAIN_COM_IF, spiCookie); + new SusHandler(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, 4, objects::SUS_POLLING_TASK, spiCookie); fdir = new SusFdir(objects::SUS_4_N_LOC_XMYFZF_PT_ZF); susHandlers[4]->setCustomFdir(fdir); - spiCookie = new SpiCookie(addresses::SUS_5, gpioIds::CS_SUS_5, SUS::MAX_CMD_SIZE, + spiCookie = new SpiCookie(addresses::SUS_5, gpioIds::CS_SUS_5, susMax1227::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT); susHandlers[5] = - new SusHandler(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, 5, objects::SPI_MAIN_COM_IF, spiCookie); + new SusHandler(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, 5, objects::SUS_POLLING_TASK, spiCookie); fdir = new SusFdir(objects::SUS_5_N_LOC_XFYMZB_PT_ZB); susHandlers[5]->setCustomFdir(fdir); - spiCookie = new SpiCookie(addresses::SUS_6, gpioIds::CS_SUS_6, SUS::MAX_CMD_SIZE, + spiCookie = new SpiCookie(addresses::SUS_6, gpioIds::CS_SUS_6, susMax1227::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT); susHandlers[6] = - new SusHandler(objects::SUS_6_R_LOC_XFYBZM_PT_XF, 6, objects::SPI_MAIN_COM_IF, spiCookie); + new SusHandler(objects::SUS_6_R_LOC_XFYBZM_PT_XF, 6, objects::SUS_POLLING_TASK, spiCookie); fdir = new SusFdir(objects::SUS_6_R_LOC_XFYBZM_PT_XF); susHandlers[6]->setCustomFdir(fdir); - spiCookie = new SpiCookie(addresses::SUS_7, gpioIds::CS_SUS_7, SUS::MAX_CMD_SIZE, + spiCookie = new SpiCookie(addresses::SUS_7, gpioIds::CS_SUS_7, susMax1227::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT); susHandlers[7] = - new SusHandler(objects::SUS_7_R_LOC_XBYBZM_PT_XB, 7, objects::SPI_MAIN_COM_IF, spiCookie); + new SusHandler(objects::SUS_7_R_LOC_XBYBZM_PT_XB, 7, objects::SUS_POLLING_TASK, spiCookie); fdir = new SusFdir(objects::SUS_7_R_LOC_XBYBZM_PT_XB); susHandlers[7]->setCustomFdir(fdir); - spiCookie = new SpiCookie(addresses::SUS_8, gpioIds::CS_SUS_8, SUS::MAX_CMD_SIZE, + spiCookie = new SpiCookie(addresses::SUS_8, gpioIds::CS_SUS_8, susMax1227::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT); susHandlers[8] = - new SusHandler(objects::SUS_8_R_LOC_XBYBZB_PT_YB, 8, objects::SPI_MAIN_COM_IF, spiCookie); + new SusHandler(objects::SUS_8_R_LOC_XBYBZB_PT_YB, 8, objects::SUS_POLLING_TASK, spiCookie); fdir = new SusFdir(objects::SUS_8_R_LOC_XBYBZB_PT_YB); susHandlers[8]->setCustomFdir(fdir); - spiCookie = new SpiCookie(addresses::SUS_9, gpioIds::CS_SUS_9, SUS::MAX_CMD_SIZE, + spiCookie = new SpiCookie(addresses::SUS_9, gpioIds::CS_SUS_9, susMax1227::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT); susHandlers[9] = - new SusHandler(objects::SUS_9_R_LOC_XBYBZB_PT_YF, 9, objects::SPI_MAIN_COM_IF, spiCookie); + new SusHandler(objects::SUS_9_R_LOC_XBYBZB_PT_YF, 9, objects::SUS_POLLING_TASK, spiCookie); fdir = new SusFdir(objects::SUS_9_R_LOC_XBYBZB_PT_YF); susHandlers[9]->setCustomFdir(fdir); - spiCookie = new SpiCookie(addresses::SUS_10, gpioIds::CS_SUS_10, SUS::MAX_CMD_SIZE, + spiCookie = new SpiCookie(addresses::SUS_10, gpioIds::CS_SUS_10, susMax1227::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT); susHandlers[10] = - new SusHandler(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, 10, objects::SPI_MAIN_COM_IF, spiCookie); + new SusHandler(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, 10, objects::SUS_POLLING_TASK, spiCookie); fdir = new SusFdir(objects::SUS_10_N_LOC_XMYBZF_PT_ZF); susHandlers[10]->setCustomFdir(fdir); - spiCookie = new SpiCookie(addresses::SUS_11, gpioIds::CS_SUS_11, SUS::MAX_CMD_SIZE, + spiCookie = new SpiCookie(addresses::SUS_11, gpioIds::CS_SUS_11, susMax1227::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT); susHandlers[11] = - new SusHandler(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, 11, objects::SPI_MAIN_COM_IF, spiCookie); + new SusHandler(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, 11, objects::SUS_POLLING_TASK, spiCookie); fdir = new SusFdir(objects::SUS_11_R_LOC_XBYMZB_PT_ZB); susHandlers[11]->setCustomFdir(fdir); @@ -337,9 +341,3 @@ void ObjectFactory::gpioChecker(ReturnValue_t result, std::string output) { sif::error << "ObjectFactory: Adding GPIOs failed for " << output << std::endl; } } - -void ObjectFactory::addTmtcIpCoresToFunnels(CcsdsIpCoreHandler& ipCoreHandler, - PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel) { - cfdpFunnel.addDestination("PTME IP Core", ipCoreHandler, config::LIVE_TM); - pusFunnel.addDestination("PTME IP Core", ipCoreHandler, config::LIVE_TM); -} diff --git a/linux/ObjectFactory.h b/linux/ObjectFactory.h index 0a7ab516..056dbd65 100644 --- a/linux/ObjectFactory.h +++ b/linux/ObjectFactory.h @@ -33,6 +33,4 @@ void gpioChecker(ReturnValue_t result, std::string output); AcsController* createAcsController(bool connectSubsystem); -void addTmtcIpCoresToFunnels(CcsdsIpCoreHandler& ipCoreHandler, PusTmFunnel& pusFunnel, - CfdpTmFunnel& cfdpFunnel); } // namespace ObjectFactory diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp new file mode 100644 index 00000000..307ddebb --- /dev/null +++ b/linux/devices/AcsBoardPolling.cpp @@ -0,0 +1,732 @@ +#include "AcsBoardPolling.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "devices/gpioIds.h" + +using namespace returnvalue; + +AcsBoardPolling::AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF) + : SystemObject(objectId), spiComIF(lowLevelComIF), gpioIF(gpioIF) { + semaphore = SemaphoreFactory::instance()->createBinarySemaphore(); + semaphore->acquire(); + ipcLock = MutexFactory::instance()->createMutex(); +} + +ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) { + while (true) { + ipcLock->lockMutex(LOCK_TYPE, LOCK_TIMEOUT); + state = InternalState::IDLE; + ipcLock->unlockMutex(); + semaphore->acquire(); + // Give all tasks or the PST some time to submit all consecutive requests. + TaskFactory::delayTask(2); + { + // Measured to take 0-1 ms in debug build. + // Stopwatch watch; + gyroAdisHandler(gyro0Adis); + gyroAdisHandler(gyro2Adis); + gyroL3gHandler(gyro1L3g); + gyroL3gHandler(gyro3L3g); + mgmRm3100Handler(mgm1Rm3100); + mgmRm3100Handler(mgm3Rm3100); + mgmLis3Handler(mgm0Lis3); + mgmLis3Handler(mgm2Lis3); + } + // To prevent task being not reactivated by tardy tasks + TaskFactory::delayTask(20); + } + return returnvalue::OK; +} + +ReturnValue_t AcsBoardPolling::initialize() { return returnvalue::OK; } + +ReturnValue_t AcsBoardPolling::initializeInterface(CookieIF* cookie) { + SpiCookie* spiCookie = dynamic_cast(cookie); + if (spiCookie == nullptr) { + return returnvalue::FAILED; + } + switch (spiCookie->getChipSelectPin()) { + case (gpioIds::MGM_0_LIS3_CS): { + mgm0Lis3.cookie = spiCookie; + break; + } + case (gpioIds::MGM_1_RM3100_CS): { + mgm1Rm3100.cookie = spiCookie; + break; + } + case (gpioIds::MGM_2_LIS3_CS): { + mgm2Lis3.cookie = spiCookie; + break; + } + case (gpioIds::MGM_3_RM3100_CS): { + mgm3Rm3100.cookie = spiCookie; + break; + } + case (gpioIds::GYRO_0_ADIS_CS): { + gyro0Adis.cookie = spiCookie; + break; + } + case (gpioIds::GYRO_1_L3G_CS): { + gyro1L3g.cookie = spiCookie; + break; + } + case (gpioIds::GYRO_2_ADIS_CS): { + gyro2Adis.cookie = spiCookie; + break; + } + case (gpioIds::GYRO_3_L3G_CS): { + gyro3L3g.cookie = spiCookie; + break; + } + default: { + sif::error << "AcsBoardPollingTask: invalid spi cookie" << std::endl; + } + } + return spiComIF.initializeInterface(cookie); +} + +ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData, + size_t sendLen) { + SpiCookie* spiCookie = dynamic_cast(cookie); + if (spiCookie == nullptr) { + return returnvalue::FAILED; + } + auto handleAdisRequest = [&](GyroAdis& adis) { + if (sendLen != sizeof(acs::Adis1650XRequest)) { + sif::error << "AcsBoardPolling: invalid adis request send length"; + adis.replyResult = returnvalue::FAILED; + return returnvalue::FAILED; + } + auto* req = reinterpret_cast(sendData); + if (req->mode != adis.mode) { + if (req->mode == acs::SimpleSensorMode::NORMAL) { + adis.type = req->type; + adis.countdown.setTimeout(adis1650x::START_UP_TIME); + if (adis.type == adis1650x::Type::ADIS16507) { + adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16507; + } else if (adis.type == adis1650x::Type::ADIS16505) { + adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16505; + } else { + sif::warning << "AcsBoardPolling: Unknown ADIS type" << std::endl; + } + adis.performStartup = true; + } else if (req->mode == acs::SimpleSensorMode::OFF) { + adis.performStartup = false; + adis.ownReply.cfgWasSet = false; + adis.ownReply.dataWasSet = false; + } + adis.mode = req->mode; + } + return returnvalue::OK; + }; + auto handleL3gRequest = [&](GyroL3g& gyro) { + if (sendLen != sizeof(acs::GyroL3gRequest)) { + sif::error << "AcsBoardPolling: invalid l3g request send length"; + gyro.replyResult = returnvalue::FAILED; + return returnvalue::FAILED; + } + auto* req = reinterpret_cast(sendData); + if (req->mode != gyro.mode) { + if (req->mode == acs::SimpleSensorMode::NORMAL) { + std::memcpy(gyro.sensorCfg, req->ctrlRegs, 5); + gyro.performStartup = true; + } else { + gyro.ownReply.cfgWasSet = false; + } + gyro.mode = req->mode; + } + return returnvalue::OK; + }; + auto handleLis3Request = [&](MgmLis3& mgm) { + if (sendLen != sizeof(acs::MgmLis3Request)) { + sif::error << "AcsBoardPolling: invalid lis3 request send length"; + mgm.replyResult = returnvalue::FAILED; + return returnvalue::FAILED; + } + auto* req = reinterpret_cast(sendData); + if (req->mode != mgm.mode) { + if (req->mode == acs::SimpleSensorMode::NORMAL) { + mgm.performStartup = true; + } else { + mgm.ownReply.dataWasSet = false; + mgm.ownReply.temperatureWasSet = false; + } + mgm.mode = req->mode; + } + return returnvalue::OK; + }; + auto handleRm3100Request = [&](MgmRm3100& mgm) { + if (sendLen != sizeof(acs::MgmRm3100Request)) { + sif::error << "AcsBoardPolling: invalid rm3100 request send length"; + mgm.replyResult = returnvalue::FAILED; + return returnvalue::FAILED; + } + auto* req = reinterpret_cast(sendData); + if (req->mode != mgm.mode) { + if (req->mode == acs::SimpleSensorMode::NORMAL) { + mgm.performStartup = true; + } else { + mgm.ownReply.dataWasRead = false; + } + mgm.mode = req->mode; + } + return returnvalue::OK; + }; + { + MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + switch (spiCookie->getChipSelectPin()) { + case (gpioIds::MGM_0_LIS3_CS): { + handleLis3Request(mgm0Lis3); + break; + } + case (gpioIds::MGM_1_RM3100_CS): { + handleRm3100Request(mgm1Rm3100); + break; + } + case (gpioIds::MGM_2_LIS3_CS): { + handleLis3Request(mgm2Lis3); + break; + } + case (gpioIds::MGM_3_RM3100_CS): { + handleRm3100Request(mgm3Rm3100); + break; + } + case (gpioIds::GYRO_0_ADIS_CS): { + handleAdisRequest(gyro0Adis); + break; + } + case (gpioIds::GYRO_2_ADIS_CS): { + handleAdisRequest(gyro2Adis); + break; + } + case (gpioIds::GYRO_1_L3G_CS): { + handleL3gRequest(gyro1L3g); + break; + } + case (gpioIds::GYRO_3_L3G_CS): { + handleL3gRequest(gyro3L3g); + break; + } + } + if (state == InternalState::IDLE) { + state = InternalState::BUSY; + } + } + semaphore->release(); + return returnvalue::OK; +} + +ReturnValue_t AcsBoardPolling::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; } + +ReturnValue_t AcsBoardPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { + return returnvalue::OK; +} + +ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, + size_t* size) { + SpiCookie* spiCookie = dynamic_cast(cookie); + if (spiCookie == nullptr) { + return returnvalue::FAILED; + } + MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + auto handleAdisReply = [&](GyroAdis& gyro) { + std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::Adis1650XReply)); + *buffer = reinterpret_cast(&gyro.readerReply); + *size = sizeof(acs::Adis1650XReply); + return gyro.replyResult; + }; + auto handleL3gReply = [&](GyroL3g& gyro) { + std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::GyroL3gReply)); + *buffer = reinterpret_cast(&gyro.readerReply); + *size = sizeof(acs::GyroL3gReply); + return gyro.replyResult; + }; + auto handleRm3100Reply = [&](MgmRm3100& mgm) { + std::memcpy(&mgm.readerReply, &mgm.ownReply, sizeof(acs::MgmRm3100Reply)); + *buffer = reinterpret_cast(&mgm.readerReply); + *size = sizeof(acs::MgmRm3100Reply); + return mgm.replyResult; + }; + auto handleLis3Reply = [&](MgmLis3& mgm) { + std::memcpy(&mgm.readerReply, &mgm.ownReply, sizeof(acs::MgmLis3Reply)); + *buffer = reinterpret_cast(&mgm.readerReply); + *size = sizeof(acs::MgmLis3Reply); + return mgm.replyResult; + }; + switch (spiCookie->getChipSelectPin()) { + case (gpioIds::MGM_0_LIS3_CS): { + return handleLis3Reply(mgm0Lis3); + } + case (gpioIds::MGM_1_RM3100_CS): { + return handleRm3100Reply(mgm1Rm3100); + } + case (gpioIds::MGM_2_LIS3_CS): { + return handleLis3Reply(mgm2Lis3); + } + case (gpioIds::MGM_3_RM3100_CS): { + return handleRm3100Reply(mgm3Rm3100); + } + case (gpioIds::GYRO_0_ADIS_CS): { + return handleAdisReply(gyro0Adis); + } + case (gpioIds::GYRO_2_ADIS_CS): { + return handleAdisReply(gyro2Adis); + } + case (gpioIds::GYRO_1_L3G_CS): { + return handleL3gReply(gyro1L3g); + } + case (gpioIds::GYRO_3_L3G_CS): { + return handleL3gReply(gyro3L3g); + } + } + return returnvalue::OK; +} + +void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) { + ReturnValue_t result; + acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; + bool gyroPerformStartup = false; + { + MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + mode = l3g.mode; + gyroPerformStartup = l3g.performStartup; + } + if (mode == acs::SimpleSensorMode::NORMAL) { + if (gyroPerformStartup) { + cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK; + std::memcpy(cmdBuf.data() + 1, l3g.sensorCfg, 5); + result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::OK; + } + // Ignore useless reply and red config + cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK; + std::memset(cmdBuf.data() + 1, 0, 5); + result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::OK; + } + result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::OK; + } + MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + // Cross check configuration as verification that communication is working + for (uint8_t idx = 0; idx < 5; idx++) { + if (rawReply[idx + 1] != l3g.sensorCfg[idx]) { + sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl; + l3g.replyResult = returnvalue::FAILED; + return; + } + } + l3g.performStartup = false; + l3g.ownReply.cfgWasSet = true; + l3g.ownReply.sensitivity = l3gd20h::ctrlReg4ToSensitivity(l3g.sensorCfg[3]); + } + cmdBuf[0] = l3gd20h::READ_START | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK; + std::memset(cmdBuf.data() + 1, 0, l3gd20h::READ_LEN); + result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), l3gd20h::READ_LEN + 1); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::FAILED; + return; + } + result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::FAILED; + return; + } + MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + // The regular read function always returns the full sensor config as well. Use that + // to verify communications. + for (uint8_t idx = 0; idx < 5; idx++) { + if (rawReply[idx + 1] != l3g.sensorCfg[idx]) { + sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl; + l3g.replyResult = returnvalue::FAILED; + return; + } + } + l3g.ownReply.statusReg = rawReply[l3gd20h::STATUS_IDX]; + l3g.ownReply.angVelocities[0] = (rawReply[l3gd20h::OUT_X_H] << 8) | rawReply[l3gd20h::OUT_X_L]; + l3g.ownReply.angVelocities[1] = (rawReply[l3gd20h::OUT_Y_H] << 8) | rawReply[l3gd20h::OUT_Y_L]; + l3g.ownReply.angVelocities[2] = (rawReply[l3gd20h::OUT_Z_H] << 8) | rawReply[l3gd20h::OUT_Z_L]; + l3g.ownReply.tempOffsetRaw = rawReply[l3gd20h::TEMPERATURE_IDX]; + } +} + +ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) { + ReturnValue_t result = returnvalue::OK; + int retval = 0; + // Prepare transfer + int fileDescriptor = 0; + std::string device = spiComIF.getSpiDev(); + UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); + if (fileHelper.getOpenResult() != returnvalue::OK) { + return SpiComIF::OPENING_FILE_FAILED; + } + spi::SpiModes spiMode = spi::SpiModes::MODE_0; + uint32_t spiSpeed = 0; + cookie.getSpiParameters(spiMode, spiSpeed, nullptr); + spiComIF.setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); + cookie.assignWriteBuffer(cmdBuf.data()); + cookie.setTransferSize(2); + + gpioId_t gpioId = cookie.getChipSelectPin(); + MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; + uint32_t timeoutMs = 0; + MutexIF* mutex = spiComIF.getCsMutex(); + cookie.getMutexParams(timeoutType, timeoutMs); + if (mutex == nullptr) { + sif::warning << "GyroADIS16507Handler::spiSendCallback: " + "Mutex or GPIO interface invalid" + << std::endl; + return returnvalue::FAILED; + } + + size_t idx = 0; + spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle(); + uint64_t origTx = transferStruct->tx_buf; + uint64_t origRx = transferStruct->rx_buf; + while (idx < transferLen) { + result = mutex->lockMutex(timeoutType, timeoutMs); + if (result != returnvalue::OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "AcsBoardPolling: Failed to lock mutex" << std::endl; +#endif + return result; + } + // Pull SPI CS low. For now, no support for active high given + if (gpioId != gpio::NO_GPIO) { + gpioIF.pullLow(gpioId); + } + + // Execute transfer + // Initiate a full duplex SPI transfer. + retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie.getTransferStructHandle()); + if (retval < 0) { + utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); + result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED; + } +#if FSFW_HAL_SPI_WIRETAPPING == 1 + comIf->performSpiWiretapping(cookie); +#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ + + if (gpioId != gpio::NO_GPIO) { + gpioIF.pullHigh(gpioId); + } + mutex->unlockMutex(); + + idx += 2; + transferStruct->tx_buf += 2; + transferStruct->rx_buf += 2; + if (idx < transferLen) { + usleep(adis1650x::STALL_TIME_MICROSECONDS); + } + } + transferStruct->tx_buf = origTx; + transferStruct->rx_buf = origRx; + cookie.setTransferSize(transferLen); + return returnvalue::OK; +} + +void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { + ReturnValue_t result; + acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; + bool cdHasTimedOut = false; + bool mustPerformStartup = false; + { + MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + mode = gyro.mode; + cdHasTimedOut = gyro.countdown.hasTimedOut(); + mustPerformStartup = gyro.performStartup; + } + if (mode == acs::SimpleSensorMode::NORMAL and cdHasTimedOut) { + if (mustPerformStartup) { + uint8_t regList[6]; + // Read configuration + regList[0] = adis1650x::DIAG_STAT_REG; + regList[1] = adis1650x::FILTER_CTRL_REG; + regList[2] = adis1650x::RANG_MDL_REG; + regList[3] = adis1650x::MSC_CTRL_REG; + regList[4] = adis1650x::DEC_RATE_REG; + regList[5] = adis1650x::PROD_ID_REG; + size_t transferLen = + adis1650x::prepareReadCommand(regList, sizeof(regList), cmdBuf.data(), cmdBuf.size()); + result = readAdisCfg(*gyro.cookie, transferLen); + if (result != returnvalue::OK) { + gyro.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy); + if (result != returnvalue::OK or rawReply == nullptr) { + gyro.replyResult = result; + return; + } + uint16_t prodId = (rawReply[12] << 8) | rawReply[13]; + if (((gyro.type == adis1650x::Type::ADIS16505) and (prodId != adis1650x::PROD_ID_16505)) or + ((gyro.type == adis1650x::Type::ADIS16507) and (prodId != adis1650x::PROD_ID_16507))) { + sif::warning << "AcsPollingTask: Invalid ADIS product ID " << prodId << std::endl; + gyro.replyResult = returnvalue::FAILED; + return; + } + MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + gyro.ownReply.cfgWasSet = true; + gyro.ownReply.cfg.diagStat = (rawReply[2] << 8) | rawReply[3]; + gyro.ownReply.cfg.filterSetting = (rawReply[4] << 8) | rawReply[5]; + gyro.ownReply.cfg.rangMdl = (rawReply[6] << 8) | rawReply[7]; + gyro.ownReply.cfg.mscCtrlReg = (rawReply[8] << 8) | rawReply[9]; + gyro.ownReply.cfg.decRateReg = (rawReply[10] << 8) | rawReply[11]; + gyro.ownReply.cfg.prodId = prodId; + gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl); + gyro.performStartup = false; + } + // Read regular registers + std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(), + adis1650x::BURST_READ_ENABLE.size()); + std::memset(cmdBuf.data() + 2, 0, 10 * 2); + result = spiComIF.sendMessage(gyro.cookie, cmdBuf.data(), adis1650x::SENSOR_READOUT_SIZE); + if (result != returnvalue::OK) { + gyro.replyResult = returnvalue::FAILED; + return; + } + result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy); + if (result != returnvalue::OK or rawReply == nullptr) { + gyro.replyResult = returnvalue::FAILED; + return; + } + uint16_t checksum = (rawReply[20] << 8) | rawReply[21]; + + // Now verify the read checksum with the expected checksum according to datasheet p. 20 + uint16_t calcChecksum = 0; + for (size_t idx = 2; idx < 20; idx++) { + calcChecksum += rawReply[idx]; + } + if (checksum != calcChecksum) { + sif::warning << "AcsPollingTask: Invalid ADIS reply checksum" << std::endl; + gyro.replyResult = returnvalue::FAILED; + return; + } + + auto burstMode = adis1650x::burstModeFromMscCtrl(gyro.ownReply.cfg.mscCtrlReg); + if (burstMode != adis1650x::BurstModes::BURST_16_BURST_SEL_0) { + sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Analysis for select burst mode" + " not implemented!" + << std::endl; + gyro.replyResult = returnvalue::FAILED; + return; + } + + MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + gyro.ownReply.dataWasSet = true; + gyro.ownReply.cfg.diagStat = rawReply[2] << 8 | rawReply[3]; + gyro.ownReply.data.angVelocities[0] = (rawReply[4] << 8) | rawReply[5]; + gyro.ownReply.data.angVelocities[1] = (rawReply[6] << 8) | rawReply[7]; + gyro.ownReply.data.angVelocities[2] = (rawReply[8] << 8) | rawReply[9]; + + gyro.ownReply.data.accelerations[0] = (rawReply[10] << 8) | rawReply[11]; + gyro.ownReply.data.accelerations[1] = (rawReply[12] << 8) | rawReply[13]; + gyro.ownReply.data.accelerations[2] = (rawReply[14] << 8) | rawReply[15]; + + gyro.ownReply.data.temperatureRaw = (rawReply[16] << 8) | rawReply[17]; + } +} + +void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) { + ReturnValue_t result; + acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; + bool mustPerformStartup = false; + { + MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + mode = mgm.mode; + mustPerformStartup = mgm.performStartup; + } + if (mode == acs::SimpleSensorMode::NORMAL) { + if (mustPerformStartup) { + // To check valid communication, read back identification + // register which should always be the same value. + cmdBuf[0] = mgmLis3::readCommand(mgmLis3::IDENTIFY_DEVICE_REG_ADDR); + cmdBuf[1] = 0x00; + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2); + if (result != OK) { + mgm.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy); + if (result != OK) { + mgm.replyResult = result; + return; + } + if (rawReply[1] != mgmLis3::DEVICE_ID) { + sif::error << "AcsPollingTask: invalid MGM lis3 device ID" << std::endl; + mgm.replyResult = result; + return; + } + mgm.cfg[0] = mgmLis3::CTRL_REG1_DEFAULT; + mgm.cfg[1] = mgmLis3::CTRL_REG2_DEFAULT; + mgm.cfg[2] = mgmLis3::CTRL_REG3_DEFAULT; + mgm.cfg[3] = mgmLis3::CTRL_REG4_DEFAULT; + mgm.cfg[4] = mgmLis3::CTRL_REG5_DEFAULT; + cmdBuf[0] = mgmLis3::writeCommand(mgmLis3::CTRL_REG1, true); + std::memcpy(cmdBuf.data() + 1, mgm.cfg, 5); + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 6); + if (result != OK) { + mgm.replyResult = result; + return; + } + // Done here. We can always read back config and data during periodic handling + mgm.performStartup = false; + } + cmdBuf[0] = mgmLis3::readCommand(mgmLis3::CTRL_REG1, true); + std::memset(cmdBuf.data() + 1, 0, mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS); + result = + spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS + 1); + if (result != returnvalue::OK) { + mgm.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy); + if (result != returnvalue::OK) { + mgm.replyResult = result; + return; + } + // Verify communication by re-checking config + if (rawReply[1] != mgm.cfg[0] or rawReply[2] != mgm.cfg[1] or rawReply[3] != mgm.cfg[2] or + rawReply[4] != mgm.cfg[3] or rawReply[5] != mgm.cfg[4]) { + mgm.replyResult = result; + return; + } + { + MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + mgm.ownReply.dataWasSet = true; + mgm.ownReply.sensitivity = mgmLis3::getSensitivityFactor(mgmLis3::getSensitivity(mgm.cfg[1])); + mgm.ownReply.mgmValuesRaw[0] = + (rawReply[mgmLis3::X_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::X_LOWBYTE_IDX]; + mgm.ownReply.mgmValuesRaw[1] = + (rawReply[mgmLis3::Y_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::Y_LOWBYTE_IDX]; + mgm.ownReply.mgmValuesRaw[2] = + (rawReply[mgmLis3::Z_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::Z_LOWBYTE_IDX]; + } + // Read tempetature + cmdBuf[0] = mgmLis3::readCommand(mgmLis3::TEMP_LOWBYTE, true); + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 3); + if (result != returnvalue::OK) { + mgm.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy); + if (result != returnvalue::OK) { + mgm.replyResult = result; + return; + } + MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + mgm.ownReply.temperatureWasSet = true; + mgm.ownReply.temperatureRaw = (rawReply[2] << 8) | rawReply[1]; + } +} + +void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) { + ReturnValue_t result; + acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; + bool mustPerformStartup = false; + { + MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + mode = mgm.mode; + mustPerformStartup = mgm.performStartup; + } + if (mode == acs::SimpleSensorMode::NORMAL) { + if (mustPerformStartup) { + // Configure CMM first + cmdBuf[0] = mgmRm3100::CMM_REGISTER; + cmdBuf[1] = mgmRm3100::CMM_VALUE; + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2); + if (result != OK) { + mgm.replyResult = result; + return; + } + // Read back register + cmdBuf[0] = mgmRm3100::CMM_REGISTER | mgmRm3100::READ_MASK; + cmdBuf[1] = 0; + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2); + if (result != OK) { + mgm.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy); + if (result != OK) { + mgm.replyResult = result; + return; + } + if (rawReply[1] != mgmRm3100::CMM_VALUE) { + sif::error << "AcsBoardPolling: MGM RM3100 read back CMM invalid" << std::endl; + mgm.replyResult = result; + return; + } + // Configure TMRC register + cmdBuf[0] = mgmRm3100::TMRC_REGISTER; + // hardcoded for now + cmdBuf[1] = mgm.tmrcValue; + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2); + if (result != OK) { + mgm.replyResult = result; + return; + } + // Read back and verify value + cmdBuf[0] = mgmRm3100::TMRC_REGISTER | mgmRm3100::READ_MASK; + cmdBuf[1] = 0; + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2); + if (result != OK) { + mgm.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy); + if (result != OK) { + mgm.replyResult = result; + return; + } + if (rawReply[1] != mgm.tmrcValue) { + sif::error << "AcsBoardPolling: MGM RM3100 read back TMRC invalid" << std::endl; + mgm.replyResult = result; + return; + } + mgm.performStartup = false; + } + // Regular read operation + cmdBuf[0] = mgmRm3100::MEASUREMENT_REG_START | mgmRm3100::READ_MASK; + std::memset(cmdBuf.data() + 1, 0, 9); + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 10); + if (result != OK) { + mgm.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy); + if (result != OK) { + mgm.replyResult = result; + return; + } + MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + for (uint8_t idx = 0; idx < 3; idx++) { + // Hardcoded, but note that the gain depends on the cycle count + // value which is configurable! + mgm.ownReply.scaleFactors[idx] = 1.0 / mgmRm3100::DEFAULT_GAIN; + } + mgm.ownReply.dataWasRead = true; + // Bitshift trickery to account for 24 bit signed value. + mgm.ownReply.mgmValuesRaw[0] = + ((rawReply[1] << 24) | (rawReply[2] << 16) | (rawReply[3] << 8)) >> 8; + mgm.ownReply.mgmValuesRaw[1] = + ((rawReply[4] << 24) | (rawReply[5] << 16) | (rawReply[6] << 8)) >> 8; + mgm.ownReply.mgmValuesRaw[2] = + ((rawReply[7] << 24) | (rawReply[8] << 16) | (rawReply[9] << 8)) >> 8; + } +} diff --git a/linux/devices/AcsBoardPolling.h b/linux/devices/AcsBoardPolling.h new file mode 100644 index 00000000..e3c3bd24 --- /dev/null +++ b/linux/devices/AcsBoardPolling.h @@ -0,0 +1,91 @@ +#ifndef LINUX_DEVICES_ACSBOARDPOLLING_H_ +#define LINUX_DEVICES_ACSBOARDPOLLING_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +class AcsBoardPolling : public SystemObject, + public ExecutableObjectIF, + public DeviceCommunicationIF { + public: + AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF); + + ReturnValue_t performOperation(uint8_t operationCode) override; + ReturnValue_t initialize() override; + + private: + enum class InternalState { IDLE, BUSY } state = InternalState::IDLE; + MutexIF* ipcLock; + static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING; + static constexpr uint32_t LOCK_TIMEOUT = 20; + static constexpr char LOCK_CTX[] = "AcsBoardPolling"; + SemaphoreIF* semaphore; + std::array cmdBuf; + + struct DevBase { + SpiCookie* cookie = nullptr; + bool performStartup = false; + acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; + ReturnValue_t replyResult = returnvalue::OK; + }; + + struct GyroAdis : public DevBase { + adis1650x::Type type; + Countdown countdown; + acs::Adis1650XReply ownReply; + acs::Adis1650XReply readerReply; + }; + GyroAdis gyro0Adis{}; + GyroAdis gyro2Adis{}; + + struct GyroL3g : public DevBase { + uint8_t sensorCfg[5]; + acs::GyroL3gReply ownReply; + acs::GyroL3gReply readerReply; + }; + GyroL3g gyro1L3g{}; + GyroL3g gyro3L3g{}; + + struct MgmRm3100 : public DevBase { + uint8_t tmrcValue = mgmRm3100::TMRC_DEFAULT_37HZ_VALUE; + acs::MgmRm3100Reply ownReply; + acs::MgmRm3100Reply readerReply; + }; + MgmRm3100 mgm1Rm3100; + MgmRm3100 mgm3Rm3100; + + struct MgmLis3 : public DevBase { + uint8_t cfg[5]{}; + acs::MgmLis3Reply ownReply; + acs::MgmLis3Reply readerReply; + }; + MgmLis3 mgm0Lis3; + MgmLis3 mgm2Lis3; + + uint8_t* rawReply = nullptr; + size_t dummy = 0; + + SpiComIF& spiComIF; + GpioIF& gpioIF; + + ReturnValue_t initializeInterface(CookieIF* cookie) override; + ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override; + ReturnValue_t getSendSuccess(CookieIF* cookie) override; + ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override; + ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override; + + void gyroL3gHandler(GyroL3g& l3g); + void gyroAdisHandler(GyroAdis& gyro); + void mgmLis3Handler(MgmLis3& mgm); + void mgmRm3100Handler(MgmRm3100& mgm); + // Special readout: 16us stall time between small 2 byte transfers. + ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen); +}; + +#endif /* LINUX_DEVICES_ACSBOARDPOLLING_H_ */ diff --git a/linux/devices/CMakeLists.txt b/linux/devices/CMakeLists.txt index 22b39840..8b23566b 100644 --- a/linux/devices/CMakeLists.txt +++ b/linux/devices/CMakeLists.txt @@ -4,8 +4,14 @@ endif() target_sources( ${OBSW_NAME} - PRIVATE Max31865RtdPolling.cpp ScexUartReader.cpp ImtqPollingTask.cpp - ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp) + PRIVATE Max31865RtdPolling.cpp + ScexUartReader.cpp + ImtqPollingTask.cpp + SusPolling.cpp + ScexDleParser.cpp + ScexHelper.cpp + RwPollingTask.cpp + AcsBoardPolling.cpp) add_subdirectory(ploc) diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index 3b396504..18461c14 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -19,9 +19,7 @@ GpsHyperionLinuxController::GpsHyperionLinuxController(object_id_t objectId, object_id_t parentId, bool debugHyperionGps) - : ExtendedControllerBase(objectId), gpsSet(this), debugHyperionGps(debugHyperionGps) { - timeUpdateCd.resetTimer(); -} + : ExtendedControllerBase(objectId), gpsSet(this), debugHyperionGps(debugHyperionGps) {} GpsHyperionLinuxController::~GpsHyperionLinuxController() { gps_stream(&gps, WATCH_DISABLE, nullptr); @@ -196,8 +194,8 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { if (mode != MODE_OFF) { if (maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) { sif::warning << "GpsHyperionLinuxController: No mode could be set in allowed " - << maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl; - triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout); + << maxTimeToReachFix.getTimeoutMs() / 1000 << " seconds" << std::endl; + triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs()); oneShotSwitches.cantGetFixSwitch = false; } modeIsSet = false; @@ -216,16 +214,14 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { } bool validFix = false; + uint8_t newFix = 0; if (modeIsSet) { // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix if (gps.fix.mode == 2 or gps.fix.mode == 3) { validFix = true; } - if (gpsSet.fixMode.value != gps.fix.mode) { - triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, gps.fix.mode); - } - gpsSet.fixMode.value = gps.fix.mode; - if (gps.fix.mode == 0 or gps.fix.mode == 1) { + newFix = gps.fix.mode; + if (newFix == 0 or newFix == 1) { if (modeCommanded and maxTimeToReachFix.hasTimedOut()) { // We are supposed to be on and functioning, but no fix was found if (mode == MODE_ON or mode == MODE_NORMAL) { @@ -235,6 +231,10 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { } } } + if (gpsSet.fixMode.value != newFix) { + triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFix); + } + gpsSet.fixMode = newFix; gpsSet.fixMode.setValid(modeIsSet); // Only set on specific messages, so only set a valid flag to invalid diff --git a/linux/devices/GpsHyperionLinuxController.h b/linux/devices/GpsHyperionLinuxController.h index df08d8a4..f00724bd 100644 --- a/linux/devices/GpsHyperionLinuxController.h +++ b/linux/devices/GpsHyperionLinuxController.h @@ -23,7 +23,8 @@ */ class GpsHyperionLinuxController : public ExtendedControllerBase { public: - static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 60 * 5; + // 30 minutes + static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 30; enum ReadModes { SHM = 0, SOCKET = 1 }; @@ -79,7 +80,6 @@ class GpsHyperionLinuxController : public ExtendedControllerBase { bool debugHyperionGps = false; int32_t noModeSetCntr = 0; - Countdown timeUpdateCd = Countdown(60); // Returns true if the function should be called again or false if other // controller handling can be done. diff --git a/linux/devices/ImtqPollingTask.cpp b/linux/devices/ImtqPollingTask.cpp index 4d8790ee..fbd5f847 100644 --- a/linux/devices/ImtqPollingTask.cpp +++ b/linux/devices/ImtqPollingTask.cpp @@ -28,6 +28,8 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) { // Stopwatch watch; switch (currentRequest) { case imtq::RequestType::MEASURE_NO_ACTUATION: { + // Measured to take 24 ms for debug and release builds. + // Stopwatch watch; handleMeasureStep(); break; } @@ -35,6 +37,9 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) { handleActuateStep(); break; } + default: { + break; + } }; } return returnvalue::OK; @@ -44,6 +49,9 @@ void ImtqPollingTask::handleMeasureStep() { size_t replyLen = 0; uint8_t* replyPtr; ImtqRepliesDefault replies(replyBuf.data()); + // If some startup handling is added later, set configured after it was done once. + replies.setConfigured(); + // Can be used later to verify correct timing (e.g. all data has been read) clearReadFlagsDefault(replies); auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) { @@ -112,18 +120,30 @@ void ImtqPollingTask::handleMeasureStep() { } } + // The I2C IP core on EIVE sometimes glitches out. Send start MTM measurement twice. + cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT; + if (i2cCmdExecMeasure(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) { + return; + } cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT; if (i2cCmdExecMeasure(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) { return; } // Takes a bit of time to take measurements. Subtract a bit because of the delay of previous // commands. - TaskFactory::delayTask(currentIntegrationTimeMs); + TaskFactory::delayTask(currentIntegrationTimeMs + MGM_READ_BUFFER_TIME_MS); cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT; if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { return; } + bool mgmMeasurementTooOld = false; + // See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably + // have old data. Which can be really bad for ACS. And everything. + if ((replyPtr[2] >> 7) == 0) { + replyPtr[0] = false; + mgmMeasurementTooOld = true; + } cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA; if (i2cCmdExecMeasure(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) { @@ -134,7 +154,9 @@ void ImtqPollingTask::handleMeasureStep() { if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) { return; } - // sif::debug << "measure done" << std::endl; + if (mgmMeasurementTooOld) { + sif::error << "IMTQ: MGM measurement too old" << std::endl; + } return; } @@ -157,23 +179,41 @@ void ImtqPollingTask::handleActuateStep() { return; } + TaskFactory::delayTask(10); + cmdLen = 1; + // The I2C IP core on EIVE sometimes glitches out. Send start MTM measurement twice. + cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT; + if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) { + return; + } cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT; if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) { return; } - TaskFactory::delayTask(currentIntegrationTimeMs); + TaskFactory::delayTask(currentIntegrationTimeMs + MGM_READ_BUFFER_TIME_MS); cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT; if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { return; } + bool measurementWasTooOld = false; + // See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably + // have old data. Which can be really bad for ACS. And everything. + if ((replyPtr[2] >> 7) == 0) { + measurementWasTooOld = true; + replyPtr[0] = false; + } + cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA; if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) { return; } - // sif::debug << "measure with torque done" << std::endl; + + if (measurementWasTooOld) { + sif::error << "IMTQ: MGM measurement too old" << std::endl; + } return; } @@ -192,15 +232,15 @@ ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) { ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { - ImtqRequest request(sendData, sendLen); + const auto* imtqReq = reinterpret_cast(sendData); { MutexGuard mg(ipcLock); - currentRequest = request.getRequestType(); - if (currentRequest == imtq::RequestType::ACTUATE) { - std::memcpy(dipoles, request.getDipoles(), 6); - torqueDuration = request.getTorqueDuration(); + if (imtqReq->request == imtq::RequestType::ACTUATE) { + std::memcpy(dipoles, imtqReq->dipoles, sizeof(dipoles)); + torqueDuration = imtqReq->torqueDuration; } - specialRequest = request.getSpecialRequest(); + currentRequest = imtqReq->request; + specialRequest = imtqReq->specialRequest; if (state != InternalState::IDLE) { return returnvalue::FAILED; } @@ -309,6 +349,8 @@ void ImtqPollingTask::buildDipoleCommand() { } SerializeAdapter::serialize(&torqueDuration, &serPtr, &serLen, cmdBuf.size(), SerializeIF::Endianness::LITTLE); + // sif::debug << "Dipole X: " << dipoles[0] << std::endl; + // sif::debug << "Torqeu Dur: " << torqueDuration << std::endl; cmdLen = 1 + serLen; } @@ -325,9 +367,11 @@ ReturnValue_t ImtqPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** b if (currentRequest == imtq::RequestType::MEASURE_NO_ACTUATION) { replyLen = getExchangeBufLen(specialRequest); memcpy(exchangeBuf.data(), replyBuf.data(), replyLen); - } else { + } else if (currentRequest == imtq::RequestType::ACTUATE) { replyLen = ImtqRepliesWithTorque::BASE_LEN; memcpy(exchangeBuf.data(), replyBufActuation.data(), replyLen); + } else { + *size = 0; } *buffer = exchangeBuf.data(); *size = replyLen; diff --git a/linux/devices/ImtqPollingTask.h b/linux/devices/ImtqPollingTask.h index 00a05fc6..cb2d3882 100644 --- a/linux/devices/ImtqPollingTask.h +++ b/linux/devices/ImtqPollingTask.h @@ -32,12 +32,13 @@ class ImtqPollingTask : public SystemObject, const char* i2cDev = nullptr; address_t i2cAddr = 0; uint32_t currentIntegrationTimeMs = 10; + // Required in addition to integration time, otherwise old data might be read. + static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 6; bool ignoreNextActuateRequest = false; imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE; int16_t dipoles[3] = {}; uint16_t torqueDuration = 0; - // uint8_t startActuateRawBuf[3] = {}; std::array cmdBuf; std::array replyBuf; diff --git a/linux/devices/Max31865RtdPolling.cpp b/linux/devices/Max31865RtdPolling.cpp index e59c2ef2..db13a76c 100644 --- a/linux/devices/Max31865RtdPolling.cpp +++ b/linux/devices/Max31865RtdPolling.cpp @@ -19,25 +19,19 @@ static constexpr uint8_t BASE_CFG = Max31865RtdPolling::Max31865RtdPolling(object_id_t objectId, SpiComIF* lowLevelComIF, GpioIF* gpioIF) : SystemObject(objectId), rtds(EiveMax31855::NUM_RTDS), comIF(lowLevelComIF), gpioIF(gpioIF) { - readerMutex = MutexFactory::instance()->createMutex(); + readerLock = MutexFactory::instance()->createMutex(); } ReturnValue_t Max31865RtdPolling::performOperation(uint8_t operationCode) { using namespace MAX31865; ReturnValue_t result = returnvalue::OK; static_cast(result); + // Measured to take 0-1 ms in debug build // Stopwatch watch; - if (periodicInitHandling()) { -#if OBSW_RTD_AUTO_MODE == 0 - // 10 ms delay for VBIAS startup - TaskFactory::delayTask(10); -#endif - } else { - // No devices usable (e.g. TCS board off) - return returnvalue::OK; - } - + periodicInitHandling(); #if OBSW_RTD_AUTO_MODE == 0 + // 10 ms delay for VBIAS startup + TaskFactory::delayTask(10); result = periodicReadReqHandling(); if (result != returnvalue::OK) { return result; @@ -56,19 +50,28 @@ bool Max31865RtdPolling::rtdIsActive(uint8_t idx) { return false; } -bool Max31865RtdPolling::periodicInitHandling() { +ReturnValue_t Max31865RtdPolling::periodicInitHandling() { using namespace MAX31865; ReturnValue_t result = returnvalue::OK; for (auto& rtd : rtds) { if (rtd == nullptr) { continue; } - MutexGuard mg(readerMutex); - if (mg.getLockResult() != returnvalue::OK) { - sif::warning << "Max31865RtdReader::periodicInitHandling: Mutex lock failed" << std::endl; - return false; + bool mustPerformInitHandling = false; + bool doWriteLowThreshold = false; + bool doWriteHighThreshold = false; + { + MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + if (mg.getLockResult() != returnvalue::OK) { + sif::warning << "Max31865RtdReader::periodicInitHandling: Mutex lock failed" << std::endl; + continue; + } + mustPerformInitHandling = + (rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut(); + doWriteHighThreshold = rtd->writeHighThreshold; + doWriteLowThreshold = rtd->writeLowThreshold; } - if ((rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut()) { + if (mustPerformInitHandling) { // Please note that using the manual CS lock wrapper here is problematic. Might be a SPI // or hardware specific issue where the CS needs to be pulled high and then low again // between transfers @@ -77,13 +80,13 @@ bool Max31865RtdPolling::periodicInitHandling() { handleSpiError(rtd, result, "writeCfgReg"); continue; } - if (rtd->writeLowThreshold) { + if (doWriteLowThreshold) { result = writeLowThreshold(rtd->spiCookie, rtd->lowThreshold); if (result != returnvalue::OK) { handleSpiError(rtd, result, "writeLowThreshold"); } } - if (rtd->writeHighThreshold) { + if (doWriteHighThreshold) { result = writeHighThreshold(rtd->spiCookie, rtd->highThreshold); if (result != returnvalue::OK) { handleSpiError(rtd, result, "writeHighThreshold"); @@ -93,38 +96,23 @@ bool Max31865RtdPolling::periodicInitHandling() { if (result != returnvalue::OK) { handleSpiError(rtd, result, "clearFaultStatus"); } + MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); rtd->db.configured = true; rtd->db.active = true; } } - bool someRtdUsable = false; - for (auto& rtd : rtds) { - if (rtd == nullptr) { - continue; - } - if (rtdIsActive(rtd->idx)) { -#if OBSW_RTD_AUTO_MODE == 0 - result = writeBiasSel(Bias::ON, rtd->spiCookie, BASE_CFG); -#endif - someRtdUsable = true; - } - } - return someRtdUsable; + return returnvalue::OK; } ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() { using namespace MAX31865; + updateActiveRtdsArray(); // Now request one shot config for all active RTDs for (auto& rtd : rtds) { if (rtd == nullptr) { continue; } - MutexGuard mg(readerMutex); - if (mg.getLockResult() != returnvalue::OK) { - sif::warning << "Max31865RtdReader::periodicReadReqHandling: Mutex lock failed" << std::endl; - return returnvalue::FAILED; - } - if (rtdIsActive(rtd->idx)) { + if (activeRtdsArray[rtd->idx]) { ReturnValue_t result = writeCfgReg(rtd->spiCookie, BASE_CFG | (1 << CfgBitPos::ONE_SHOT)); if (result != returnvalue::OK) { handleSpiError(rtd, result, "writeCfgReg"); @@ -139,17 +127,13 @@ ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() { ReturnValue_t Max31865RtdPolling::periodicReadHandling() { using namespace MAX31865; auto result = returnvalue::OK; + updateActiveRtdsArray(); // Now read the RTD values for (auto& rtd : rtds) { if (rtd == nullptr) { continue; } - MutexGuard mg(readerMutex); - if (mg.getLockResult() != returnvalue::OK) { - sif::warning << "Max31865RtdReader::periodicReadHandling: Mutex lock failed" << std::endl; - return returnvalue::FAILED; - } - if (rtdIsActive(rtd->idx)) { + if (activeRtdsArray[rtd->idx]) { // Please note that using the manual CS lock wrapper here is problematic. Might be a SPI // or hardware specific issue where the CS needs to be pulled high and then low again // between transfers @@ -166,6 +150,7 @@ ReturnValue_t Max31865RtdPolling::periodicReadHandling() { handleSpiError(rtd, result, "readRtdVal"); continue; } + MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); if (faultBitSet) { rtd->db.faultBitSet = faultBitSet; } @@ -200,7 +185,7 @@ ReturnValue_t Max31865RtdPolling::initializeInterface(CookieIF* cookie) { throw std::invalid_argument("Invalid RTD index"); } rtds[rtdCookie->idx] = rtdCookie; - MutexGuard mg(readerMutex); + MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); if (dbLen == 0) { dbLen = rtdCookie->db.getSerializedSize(); } @@ -212,16 +197,19 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s if (cookie == nullptr) { return returnvalue::FAILED; } + auto* rtdCookie = dynamic_cast(cookie); + if (rtdCookie == nullptr) { + return returnvalue::FAILED; + } // Empty command.. don't fail for now if (sendLen < 1) { return returnvalue::OK; } - MutexGuard mg(readerMutex); + MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); if (mg.getLockResult() != returnvalue::OK) { sif::warning << "Max31865RtdReader::sendMessage: Mutex lock failed" << std::endl; return returnvalue::FAILED; } - auto* rtdCookie = dynamic_cast(cookie); uint8_t cmdRaw = sendData[0]; if (cmdRaw > EiveMax31855::RtdCommands::NUM_CMDS) { sif::warning << "Max31865RtdReader::sendMessage: Invalid command" << std::endl; @@ -240,7 +228,6 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s case (EiveMax31855::RtdCommands::ON): { if (not rtdCookie->on) { rtdCookie->cd.setTimeout(MAX31865::WARMUP_MS); - rtdCookie->cd.resetTimer(); rtdCookie->on = true; rtdCookie->db.active = false; rtdCookie->db.configured = false; @@ -253,7 +240,6 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s case (EiveMax31855::RtdCommands::ACTIVE): { if (not rtdCookie->on) { rtdCookie->cd.setTimeout(MAX31865::WARMUP_MS); - rtdCookie->cd.resetTimer(); rtdCookie->on = true; rtdCookie->db.active = true; rtdCookie->db.configured = false; @@ -312,15 +298,15 @@ ReturnValue_t Max31865RtdPolling::requestReceiveMessage(CookieIF* cookie, size_t ReturnValue_t Max31865RtdPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) { - MutexGuard mg(readerMutex); - if (mg.getLockResult() != returnvalue::OK) { - // TODO: Emit warning - return returnvalue::FAILED; - } auto* rtdCookie = dynamic_cast(cookie); if (rtdCookie == nullptr) { return returnvalue::FAILED; } + MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + if (mg.getLockResult() != returnvalue::OK) { + // TODO: Emit warning + return returnvalue::FAILED; + } uint8_t* exchangePtr = rtdCookie->exchangeBuf.data(); size_t serLen = 0; auto result = rtdCookie->db.serialize(&exchangePtr, &serLen, rtdCookie->exchangeBuf.size(), @@ -461,6 +447,18 @@ ReturnValue_t Max31865RtdPolling::readNFromReg(SpiCookie* cookie, uint8_t reg, s return returnvalue::OK; } +ReturnValue_t Max31865RtdPolling::updateActiveRtdsArray() { + MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + if (mg.getLockResult() != returnvalue::OK) { + sif::warning << "Max31865RtdReader::periodicReadHandling: Mutex lock failed" << std::endl; + return returnvalue::FAILED; + } + for (const auto& rtd : rtds) { + activeRtdsArray[rtd->idx] = rtdIsActive(rtd->idx); + } + return returnvalue::OK; +} + ReturnValue_t Max31865RtdPolling::handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result, const char* ctx) { cookie->db.spiErrorCount.value += 1; diff --git a/linux/devices/Max31865RtdPolling.h b/linux/devices/Max31865RtdPolling.h index a34c8e53..ef73fb8b 100644 --- a/linux/devices/Max31865RtdPolling.h +++ b/linux/devices/Max31865RtdPolling.h @@ -47,8 +47,12 @@ class Max31865RtdPolling : public SystemObject, private: std::vector rtds; std::array cmdBuf = {}; + std::array activeRtdsArray{}; size_t dbLen = 0; - MutexIF* readerMutex; + MutexIF* readerLock; + static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING; + static constexpr uint32_t LOCK_TIMEOUT = 20; + static constexpr char LOCK_CTX[] = "Max31865RtdPolling"; SpiComIF* comIF; GpioIF* gpioIF; @@ -56,7 +60,7 @@ class Max31865RtdPolling : public SystemObject, uint32_t csTimeoutMs = spi::RTD_CS_TIMEOUT; MutexIF* csLock = nullptr; - bool periodicInitHandling(); + ReturnValue_t periodicInitHandling(); ReturnValue_t periodicReadReqHandling(); ReturnValue_t periodicReadHandling(); @@ -81,6 +85,8 @@ class Max31865RtdPolling : public SystemObject, ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override; ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override; + ReturnValue_t updateActiveRtdsArray(); + ReturnValue_t handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result, const char* ctx); }; diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp index 2d05ac8b..7dff31bb 100644 --- a/linux/devices/RwPollingTask.cpp +++ b/linux/devices/RwPollingTask.cpp @@ -220,7 +220,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf } pullCsLow(gpioId, gpioIF); bool lastByteWasFrameMarker = false; - Countdown cd(3000); + Countdown cd(2000); size_t readIdx = 0; while (true) { diff --git a/linux/devices/SusPolling.cpp b/linux/devices/SusPolling.cpp new file mode 100644 index 00000000..9ee0051a --- /dev/null +++ b/linux/devices/SusPolling.cpp @@ -0,0 +1,220 @@ +#include "SusPolling.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "mission/devices/devicedefinitions/susMax1227Helpers.h" + +using namespace returnvalue; + +SusPolling::SusPolling(object_id_t objectId, SpiComIF& spiComIF, GpioIF& gpioIF) + : SystemObject(objectId), spiComIF(spiComIF), gpioIF(gpioIF) { + semaphore = SemaphoreFactory::instance()->createBinarySemaphore(); + semaphore->acquire(); + ipcLock = MutexFactory::instance()->createMutex(); +} + +ReturnValue_t SusPolling::performOperation(uint8_t operationCode) { + while (true) { + ipcLock->lockMutex(); + state = InternalState::IDLE; + ipcLock->unlockMutex(); + semaphore->acquire(); + // Give SUS handlers a chance to submit all requests. + TaskFactory::delayTask(2); + { + // Takes 4-5 ms in debug mode. + // Stopwatch watch; + handleSusPolling(); + } + // Protection against tardy tasks unlocking the thread again immediately. + TaskFactory::delayTask(20); + } + return OK; +} + +ReturnValue_t SusPolling::initialize() { return OK; } + +ReturnValue_t SusPolling::initializeInterface(CookieIF* cookie) { + auto* spiCookie = dynamic_cast(cookie); + if (spiCookie == nullptr) { + return FAILED; + } + int susIdx = addressToIndex(spiCookie->getSpiAddress()); + if (susIdx < 0) { + return FAILED; + } + susDevs[susIdx].cookie = spiCookie; + return spiComIF.initializeInterface(cookie); +} + +ReturnValue_t SusPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { + auto* spiCookie = dynamic_cast(cookie); + if (spiCookie == nullptr) { + return FAILED; + } + int susIdx = addressToIndex(spiCookie->getSpiAddress()); + if (susIdx < 0) { + return FAILED; + } + if (sendLen != sizeof(acs::SusRequest)) { + return FAILED; + } + const auto* susReq = reinterpret_cast(sendData); + MutexGuard mg(ipcLock); + if (susDevs[susIdx].mode != susReq->mode) { + if (susReq->mode == acs::SimpleSensorMode::NORMAL) { + susDevs[susIdx].performStartup = true; + } else { + susDevs[susIdx].ownReply.cfgWasSet = false; + susDevs[susIdx].ownReply.dataWasSet = false; + } + susDevs[susIdx].mode = susReq->mode; + } + if (state == InternalState::IDLE) { + state = InternalState::BUSY; + semaphore->release(); + } + return OK; +} + +ReturnValue_t SusPolling::getSendSuccess(CookieIF* cookie) { return OK; } + +ReturnValue_t SusPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { return OK; } + +ReturnValue_t SusPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) { + auto* spiCookie = dynamic_cast(cookie); + if (spiCookie == nullptr) { + return FAILED; + } + int susIdx = addressToIndex(spiCookie->getSpiAddress()); + if (susIdx < 0) { + return FAILED; + } + MutexGuard mg(ipcLock); + std::memcpy(&susDevs[susIdx].readerReply, &susDevs[susIdx].ownReply, sizeof(acs::SusReply)); + *buffer = reinterpret_cast(&susDevs[susIdx].readerReply); + *size = sizeof(acs::SusReply); + return OK; +} + +ReturnValue_t SusPolling::handleSusPolling() { + ReturnValue_t result; + acs::SimpleSensorMode modes[12]; + bool performStartups[12]{}; + bool cfgsWereSet[12]{}; + uint8_t idx = 0; + { + MutexGuard mg(ipcLock); + for (idx = 0; idx < 12; idx++) { + modes[idx] = susDevs[idx].mode; + performStartups[idx] = susDevs[idx].performStartup; + } + } + for (idx = 0; idx < 12; idx++) { + if (modes[idx] == acs::SimpleSensorMode::NORMAL) { + if (performStartups[idx]) { + // Startup handling. + cmdBuf[0] = susMax1227::SETUP_INT_CLOKED; + result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(), 1); + if (result != OK) { + susDevs[idx].replyResult = result; + continue; + } + MutexGuard mg(ipcLock); + susDevs[idx].ownReply.cfgWasSet = true; + cfgsWereSet[idx] = true; + susDevs[idx].performStartup = true; + } + } + } + for (idx = 0; idx < 12; idx++) { + if (modes[idx] == acs::SimpleSensorMode::NORMAL and cfgsWereSet[idx]) { + // Regular sensor polling. + cmdBuf[0] = max1227::buildResetByte(true); + cmdBuf[1] = susMax1227::CONVERSION; + result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(), 2); + if (result != OK) { + susDevs[idx].replyResult = result; + continue; + } + } + } + + // Internal conversion time is 3.5 us + usleep(4); + + for (idx = 0; idx < 12; idx++) { + if (modes[idx] == acs::SimpleSensorMode::NORMAL and cfgsWereSet[idx]) { + std::memset(cmdBuf.data(), 0, susMax1227::SIZE_READ_INT_CONVERSIONS); + result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(), + susMax1227::SIZE_READ_INT_CONVERSIONS); + if (result != OK) { + susDevs[idx].replyResult = result; + continue; + } + result = spiComIF.readReceivedMessage(susDevs[idx].cookie, &rawReply, &dummy); + if (result != OK) { + susDevs[idx].replyResult = result; + continue; + } + MutexGuard mg(ipcLock); + susDevs[idx].ownReply.tempRaw = ((rawReply[0] & 0x0f) << 8) | rawReply[1]; + for (unsigned chIdx = 0; chIdx < 6; chIdx++) { + susDevs[idx].ownReply.channelsRaw[chIdx] = + (rawReply[chIdx * 2 + 2] << 8) | rawReply[chIdx * 2 + 3]; + } + susDevs[idx].ownReply.dataWasSet = true; + } + } + return OK; +} + +int SusPolling::addressToIndex(address_t addr) { + switch (addr) { + case (addresses::SUS_0): + return 0; + break; + case (addresses::SUS_1): + return 1; + break; + case (addresses::SUS_2): + return 2; + break; + case (addresses::SUS_3): + return 3; + break; + case (addresses::SUS_4): + return 4; + break; + case (addresses::SUS_5): + return 5; + break; + case (addresses::SUS_6): + return 6; + break; + case (addresses::SUS_7): + return 7; + break; + case (addresses::SUS_8): + return 8; + break; + case (addresses::SUS_9): + return 9; + break; + case (addresses::SUS_10): + return 10; + break; + case (addresses::SUS_11): + return 11; + break; + default: { + return -1; + } + } +} diff --git a/linux/devices/SusPolling.h b/linux/devices/SusPolling.h new file mode 100644 index 00000000..3afb1d9d --- /dev/null +++ b/linux/devices/SusPolling.h @@ -0,0 +1,52 @@ +#ifndef LINUX_DEVICES_SUSPOLLING_H_ +#define LINUX_DEVICES_SUSPOLLING_H_ + +#include +#include +#include +#include +#include + +#include "devices/addresses.h" +#include "mission/devices/devicedefinitions/acsPolling.h" + +class SusPolling : public SystemObject, public ExecutableObjectIF, public DeviceCommunicationIF { + public: + SusPolling(object_id_t objectId, SpiComIF& spiComIF, GpioIF& gpioIF); + + ReturnValue_t performOperation(uint8_t operationCode) override; + ReturnValue_t initialize() override; + + private: + enum class InternalState { IDLE, BUSY } state = InternalState::IDLE; + + struct SusDev { + SpiCookie* cookie = nullptr; + bool performStartup = false; + acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; + ReturnValue_t replyResult = returnvalue::OK; + acs::SusReply ownReply{}; + acs::SusReply readerReply{}; + }; + + MutexIF* ipcLock; + SemaphoreIF* semaphore; + uint8_t* rawReply = nullptr; + size_t dummy = 0; + SpiComIF& spiComIF; + GpioIF& gpioIF; + + std::array susDevs; + std::array cmdBuf; + + ReturnValue_t initializeInterface(CookieIF* cookie) override; + ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override; + ReturnValue_t getSendSuccess(CookieIF* cookie) override; + ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override; + ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override; + + ReturnValue_t handleSusPolling(); + static int addressToIndex(address_t addr); +}; + +#endif /* LINUX_DEVICES_SUSPOLLING_H_ */ diff --git a/linux/devices/startracker/ArcsecJsonParamBase.cpp b/linux/devices/startracker/ArcsecJsonParamBase.cpp index 05864a3f..a63025f1 100644 --- a/linux/devices/startracker/ArcsecJsonParamBase.cpp +++ b/linux/devices/startracker/ArcsecJsonParamBase.cpp @@ -5,14 +5,14 @@ ArcsecJsonParamBase::ArcsecJsonParamBase(std::string setName) : setName(setName) {} ReturnValue_t ArcsecJsonParamBase::create(std::string fullname, uint8_t* buffer) { - ReturnValue_t result = returnvalue::OK; - result = init(fullname); - if (result != returnvalue::OK) { - sif::warning << "ArcsecJsonParamBase::create: Failed to init parameter command for set " - << setName << std::endl; - return result; - } - result = createCommand(buffer); + // ReturnValue_t result = returnvalue::OK; + // result = init(fullname); + // if (result != returnvalue::OK) { + // sif::warning << "ArcsecJsonParamBase::create: Failed to init parameter command for set " + // << setName << std::endl; + // return result; + // } + ReturnValue_t result = createCommand(buffer); if (result != returnvalue::OK) { sif::warning << "ArcsecJsonParamBase::create: Failed to create parameter command for set " << setName << std::endl; @@ -74,12 +74,17 @@ ReturnValue_t ArcsecJsonParamBase::init(const std::string filename) { << std::endl; return JSON_FILE_NOT_EXISTS; } - createJsonObject(filename); - result = initSet(); - if (result != returnvalue::OK) { - return result; + try { + createJsonObject(filename); + result = initSet(); + if (result != returnvalue::OK) { + return result; + } + return returnvalue::OK; + } catch (json::exception& e) { + // TODO: Re-create json file from backup here. + return returnvalue::FAILED; } - return returnvalue::OK; } void ArcsecJsonParamBase::createJsonObject(const std::string fullname) { diff --git a/linux/devices/startracker/ArcsecJsonParamBase.h b/linux/devices/startracker/ArcsecJsonParamBase.h index a7a4f421..49d0dbba 100644 --- a/linux/devices/startracker/ArcsecJsonParamBase.h +++ b/linux/devices/startracker/ArcsecJsonParamBase.h @@ -41,6 +41,17 @@ class ArcsecJsonParamBase { */ ArcsecJsonParamBase(std::string setName); + /** + * @brief Initializes the properties json object and the set json object + * + * @param fullname Name including absolute path to json file + * @param setName The name of the set to work on + * + * @param return JSON_FILE_NOT_EXISTS if specified file does not exist, otherwise + * returnvalue::OK + */ + ReturnValue_t init(const std::string filename); + /** * @brief Fills a buffer with a parameter set * @@ -124,17 +135,6 @@ class ArcsecJsonParamBase { */ virtual ReturnValue_t createCommand(uint8_t* buffer) = 0; - /** - * @brief Initializes the properties json object and the set json object - * - * @param fullname Name including absolute path to json file - * @param setName The name of the set to work on - * - * @param return JSON_FILE_NOT_EXISTS if specified file does not exist, otherwise - * returnvalue::OK - */ - ReturnValue_t init(const std::string filename); - void createJsonObject(const std::string fullname); /** diff --git a/linux/devices/startracker/StarTrackerHandler.cpp b/linux/devices/startracker/StarTrackerHandler.cpp index a868a22b..0edf6de1 100644 --- a/linux/devices/startracker/StarTrackerHandler.cpp +++ b/linux/devices/startracker/StarTrackerHandler.cpp @@ -1,8 +1,11 @@ #include "StarTrackerHandler.h" #include +#include +#include #include +#include #include "OBSWConfig.h" #include "StarTrackerJsonCommands.h" @@ -14,8 +17,11 @@ extern "C" { #include "common/misc.h" } +std::atomic_bool JCFG_DONE(false); + StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, - StrHelper* strHelper, power::Switch_t powerSwitch) + const char* jsonFileStr, StrHelper* strHelper, + power::Switch_t powerSwitch) : DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), versionSet(this), @@ -40,6 +46,7 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, logSubscriptionSet(this), debugCameraSet(this), strHelper(strHelper), + paramJsonFile(jsonFileStr), powerSwitch(powerSwitch) { if (comCookie == nullptr) { sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl; @@ -59,6 +66,11 @@ ReturnValue_t StarTrackerHandler::initialize() { return result; } + // Spin up a thread to do the JSON initialization, takes 200-250 ms which would + // delay whole satellite boot process. + jcfgCountdown.resetTimer(); + jsonCfgTask = std::thread{setUpJsonCfgs, std::ref(jcfgs), paramJsonFile.c_str()}; + EventManagerIF* manager = ObjectManager::instance()->get(objects::EVENT_MANAGER); if (manager == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -240,8 +252,19 @@ void StarTrackerHandler::doStartUp() { // the device handler's submode to the star tracker's mode return; case StartupState::DONE: + if (jcfgCountdown.isBusy()) { + startupState = StartupState::WAIT_JCFG; + return; + } startupState = StartupState::IDLE; break; + case StartupState::WAIT_JCFG: { + if (jcfgCountdown.hasTimedOut()) { + startupState = StartupState::IDLE; + break; + } + return; + } default: return; } @@ -419,8 +442,7 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi return returnvalue::OK; } case (startracker::SUBSCRIPTION): { - Subscription subscription; - result = prepareParamCommand(commandData, commandDataLen, subscription); + result = prepareParamCommand(commandData, commandDataLen, jcfgs.subscription); return returnvalue::OK; } case (startracker::REQ_SOLUTION): { @@ -436,68 +458,55 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi return returnvalue::OK; } case (startracker::LIMITS): { - Limits limits; - result = prepareParamCommand(commandData, commandDataLen, limits); + result = prepareParamCommand(commandData, commandDataLen, jcfgs.limits); return result; } case (startracker::MOUNTING): { - Mounting mounting; - result = prepareParamCommand(commandData, commandDataLen, mounting); + result = prepareParamCommand(commandData, commandDataLen, jcfgs.mounting); return result; } case (startracker::IMAGE_PROCESSOR): { - ImageProcessor imageProcessor; - result = prepareParamCommand(commandData, commandDataLen, imageProcessor); + result = prepareParamCommand(commandData, commandDataLen, jcfgs.imageProcessor); return result; } case (startracker::CAMERA): { - Camera camera; - result = prepareParamCommand(commandData, commandDataLen, camera); + result = prepareParamCommand(commandData, commandDataLen, jcfgs.camera); return result; } case (startracker::CENTROIDING): { - Centroiding centroiding; - result = prepareParamCommand(commandData, commandDataLen, centroiding); + result = prepareParamCommand(commandData, commandDataLen, jcfgs.centroiding); return result; } case (startracker::LISA): { - Lisa lisa; - result = prepareParamCommand(commandData, commandDataLen, lisa); + result = prepareParamCommand(commandData, commandDataLen, jcfgs.lisa); return result; } case (startracker::MATCHING): { - Matching matching; - result = prepareParamCommand(commandData, commandDataLen, matching); + result = prepareParamCommand(commandData, commandDataLen, jcfgs.matching); return result; } case (startracker::VALIDATION): { - Validation validation; - result = prepareParamCommand(commandData, commandDataLen, validation); + result = prepareParamCommand(commandData, commandDataLen, jcfgs.validation); return result; } case (startracker::ALGO): { - Algo algo; - result = prepareParamCommand(commandData, commandDataLen, algo); + result = prepareParamCommand(commandData, commandDataLen, jcfgs.algo); return result; } case (startracker::TRACKING): { - Tracking tracking; - result = prepareParamCommand(commandData, commandDataLen, tracking); + result = prepareParamCommand(commandData, commandDataLen, jcfgs.tracking); return result; } case (startracker::LOGLEVEL): { - LogLevel logLevel; - result = prepareParamCommand(commandData, commandDataLen, logLevel); + result = prepareParamCommand(commandData, commandDataLen, jcfgs.logLevel); return result; } case (startracker::LOGSUBSCRIPTION): { - LogSubscription logSubscription; - result = prepareParamCommand(commandData, commandDataLen, logSubscription); + result = prepareParamCommand(commandData, commandDataLen, jcfgs.logSubscription); return result; } case (startracker::DEBUG_CAMERA): { - DebugCamera debugCamera; - result = prepareParamCommand(commandData, commandDataLen, debugCamera); + result = prepareParamCommand(commandData, commandDataLen, jcfgs.debugCamera); return result; } case (startracker::CHECKSUM): { @@ -746,6 +755,24 @@ void StarTrackerHandler::bootFirmware(Mode_t toMode) { } } +void StarTrackerHandler::setUpJsonCfgs(JsonConfigs& cfgs, const char* paramJsonFile) { + cfgs.tracking.init(paramJsonFile); + cfgs.logLevel.init(paramJsonFile); + cfgs.logSubscription.init(paramJsonFile); + cfgs.debugCamera.init(paramJsonFile); + cfgs.algo.init(paramJsonFile); + cfgs.validation.init(paramJsonFile); + cfgs.matching.init(paramJsonFile); + cfgs.lisa.init(paramJsonFile); + cfgs.centroiding.init(paramJsonFile); + cfgs.camera.init(paramJsonFile); + cfgs.imageProcessor.init(paramJsonFile); + cfgs.mounting.init(paramJsonFile); + cfgs.limits.init(paramJsonFile); + cfgs.subscription.init(paramJsonFile); + JCFG_DONE = true; +} + void StarTrackerHandler::bootBootloader() { if (internalState == InternalState::IDLE) { internalState = InternalState::BOOT_BOOTLOADER; @@ -1650,6 +1677,7 @@ void StarTrackerHandler::prepareHistogramRequest() { ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData, size_t commandDataLen, ArcsecJsonParamBase& paramSet) { + // Stopwatch watch; ReturnValue_t result = returnvalue::OK; if (commandDataLen > MAX_PATH_SIZE) { return FILE_PATH_TOO_LONG; diff --git a/linux/devices/startracker/StarTrackerHandler.h b/linux/devices/startracker/StarTrackerHandler.h index 3ed81047..11cf7fc3 100644 --- a/linux/devices/startracker/StarTrackerHandler.h +++ b/linux/devices/startracker/StarTrackerHandler.h @@ -2,6 +2,9 @@ #define MISSION_DEVICES_STARTRACKERHANDLER_H_ #include +#include + +#include #include "ArcsecDatalinkLayer.h" #include "ArcsecJsonParamBase.h" @@ -35,7 +38,7 @@ class StarTrackerHandler : public DeviceHandlerBase { * to high to enable the device. */ StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, - StrHelper* strHelper, power::Switch_t powerSwitch); + const char* jsonFileStr, StrHelper* strHelper, power::Switch_t powerSwitch); virtual ~StarTrackerHandler(); ReturnValue_t initialize() override; @@ -216,15 +219,29 @@ class StarTrackerHandler : public DeviceHandlerBase { // Loading firmware requires some time and the command will not trigger a reply when executed Countdown bootCountdown; -#ifdef EGSE - std::string paramJsonFile = "/home/pi/arcsec/json/flight-config.json"; -#else -#if OBSW_STAR_TRACKER_GROUND_CONFIG == 1 - std::string paramJsonFile = "/mnt/sd0/startracker/ground-config.json"; -#else - std::string paramJsonFile = "/mnt/sd0/startracker/flight-config.json"; -#endif -#endif + struct JsonConfigs { + Tracking tracking; + LogLevel logLevel; + LogSubscription logSubscription; + DebugCamera debugCamera; + Algo algo; + Validation validation; + Matching matching; + Lisa lisa; + Centroiding centroiding; + Camera camera; + ImageProcessor imageProcessor; + Mounting mounting; + Limits limits; + Subscription subscription; + }; + JsonConfigs jcfgs; + Countdown jcfgCountdown = Countdown(250); + bool commandExecuted = false; + std::thread jsonCfgTask; + static void setUpJsonCfgs(JsonConfigs& cfgs, const char* paramJsonFile); + + std::string paramJsonFile; enum class NormalState { TEMPERATURE_REQUEST, SOLUTION_REQUEST }; @@ -262,7 +279,14 @@ class StarTrackerHandler : public DeviceHandlerBase { InternalState internalState = InternalState::IDLE; - enum class StartupState { IDLE, CHECK_PROGRAM, WAIT_CHECK_PROGRAM, BOOT_BOOTLOADER, DONE }; + enum class StartupState { + IDLE, + CHECK_PROGRAM, + WAIT_CHECK_PROGRAM, + BOOT_BOOTLOADER, + WAIT_JCFG, + DONE + }; StartupState startupState = StartupState::IDLE; diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index c979ff5e..94d829fe 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 263 translations. + * @brief Auto-generated event translation file. Contains 277 translations. * @details - * Generated on: 2023-02-24 16:57:00 + * Generated on: 2023-03-11 15:01:05 */ #include "translateEvents.h" @@ -158,6 +158,8 @@ const char *LOST_BIT_LOCK_PDEC_STRING = "LOST_BIT_LOCK_PDEC"; const char *TOO_MANY_IRQS_STRING = "TOO_MANY_IRQS"; const char *POLL_SYSCALL_ERROR_PDEC_STRING = "POLL_SYSCALL_ERROR_PDEC"; const char *WRITE_SYSCALL_ERROR_PDEC_STRING = "WRITE_SYSCALL_ERROR_PDEC"; +const char *PDEC_RESET_FAILED_STRING = "PDEC_RESET_FAILED"; +const char *OPEN_IRQ_FILE_FAILED_STRING = "OPEN_IRQ_FILE_FAILED"; 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"; @@ -255,7 +257,8 @@ const char *REBOOT_HW_STRING = "REBOOT_HW"; const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE"; const char *VERSION_INFO_STRING = "VERSION_INFO"; const char *CURRENT_IMAGE_INFO_STRING = "CURRENT_IMAGE_INFO"; -const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; +const char *REBOOT_COUNTER_STRING = "REBOOT_COUNTER"; +const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS"; const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE"; const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE"; const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING"; @@ -263,6 +266,17 @@ const char *PLOC_OVERHEATING_STRING = "PLOC_OVERHEATING"; const char *OBC_OVERHEATING_STRING = "OBC_OVERHEATING"; const char *HPA_OVERHEATING_STRING = "HPA_OVERHEATING"; const char *PLPCDU_OVERHEATING_STRING = "PLPCDU_OVERHEATING"; +const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED"; +const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON"; +const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION"; +const char *FILE_TOO_LARGE_STRING = "FILE_TOO_LARGE"; +const char *BUSY_DUMPING_EVENT_STRING = "BUSY_DUMPING_EVENT"; +const char *DUMP_WAS_CANCELLED_STRING = "DUMP_WAS_CANCELLED"; +const char *DUMP_OK_STORE_DONE_STRING = "DUMP_OK_STORE_DONE"; +const char *DUMP_NOK_STORE_DONE_STRING = "DUMP_NOK_STORE_DONE"; +const char *DUMP_MISC_STORE_DONE_STRING = "DUMP_MISC_STORE_DONE"; +const char *DUMP_HK_STORE_DONE_STRING = "DUMP_HK_STORE_DONE"; +const char *DUMP_CFDP_STORE_DONE_STRING = "DUMP_CFDP_STORE_DONE"; const char *translateEvents(Event event) { switch ((event & 0xFFFF)) { @@ -572,6 +586,10 @@ const char *translateEvents(Event event) { return POLL_SYSCALL_ERROR_PDEC_STRING; case (12409): return WRITE_SYSCALL_ERROR_PDEC_STRING; + case (12410): + return PDEC_RESET_FAILED_STRING; + case (12411): + return OPEN_IRQ_FILE_FAILED_STRING; case (12500): return IMAGE_UPLOAD_FAILED_STRING; case (12501): @@ -766,22 +784,46 @@ const char *translateEvents(Event event) { return VERSION_INFO_STRING; case (14006): return CURRENT_IMAGE_INFO_STRING; + case (14007): + return REBOOT_COUNTER_STRING; + case (14008): + return INDIVIDUAL_BOOT_COUNTS_STRING; case (14100): - return POSSIBLE_FILE_CORRUPTION_STRING; - case (14200): return NO_VALID_SENSOR_TEMPERATURE_STRING; - case (14201): + case (14101): return NO_HEALTHY_HEATER_AVAILABLE_STRING; - case (14202): + case (14102): return SYRLINKS_OVERHEATING_STRING; - case (14203): + case (14103): return PLOC_OVERHEATING_STRING; - case (14204): + case (14104): return OBC_OVERHEATING_STRING; - case (14205): + case (14105): return HPA_OVERHEATING_STRING; - case (14206): + case (14106): return PLPCDU_OVERHEATING_STRING; + case (14201): + return TX_TIMER_EXPIRED_STRING; + case (14202): + return BIT_LOCK_TX_ON_STRING; + case (14300): + return POSSIBLE_FILE_CORRUPTION_STRING; + case (14301): + return FILE_TOO_LARGE_STRING; + case (14302): + return BUSY_DUMPING_EVENT_STRING; + case (14303): + return DUMP_WAS_CANCELLED_STRING; + case (14305): + return DUMP_OK_STORE_DONE_STRING; + case (14306): + return DUMP_NOK_STORE_DONE_STRING; + case (14307): + return DUMP_MISC_STORE_DONE_STRING; + case (14308): + return DUMP_HK_STORE_DONE_STRING; + case (14309): + return DUMP_CFDP_STORE_DONE_STRING; default: return "UNKNOWN_EVENT"; } diff --git a/linux/fsfwconfig/objects/systemObjectList.h b/linux/fsfwconfig/objects/systemObjectList.h index 6cb70a06..26124b76 100644 --- a/linux/fsfwconfig/objects/systemObjectList.h +++ b/linux/fsfwconfig/objects/systemObjectList.h @@ -47,7 +47,6 @@ enum sourceObjects : uint32_t { GPIO_IF = 0x49010005, /* Custom device handler */ - RW_POLLING_TASK = 0x49020005, /* 0x54 ('T') for test handlers */ TEST_TASK = 0x54694269, diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index 93c4fb0f..212322bc 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 159 translations. - * Generated on: 2023-02-24 16:57:00 + * Contains 173 translations. + * Generated on: 2023-03-11 15:01:05 */ #include "translateObjects.h" @@ -53,6 +53,10 @@ const char *STR_HELPER_STRING = "STR_HELPER"; const char *PLOC_MPSOC_HELPER_STRING = "PLOC_MPSOC_HELPER"; const char *AXI_PTME_CONFIG_STRING = "AXI_PTME_CONFIG"; const char *PTME_CONFIG_STRING = "PTME_CONFIG"; +const char *PTME_VC0_LIVE_TM_STRING = "PTME_VC0_LIVE_TM"; +const char *PTME_VC1_LOG_TM_STRING = "PTME_VC1_LOG_TM"; +const char *PTME_VC2_HK_TM_STRING = "PTME_VC2_HK_TM"; +const char *PTME_VC3_CFDP_TM_STRING = "PTME_VC3_CFDP_TM"; const char *PLOC_MPSOC_HANDLER_STRING = "PLOC_MPSOC_HANDLER"; const char *PLOC_SUPERVISOR_HANDLER_STRING = "PLOC_SUPERVISOR_HANDLER"; const char *PLOC_SUPERVISOR_HELPER_STRING = "PLOC_SUPERVISOR_HELPER"; @@ -85,11 +89,13 @@ const char *ARDUINO_COM_IF_STRING = "ARDUINO_COM_IF"; const char *GPIO_IF_STRING = "GPIO_IF"; const char *SCEX_UART_READER_STRING = "SCEX_UART_READER"; const char *SPI_MAIN_COM_IF_STRING = "SPI_MAIN_COM_IF"; -const char *RW_POLLING_TASK_STRING = "RW_POLLING_TASK"; -const char *SPI_RTD_COM_IF_STRING = "SPI_RTD_COM_IF"; const char *UART_COM_IF_STRING = "UART_COM_IF"; const char *I2C_COM_IF_STRING = "I2C_COM_IF"; const char *CSP_COM_IF_STRING = "CSP_COM_IF"; +const char *ACS_BOARD_POLLING_TASK_STRING = "ACS_BOARD_POLLING_TASK"; +const char *RW_POLLING_TASK_STRING = "RW_POLLING_TASK"; +const char *SPI_RTD_COM_IF_STRING = "SPI_RTD_COM_IF"; +const char *SUS_POLLING_TASK_STRING = "SUS_POLLING_TASK"; const char *CCSDS_PACKET_DISTRIBUTOR_STRING = "CCSDS_PACKET_DISTRIBUTOR"; const char *PUS_PACKET_DISTRIBUTOR_STRING = "PUS_PACKET_DISTRIBUTOR"; const char *TCP_TMTC_SERVER_STRING = "TCP_TMTC_SERVER"; @@ -141,12 +147,15 @@ const char *HEATER_3_OBC_BRD_STRING = "HEATER_3_OBC_BRD"; const char *HEATER_4_CAMERA_STRING = "HEATER_4_CAMERA"; const char *HEATER_5_STR_STRING = "HEATER_5_STR"; const char *HEATER_6_DRO_STRING = "HEATER_6_DRO"; -const char *HEATER_7_HPA_STRING = "HEATER_7_HPA"; +const char *HEATER_7_SYRLINKS_STRING = "HEATER_7_SYRLINKS"; 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 *RW_ASSY_STRING = "RW_ASSY"; const char *CAM_SWITCHER_STRING = "CAM_SWITCHER"; +const char *SYRLINKS_ASSY_STRING = "SYRLINKS_ASSY"; +const char *IMTQ_ASSY_STRING = "IMTQ_ASSY"; +const char *STR_ASSY_STRING = "STR_ASSY"; 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"; @@ -162,6 +171,11 @@ const char *OK_TM_STORE_STRING = "OK_TM_STORE"; const char *NOT_OK_TM_STORE_STRING = "NOT_OK_TM_STORE"; const char *HK_TM_STORE_STRING = "HK_TM_STORE"; const char *CFDP_TM_STORE_STRING = "CFDP_TM_STORE"; +const char *LIVE_TM_TASK_STRING = "LIVE_TM_TASK"; +const char *LOG_STORE_AND_TM_TASK_STRING = "LOG_STORE_AND_TM_TASK"; +const char *HK_STORE_AND_TM_TASK_STRING = "HK_STORE_AND_TM_TASK"; +const char *CFDP_STORE_AND_TM_TASK_STRING = "CFDP_STORE_AND_TM_TASK"; +const char *DOWNLINK_RAM_STORE_STRING = "DOWNLINK_RAM_STORE"; const char *CCSDS_IP_CORE_BRIDGE_STRING = "CCSDS_IP_CORE_BRIDGE"; const char *THERMAL_TEMP_INSERTER_STRING = "THERMAL_TEMP_INSERTER"; const char *NO_OBJECT_STRING = "NO_OBJECT"; @@ -262,6 +276,14 @@ const char *translateObject(object_id_t object) { return AXI_PTME_CONFIG_STRING; case 0x44330005: return PTME_CONFIG_STRING; + case 0x44330006: + return PTME_VC0_LIVE_TM_STRING; + case 0x44330007: + return PTME_VC1_LOG_TM_STRING; + case 0x44330008: + return PTME_VC2_HK_TM_STRING; + case 0x44330009: + return PTME_VC3_CFDP_TM_STRING; case 0x44330015: return PLOC_MPSOC_HANDLER_STRING; case 0x44330016: @@ -326,16 +348,20 @@ const char *translateObject(object_id_t object) { return SCEX_UART_READER_STRING; case 0x49020004: return SPI_MAIN_COM_IF_STRING; - case 0x49020005: - return RW_POLLING_TASK_STRING; - case 0x49020006: - return SPI_RTD_COM_IF_STRING; case 0x49030003: return UART_COM_IF_STRING; case 0x49040002: return I2C_COM_IF_STRING; case 0x49050001: return CSP_COM_IF_STRING; + case 0x49060004: + return ACS_BOARD_POLLING_TASK_STRING; + case 0x49060005: + return RW_POLLING_TASK_STRING; + case 0x49060006: + return SPI_RTD_COM_IF_STRING; + case 0x49060007: + return SUS_POLLING_TASK_STRING; case 0x50000100: return CCSDS_PACKET_DISTRIBUTOR_STRING; case 0x50000200: @@ -439,7 +465,7 @@ const char *translateObject(object_id_t object) { case 0x60000006: return HEATER_6_DRO_STRING; case 0x60000007: - return HEATER_7_HPA_STRING; + return HEATER_7_SYRLINKS_STRING; case 0x73000001: return ACS_BOARD_ASS_STRING; case 0x73000002: @@ -447,9 +473,15 @@ const char *translateObject(object_id_t object) { case 0x73000003: return TCS_BOARD_ASS_STRING; case 0x73000004: - return RW_ASS_STRING; + return RW_ASSY_STRING; case 0x73000006: return CAM_SWITCHER_STRING; + case 0x73000007: + return SYRLINKS_ASSY_STRING; + case 0x73000008: + return IMTQ_ASSY_STRING; + case 0x73000009: + return STR_ASSY_STRING; case 0x73000100: return TM_FUNNEL_STRING; case 0x73000101: @@ -480,6 +512,16 @@ const char *translateObject(object_id_t object) { return HK_TM_STORE_STRING; case 0x73030000: return CFDP_TM_STORE_STRING; + case 0x73040000: + return LIVE_TM_TASK_STRING; + case 0x73040001: + return LOG_STORE_AND_TM_TASK_STRING; + case 0x73040002: + return HK_STORE_AND_TM_TASK_STRING; + case 0x73040003: + return CFDP_STORE_AND_TM_TASK_STRING; + case 0x73040004: + return DOWNLINK_RAM_STORE_STRING; case 0x73500000: return CCSDS_IP_CORE_BRIDGE_STRING; case 0x90000003: diff --git a/linux/fsfwconfig/returnvalues/classIds.h b/linux/fsfwconfig/returnvalues/classIds.h index 0a71a8b6..f4db7ffa 100644 --- a/linux/fsfwconfig/returnvalues/classIds.h +++ b/linux/fsfwconfig/returnvalues/classIds.h @@ -13,8 +13,8 @@ namespace CLASS_ID { enum { CLASS_ID_START = COMMON_CLASS_ID_END, - SCRATCH_BUFFER, // SCBU - CLASS_ID_END // [EXPORT] : [END] + SCRATCH_BUFFER, // SCBU + CLASS_ID_END // [EXPORT] : [END] }; } diff --git a/linux/ipcore/PapbVcInterface.cpp b/linux/ipcore/PapbVcInterface.cpp index b8b12c7a..51afc15d 100644 --- a/linux/ipcore/PapbVcInterface.cpp +++ b/linux/ipcore/PapbVcInterface.cpp @@ -41,7 +41,7 @@ void PapbVcInterface::startPacketTransfer() { *vcBaseReg = CONFIG_START; } void PapbVcInterface::endPacketTransfer() { *vcBaseReg = CONFIG_END; } -ReturnValue_t PapbVcInterface::pollPapbBusySignal() { +ReturnValue_t PapbVcInterface::pollPapbBusySignal() const { gpio::Levels papbBusyState = gpio::Levels::LOW; ReturnValue_t result = returnvalue::OK; @@ -53,7 +53,6 @@ ReturnValue_t PapbVcInterface::pollPapbBusySignal() { return returnvalue::FAILED; } if (papbBusyState == gpio::Levels::LOW) { - sif::warning << "PapbVcInterface::pollPapbBusySignal: PAPB busy" << std::endl; return PAPB_BUSY; } @@ -80,6 +79,8 @@ void PapbVcInterface::isVcInterfaceBufferEmpty() { return; } +bool PapbVcInterface::isBusy() const { return pollPapbBusySignal() == PAPB_BUSY; } + ReturnValue_t PapbVcInterface::sendTestFrame() { /** Size of one complete transfer frame data field amounts to 1105 bytes */ uint8_t testPacket[1105]; diff --git a/linux/ipcore/PapbVcInterface.h b/linux/ipcore/PapbVcInterface.h index 83081d9d..d4694a62 100644 --- a/linux/ipcore/PapbVcInterface.h +++ b/linux/ipcore/PapbVcInterface.h @@ -3,10 +3,10 @@ #include #include +#include #include "OBSWConfig.h" #include "fsfw/returnvalues/returnvalue.h" -#include "linux/ipcore/VcInterfaceIF.h" /** * @brief This class handles the transmission of data to a virtual channel of the PTME IP Core @@ -14,7 +14,7 @@ * * @author J. Meier */ -class PapbVcInterface : public VcInterfaceIF { +class PapbVcInterface : public VirtualChannelIF { public: /** * @brief Constructor @@ -32,6 +32,13 @@ class PapbVcInterface : public VcInterfaceIF { std::string uioFile, int mapNum); virtual ~PapbVcInterface(); + bool isBusy() const override; + /** + * + * @param data + * @param size + * @return returnvalue::OK on successfull write, PAPB_BUSY if PAPB is busy. + */ ReturnValue_t write(const uint8_t* data, size_t size) override; ReturnValue_t initialize() override; @@ -95,7 +102,7 @@ class PapbVcInterface : public VcInterfaceIF { * * @return returnvalue::OK when ready to receive data else PAPB_BUSY. */ - ReturnValue_t pollPapbBusySignal(); + ReturnValue_t pollPapbBusySignal() const; /** * @brief This function can be used for debugging to check whether there are packets in diff --git a/linux/ipcore/PdecHandler.cpp b/linux/ipcore/PdecHandler.cpp index 80327c94..d5e5c0c8 100644 --- a/linux/ipcore/PdecHandler.cpp +++ b/linux/ipcore/PdecHandler.cpp @@ -164,8 +164,8 @@ ReturnValue_t PdecHandler::polledOperation() { // See https://yurovsky.github.io/2014/10/10/linux-uio-gpio-interrupt.html for more information. ReturnValue_t PdecHandler::irqOperation() { - ReturnValue_t result = returnvalue::OK; - int fd = -1; + ReturnValue_t result = returnvalue::OK; + int fd = -1; // Used to unmask IRQ uint32_t info = 1; @@ -183,7 +183,7 @@ ReturnValue_t PdecHandler::irqOperation() { case State::INIT: { result = handleInitState(); if (result == returnvalue::OK) { - openIrqFile(&fd); + openIrqFile(&fd); } break; } @@ -196,7 +196,7 @@ ReturnValue_t PdecHandler::irqOperation() { break; } case State::RUNNING: { - checkLocks(); + checkLocks(); checkAndHandleIrqs(fd, info); break; } @@ -238,13 +238,13 @@ ReturnValue_t PdecHandler::handleInitState() { } void PdecHandler::openIrqFile(int* fd) { - *fd = open(uioNames.irq, O_RDWR); - if (*fd < 0) { - sif::error << "PdecHandler::irqOperation: Opening UIO IRQ file" << uioNames.irq << " failed" - << std::endl; - triggerEvent(OPEN_IRQ_FILE_FAILED); - state = State::WAIT_FOR_RECOVERY; - } + *fd = open(uioNames.irq, O_RDWR); + if (*fd < 0) { + sif::error << "PdecHandler::irqOperation: Opening UIO IRQ file" << uioNames.irq << " failed" + << std::endl; + triggerEvent(OPEN_IRQ_FILE_FAILED); + state = State::WAIT_FOR_RECOVERY; + } } ReturnValue_t PdecHandler::checkAndHandleIrqs(int fd, uint32_t& info) { diff --git a/linux/ipcore/PdecHandler.h b/linux/ipcore/PdecHandler.h index e98e939c..2f0bcca2 100644 --- a/linux/ipcore/PdecHandler.h +++ b/linux/ipcore/PdecHandler.h @@ -105,8 +105,7 @@ class PdecHandler : public SystemObject, //! [EXPORT] : [COMMENT] Failed to pull PDEC reset to low static constexpr Event PDEC_RESET_FAILED = event::makeEvent(SUBSYSTEM_ID, 10, severity::HIGH); //! [EXPORT] : [COMMENT] Failed to open the IRQ uio file - static constexpr Event OPEN_IRQ_FILE_FAILED = - event::makeEvent(SUBSYSTEM_ID, 11, severity::HIGH); + static constexpr Event OPEN_IRQ_FILE_FAILED = event::makeEvent(SUBSYSTEM_ID, 11, severity::HIGH); private: static const uint8_t INTERFACE_ID = CLASS_ID::PDEC_HANDLER; diff --git a/linux/ipcore/Ptme.cpp b/linux/ipcore/Ptme.cpp index 50b1a37c..69d772ff 100644 --- a/linux/ipcore/Ptme.cpp +++ b/linux/ipcore/Ptme.cpp @@ -32,7 +32,7 @@ ReturnValue_t Ptme::writeToVc(uint8_t vcId, const uint8_t* data, size_t size) { return result; } -void Ptme::addVcInterface(VcId_t vcId, VcInterfaceIF* vc) { +void Ptme::addVcInterface(VcId_t vcId, VirtualChannelIF* vc) { if (vcId > config::NUMBER_OF_VIRTUAL_CHANNELS) { sif::warning << "Ptme::addVcInterface: Invalid virtual channel ID" << std::endl; return; @@ -51,3 +51,14 @@ void Ptme::addVcInterface(VcId_t vcId, VcInterfaceIF* vc) { return; } } + +bool Ptme::isBusy(uint8_t vcId) const { + const auto& vcInterfaceMapIter = vcInterfaceMap.find(vcId); + if (vcInterfaceMapIter == vcInterfaceMap.end()) { + sif::warning << "Ptme::writeToVc: No virtual channel interface found for the virtual " + "channel with id " + << static_cast(vcId) << std::endl; + return UNKNOWN_VC_ID; + } + return vcInterfaceMapIter->second->isBusy(); +} diff --git a/linux/ipcore/Ptme.h b/linux/ipcore/Ptme.h index 2de85a38..3c076085 100644 --- a/linux/ipcore/Ptme.h +++ b/linux/ipcore/Ptme.h @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -10,13 +11,15 @@ #include "OBSWConfig.h" #include "fsfw/returnvalues/returnvalue.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 - * encoding of telemetry packets according to the CCSDS standards CCSDS 131.0-B-3 (TM - * Synchro- nization and channel coding) and CCSDS 132.0-B-2 (TM Space Data Link Protocoll). The IP - * cores are implemented on the programmable logic and are accessible through the linux UIO driver. + * @brief This class handles the interfacing to the telemetry (PTME) IP core. + * + * @details + * This module is responsible for the encoding of telemetry packets according to the CCSDS + * standards CCSDS 131.0-B-3 (TM Synchronization and channel coding) and CCSDS 132.0-B-2 + * (TM Space Data Link Protocoll). The IP cores are implemented on the programmable logic and are + * accessible through the linux UIO driver. */ class Ptme : public PtmeIF, public SystemObject { public: @@ -32,12 +35,13 @@ class Ptme : public PtmeIF, public SystemObject { ReturnValue_t initialize() override; ReturnValue_t writeToVc(uint8_t vcId, const uint8_t* data, size_t size) override; + bool isBusy(uint8_t vcId) const override; /** * @brief This function adds the reference to a virtual channel interface to the vcInterface * map. */ - void addVcInterface(VcId_t vcId, VcInterfaceIF* vc); + void addVcInterface(VcId_t vcId, VirtualChannelIF* vc); private: static const uint8_t INTERFACE_ID = CLASS_ID::PTME; @@ -70,7 +74,7 @@ class Ptme : public PtmeIF, public SystemObject { uint32_t* ptmeBaseAddress = nullptr; - using VcInterfaceMap = std::unordered_map; + using VcInterfaceMap = std::unordered_map; using VcInterfaceMapIter = VcInterfaceMap::iterator; VcInterfaceMap vcInterfaceMap; diff --git a/linux/ipcore/PtmeIF.h b/linux/ipcore/PtmeIF.h index 44aa39f2..06b1cbe7 100644 --- a/linux/ipcore/PtmeIF.h +++ b/linux/ipcore/PtmeIF.h @@ -22,6 +22,7 @@ class PtmeIF { * @param size Number of bytes to write */ virtual ReturnValue_t writeToVc(uint8_t vcId, const uint8_t* data, size_t size) = 0; + virtual bool isBusy(uint8_t vcId) const = 0; }; #endif /* LINUX_OBC_PTMEIF_H_ */ diff --git a/linux/ipcore/VcInterfaceIF.h b/linux/ipcore/VirtualChannelIF.h similarity index 52% rename from linux/ipcore/VcInterfaceIF.h rename to linux/ipcore/VirtualChannelIF.h index de1bb61e..266a56c3 100644 --- a/linux/ipcore/VcInterfaceIF.h +++ b/linux/ipcore/VirtualChannelIF.h @@ -1,6 +1,7 @@ #ifndef LINUX_OBC_VCINTERFACEIF_H_ #define LINUX_OBC_VCINTERFACEIF_H_ +#include #include #include "fsfw/returnvalues/returnvalue.h" @@ -8,21 +9,13 @@ /** * @brief Interface class for managing different virtual channels of the PTME IP core implemented * in the programmable logic. - * + * @details + * Also implements @DirectTmSinkIF to allow wiriting to the VC directly. * @author J. Meier */ -class VcInterfaceIF { +class VirtualChannelIF : public DirectTmSinkIF { public: - virtual ~VcInterfaceIF(){}; - - /** - * @brief Implememts the functionality to write data in the virtual channel of the PTME IP - * Core. - * - * @param data Pointer to buffer holding the data to write - * @param size Number of bytes to write - */ - virtual ReturnValue_t write(const uint8_t* data, size_t size) = 0; + virtual ~VirtualChannelIF(){}; virtual ReturnValue_t initialize() = 0; }; diff --git a/mission/acsDefs.cpp b/mission/acsDefs.cpp index 00686b15..4027896c 100644 --- a/mission/acsDefs.cpp +++ b/mission/acsDefs.cpp @@ -11,10 +11,6 @@ const char* acs::getModeStr(AcsMode mode) { modeStr = "SAFE"; break; } - case (acs::AcsMode::DETUMBLE): { - modeStr = "DETUBMLE"; - break; - } case (acs::AcsMode::PTG_NADIR): { modeStr = "POITNING NADIR"; break; diff --git a/mission/acsDefs.h b/mission/acsDefs.h index 61a3c644..a724bbd7 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -10,14 +10,17 @@ namespace acs { enum AcsMode : Mode_t { OFF = HasModesIF::MODE_OFF, SAFE = 10, - DETUMBLE = 11, - PTG_IDLE = 12, - PTG_NADIR = 13, - PTG_TARGET = 14, - PTG_TARGET_GS = 15, - PTG_INERTIAL = 16, + PTG_IDLE = 11, + PTG_NADIR = 12, + PTG_TARGET = 13, + PTG_TARGET_GS = 14, + PTG_INERTIAL = 15, }; +enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 }; + +// static constexpr uint8_t ACS_SYSTEM_DETUMBLE_SUBMODE = 1; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; //!< The limits for the rotation in safe mode were violated. static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); diff --git a/mission/config/comCfg.cpp b/mission/config/comCfg.cpp index 9e350a9d..8387214d 100644 --- a/mission/config/comCfg.cpp +++ b/mission/config/comCfg.cpp @@ -9,12 +9,12 @@ MutexIF* DATARATE_LOCK = nullptr; MutexIF* lazyLock(); com::Datarate com::getCurrentDatarate() { - MutexGuard mg(lazyLock()); + MutexGuard mg(lazyLock(), MutexIF::TimeoutType::WAITING, 20, "com"); return DATARATE_CFG_RAW; } void com::setCurrentDatarate(com::Datarate newRate) { - MutexGuard mg(lazyLock()); + MutexGuard mg(lazyLock(), MutexIF::TimeoutType::WAITING, 20, "com"); DATARATE_CFG_RAW = newRate; } diff --git a/mission/config/torquer.h b/mission/config/torquer.h index 10a27991..bb5a101f 100644 --- a/mission/config/torquer.h +++ b/mission/config/torquer.h @@ -9,6 +9,9 @@ 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; +static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING; +static constexpr uint32_t LOCK_TIMEOUT = 20; +static constexpr char LOCK_CTX[] = "torquer"; MutexIF* lazyLock(); extern bool TORQUEING; diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index f3269285..b4e7c5d3 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -6,12 +6,8 @@ AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId), - sensorProcessing(&acsParameters), - navigation(&acsParameters), - actuatorCmd(&acsParameters), guidance(&acsParameters), safeCtrl(&acsParameters), - detumble(&acsParameters), ptgCtrl(&acsParameters), parameterHelper(this), mgmDataRaw(this), @@ -112,12 +108,16 @@ void AcsController::performControlOperation() { } case InternalState::READY: { if (mode != MODE_OFF) { - switch (submode) { + switch (mode) { case acs::SAFE: - performSafe(); - break; - case acs::DETUMBLE: - performDetumble(); + switch (submode) { + case SUBMODE_NONE: + performSafe(); + break; + case acs::DETUMBLE: + performDetumble(); + break; + } break; case acs::PTG_IDLE: case acs::PTG_TARGET: @@ -142,7 +142,7 @@ void AcsController::performSafe() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData); + &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { @@ -157,23 +157,28 @@ void AcsController::performSafe() { guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); // if MEKF is working double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; - bool magMomMtqValid = false; if (result == MultiplicativeKalmanFilter::MEKF_RUNNING) { - safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), - mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(), - susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), - mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(), - sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); + result = safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), + mgmDataProcessed.magIgrfModel.value, + mgmDataProcessed.magIgrfModel.isValid(), + susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), + mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(), + sunTargetDir, satRateSafe, &errAng, magMomMtq); } else { - safeCtrl.safeNoMekf( + result = safeCtrl.safeNoMekf( now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(), susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), - sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); + sunTargetDir, satRateSafe, &errAng, magMomMtq); + } + if (result == returnvalue::FAILED) { + // ToDo: this should never ever happen or we are dead. prob add an event at least } - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, + *acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipolMax); // detumble check and switch if (mekfData.satRotRateMekf.isValid() && @@ -184,20 +189,21 @@ void AcsController::performSafe() { VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; - } else { - detumbleCounter = 0; + } else if (detumbleCounter > 0) { + detumbleCounter -= 1; } if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { detumbleCounter = 0; // Triggers detumble mode transition in subsystem triggerEvent(acs::SAFE_RATE_VIOLATION); + startTransition(mode, acs::SafeSubmode::DETUMBLE); } updateCtrlValData(errAng); updateActuatorCmdData(cmdDipolMtqs); - // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], - // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, - // acsParameters.rwHandlingParameters.rampTime); + // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, + // acsParameters.rwHandlingParameters.rampTime); } void AcsController::performDetumble() { @@ -207,7 +213,7 @@ void AcsController::performDetumble() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData); + &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { @@ -220,8 +226,11 @@ void AcsController::performDetumble() { double magMomMtq[3] = {0, 0, 0}; detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); + mgmDataProcessed.mgmVecTot.isValid(), magMomMtq, + acsParameters.detumbleParameter.gainD); + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, + *acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipolMax); if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < @@ -231,13 +240,14 @@ void AcsController::performDetumble() { VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; - } else { - detumbleCounter = 0; + } else if (detumbleCounter > 0) { + detumbleCounter -= 1; } if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { detumbleCounter = 0; // Triggers safe mode transition in subsystem triggerEvent(acs::SAFE_RATE_RECOVERY); + startTransition(mode, acs::SafeSubmode::DEFAULT); } disableCtrlValData(); @@ -254,15 +264,16 @@ void AcsController::performPointingCtrl() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData); + &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; } - if (mekfInvalidCounter > 4) { - triggerEvent(acs::MEKF_INVALID_MODE_VIOLATION); + if (mekfInvalidCounter == 5) { + // Trigger this so STR FDIR can set the device faulty. + EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0); } mekfInvalidCounter++; // commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration, @@ -278,7 +289,7 @@ void AcsController::performPointingCtrl() { double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); if (result == returnvalue::FAILED) { - if (multipleRwUnavailableCounter > 4) { + if (multipleRwUnavailableCounter == 5) { triggerEvent(acs::MULTIPLE_RW_INVALID); } multipleRwUnavailableCounter++; @@ -286,24 +297,26 @@ void AcsController::performPointingCtrl() { } else { multipleRwUnavailableCounter = 0; } - double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}; - double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; - double mgtDpDes[3] = {0, 0, 0}; // Variables required for guidance double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1}, errorAngle = 0, errorSatRotRate[3] = {0, 0, 0}; - switch (submode) { + // Variables required for setting actuators + double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0}, + mgtDpDes[3] = {0, 0, 0}; + switch (mode) { case acs::PTG_IDLE: guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); + ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, + *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -327,7 +340,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -341,20 +354,20 @@ void AcsController::performPointingCtrl() { susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, + ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( - &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), + &acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, + &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; + enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; break; case acs::PTG_NADIR: @@ -372,7 +385,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -395,7 +408,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -403,20 +416,26 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; + default: + sif::error << "AcsController: Invalid mode for performPointingCtrl"; + break; } + actuatorCmd.cmdSpeedToRws( + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws, + cmdSpeedRws, acsParameters.onBoardParams.sampleTime, + acsParameters.rwHandlingParameters.maxRwSpeed, + acsParameters.rwHandlingParameters.inertiaWheel); if (enableAntiStiction) { - ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled); + ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); } - - actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws); - actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs); + actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, + *acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipolMax); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); - updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs); + updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], @@ -429,7 +448,8 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, uint16_t rampTime) { { PoolReadGuard pg(&dipoleSet); - MutexGuard mg(torquer::lazyLock()); + MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT, + torquer::LOCK_CTX); torquer::NEW_ACTUATION_FLAG = true; dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration); } @@ -573,6 +593,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD // GPS Processed localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); + localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); @@ -631,8 +652,14 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, } else { return INVALID_SUBMODE; } - } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { - if ((submode < acs::AcsMode::SAFE) or (submode > acs::AcsMode::PTG_INERTIAL)) { + } else if (not((mode < acs::AcsMode::SAFE) or (mode > acs::AcsMode::PTG_INERTIAL))) { + if (mode == acs::AcsMode::SAFE) { + if (not((submode == SUBMODE_NONE) or (submode == acs::SafeSubmode::DETUMBLE))) { + return INVALID_SUBMODE; + } else { + return returnvalue::OK; + } + } else if (not(submode == SUBMODE_NONE)) { return INVALID_SUBMODE; } else { return returnvalue::OK; @@ -649,19 +676,25 @@ void AcsController::announceMode(bool recursive) { const char *modeStr = "UNKNOWN"; if (mode == HasModesIF::MODE_OFF) { modeStr = "OFF"; - } else if (mode == HasModesIF::MODE_ON) { - modeStr = "ON"; - } else if (mode == DeviceHandlerIF::MODE_NORMAL) { - modeStr = "NORMAL"; + } else { + modeStr = acs::getModeStr(static_cast(mode)); + } + const char *submodeStr = "UNKNOWN"; + if (submode == HasModesIF::SUBMODE_NONE) { + submodeStr = "NONE"; + } + if (mode == acs::AcsMode::SAFE) { + acs::SafeSubmode safeSubmode = static_cast(this->submode); + if (safeSubmode == acs::SafeSubmode::DETUMBLE) { + submodeStr = "DETUMBLE"; + } } - const char *submodeStr = acs::getModeStr(static_cast(submode)); sif::info << "ACS controller is now in " << modeStr << " mode with " << submodeStr << " submode" << std::endl; return ExtendedControllerBase::announceMode(recursive); } void AcsController::copyMgmData() { - ACS::SensorValues sensorValues; { PoolReadGuard pg(&sensorValues.mgm0Lis3Set); if (pg.getReadResult() == returnvalue::OK) { @@ -806,7 +839,6 @@ void AcsController::copySusData() { } void AcsController::copyGyrData() { - ACS::SensorValues sensorValues; { PoolReadGuard pg(&sensorValues.gyr0AdisSet); if (pg.getReadResult() == returnvalue::OK) { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 6d31ae49..c0376127 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -4,13 +4,14 @@ #include #include #include +#include #include #include #include #include -#include #include #include +#include #include #include "acs/ActuatorCmd.h" @@ -189,6 +190,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes acsctrl::GpsDataProcessed gpsDataProcessed; PoolEntry gcLatitude = PoolEntry(); PoolEntry gdLongitude = PoolEntry(); + PoolEntry altitude = PoolEntry(); PoolEntry gpsPosition = PoolEntry(3); PoolEntry gpsVelocity = PoolEntry(3); diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 98f65427..2df7af42 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -3,13 +3,13 @@ #include #include #include -#include +#include #include #include #include -#include #include #include +#include #include #include #include @@ -474,8 +474,8 @@ void ThermalController::copySus() { { PoolReadGuard pg0(&susSet0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg0.getReadResult() == returnvalue::OK) { - susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.value = susSet0.temperatureCelcius.value; - susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.setValid(susSet0.temperatureCelcius.isValid()); + susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.value = susSet0.tempC.value; + susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.setValid(susSet0.tempC.isValid()); if (not susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.isValid()) { susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.value = INVALID_TEMPERATURE; } @@ -485,8 +485,8 @@ void ThermalController::copySus() { { PoolReadGuard pg1(&susSet1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg1.getReadResult() == returnvalue::OK) { - susTemperatures.sus_6_r_loc_xfybzm_pt_xf.value = susSet1.temperatureCelcius.value; - susTemperatures.sus_6_r_loc_xfybzm_pt_xf.setValid(susSet1.temperatureCelcius.isValid()); + susTemperatures.sus_6_r_loc_xfybzm_pt_xf.value = susSet1.tempC.value; + susTemperatures.sus_6_r_loc_xfybzm_pt_xf.setValid(susSet1.tempC.isValid()); if (not susTemperatures.sus_6_r_loc_xfybzm_pt_xf.isValid()) { susTemperatures.sus_6_r_loc_xfybzm_pt_xf.value = INVALID_TEMPERATURE; } @@ -496,8 +496,8 @@ void ThermalController::copySus() { { PoolReadGuard pg2(&susSet2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg2.getReadResult() == returnvalue::OK) { - susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.value = susSet2.temperatureCelcius.value; - susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.setValid(susSet2.temperatureCelcius.isValid()); + susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.value = susSet2.tempC.value; + susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.setValid(susSet2.tempC.isValid()); if (not susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.isValid()) { susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.value = INVALID_TEMPERATURE; } @@ -507,8 +507,8 @@ void ThermalController::copySus() { { PoolReadGuard pg3(&susSet3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg3.getReadResult() == returnvalue::OK) { - susTemperatures.sus_7_r_loc_xbybzm_pt_xb.value = susSet3.temperatureCelcius.value; - susTemperatures.sus_7_r_loc_xbybzm_pt_xb.setValid(susSet3.temperatureCelcius.isValid()); + susTemperatures.sus_7_r_loc_xbybzm_pt_xb.value = susSet3.tempC.value; + susTemperatures.sus_7_r_loc_xbybzm_pt_xb.setValid(susSet3.tempC.isValid()); if (not susTemperatures.sus_7_r_loc_xbybzm_pt_xb.isValid()) { susTemperatures.sus_7_r_loc_xbybzm_pt_xb.value = INVALID_TEMPERATURE; } @@ -518,8 +518,8 @@ void ThermalController::copySus() { { PoolReadGuard pg4(&susSet4, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg4.getReadResult() == returnvalue::OK) { - susTemperatures.sus_2_n_loc_xfybzb_pt_yb.value = susSet4.temperatureCelcius.value; - susTemperatures.sus_2_n_loc_xfybzb_pt_yb.setValid(susSet4.temperatureCelcius.isValid()); + susTemperatures.sus_2_n_loc_xfybzb_pt_yb.value = susSet4.tempC.value; + susTemperatures.sus_2_n_loc_xfybzb_pt_yb.setValid(susSet4.tempC.isValid()); if (not susTemperatures.sus_2_n_loc_xfybzb_pt_yb.isValid()) { susTemperatures.sus_2_n_loc_xfybzb_pt_yb.value = INVALID_TEMPERATURE; } @@ -529,8 +529,8 @@ void ThermalController::copySus() { { PoolReadGuard pg5(&susSet5, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg5.getReadResult() == returnvalue::OK) { - susTemperatures.sus_8_r_loc_xbybzb_pt_yb.value = susSet5.temperatureCelcius.value; - susTemperatures.sus_8_r_loc_xbybzb_pt_yb.setValid(susSet5.temperatureCelcius.isValid()); + susTemperatures.sus_8_r_loc_xbybzb_pt_yb.value = susSet5.tempC.value; + susTemperatures.sus_8_r_loc_xbybzb_pt_yb.setValid(susSet5.tempC.isValid()); if (not susTemperatures.sus_8_r_loc_xbybzb_pt_yb.isValid()) { susTemperatures.sus_8_r_loc_xbybzb_pt_yb.value = INVALID_TEMPERATURE; } @@ -540,8 +540,8 @@ void ThermalController::copySus() { { PoolReadGuard pg6(&susSet6, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg6.getReadResult() == returnvalue::OK) { - susTemperatures.sus_3_n_loc_xfybzf_pt_yf.value = susSet6.temperatureCelcius.value; - susTemperatures.sus_3_n_loc_xfybzf_pt_yf.setValid(susSet6.temperatureCelcius.isValid()); + susTemperatures.sus_3_n_loc_xfybzf_pt_yf.value = susSet6.tempC.value; + susTemperatures.sus_3_n_loc_xfybzf_pt_yf.setValid(susSet6.tempC.isValid()); if (not susTemperatures.sus_3_n_loc_xfybzf_pt_yf.isValid()) { susTemperatures.sus_3_n_loc_xfybzf_pt_yf.value = INVALID_TEMPERATURE; } @@ -551,8 +551,8 @@ void ThermalController::copySus() { { PoolReadGuard pg7(&susSet7, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg7.getReadResult() == returnvalue::OK) { - susTemperatures.sus_9_r_loc_xbybzb_pt_yf.value = susSet7.temperatureCelcius.value; - susTemperatures.sus_9_r_loc_xbybzb_pt_yf.setValid(susSet7.temperatureCelcius.isValid()); + susTemperatures.sus_9_r_loc_xbybzb_pt_yf.value = susSet7.tempC.value; + susTemperatures.sus_9_r_loc_xbybzb_pt_yf.setValid(susSet7.tempC.isValid()); if (not susTemperatures.sus_9_r_loc_xbybzb_pt_yf.isValid()) { susTemperatures.sus_9_r_loc_xbybzb_pt_yf.value = INVALID_TEMPERATURE; } @@ -562,8 +562,8 @@ void ThermalController::copySus() { { PoolReadGuard pg8(&susSet8, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg8.getReadResult() == returnvalue::OK) { - susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.value = susSet8.temperatureCelcius.value; - susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.setValid(susSet8.temperatureCelcius.isValid()); + susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.value = susSet8.tempC.value; + susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.setValid(susSet8.tempC.isValid()); if (not susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.isValid()) { susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.value = INVALID_TEMPERATURE; } @@ -573,8 +573,8 @@ void ThermalController::copySus() { { PoolReadGuard pg9(&susSet9, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg9.getReadResult() == returnvalue::OK) { - susTemperatures.sus_10_n_loc_xmybzf_pt_zf.value = susSet9.temperatureCelcius.value; - susTemperatures.sus_10_n_loc_xmybzf_pt_zf.setValid(susSet9.temperatureCelcius.isValid()); + susTemperatures.sus_10_n_loc_xmybzf_pt_zf.value = susSet9.tempC.value; + susTemperatures.sus_10_n_loc_xmybzf_pt_zf.setValid(susSet9.tempC.isValid()); if (not susTemperatures.sus_10_n_loc_xmybzf_pt_zf.isValid()) { susTemperatures.sus_10_n_loc_xmybzf_pt_zf.value = INVALID_TEMPERATURE; } @@ -584,8 +584,8 @@ void ThermalController::copySus() { { PoolReadGuard pg10(&susSet10, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg10.getReadResult() == returnvalue::OK) { - susTemperatures.sus_5_n_loc_xfymzb_pt_zb.value = susSet10.temperatureCelcius.value; - susTemperatures.sus_5_n_loc_xfymzb_pt_zb.setValid(susSet10.temperatureCelcius.isValid()); + susTemperatures.sus_5_n_loc_xfymzb_pt_zb.value = susSet10.tempC.value; + susTemperatures.sus_5_n_loc_xfymzb_pt_zb.setValid(susSet10.tempC.isValid()); if (not susTemperatures.sus_5_n_loc_xfymzb_pt_zb.isValid()) { susTemperatures.sus_5_n_loc_xfymzb_pt_zb.value = INVALID_TEMPERATURE; } @@ -595,8 +595,8 @@ void ThermalController::copySus() { { PoolReadGuard pg11(&susSet11, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg11.getReadResult() == returnvalue::OK) { - susTemperatures.sus_11_r_loc_xbymzb_pt_zb.value = susSet11.temperatureCelcius.value; - susTemperatures.sus_11_r_loc_xbymzb_pt_zb.setValid(susSet11.temperatureCelcius.isValid()); + susTemperatures.sus_11_r_loc_xbymzb_pt_zb.value = susSet11.tempC.value; + susTemperatures.sus_11_r_loc_xbymzb_pt_zb.setValid(susSet11.tempC.isValid()); if (not susTemperatures.sus_11_r_loc_xbymzb_pt_zb.isValid()) { susTemperatures.sus_11_r_loc_xbymzb_pt_zb.value = INVALID_TEMPERATURE; } @@ -854,7 +854,7 @@ void ThermalController::copyDevices() { { lp_var_t tempGyro0 = - lp_var_t(objects::GYRO_0_ADIS_HANDLER, ADIS1650X::TEMPERATURE); + 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; @@ -867,7 +867,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempGyro1 = lp_var_t(objects::GYRO_1_L3G_HANDLER, L3GD20H::TEMPERATURE); + 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; @@ -881,7 +881,7 @@ void ThermalController::copyDevices() { { lp_var_t tempGyro2 = - lp_var_t(objects::GYRO_2_ADIS_HANDLER, ADIS1650X::TEMPERATURE); + 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; @@ -894,7 +894,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempGyro3 = lp_var_t(objects::GYRO_3_L3G_HANDLER, L3GD20H::TEMPERATURE); + 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; @@ -908,7 +908,7 @@ void ThermalController::copyDevices() { { lp_var_t tempMgm0 = - lp_var_t(objects::MGM_0_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS); + lp_var_t(objects::MGM_0_LIS3_HANDLER, mgmLis3::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; @@ -922,7 +922,7 @@ void ThermalController::copyDevices() { { lp_var_t tempMgm2 = - lp_var_t(objects::MGM_2_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS); + lp_var_t(objects::MGM_2_LIS3_HANDLER, mgmLis3::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; diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index a161b0eb..e1371aa2 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -6,8 +6,8 @@ #include #include #include -#include #include +#include #include @@ -112,18 +112,18 @@ class ThermalController : public ExtendedControllerBase { TMP1075::Tmp1075Dataset tmp1075SetIfBoard; // SUS - SUS::SusDataset susSet0; - SUS::SusDataset susSet1; - SUS::SusDataset susSet2; - SUS::SusDataset susSet3; - SUS::SusDataset susSet4; - SUS::SusDataset susSet5; - SUS::SusDataset susSet6; - SUS::SusDataset susSet7; - SUS::SusDataset susSet8; - SUS::SusDataset susSet9; - SUS::SusDataset susSet10; - SUS::SusDataset susSet11; + susMax1227::SusDataset susSet0; + susMax1227::SusDataset susSet1; + susMax1227::SusDataset susSet2; + susMax1227::SusDataset susSet3; + susMax1227::SusDataset susSet4; + susMax1227::SusDataset susSet5; + susMax1227::SusDataset susSet6; + susMax1227::SusDataset susSet7; + susMax1227::SusDataset susSet8; + susMax1227::SusDataset susSet9; + susMax1227::SusDataset susSet10; + susMax1227::SusDataset susSet11; // TempLimits TempLimits acsBoardLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0); diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 2e5fbb1b..4abe0d7a 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -30,19 +30,19 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x2: // InertiaEIVE switch (parameterId) { case 0x0: - parameterWrapper->set(inertiaEIVE.inertiaMatrix); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrix); break; case 0x1: - parameterWrapper->set(inertiaEIVE.inertiaMatrixDeployed); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixDeployed); break; case 0x2: - parameterWrapper->set(inertiaEIVE.inertiaMatrixUndeployed); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixUndeployed); break; case 0x3: - parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel1); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel1); break; case 0x4: - parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel3); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel3); break; default: return INVALID_IDENTIFIER_ID; @@ -51,58 +51,58 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x3: // MgmHandlingParameters switch (parameterId) { case 0x0: - parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm0orientationMatrix); break; case 0x1: - parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm1orientationMatrix); break; case 0x2: - parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm2orientationMatrix); break; case 0x3: - parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm3orientationMatrix); break; case 0x4: - parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm4orientationMatrix); break; case 0x5: - parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm0hardIronOffset); break; case 0x6: - parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm1hardIronOffset); break; case 0x7: - parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm2hardIronOffset); break; case 0x8: - parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm3hardIronOffset); break; case 0x9: - parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm4hardIronOffset); break; case 0xA: - parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm0softIronInverse); break; case 0xB: - parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm1softIronInverse); break; case 0xC: - parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm2softIronInverse); break; case 0xD: - parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm3softIronInverse); break; case 0xE: - parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm4softIronInverse); break; case 0xF: - parameterWrapper->set(mgmHandlingParameters.mgm02variance); + parameterWrapper->setVector(mgmHandlingParameters.mgm02variance); break; case 0x10: - parameterWrapper->set(mgmHandlingParameters.mgm13variance); + parameterWrapper->setVector(mgmHandlingParameters.mgm13variance); break; case 0x11: - parameterWrapper->set(mgmHandlingParameters.mgm4variance); + parameterWrapper->setVector(mgmHandlingParameters.mgm4variance); break; default: return INVALID_IDENTIFIER_ID; @@ -111,112 +111,112 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x4: // SusHandlingParameters switch (parameterId) { case 0x0: - parameterWrapper->set(susHandlingParameters.sus0orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus0orientationMatrix); break; case 0x1: - parameterWrapper->set(susHandlingParameters.sus1orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus1orientationMatrix); break; case 0x2: - parameterWrapper->set(susHandlingParameters.sus2orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus2orientationMatrix); break; case 0x3: - parameterWrapper->set(susHandlingParameters.sus3orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus3orientationMatrix); break; case 0x4: - parameterWrapper->set(susHandlingParameters.sus4orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus4orientationMatrix); break; case 0x5: - parameterWrapper->set(susHandlingParameters.sus5orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus5orientationMatrix); break; case 0x6: - parameterWrapper->set(susHandlingParameters.sus6orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus6orientationMatrix); break; case 0x7: - parameterWrapper->set(susHandlingParameters.sus7orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus7orientationMatrix); break; case 0x8: - parameterWrapper->set(susHandlingParameters.sus8orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus8orientationMatrix); break; case 0x9: - parameterWrapper->set(susHandlingParameters.sus9orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus9orientationMatrix); break; case 0xA: - parameterWrapper->set(susHandlingParameters.sus10orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus10orientationMatrix); break; case 0xB: - parameterWrapper->set(susHandlingParameters.sus11orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus11orientationMatrix); break; case 0xC: - parameterWrapper->set(susHandlingParameters.sus0coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus0coeffAlpha); break; case 0xD: - parameterWrapper->set(susHandlingParameters.sus0coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus0coeffBeta); break; case 0xE: - parameterWrapper->set(susHandlingParameters.sus1coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus1coeffAlpha); break; case 0xF: - parameterWrapper->set(susHandlingParameters.sus1coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus1coeffBeta); break; case 0x10: - parameterWrapper->set(susHandlingParameters.sus2coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus2coeffAlpha); break; case 0x11: - parameterWrapper->set(susHandlingParameters.sus2coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus2coeffBeta); break; case 0x12: - parameterWrapper->set(susHandlingParameters.sus3coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus3coeffAlpha); break; case 0x13: - parameterWrapper->set(susHandlingParameters.sus3coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus3coeffBeta); break; case 0x14: - parameterWrapper->set(susHandlingParameters.sus4coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus4coeffAlpha); break; case 0x15: - parameterWrapper->set(susHandlingParameters.sus4coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus4coeffBeta); break; case 0x16: - parameterWrapper->set(susHandlingParameters.sus5coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus5coeffAlpha); break; case 0x17: - parameterWrapper->set(susHandlingParameters.sus5coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus5coeffBeta); break; case 0x18: - parameterWrapper->set(susHandlingParameters.sus6coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus6coeffAlpha); break; case 0x19: - parameterWrapper->set(susHandlingParameters.sus6coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus6coeffBeta); break; case 0x1A: - parameterWrapper->set(susHandlingParameters.sus7coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus7coeffAlpha); break; case 0x1B: - parameterWrapper->set(susHandlingParameters.sus7coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus7coeffBeta); break; case 0x1C: - parameterWrapper->set(susHandlingParameters.sus8coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus8coeffAlpha); break; case 0x1D: - parameterWrapper->set(susHandlingParameters.sus8coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus8coeffBeta); break; case 0x1E: - parameterWrapper->set(susHandlingParameters.sus9coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus9coeffAlpha); break; case 0x1F: - parameterWrapper->set(susHandlingParameters.sus9coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus9coeffBeta); break; case 0x20: - parameterWrapper->set(susHandlingParameters.sus10coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus10coeffAlpha); break; case 0x21: - parameterWrapper->set(susHandlingParameters.sus10coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus10coeffBeta); break; case 0x22: - parameterWrapper->set(susHandlingParameters.sus11coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus11coeffAlpha); break; case 0x23: - parameterWrapper->set(susHandlingParameters.sus11coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta); break; default: return INVALID_IDENTIFIER_ID; @@ -225,34 +225,34 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x5): // GyrHandlingParameters switch (parameterId) { case 0x0: - parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix); + parameterWrapper->setMatrix(gyrHandlingParameters.gyr0orientationMatrix); break; case 0x1: - parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix); + parameterWrapper->setMatrix(gyrHandlingParameters.gyr1orientationMatrix); break; case 0x2: - parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix); + parameterWrapper->setMatrix(gyrHandlingParameters.gyr2orientationMatrix); break; case 0x3: - parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix); + parameterWrapper->setMatrix(gyrHandlingParameters.gyr3orientationMatrix); break; case 0x4: - parameterWrapper->set(gyrHandlingParameters.gyr0bias); + parameterWrapper->setVector(gyrHandlingParameters.gyr0bias); break; case 0x5: - parameterWrapper->set(gyrHandlingParameters.gyr1bias); + parameterWrapper->setVector(gyrHandlingParameters.gyr1bias); break; case 0x6: - parameterWrapper->set(gyrHandlingParameters.gyr2bias); + parameterWrapper->setVector(gyrHandlingParameters.gyr2bias); break; case 0x7: - parameterWrapper->set(gyrHandlingParameters.gyr3bias); + parameterWrapper->setVector(gyrHandlingParameters.gyr3bias); break; case 0x8: - parameterWrapper->set(gyrHandlingParameters.gyr02variance); + parameterWrapper->setVector(gyrHandlingParameters.gyr02variance); break; case 0x9: - parameterWrapper->set(gyrHandlingParameters.gyr13variance); + parameterWrapper->setVector(gyrHandlingParameters.gyr13variance); break; case 0xA: parameterWrapper->set(gyrHandlingParameters.preferAdis); @@ -270,15 +270,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(rwHandlingParameters.maxTrq); break; case 0x2: - parameterWrapper->set(rwHandlingParameters.stictionSpeed); + parameterWrapper->set(rwHandlingParameters.maxRwSpeed); break; case 0x3: - parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed); + parameterWrapper->set(rwHandlingParameters.stictionSpeed); break; case 0x4: - parameterWrapper->set(rwHandlingParameters.stictionTorque); + parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed); break; case 0x5: + parameterWrapper->set(rwHandlingParameters.stictionTorque); + break; + case 0x6: parameterWrapper->set(rwHandlingParameters.rampTime); break; default: @@ -288,25 +291,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x7): // RwMatrices switch (parameterId) { case 0x0: - parameterWrapper->set(rwMatrices.alignmentMatrix); + parameterWrapper->setMatrix(rwMatrices.alignmentMatrix); break; case 0x1: - parameterWrapper->set(rwMatrices.pseudoInverse); + parameterWrapper->setMatrix(rwMatrices.pseudoInverse); break; case 0x2: - parameterWrapper->set(rwMatrices.without1); + parameterWrapper->setMatrix(rwMatrices.without1); break; case 0x3: - parameterWrapper->set(rwMatrices.without2); + parameterWrapper->setMatrix(rwMatrices.without2); break; case 0x4: - parameterWrapper->set(rwMatrices.without3); + parameterWrapper->setMatrix(rwMatrices.without3); break; case 0x5: - parameterWrapper->set(rwMatrices.without4); + parameterWrapper->setMatrix(rwMatrices.without4); break; case 0x6: - parameterWrapper->set(rwMatrices.nullspace); + parameterWrapper->setVector(rwMatrices.nullspace); break; default: return INVALID_IDENTIFIER_ID; @@ -330,13 +333,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin); break; case 0x5: - parameterWrapper->set(safeModeControllerParameters.sunTargetDirLeop); + parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop); break; case 0x6: - parameterWrapper->set(safeModeControllerParameters.sunTargetDir); + parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir); break; case 0x7: - parameterWrapper->set(safeModeControllerParameters.satRateRef); + parameterWrapper->setVector(safeModeControllerParameters.satRateRef); break; default: return INVALID_IDENTIFIER_ID; @@ -345,31 +348,31 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x9): // IdleModeControllerParameters switch (parameterId) { case 0x0: - parameterWrapper->set(targetModeControllerParameters.zeta); + parameterWrapper->set(idleModeControllerParameters.zeta); break; case 0x1: - parameterWrapper->set(targetModeControllerParameters.om); + parameterWrapper->set(idleModeControllerParameters.om); break; case 0x2: - parameterWrapper->set(targetModeControllerParameters.omMax); + parameterWrapper->set(idleModeControllerParameters.omMax); break; case 0x3: - parameterWrapper->set(targetModeControllerParameters.qiMin); + parameterWrapper->set(idleModeControllerParameters.qiMin); break; case 0x4: - parameterWrapper->set(targetModeControllerParameters.gainNullspace); + parameterWrapper->set(idleModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef); break; case 0x6: - parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + parameterWrapper->set(idleModeControllerParameters.deSatGainFactor); break; case 0x7: - parameterWrapper->set(targetModeControllerParameters.desatOn); + parameterWrapper->set(idleModeControllerParameters.desatOn); break; case 0x8: - parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); break; default: @@ -394,7 +397,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); break; case 0x6: parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); @@ -406,13 +409,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); break; case 0x9: - parameterWrapper->set(targetModeControllerParameters.refDirection); + parameterWrapper->setVector(targetModeControllerParameters.refDirection); break; case 0xA: - parameterWrapper->set(targetModeControllerParameters.refRotRate); + parameterWrapper->setVector(targetModeControllerParameters.refRotRate); break; case 0xB: - parameterWrapper->set(targetModeControllerParameters.quatRef); + parameterWrapper->setVector(targetModeControllerParameters.quatRef); break; case 0xC: parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); @@ -445,52 +448,46 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0xB): // GsTargetModeControllerParameters switch (parameterId) { case 0x0: - parameterWrapper->set(targetModeControllerParameters.zeta); + parameterWrapper->set(gsTargetModeControllerParameters.zeta); break; case 0x1: - parameterWrapper->set(targetModeControllerParameters.om); + parameterWrapper->set(gsTargetModeControllerParameters.om); break; case 0x2: - parameterWrapper->set(targetModeControllerParameters.omMax); + parameterWrapper->set(gsTargetModeControllerParameters.omMax); break; case 0x3: - parameterWrapper->set(targetModeControllerParameters.qiMin); + parameterWrapper->set(gsTargetModeControllerParameters.qiMin); break; case 0x4: - parameterWrapper->set(targetModeControllerParameters.gainNullspace); + parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef); break; case 0x6: - parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor); break; case 0x7: - parameterWrapper->set(targetModeControllerParameters.desatOn); + parameterWrapper->set(gsTargetModeControllerParameters.desatOn); break; case 0x8: - parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); break; case 0x9: - parameterWrapper->set(targetModeControllerParameters.refDirection); + parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); break; case 0xA: - parameterWrapper->set(targetModeControllerParameters.refRotRate); + parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); break; case 0xB: - parameterWrapper->set(targetModeControllerParameters.quatRef); + parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); break; case 0xC: - parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); + parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); break; case 0xD: - parameterWrapper->set(targetModeControllerParameters.latitudeTgt); - break; - case 0xE: - parameterWrapper->set(targetModeControllerParameters.longitudeTgt); - break; - case 0xF: - parameterWrapper->set(targetModeControllerParameters.altitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); break; default: return INVALID_IDENTIFIER_ID; @@ -514,7 +511,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(nadirModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef); break; case 0x6: parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); @@ -526,10 +523,10 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); break; case 0x9: - parameterWrapper->set(nadirModeControllerParameters.refDirection); + parameterWrapper->setVector(nadirModeControllerParameters.refDirection); break; case 0xA: - parameterWrapper->set(nadirModeControllerParameters.quatRef); + parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; case 0xC: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); @@ -556,7 +553,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(inertialModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef); break; case 0x6: parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); @@ -568,13 +565,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); break; case 0x9: - parameterWrapper->set(inertialModeControllerParameters.tgtQuat); + parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); break; case 0xA: - parameterWrapper->set(inertialModeControllerParameters.refRotRate); + parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); break; case 0xC: - parameterWrapper->set(inertialModeControllerParameters.quatRef); + parameterWrapper->setVector(inertialModeControllerParameters.quatRef); break; default: return INVALID_IDENTIFIER_ID; @@ -586,7 +583,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(strParameters.exclusionAngle); break; case 0x1: - parameterWrapper->set(strParameters.boresightAxis); + parameterWrapper->setVector(strParameters.boresightAxis); break; default: return INVALID_IDENTIFIER_ID; @@ -597,6 +594,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x0: parameterWrapper->set(gpsParameters.timeDiffVelocityMax); break; + case 0x1: + parameterWrapper->set(gpsParameters.minimumFdirAltitude); + break; + case 0x2: + parameterWrapper->set(gpsParameters.maximumFdirAltitude); + break; + case 0x3: + parameterWrapper->set(gpsParameters.fdirAltitude); + break; default: return INVALID_IDENTIFIER_ID; } @@ -658,25 +664,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x12): // MagnetorquesParameter switch (parameterId) { case 0x0: - parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); + parameterWrapper->setMatrix(magnetorquerParameter.mtq0orientationMatrix); break; case 0x1: - parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix); + parameterWrapper->setMatrix(magnetorquerParameter.mtq1orientationMatrix); break; case 0x2: - parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix); + parameterWrapper->setMatrix(magnetorquerParameter.mtq2orientationMatrix); break; case 0x3: - parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq); + parameterWrapper->setMatrix(magnetorquerParameter.alignmentMatrixMtq); break; case 0x4: - parameterWrapper->set(magnetorquesParameter.inverseAlignment); + parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment); break; case 0x5: - parameterWrapper->set(magnetorquesParameter.DipolMax); + parameterWrapper->set(magnetorquerParameter.dipolMax); break; case 0x6: - parameterWrapper->set(magnetorquesParameter.torqueDuration); + parameterWrapper->set(magnetorquerParameter.torqueDuration); break; default: return INVALID_IDENTIFIER_ID; diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 11de10a3..822cfabc 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -772,7 +772,7 @@ class AcsParameters : public HasParametersIF { double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594}; double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845}; - /* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is + /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is * assumed to be equal for the same class of sensors */ float gyr02variance[3] = {pow(3.0e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms pow(3.0e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms @@ -783,9 +783,10 @@ class AcsParameters : public HasParametersIF { struct RwHandlingParameters { double inertiaWheel = 0.000028198; - double maxTrq = 0.0032; // 3.2 [mNm] - int32_t stictionSpeed = 100; // RPM - int32_t stictionReleaseSpeed = 120; // RPM + double maxTrq = 0.0032; // 3.2 [mNm] + int32_t maxRwSpeed = 65000; // 0.1 RPM + int32_t stictionSpeed = 1000; // 0.1 RPM + int32_t stictionReleaseSpeed = 1000; // 0.1 RPM double stictionTorque = 0.0006; uint16_t rampTime = 10; @@ -817,7 +818,7 @@ class AcsParameters : public HasParametersIF { double sunMagAngleMin = 5 * M_PI / 180; - double sunTargetDirLeop[3] = {0, .5, .5}; + double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)}; double sunTargetDir[3] = {0, 0, 1}; double satRateRef[3] = {0, 0, 0}; @@ -843,7 +844,7 @@ class AcsParameters : public HasParametersIF { double refDirection[3] = {-1, 0, 0}; // Antenna double refRotRate[3] = {0, 0, 0}; double quatRef[4] = {0, 0, 0, 1}; - int8_t timeElapsedMax = 10; // rot rate calculations + uint8_t timeElapsedMax = 10; // rot rate calculations // Default is Stuttgart GS double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude @@ -859,7 +860,7 @@ class AcsParameters : public HasParametersIF { struct GsTargetModeControllerParameters : PointingLawParameters { double refDirection[3] = {-1, 0, 0}; // Antenna - int8_t timeElapsedMax = 10; // rot rate calculations + uint8_t timeElapsedMax = 10; // rot rate calculations // Default is Stuttgart GS double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude @@ -871,7 +872,7 @@ class AcsParameters : public HasParametersIF { double refDirection[3] = {-1, 0, 0}; // Antenna double quatRef[4] = {0, 0, 0, 1}; double refRotRate[3] = {0, 0, 0}; - int8_t timeElapsedMax = 10; // rot rate calculations + uint8_t timeElapsedMax = 10; // rot rate calculations } nadirModeControllerParameters; struct InertialModeControllerParameters : PointingLawParameters { @@ -886,7 +887,10 @@ class AcsParameters : public HasParametersIF { } strParameters; struct GpsParameters { - double timeDiffVelocityMax = 30; //[s] + double timeDiffVelocityMax = 30; // [s] + double minimumFdirAltitude = 475 * 1e3; // [m] + double maximumFdirAltitude = 575 * 1e3; // [m] + double fdirAltitude = 525 * 1e3; // [m] } gpsParameters; struct SunModelParameters { @@ -912,16 +916,16 @@ class AcsParameters : public HasParametersIF { double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability } kalmanFilterParameters; - struct MagnetorquesParameter { + struct MagnetorquerParameter { double mtq0orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; double mtq1orientationMatrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}}; double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; - double DipolMax = 0.2; // [Am^2] + double dipolMax = 0.2; // [Am^2] uint16_t torqueDuration = 300; // [ms] - } magnetorquesParameter; + } magnetorquerParameter; struct DetumbleParameter { uint8_t detumblecounter = 75; // 30 s diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 8cda84d6..457cacce 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -10,40 +10,32 @@ #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" -ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; } +ActuatorCmd::ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {} -void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) { - // Scaling the commanded torque to a maximum value - double maxTrq = acsParameters.rwHandlingParameters.maxTrq; +void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) { + uint8_t maxIdx = 0; + VectorOperations::maxAbsValue(rwTrq, 4, &maxIdx); + double maxValue = rwTrq[maxIdx]; - double maxValue = 0; - for (int i = 0; i < 4; i++) { // size of torque, always 4 ? - if (abs(rwTrq[i]) > maxValue) { - maxValue = abs(rwTrq[i]); - } - } - - if (maxValue > maxTrq) { - double scalingFactor = maxTrq / maxValue; - VectorOperations::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4); + if (maxValue > maxTorque) { + double scalingFactor = maxTorque / maxValue; + VectorOperations::mulScalar(rwTrq, scalingFactor, rwTrq, 4); } } -void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, - const int32_t speedRw2, const int32_t speedRw3, - const double *rwTorque, int32_t *rwCmdSpeed) { +void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, + int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed, + double sampleTime, int32_t maxRwSpeed, double inertiaWheel) { using namespace Math; // Calculating the commanded speed in RPM for every reaction wheel int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3}; double deltaSpeed[4] = {0, 0, 0, 0}; - double commandTime = acsParameters.onBoardParams.sampleTime, - inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel; double radToRpm = 60 / (2 * PI); // factor for conversion to RPM // W_RW = Torque_RW / I_RW * delta t [rad/s] - double factor = commandTime / inertiaWheel * radToRpm; + double factor = sampleTime / inertiaWheel * radToRpm; int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); for (int i = 0; i < 4; i++) { @@ -51,23 +43,27 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, } VectorOperations::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4); VectorOperations::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4); + for (uint8_t i = 0; i < 4; i++) { + if (rwCmdSpeed[i] > maxRwSpeed) { + rwCmdSpeed[i] = maxRwSpeed; + } else if (rwCmdSpeed[i] < -maxRwSpeed) { + rwCmdSpeed[i] = -maxRwSpeed; + } + } } -void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) { +void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, + const double *inverseAlignment, double maxDipol) { // Convert to actuator frame double dipolMomentActuatorDouble[3] = {0, 0, 0}; - MatrixOperations::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, - dipolMoment, dipolMomentActuatorDouble, 3, 3, 1); + MatrixOperations::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3, + 1); // Scaling along largest element if dipol exceeds maximum - double maxDipol = acsParameters.magnetorquesParameter.DipolMax; - double maxValue = 0; - for (int i = 0; i < 3; i++) { - if (abs(dipolMomentActuator[i]) > maxDipol) { - maxValue = abs(dipolMomentActuator[i]); - } - } - if (maxValue > maxDipol) { - double scalingFactor = maxDipol / maxValue; + uint8_t maxIdx = 0; + VectorOperations::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx); + double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]); + if (maxAbsValue > maxDipol) { + double scalingFactor = maxDipol / maxAbsValue; VectorOperations::mulScalar(dipolMomentActuatorDouble, scalingFactor, dipolMomentActuatorDouble, 3); } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 969bd782..5d5d47f3 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -1,23 +1,22 @@ #ifndef ACTUATORCMD_H_ #define ACTUATORCMD_H_ -#include "AcsParameters.h" #include "MultiplicativeKalmanFilter.h" #include "SensorProcessing.h" #include "SensorValues.h" class ActuatorCmd { public: - ActuatorCmd(AcsParameters *acsParameters_); // Input mode ? + ActuatorCmd(); virtual ~ActuatorCmd(); /* * @brief: scalingTorqueRws() scales the torque via maximum part in case this part is * higher then the maximum torque - * @param: rwTrq given torque for reaction wheels - * rwTrqScaled possible scaled torque + * @param: rwTrq given torque for reaction wheels which will be + * scaled if needed to be */ - void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled); + void scalingTorqueRws(double *rwTrq, double maxTorque); /* * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction @@ -28,8 +27,9 @@ class ActuatorCmd { * rwCmdSpeed output revolutions per minute for every * reaction wheel */ - void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, - const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed); + void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3, + const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime, + int32_t maxRwSpeed, double inertiaWheel); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques @@ -37,11 +37,11 @@ class ActuatorCmd { * @param: dipolMoment given dipol moment in spacecraft frame * dipolMomentActuator resulting dipol moment in actuator reference frame */ - void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator); + void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, + const double *inverseAlignment, double maxDipol); protected: private: - AcsParameters acsParameters; }; #endif /* ACTUATORCMD_H_ */ diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 09f0be20..35a2f025 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -12,7 +12,7 @@ #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" -Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {} +Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } Guidance::~Guidance() {} @@ -26,9 +26,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, - acsParameters.targetModeControllerParameters.longitudeTgt, - acsParameters.targetModeControllerParameters.altitudeTgt, targetE); + acsParameters->targetModeControllerParameters.latitudeTgt, + acsParameters->targetModeControllerParameters.longitudeTgt, + acsParameters->targetModeControllerParameters.altitudeTgt, targetE); // target direction in the ECEF frame double targetDirE[3] = {0, 0, 0}; @@ -57,9 +57,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve // rotation quaternion from two vectors double refDir[3] = {0, 0, 0}; - refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0]; - refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1]; - refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2]; + refDir[0] = acsParameters->targetModeControllerParameters.refDirection[0]; + refDir[1] = acsParameters->targetModeControllerParameters.refDirection[1]; + refDir[2] = acsParameters->targetModeControllerParameters.refDirection[2]; double noramlizedTargetDirB[3] = {0, 0, 0}; VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); VectorOperations::normalize(refDir, refDir, 3); @@ -96,15 +96,15 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve //------------------------------------------------------------------------------------- // Calculation of reference rotation rate in case of star tracker blinding //------------------------------------------------------------------------------------- - if (acsParameters.targetModeControllerParameters.avoidBlindStr) { + if (acsParameters->targetModeControllerParameters.avoidBlindStr) { double sunDirB[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1); - double exclAngle = acsParameters.strParameters.exclusionAngle, - blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, - blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop; + double exclAngle = acsParameters->strParameters.exclusionAngle, + blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart, + blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop; double sightAngleSun = - VectorOperations::dot(acsParameters.strParameters.boresightAxis, sunDirB); + VectorOperations::dot(acsParameters->strParameters.boresightAxis, sunDirB); if (!(strBlindAvoidFlag)) { double critSightAngle = blindStart * exclAngle; @@ -113,7 +113,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve } } else { if (sightAngleSun < blindEnd * exclAngle) { - double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate; + double normBlindRefRate = acsParameters->targetModeControllerParameters.blindRotRate; double blindRefRate[3] = {0, 0, 0}; if (sunDirB[1] < 0) { blindRefRate[0] = normBlindRefRate; @@ -144,9 +144,9 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, - acsParameters.targetModeControllerParameters.longitudeTgt, - acsParameters.targetModeControllerParameters.altitudeTgt, targetE); + acsParameters->targetModeControllerParameters.latitudeTgt, + acsParameters->targetModeControllerParameters.longitudeTgt, + acsParameters->targetModeControllerParameters.altitudeTgt, targetE); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetE, posSatE, targetDirE, 3); @@ -198,7 +198,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel {xAxis[2], yAxis[2], zAxis[2]}}; QuaternionOperations::fromDcm(dcmIX, targetQuat); - int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; + int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax; targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } @@ -211,9 +211,9 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] double groundStationE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.gsTargetModeControllerParameters.latitudeTgt, - acsParameters.gsTargetModeControllerParameters.longitudeTgt, - acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE); + acsParameters->gsTargetModeControllerParameters.latitudeTgt, + acsParameters->gsTargetModeControllerParameters.longitudeTgt, + acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(groundStationE, posSatE, targetDirE, 3); @@ -262,7 +262,7 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] {xAxis[2], yAxis[2], zAxis[2]}}; QuaternionOperations::fromDcm(dcmTgt, targetQuat); - int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax; + int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } @@ -332,9 +332,9 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub // rotation quaternion from two vectors double refDir[3] = {0, 0, 0}; - refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0]; - refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1]; - refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2]; + refDir[0] = acsParameters->nadirModeControllerParameters.refDirection[0]; + refDir[1] = acsParameters->nadirModeControllerParameters.refDirection[1]; + refDir[2] = acsParameters->nadirModeControllerParameters.refDirection[2]; double noramlizedTargetDirB[3] = {0, 0, 0}; VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); VectorOperations::normalize(refDir, refDir, 3); @@ -406,7 +406,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl {xAxis[2], yAxis[2], zAxis[2]}}; QuaternionOperations::fromDcm(dcmTgt, targetQuat); - int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax; + int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax; targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate); } @@ -516,19 +516,19 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid()); if (rw1valid && rw2valid && rw3valid && rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); return returnvalue::OK; } else if (!rw1valid && rw2valid && rw3valid && rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without1, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double)); return returnvalue::OK; } else if (rw1valid && !rw2valid && rw3valid && rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without2, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double)); return returnvalue::OK; } else if (rw1valid && rw2valid && !rw3valid && rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without3, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double)); return returnvalue::OK; } else if (rw1valid && rw2valid && rw3valid && !rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without4, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double)); return returnvalue::OK; } else { // @note: This one takes the normal pseudoInverse of all four raction wheels valid. @@ -540,28 +540,30 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, } void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { - if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or - not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore - std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, + std::error_code e; + if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or + not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) { + std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir, 3 * sizeof(double)); } else { - std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop, + std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop, 3 * sizeof(double)); } - std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef, + std::memcpy(satRateSafe, acsParameters->safeModeControllerParameters.satRateRef, 3 * sizeof(double)); } ReturnValue_t Guidance::solarArrayDeploymentComplete() { - if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE)) { + std::error_code e; + if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e)) { std::remove(SD_0_SKEWED_PTG_FILE); - if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE)) { + if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e)) { return returnvalue::FAILED; } } - if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { + if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) { std::remove(SD_1_SKEWED_PTG_FILE); - if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { + if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) { return returnvalue::FAILED; } } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index da9d429b..f3369092 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -55,14 +55,15 @@ class Guidance { ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); private: - AcsParameters acsParameters; + const AcsParameters *acsParameters; + bool strBlindAvoidFlag = false; timeval timeSavedQuaternion; double savedQuaternion[4] = {0, 0, 0, 0}; double omegaRefSaved[3] = {0, 0, 0}; - static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/deployment"; - static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/deployment"; + static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm"; + static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm"; }; #endif /* ACS_GUIDANCE_H_ */ diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index a700c6a6..77a3ef00 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -12,33 +12,22 @@ #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" -/*Initialisation of values for parameters in constructor*/ -MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) - : initialQuaternion{0, 0, 0, 1}, - initialCovarianceMatrix{{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}} { - loadAcsParameters(acsParameters_); -} +MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {} MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {} -void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) { - inertiaEIVE = &(acsParameters_->inertiaEIVE); - kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); -} - ReturnValue_t MultiplicativeKalmanFilter::init( const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, - const bool validMagModel, acsctrl::MekfData *mekfData) { // valids for "model measurements"? + const bool validMagModel, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters) { // valids for "model measurements"? // check for valid mag/sun if (validMagField_ && validSS && validSSModel && validMagModel) { - validInit = true; // QUEST ALGO ----------------------------------------------------------------------- double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; - sigmaSun = kalmanFilterParameters->sensorNoiseSS; - sigmaMag = kalmanFilterParameters->sensorNoiseMAG; - sigmaGyro = kalmanFilterParameters->sensorNoiseGYR; + sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS; + sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG; + sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGYR; double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, normSunJ[3] = {0, 0, 0}; @@ -192,21 +181,18 @@ ReturnValue_t MultiplicativeKalmanFilter::init( return MEKF_INITIALIZED; } else { // no initialisation possible, no valid measurements - validInit = false; updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); return MEKF_UNINITIALIZED; } } // --------------- MEKF DISCRETE TIME STEP ------------------------------- -ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, const bool validSTR_, - const double *rateGYRs_, const bool validGYRs_, - const double *magneticField_, - const bool validMagField_, const double *sunDir_, - const bool validSS, const double *sunDirJ, - const bool validSSModel, const double *magFieldJ, - const bool validMagModel, double sampleTime, - acsctrl::MekfData *mekfData) { +ReturnValue_t MultiplicativeKalmanFilter::mekfEst( + const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_, + const bool validGYRs_, const double *magneticField_, const bool validMagField_, + const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, + const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters) { // Check for GYR Measurements int MDF = 0; // Matrix Dimension Factor if (!validGYRs_) { @@ -248,9 +234,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c // If we are here, MEKF will perform double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0; - sigmaSun = kalmanFilterParameters->sensorNoiseSS; - sigmaMag = kalmanFilterParameters->sensorNoiseMAG; - sigmaStr = kalmanFilterParameters->sensorNoiseSTR; + sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS; + sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG; + sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseSTR; double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, normSunJ[3] = {0, 0, 0}; @@ -912,8 +898,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c biasGYR[2] = updatedGyroBias[2]; /* ----------- PROPAGATION ----------*/ - double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR; - double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR; + double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseBsGYR; + double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseArwGYR; double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; @@ -931,27 +917,31 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, covQ22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, covQ12trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - if (normRotEst * sampleTime < M_PI / 10) { - double fact11 = pow(sigmaV, 2) * sampleTime + 1. / 3. * pow(sigmaU, 2) * pow(sampleTime, 3); + if (normRotEst * acsParameters->onBoardParams.sampleTime < M_PI / 10) { + double fact11 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime + + 1. / 3. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 3); MatrixOperations::multiplyScalar(*identityMatrix3, fact11, *covQ11, 3, 3); - double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(sampleTime, 2)); + double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 2)); MatrixOperations::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3); std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double)); - double fact22 = pow(sigmaU, 2) * sampleTime; + double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime; MatrixOperations::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3); } else { - double fact22 = pow(sigmaU, 2) * sampleTime; + double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime; MatrixOperations::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3); double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3]; - double fact12_0 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3)); + double fact12_0 = + (normRotEst * acsParameters->onBoardParams.sampleTime - + sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3)); MatrixOperations::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3); - double fact12_1 = 1. / 2. * pow(sampleTime, 2); + double fact12_1 = 1. / 2. * pow(acsParameters->onBoardParams.sampleTime, 2); MatrixOperations::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3); double fact12_2 = - (1. / 2. * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) / + (1. / 2. * pow(normRotEst, 2) * pow(acsParameters->onBoardParams.sampleTime, 2) + + cos(normRotEst * acsParameters->onBoardParams.sampleTime) - 1) / pow(normRotEst, 4); MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3); MatrixOperations::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3); @@ -961,13 +951,15 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c MatrixOperations::transpose(*covQ12, *covQ12trans, 3); double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3]; - double fact11_0 = pow(sigmaV, 2) * sampleTime; + double fact11_0 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime; MatrixOperations::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3); - double fact11_1 = 1. / 3. * pow(sampleTime, 3); + double fact11_1 = 1. / 3. * pow(acsParameters->onBoardParams.sampleTime, 3); MatrixOperations::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3); - double fact11_2 = (2 * normRotEst * sampleTime - 2 * sin(normRotEst * sampleTime) - - 1. / 3. * pow(normRotEst, 3) * pow(sampleTime, 3)) / - pow(normRotEst, 5); + double fact11_2 = + (2 * normRotEst * acsParameters->onBoardParams.sampleTime - + 2 * sin(normRotEst * acsParameters->onBoardParams.sampleTime) - + 1. / 3. * pow(normRotEst, 3) * pow(acsParameters->onBoardParams.sampleTime, 3)) / + pow(normRotEst, 5); MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3); MatrixOperations::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3); MatrixOperations::subtract(*covQ11_1, *covQ11_2, *covQ11_12, 3, 3); @@ -1017,9 +1009,10 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3]; - double fact11_1 = sin(normRotEst * sampleTime) / normRotEst; + double fact11_1 = sin(normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst; MatrixOperations::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3); - double fact11_2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2); + double fact11_2 = + (1 - cos(normRotEst * acsParameters->onBoardParams.sampleTime)) / pow(normRotEst, 2); MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3); MatrixOperations::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3); MatrixOperations::subtract(*identityMatrix3, *phi11_1, *phi11_01, 3, 3); @@ -1028,8 +1021,11 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c double phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3]; double fact12_0 = fact11_2; MatrixOperations::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3); - MatrixOperations::multiplyScalar(*identityMatrix3, sampleTime, *phi12_1, 3, 3); - double fact12_2 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3)); + MatrixOperations::multiplyScalar(*identityMatrix3, + acsParameters->onBoardParams.sampleTime, *phi12_1, 3, 3); + double fact12_2 = + (normRotEst * acsParameters->onBoardParams.sampleTime - + sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3)); MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3); MatrixOperations::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3); MatrixOperations::subtract(*phi12_0, *phi12_1, *phi12_01, 3, 3); @@ -1056,8 +1052,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c // Propagated Quaternion double rotSin[3] = {0, 0, 0}, rotCosMat[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double rotCos = cos(0.5 * normRotEst * sampleTime); - double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst; + double rotCos = cos(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime); + double sinFac = sin(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst; VectorOperations::mulScalar(rotRateEst, sinFac, rotSin, 3); double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index 47e1f807..ceb98339 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -17,9 +17,8 @@ class MultiplicativeKalmanFilter { */ public: /* @brief: Constructor - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ - MultiplicativeKalmanFilter(AcsParameters *acsParameters_); + MultiplicativeKalmanFilter(); virtual ~MultiplicativeKalmanFilter(); ReturnValue_t reset(acsctrl::MekfData *mekfData); @@ -33,8 +32,8 @@ class MultiplicativeKalmanFilter { */ ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, - const double *magFieldJ, const bool validMagModel, - acsctrl::MekfData *mekfData); + const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters); /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter * for the current step after the initalization @@ -54,7 +53,8 @@ class MultiplicativeKalmanFilter { const bool validGYRs_, const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, - const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData); + const bool validMagModel, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters); enum MekfStatus : uint8_t { UNINITIALIZED = 0, @@ -79,23 +79,21 @@ class MultiplicativeKalmanFilter { private: /*Parameters*/ - AcsParameters::InertiaEIVE *inertiaEIVE; - AcsParameters::KalmanFilterParameters *kalmanFilterParameters; double quaternion_STR_SB[4]; - bool validInit; /*States*/ - double initialQuaternion[4]; /*after reset?QUEST*/ - double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/ - double propagatedQuaternion[4]; /*Filter Quaternion for next step*/ - uint8_t sensorsAvail; + double initialQuaternion[4] = {0, 0, 0, 1}; /*after reset?QUEST*/ + double initialCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; + double propagatedQuaternion[4]; /*Filter Quaternion for next step*/ + uint8_t sensorsAvail = 0; /*Outputs*/ double quatBJ[4]; /* Output Quaternion */ double biasGYR[3]; /*Between measured and estimated sat Rate*/ /*Parameter INIT*/ /*Functions*/ - void loadAcsParameters(AcsParameters *acsParameters_); void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus); void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4], double satRotRate[3]); diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 03446609..9e8b3719 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -8,9 +8,7 @@ #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" -Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilter(acsParameters_) { - acsParameters = *acsParameters_; -} +Navigation::Navigation() {} Navigation::~Navigation() {} @@ -18,7 +16,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::MekfData *mekfData) { + acsctrl::MekfData *mekfData, AcsParameters *acsParameters) { double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; bool quatIBValid = sensorValues->strSet.caliQx.isValid() && @@ -30,7 +28,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), - mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData); + mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData, + acsParameters); return mekfStatus; } else { mekfStatus = multiplicativeKalmanFilter.mekfEst( @@ -39,7 +38,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value, - mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData); + mgmDataProcessed->magIgrfModel.isValid(), mekfData, acsParameters); return mekfStatus; } } diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index cf9e81e3..b567fbdd 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -9,19 +9,19 @@ class Navigation { public: - Navigation(AcsParameters *acsParameters_); + Navigation(); virtual ~Navigation(); ReturnValue_t useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData); + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters); void resetMekf(acsctrl::MekfData *mekfData); protected: private: MultiplicativeKalmanFilter multiplicativeKalmanFilter; - AcsParameters acsParameters; ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED; }; diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index e268d786..b2812f55 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -14,7 +14,7 @@ using namespace Math; -SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) {} +SensorProcessing::SensorProcessing() {} SensorProcessing::~SensorProcessing() {} @@ -26,7 +26,8 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude, bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed) { - // ---------------- IGRF- 13 Implementation here ------------------------------------------------ + // ---------------- IGRF- 13 Implementation here + // ------------------------------------------------ double magIgrfModel[3] = {0.0, 0.0, 0.0}; if (gpsValid) { // Should be existing class object which will be called and modified here. @@ -469,10 +470,10 @@ void SensorProcessing::processGyr( float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0}; if (gyr0valid) { - const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; + double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; + VectorOperations::subtract(gyr0Value, gyrParameters->gyr0bias, gyr0Value, 3); MatrixOperations::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, gyr0ValueBody, 3, 3, 1); - VectorOperations::subtract(gyr0ValueBody, gyrParameters->gyr0bias, gyr0ValueBody, 3); VectorOperations::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i]; @@ -480,10 +481,10 @@ void SensorProcessing::processGyr( } } if (gyr1valid) { - const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; + double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; + VectorOperations::subtract(gyr1Value, gyrParameters->gyr1bias, gyr1Value, 3); MatrixOperations::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, gyr1ValueBody, 3, 3, 1); - VectorOperations::subtract(gyr1ValueBody, gyrParameters->gyr1bias, gyr1ValueBody, 3); VectorOperations::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i]; @@ -491,10 +492,10 @@ void SensorProcessing::processGyr( } } if (gyr2valid) { - const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; + double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; + VectorOperations::subtract(gyr2Value, gyrParameters->gyr2bias, gyr2Value, 3); MatrixOperations::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, gyr2ValueBody, 3, 3, 1); - VectorOperations::subtract(gyr2ValueBody, gyrParameters->gyr2bias, gyr2ValueBody, 3); VectorOperations::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i]; @@ -502,10 +503,10 @@ void SensorProcessing::processGyr( } } if (gyr3valid) { - const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; + double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; + VectorOperations::subtract(gyr3Value, gyrParameters->gyr3bias, gyr3Value, 3); MatrixOperations::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, gyr3ValueBody, 3, 3, 1); - VectorOperations::subtract(gyr3ValueBody, gyrParameters->gyr3bias, gyr3ValueBody, 3); VectorOperations::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i]; @@ -549,8 +550,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong const bool validGps, const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed) { - // name to convert not process - double gdLongitude = 0, gcLatitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0}; + double gdLongitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0}, + gpsVelocityE[3] = {0, 0, 0}; if (validGps) { // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic gdLongitude = gpsLongitude * PI / 180.; @@ -559,9 +560,17 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong double factor = 1 - pow(eccentricityWgs84, 2); gcLatitude = atan(factor * tan(latitudeRad)); + // Altitude FDIR + if (gpsAltitude > gpsParameters->maximumFdirAltitude || + gpsAltitude < gpsParameters->maximumFdirAltitude) { + altitude = gpsParameters->fdirAltitude; + } else { + altitude = gpsAltitude; + } + // Calculation of the satellite velocity in earth fixed frame double deltaDistance[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE); + MathOperations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE); if (validSavedPosSatE && (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); @@ -580,6 +589,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong if (pg.getReadResult() == returnvalue::OK) { gpsDataProcessed->gdLongitude.value = gdLongitude; gpsDataProcessed->gcLatitude.value = gcLatitude; + gpsDataProcessed->altitude.value = altitude; std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double)); std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double)); gpsDataProcessed->setValidity(validGps, true); diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index cdd29d8b..d845c2f3 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -15,7 +15,7 @@ class SensorProcessing { public: void reset(); - SensorProcessing(AcsParameters *acsParameters_); + SensorProcessing(); virtual ~SensorProcessing(); void process(timeval now, ACS::SensorValues *sensorValues, @@ -77,7 +77,6 @@ class SensorProcessing { bool validSavedPosSatE = false; SusConverter susConverter; - AcsParameters acsParameters; }; #endif /*SENSORPROCESSING_H_*/ diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 49e3d5fa..25946d0b 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -1,16 +1,15 @@ #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ +#include +#include +#include +#include +#include +#include #include #include - -#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h" -#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" -#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" -#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" -#include "mission/devices/devicedefinitions/GPSDefinitions.h" -#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" -#include "mission/devices/devicedefinitions/SusDefinitions.h" +#include namespace ACS { @@ -27,30 +26,28 @@ class SensorValues { ReturnValue_t updateStr(); ReturnValue_t updateRw(); - MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = - MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); - RM3100::Rm3100PrimaryDataset mgm1Rm3100Set = - RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER); - MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set = - MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER); - RM3100::Rm3100PrimaryDataset mgm3Rm3100Set = - RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); + mgmLis3::MgmPrimaryDataset mgm0Lis3Set = mgmLis3::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); + mgmRm3100::Rm3100PrimaryDataset mgm1Rm3100Set = + mgmRm3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER); + mgmLis3::MgmPrimaryDataset mgm2Lis3Set = mgmLis3::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER); + mgmRm3100::Rm3100PrimaryDataset mgm3Rm3100Set = + mgmRm3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); imtq::RawMtmMeasurementNoTorque imtqMgmSet = imtq::RawMtmMeasurementNoTorque(objects::IMTQ_HANDLER); - std::array susSets{ - SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF), - SUS::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB), - SUS::SusDataset(objects::SUS_2_N_LOC_XFYBZB_PT_YB), - SUS::SusDataset(objects::SUS_3_N_LOC_XFYBZF_PT_YF), - SUS::SusDataset(objects::SUS_4_N_LOC_XMYFZF_PT_ZF), - SUS::SusDataset(objects::SUS_5_N_LOC_XFYMZB_PT_ZB), - SUS::SusDataset(objects::SUS_6_R_LOC_XFYBZM_PT_XF), - SUS::SusDataset(objects::SUS_7_R_LOC_XBYBZM_PT_XB), - SUS::SusDataset(objects::SUS_8_R_LOC_XBYBZB_PT_YB), - SUS::SusDataset(objects::SUS_9_R_LOC_XBYBZB_PT_YF), - SUS::SusDataset(objects::SUS_10_N_LOC_XMYBZF_PT_ZF), - SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB), + std::array susSets{ + susMax1227::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF), + susMax1227::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB), + susMax1227::SusDataset(objects::SUS_2_N_LOC_XFYBZB_PT_YB), + susMax1227::SusDataset(objects::SUS_3_N_LOC_XFYBZF_PT_YF), + susMax1227::SusDataset(objects::SUS_4_N_LOC_XMYFZF_PT_ZF), + susMax1227::SusDataset(objects::SUS_5_N_LOC_XFYMZB_PT_ZB), + susMax1227::SusDataset(objects::SUS_6_R_LOC_XFYBZM_PT_XF), + susMax1227::SusDataset(objects::SUS_7_R_LOC_XBYBZM_PT_XB), + susMax1227::SusDataset(objects::SUS_8_R_LOC_XBYBZB_PT_YB), + susMax1227::SusDataset(objects::SUS_9_R_LOC_XBYBZB_PT_YF), + susMax1227::SusDataset(objects::SUS_10_N_LOC_XMYBZF_PT_ZF), + susMax1227::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB), }; AdisGyroPrimaryDataset gyr0AdisSet = AdisGyroPrimaryDataset(objects::GYRO_0_ADIS_HANDLER); diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 705bf599..893d6a4b 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -1,11 +1,3 @@ - -/* - * Detumble.cpp - * - * Created on: 17 Aug 2022 - * Author: Robin Marquardt - */ - #include "Detumble.h" #include @@ -17,33 +9,32 @@ #include "../util/MathOperations.h" -Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } +Detumble::Detumble() {} Detumble::~Detumble() {} -void Detumble::loadAcsParameters(AcsParameters *acsParameters_) { - detumbleParameter = &(acsParameters_->detumbleParameter); - magnetorquesParameter = &(acsParameters_->magnetorquesParameter); -} - ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid, - const double *magField, const bool magFieldValid, double *magMom) { + const double *magField, const bool magFieldValid, double *magMom, + double gain) { if (!magRateValid || !magFieldValid) { return DETUMBLE_NO_SENSORDATA; } - double gain = detumbleParameter->gainD; - double factor = -gain / pow(VectorOperations::norm(magField, 3), 2); - VectorOperations::mulScalar(magRate, factor, magMom, 3); + // convert uT to T + double magFieldT[3], magRateT[3]; + VectorOperations::mulScalar(magField, 1e-6, magFieldT, 3); + VectorOperations::mulScalar(magRate, 1e-6, magRateT, 3); + // control law + double factor = -gain / pow(VectorOperations::norm(magFieldT, 3), 2); + VectorOperations::mulScalar(magRateT, factor, magMom, 3); return returnvalue::OK; } -ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, - double *magMom) { +ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, double *magMom, + double dipolMax) { if (!magRateValid) { return DETUMBLE_NO_SENSORDATA; } - double dipolMax = magnetorquesParameter->DipolMax; for (int i = 0; i < 3; i++) { magMom[i] = -dipolMax * sign(magRate[i]); } @@ -51,14 +42,20 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal return returnvalue::OK; } -ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid, +ReturnValue_t Detumble::bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField, const bool *magFieldValid, - double *magMom) { + double *magMom, double gain) { if (!satRateValid || !magFieldValid) { return DETUMBLE_NO_SENSORDATA; } - double gain = detumbleParameter->gainD; - double factor = -gain / pow(VectorOperations::norm(magField, 3), 2); - VectorOperations::mulScalar(satRate, factor, magMom, 3); + // convert uT to T + double magFieldT[3]; + VectorOperations::mulScalar(magField, 1e-6, magFieldT, 3); + // control law + double factor = gain / pow(VectorOperations::norm(magField, 3), 2); + double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0}; + VectorOperations::normalize(magFieldT, magFieldNormed, 3); + VectorOperations::cross(satRate, magFieldNormed, crossProduct); + VectorOperations::mulScalar(crossProduct, factor, magMom, 3); return returnvalue::OK; } diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 65e5ec28..2bf3607d 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -12,30 +12,22 @@ class Detumble { public: - Detumble(AcsParameters *acsParameters_); + Detumble(); virtual ~Detumble(); static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE; static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); - /* @brief: Load AcsParameters for this class - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - void loadAcsParameters(AcsParameters *acsParameters_); - ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, const double *magField, - const bool magFieldValid, double *magMom); + const bool magFieldValid, double *magMom, double gain); - ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom); + ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom, + double dipolMax); - ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom); - - ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField, - const bool *magFieldValid, double *magMom); + ReturnValue_t bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField, + const bool *magFieldValid, double *magMom, double gain); private: - AcsParameters::DetumbleParameter *detumbleParameter; - AcsParameters::MagnetorquesParameter *magnetorquesParameter; }; #endif /*ACS_CONTROL_DETUMBLE_H_*/ diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 74a32a4a..23f51f9e 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -1,10 +1,3 @@ -/* - * PtgCtrl.cpp - * - * Created on: 17 Jul 2022 - * Author: Robin Marquardt - */ - #include "PtgCtrl.h" #include @@ -16,16 +9,10 @@ #include "../util/MathOperations.h" -PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } +PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } PtgCtrl::~PtgCtrl() {} -void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { - inertiaEIVE = &(acsParameters_->inertiaEIVE); - rwHandlingParameters = &(acsParameters_->rwHandlingParameters); - rwMatrices = &(acsParameters_->rwMatrices); -} - void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *errorQuat, const double *deltaRate, const double *rwPseudoInv, double *torqueRws) { @@ -62,8 +49,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters gainMatrixDiagonal[0][0] = gainVector[0]; gainMatrixDiagonal[1][1] = gainVector[1]; gainMatrixDiagonal[2][2] = gainVector[2]; - MatrixOperations::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), - *gainMatrix, 3, 3, 3); + MatrixOperations::multiply( + *gainMatrixDiagonal, *(acsParameters->inertiaEIVE.inertiaMatrix), *gainMatrix, 3, 3, 3); // Inverse of gainMatrix double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -72,8 +59,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters gainMatrixInverse[2][2] = 1 / gainMatrix[2][2]; double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, - 3, 3); + MatrixOperations::multiply( + *gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrix), *pMatrix, 3, 3, 3); MatrixOperations::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3); //------------------------------------------------------------------------------------------------ @@ -91,18 +78,19 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters pErrorSign[i] = pError[i]; } } - // Torque for quaternion error + // torque for quaternion error double torqueQuat[3] = {0, 0, 0}; MatrixOperations::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1); VectorOperations::mulScalar(torqueQuat, -1, torqueQuat, 3); - // Torque for rate error + // torque for rate error double torqueRate[3] = {0, 0, 0}; - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1); + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), deltaRate, + torqueRate, 3, 3, 1); VectorOperations::mulScalar(torqueRate, cInt, torqueRate, 3); VectorOperations::mulScalar(torqueRate, -1, torqueRate, 3); - // Final commanded Torque for every reaction wheel + // final commanded Torque for every reaction wheel double torque[3] = {0, 0, 0}; VectorOperations::add(torqueRate, torqueQuat, torque, 3); MatrixOperations::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); @@ -120,20 +108,22 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP return; } - // calculating momentum of satellite and momentum of reaction wheels + // calculating momentum of satellite and momentum of reaction wheels double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; - VectorOperations::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); - MatrixOperations::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, - 1); + VectorOperations::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel, + momentumRwU, 4); + MatrixOperations::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU, + momentumRw, 3, 4, 1); double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1); + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), satRate, + momentumSat, 3, 3, 1); VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); - // calculating momentum error + // calculating momentum error double deltaMomentum[3] = {0, 0, 0}; VectorOperations::subtract(momentumTotal, pointingLawParameters->desatMomentumRef, deltaMomentum, 3); - // resulting magnetic dipole command + // resulting magnetic dipole command double crossMomentumMagField[3] = {0, 0, 0}; VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); double normMag = VectorOperations::norm(magFieldEst, 3), factor = 0; @@ -147,53 +137,50 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double wheelMomentum[4] = {0, 0, 0, 0}; double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; - // Conversion to [rad/s] for further calculations + // conversion to [rad/s] for further calculations VectorOperations::mulScalar(rpmOffset, factor, rpmOffset, 4); VectorOperations::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); double diffRwSpeed[4] = {0, 0, 0, 0}; VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); - VectorOperations::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, + VectorOperations::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, wheelMomentum, 4); double gainNs = pointingLawParameters->gainNullspace; double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, + MathOperations::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace, + acsParameters->rwMatrices.nullspace, *nullSpaceMatrix, 4); MatrixOperations::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); } -void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand) { +void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) { bool rwAvailable[4] = { (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()), (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()), (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()), (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid())}; - int32_t omegaRw[4] = {sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value, - sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value}; + int32_t currRwSpeed[4] = { + sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value, + sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value}; for (uint8_t i = 0; i < 4; i++) { if (rwAvailable[i]) { - if (torqueMemory[i] != 0) { - if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) { - torqueMemory[i] = 0; - } else { - torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; - } - } else { - if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) && - (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { - if (omegaRw[i] < omegaMemory[i]) { - torqueMemory[i] = -1; - } else { - torqueMemory[i] = 1; + if (rwCmdSpeeds[i] != 0) { + if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed && + rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) { + if (currRwSpeed[i] == 0) { + if (rwCmdSpeeds[i] > 0) { + rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; + } else if (rwCmdSpeeds[i] < 0) { + rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; + } + } else if (currRwSpeed[i] < -acsParameters->rwHandlingParameters.stictionSpeed) { + rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; + } else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) { + rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; } - - torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; } } - } else { - torqueMemory[i] = 0; } - omegaMemory[i] = omegaRw[i]; } } diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index fb27fc6d..f6c6c345 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -1,16 +1,3 @@ -/* - * PtgCtrl.h - * - * Created on: 17 Jul 2022 - * Author: Robin Marquardt - * - * @brief: This class handles the pointing control mechanism. Calculation of an commanded - * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation - * - * @note: A description of the used algorithms can be found in - * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110 - */ - #ifndef PTGCTRL_H_ #define PTGCTRL_H_ @@ -23,6 +10,13 @@ #include "eive/resultClassIds.h" class PtgCtrl { + /* + * @brief: This class handles the pointing control mechanism. Calculation of an commanded + * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation + * + * @note: A description of the used algorithms can be found in + * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110 + */ public: /* @brief: Constructor * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters @@ -33,13 +27,7 @@ class PtgCtrl { static const uint8_t INTERFACE_ID = CLASS_ID::ACS_PTG; static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); - /* @brief: Load AcsParameters for this class - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - void loadAcsParameters(AcsParameters *acsParameters_); - /* @brief: Calculates the needed torque for the pointing control mechanism - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws); @@ -54,18 +42,12 @@ class PtgCtrl { const int32_t *speedRw3, double *rwTrqNs); /* @brief: Commands the stiction torque in case wheel speed is to low - * @param: sensorValues class containing all RW related values * torqueCommand modified torque after antistiction */ - void rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand); + void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed); private: - AcsParameters::RwHandlingParameters *rwHandlingParameters; - AcsParameters::InertiaEIVE *inertiaEIVE; - AcsParameters::RwMatrices *rwMatrices; - - double torqueMemory[4] = {0, 0, 0, 0}; - double omegaMemory[4] = {0, 0, 0, 0}; + const AcsParameters *acsParameters; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index aa04cbb6..392e32ba 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -1,10 +1,3 @@ -/* - * SafeCtrl.cpp - * - * Created on: 19 Apr 2022 - * Author: Robin Marquardt - */ - #include "SafeCtrl.h" #include @@ -15,32 +8,21 @@ #include "../util/MathOperations.h" -SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { - loadAcsParameters(acsParameters_); - MatrixOperations::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3, - 3); -} +SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } SafeCtrl::~SafeCtrl() {} -void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) { - safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters); - inertiaEIVE = &(acsParameters_->inertiaEIVE); -} - ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel, bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, double *satRateMekf, bool rateMekfValid, double *sunDirRef, double *satRatRef, - double *outputAngle, double *outputMagMomB, bool *outputValid) { + double *outputAngle, double *outputMagMomB) { if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) { - *outputValid = false; return SAFECTRL_MEKF_INPUT_INVALID; } - double kRate = 0, kAlign = 0; - kRate = safeModeControllerParameters->k_rate_mekf; - kAlign = safeModeControllerParameters->k_align_mekf; + double kRate = acsParameters->safeModeControllerParameters.k_rate_mekf; + double kAlign = acsParameters->safeModeControllerParameters.k_align_mekf; // Calc sunDirB ,magFieldB with mekf output and model double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -49,22 +31,22 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, MatrixOperations::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1); MatrixOperations::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1); - double crossSun[3] = {0, 0, 0}; + // change unit from uT to T + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldB, 3); + double crossSun[3] = {0, 0, 0}; VectorOperations::cross(sunDirRef, sunDirB, crossSun); double normCrossSun = VectorOperations::norm(crossSun, 3); // calc angle alpha between sunDirRef and sunDIr - double alpha = 0, dotSun = 0; - dotSun = VectorOperations::dot(sunDirRef, sunDirB); - alpha = acos(dotSun); + double dotSun = VectorOperations::dot(sunDirRef, sunDirB); + double alpha = acos(dotSun); // Law Torque calculations double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0}, torqueAll[3] = {0, 0, 0}; - double scalarFac = 0; - scalarFac = kAlign * alpha / normCrossSun; + double scalarFac = kAlign * alpha / normCrossSun; VectorOperations::mulScalar(crossSun, scalarFac, torqueAlign, 3); double rateSafeMode[3] = {0, 0, 0}; @@ -73,6 +55,9 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, VectorOperations::add(torqueRate, torqueAlign, torqueAll, 3); // Adding factor of inertia for axes + MatrixOperations::multiplyScalar(*(acsParameters->inertiaEIVE.inertiaMatrix), 10, + *gainMatrixInertia, 3, + 3); // why only for mekf one and not for no mekf MatrixOperations::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1); // MagMom B (orthogonal torque) @@ -82,39 +67,38 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, VectorOperations::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3); *outputAngle = alpha; - *outputValid = true; - return returnvalue::OK; } // Will be the version in worst case scenario in event of no working MEKF (nor GYRs) -void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, - bool sunRateBValid, double *magFieldB, bool magFieldBValid, - double *magRateB, bool magRateBValid, double *sunDirRef, - double *satRateRef, double *outputAngle, double *outputMagMomB, - bool *outputValid) { +ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, + double *sunRateB, bool sunRateBValid, double *magFieldB, + bool magFieldBValid, double *magRateB, bool magRateBValid, + double *sunDirRef, double *satRateRef, double *outputAngle, + double *outputMagMomB) { // Check for invalid Inputs if (!susDirBValid || !magFieldBValid || !magRateBValid) { - *outputValid = false; - return; + return returnvalue::FAILED; } + // change unit from uT to T + double magFieldBT[3] = {0, 0, 0}; + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); + // normalize sunDir and magDir double magDirB[3] = {0, 0, 0}; - VectorOperations::normalize(magFieldB, magDirB, 3); + VectorOperations::normalize(magFieldBT, magDirB, 3); VectorOperations::normalize(susDirB, susDirB, 3); // Cosinus angle between sunDir and magDir double cosAngleSunMag = VectorOperations::dot(magDirB, susDirB); // Rate parallel to sun direction and magnetic field direction - double rateParaSun = 0, rateParaMag = 0; - double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0; - dotSunRateMag = VectorOperations::dot(sunRateB, magDirB); - dotmagRateSun = VectorOperations::dot(magRateB, susDirB); - rateFactor = 1 - pow(cosAngleSunMag, 2); - rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor; - rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor; + double dotSunRateMag = VectorOperations::dot(sunRateB, magDirB); + double dotmagRateSun = VectorOperations::dot(magRateB, susDirB); + double rateFactor = 1 - pow(cosAngleSunMag, 2); + double rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor; + double rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor; // Full rate or estimate double estSatRate[3] = {0, 0, 0}; @@ -129,8 +113,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl /* Only valid if angle between sun direction and magnetic field direction * is sufficiently large */ double angleSunMag = acos(cosAngleSunMag); - if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) { - return; + if (angleSunMag < acsParameters->safeModeControllerParameters.sunMagAngleMin) { + return returnvalue::FAILED; } // Rate for Torque Calculation @@ -138,9 +122,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl VectorOperations::subtract(estSatRate, satRateRef, diffRate, 3); // Torque Align calculation - double kRateNoMekf = 0, kAlignNoMekf = 0; - kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; - kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; + double kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf; + double kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf; double cosAngleAlignErr = VectorOperations::dot(sunDirRef, susDirB); double crossSusSunRef[3] = {0, 0, 0}; @@ -159,17 +142,17 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl // Final torque double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0}; VectorOperations::add(torqueRate, torqueAlign, torqueAlignRate, 3); - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, - 1); + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), torqueAlignRate, + torqueB, 3, 3, 1); // Magnetic moment double magMomB[3] = {0, 0, 0}; double crossMagFieldTorque[3] = {0, 0, 0}; - VectorOperations::cross(magFieldB, torqueB, crossMagFieldTorque); - double magMomFactor = pow(VectorOperations::norm(magFieldB, 3), 2); + VectorOperations::cross(magFieldBT, torqueB, crossMagFieldTorque); + double magMomFactor = pow(VectorOperations::norm(magFieldBT, 3), 2); VectorOperations::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3); std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double)); *outputAngle = angleAlignErr; - *outputValid = true; + return returnvalue::OK; } diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 1784f9ca..ac5a754a 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -17,25 +17,20 @@ class SafeCtrl { static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE; static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); - void loadAcsParameters(AcsParameters *acsParameters_); - ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel, bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, double *satRateMekf, bool rateMekfValid, double *sunDirRef, double *satRatRef, // From Guidance (!) - double *outputAngle, double *outputMagMomB, bool *outputValid); + double *outputAngle, double *outputMagMomB); - void safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, - bool sunRateBValid, double *magFieldB, bool magFieldBValid, double *magRateB, - bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle, - double *outputMagMomB, bool *outputValid); - - void idleSunPointing(); // with reaction wheels + ReturnValue_t safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, + bool sunRateBValid, double *magFieldB, bool magFieldBValid, + double *magRateB, bool magRateBValid, double *sunDirRef, + double *satRateRef, double *outputAngle, double *outputMagMomB); protected: private: - AcsParameters::SafeModeControllerParameters *safeModeControllerParameters; - AcsParameters::InertiaEIVE *inertiaEIVE; + AcsParameters *acsParameters; double gainMatrixInertia[3][3]; double magFieldBState[3]; diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index c3509a04..f82e75f1 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -86,6 +86,7 @@ enum PoolIds : lp_id_t { // GPS Processed GC_LATITUDE, GD_LONGITUDE, + ALTITUDE, GPS_POSITION, GPS_VELOCITY, // MEKF @@ -109,7 +110,7 @@ static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12; static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15; static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4; static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; -static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4; +static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t MEKF_SET_ENTRIES = 3; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; @@ -228,6 +229,7 @@ class GpsDataProcessed : public StaticLocalDataSet { lp_var_t gcLatitude = lp_var_t(sid.objectId, GC_LATITUDE, this); lp_var_t gdLongitude = lp_var_t(sid.objectId, GD_LONGITUDE, this); + lp_var_t altitude = lp_var_t(sid.objectId, ALTITUDE, this); lp_vec_t gpsPosition = lp_vec_t(sid.objectId, GPS_POSITION, this); lp_vec_t gpsVelocity = lp_vec_t(sid.objectId, GPS_VELOCITY, this); diff --git a/mission/core/GenericFactory.cpp b/mission/core/GenericFactory.cpp index 9e929e83..76024784 100644 --- a/mission/core/GenericFactory.cpp +++ b/mission/core/GenericFactory.cpp @@ -28,12 +28,17 @@ #include #include #include +#include #include #include #include #include #include +#include +#include +#include #include +#include #include #include "OBSWConfig.h" @@ -44,9 +49,12 @@ #include "mission/system/objects/RwAssembly.h" #include "mission/system/tree/acsModeTree.h" #include "mission/system/tree/tcsModeTree.h" +#include "mission/tmtc/tmFilters.h" #include "objects/systemObjectList.h" #include "tmtc/pusIds.h" +using persTmStore::PersistentTmStores; + #if OBSW_ADD_TCPIP_SERVERS == 1 #if OBSW_ADD_TMTC_UDP_SERVER == 1 // UDP server includes @@ -84,9 +92,11 @@ EiveFaultHandler EIVE_FAULT_HANDLER; } // namespace cfdp void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFunnel** pusFunnel, - CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan) { + CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan, + StorageManagerIF** ipcStore, StorageManagerIF** tmStore, + PersistentTmStores& stores) { // Framework objects - new EventManager(objects::EVENT_MANAGER); + new EventManager(objects::EVENT_MANAGER, 120); auto healthTable = new HealthTable(objects::HEALTH_TABLE); if (healthTable_ != nullptr) { *healthTable_ = healthTable; @@ -95,8 +105,6 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun new VerificationReporter(); auto* timeStamper = new CdsShortTimeStamper(objects::TIME_STAMPER); StorageManagerIF* tcStore; - StorageManagerIF* tmStore; - StorageManagerIF* ipcStore; { PoolManager::LocalPoolConfig poolCfg = {{250, 16}, {250, 32}, {250, 64}, {150, 128}, {120, 1024}, {120, 2048}}; @@ -104,27 +112,32 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun } { - PoolManager::LocalPoolConfig poolCfg = {{400, 32}, {400, 64}, {250, 128}, - {150, 512}, {150, 1024}, {150, 2048}}; - tmStore = new PoolManager(objects::TM_STORE, poolCfg); + PoolManager::LocalPoolConfig poolCfg = {{300, 32}, {300, 32}, {400, 64}, {250, 128}, + {150, 512}, {150, 1024}, {150, 1024}, {150, 2048}}; + *tmStore = new PoolManager(objects::TM_STORE, poolCfg); } { PoolManager::LocalPoolConfig poolCfg = {{300, 16}, {200, 32}, {150, 64}, {150, 128}, {100, 256}, {50, 512}, {50, 1024}, {10, 2048}}; - ipcStore = new PoolManager(objects::IPC_STORE, poolCfg); + *ipcStore = new PoolManager(objects::IPC_STORE, poolCfg); } + PoolManager::LocalPoolConfig poolCfg = {{300, 32}, {400, 64}, {250, 128}, + {150, 512}, {150, 1024}, {150, 2048}}; + auto* ramToFileStore = new PoolManager(objects::DOWNLINK_RAM_STORE, poolCfg); #if OBSW_ADD_TCPIP_SERVERS == 1 #if OBSW_ADD_TMTC_UDP_SERVER == 1 - auto udpBridge = new UdpTmTcBridge(objects::UDP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR); + auto udpBridge = + new UdpTmTcBridge(objects::UDP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR, 50); new UdpTcPollingTask(objects::UDP_TMTC_POLLING_TASK, objects::UDP_TMTC_SERVER); sif::info << "Created UDP server for TMTC commanding with listener port " << udpBridge->getUdpPort() << std::endl; udpBridge->setMaxNumberOfPacketsStored(config::MAX_STORED_CMDS_UDP); #endif #if OBSW_ADD_TMTC_TCP_SERVER == 1 - auto tcpBridge = new TcpTmTcBridge(objects::TCP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR); + auto tcpBridge = + new TcpTmTcBridge(objects::TCP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR, 50); TcpTmTcServer::TcpConfig cfg(true, true); auto tcpServer = new TcpTmTcServer(objects::TCP_TMTC_POLLING_TASK, objects::TCP_TMTC_SERVER, cfg); // TCP is stream based. Use packet ID as start marker when parsing for space packets @@ -139,34 +152,93 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun new CcsdsDistributor(config::EIVE_PUS_APID, objects::CCSDS_PACKET_DISTRIBUTOR); new PusDistributor(config::EIVE_PUS_APID, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib); - PusTmFunnel::FunnelCfg cfdpFunnelCfg(objects::CFDP_TM_FUNNEL, *tmStore, *ipcStore, 50); - *cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, config::EIVE_CFDP_APID); - PusTmFunnel::FunnelCfg pusFunnelCfg(objects::PUS_TM_FUNNEL, *tmStore, *ipcStore, + PusTmFunnel::FunnelCfg pusFunnelCfg(objects::PUS_TM_FUNNEL, "PusTmFunnel", **tmStore, **ipcStore, config::MAX_PUS_FUNNEL_QUEUE_DEPTH); - *pusFunnel = new PusTmFunnel(pusFunnelCfg, *timeStamper, sdcMan); + // The PUS funnel routes all live TM to the live destinations and to the TM stores. + *pusFunnel = new PusTmFunnel(pusFunnelCfg, *ramToFileStore, *timeStamper, sdcMan); + + // MISC store and PUS funnel to MISC store routing + { + PersistentTmStoreArgs storeArgs(objects::MISC_TM_STORE, "tm", "misc", + + RolloverInterval::HOURLY, 2, *ramToFileStore, sdcMan); + stores.miscStore = + new PersistentTmStoreWithTmQueue(storeArgs, "MISC STORE", config::MISC_STORE_QUEUE_SIZE); + (*pusFunnel) + ->addPersistentTmStoreRouting(filters::miscFilter(), + stores.miscStore->getReportReceptionQueue(0)); + } + + // OK store and PUS Funnel to OK store routing + { + PersistentTmStoreArgs storeArgs(objects::OK_TM_STORE, "tm", "ok", RolloverInterval::MINUTELY, + 30, *ramToFileStore, sdcMan); + stores.okStore = + new PersistentTmStoreWithTmQueue(storeArgs, "OK STORE", config::OK_STORE_QUEUE_SIZE); + (*pusFunnel) + ->addPersistentTmStoreRouting(filters::okFilter(), + stores.okStore->getReportReceptionQueue(0)); + } + + // NOT OK store and PUS funnel to NOT OK store routing + { + PersistentTmStoreArgs storeArgs(objects::NOT_OK_TM_STORE, "tm", "nok", + RolloverInterval::MINUTELY, 30, *ramToFileStore, sdcMan); + stores.notOkStore = + new PersistentTmStoreWithTmQueue(storeArgs, "NOT OK STORE", config::NOK_STORE_QUEUE_SIZE); + (*pusFunnel) + ->addPersistentTmStoreRouting(filters::notOkFilter(), + stores.notOkStore->getReportReceptionQueue(0)); + } + + // HK store and PUS funnel to HK store routing + { + PersistentTmStoreArgs storeArgs(objects::HK_TM_STORE, "tm", "hk", RolloverInterval::MINUTELY, + 15, *ramToFileStore, sdcMan); + stores.hkStore = + new PersistentTmStoreWithTmQueue(storeArgs, "HK STORE", config::HK_STORE_QUEUE_SIZE); + (*pusFunnel) + ->addPersistentTmStoreRouting(filters::hkFilter(), + stores.hkStore->getReportReceptionQueue(0)); + } + + // CFDP store and PUS funnel to CFDP store routing + { + PersistentTmStoreArgs storeArgs(objects::CFDP_TM_STORE, "tm", "cfdp", + RolloverInterval::MINUTELY, 30, *ramToFileStore, sdcMan); + stores.cfdpStore = + new PersistentTmStoreWithTmQueue(storeArgs, "CFDP STORE", config::CFDP_STORE_QUEUE_SIZE); + + (*pusFunnel) + ->addPersistentTmStoreRouting(filters::cfdpFilter(), + stores.cfdpStore->getReportReceptionQueue(0)); + } + PusTmFunnel::FunnelCfg cfdpFunnelCfg(objects::CFDP_TM_FUNNEL, "CfdpTmFunnel", **tmStore, + **ipcStore, config::MAX_CFDP_FUNNEL_QUEUE_DEPTH); + *cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, stores.cfdpStore->getReportReceptionQueue(0), + *ramToFileStore, config::EIVE_CFDP_APID); + #if OBSW_ADD_TCPIP_SERVERS == 1 #if OBSW_ADD_TMTC_UDP_SERVER == 1 - (*cfdpFunnel)->addDestination("UDP Server", *udpBridge, 0); - (*pusFunnel)->addDestination("UDP Server", *udpBridge, 0); + (*cfdpFunnel)->addLiveDestination("UDP Server", *udpBridge, 0); + (*pusFunnel)->addLiveDestination("UDP Server", *udpBridge, 0); #endif #if OBSW_ADD_TMTC_TCP_SERVER == 1 - (*cfdpFunnel)->addDestination("TCP Server", *tcpBridge, 0); - (*pusFunnel)->addDestination("TCP Server", *tcpBridge, 0); + (*cfdpFunnel)->addLiveDestination("TCP Server", *tcpBridge, 0); + (*pusFunnel)->addLiveDestination("TCP Server", *tcpBridge, 0); #endif #endif - // Every TM packet goes through this funnel - new TmFunnelHandler(objects::TM_FUNNEL, **pusFunnel, **cfdpFunnel); // PUS service stack new Service1TelecommandVerification(objects::PUS_SERVICE_1_VERIFICATION, config::EIVE_PUS_APID, - pus::PUS_SERVICE_1, objects::PUS_TM_FUNNEL, 20); + pus::PUS_SERVICE_1, objects::PUS_TM_FUNNEL, 40); new Service2DeviceAccess(objects::PUS_SERVICE_2_DEVICE_ACCESS, config::EIVE_PUS_APID, pus::PUS_SERVICE_2, 3, 10); new Service3Housekeeping(objects::PUS_SERVICE_3_HOUSEKEEPING, config::EIVE_PUS_APID, pus::PUS_SERVICE_3); new Service5EventReporting( PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5), - 15, 45); + 40, 120); new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, config::EIVE_PUS_APID, pus::PUS_SERVICE_8, 16, 60); new Service9TimeManagement( @@ -194,7 +266,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun 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); @@ -221,7 +293,7 @@ void ObjectFactory::createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& {new HealthDevice(objects::HEATER_4_CAMERA, MessageQueueIF::NO_QUEUE), gpioIds::HEATER_4}, {new HealthDevice(objects::HEATER_5_STR, MessageQueueIF::NO_QUEUE), gpioIds::HEATER_5}, {new HealthDevice(objects::HEATER_6_DRO, MessageQueueIF::NO_QUEUE), gpioIds::HEATER_6}, - {new HealthDevice(objects::HEATER_7_HPA, MessageQueueIF::NO_QUEUE), gpioIds::HEATER_7}, + {new HealthDevice(objects::HEATER_7_SYRLINKS, MessageQueueIF::NO_QUEUE), gpioIds::HEATER_7}, }}); heaterHandler = new HeaterHandler(objects::HEATER_HANDLER, &gpioIF, helper, &pwrSwitcher, pcdu::Switches::PDU2_CH3_TCS_BOARD_HEATER_IN_8V); @@ -236,7 +308,7 @@ void ObjectFactory::createRwAssy(PowerSwitchIF& pwrSwitcher, power::Switch_t the std::array rws, std::array rwIds) { RwHelper rwHelper(rwIds); - auto* rwAss = new RwAssembly(objects::RW_ASS, &pwrSwitcher, theSwitch, rwHelper); + auto* rwAss = new RwAssembly(objects::RW_ASSY, &pwrSwitcher, theSwitch, rwHelper); for (size_t idx = 0; idx < rwIds.size(); idx++) { ReturnValue_t result = rws[idx]->connectModeTreeParent(*rwAss); if (result != returnvalue::OK) { diff --git a/mission/core/GenericFactory.h b/mission/core/GenericFactory.h index 5902ff7b..1c5e9ce6 100644 --- a/mission/core/GenericFactory.h +++ b/mission/core/GenericFactory.h @@ -3,12 +3,16 @@ #include #include +#include +#include #include "fsfw/objectmanager/SystemObjectIF.h" #include "fsfw/power/PowerSwitchIF.h" #include "fsfw_hal/common/gpio/GpioIF.h" #include "mission/devices/devicedefinitions/Max31865Definitions.h" +using persTmStore::PersistentTmStores; + class HeaterHandler; class HealthTableIF; class PusTmFunnel; @@ -38,7 +42,9 @@ const std::array, EiveMax31855::NUM_RTDS> RT namespace ObjectFactory { void produceGenericObjects(HealthTableIF** healthTable, PusTmFunnel** pusFunnel, - CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan); + CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan, + StorageManagerIF** ipcStore, StorageManagerIF** tmStore, + PersistentTmStores& stores); void createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& pwrSwitcher, HeaterHandler*& heaterHandler); diff --git a/mission/core/pollingSeqTables.cpp b/mission/core/pollingSeqTables.cpp index 0b134d87..88b5b95a 100644 --- a/mission/core/pollingSeqTables.cpp +++ b/mission/core/pollingSeqTables.cpp @@ -30,22 +30,6 @@ ReturnValue_t pst::pstSpiAndSyrlinks(FixedTimeslotTaskIF *thisSequence) { #endif static_cast(length); -#if OBSW_ADD_PL_PCDU == 1 - thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::GET_READ); -#endif - -#if OBSW_ADD_RAD_SENSORS == 1 - /* Radiation sensor */ - thisSequence->addSlot(objects::RAD_SENSOR, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RAD_SENSOR, length * 0.2, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RAD_SENSOR, length * 0.4, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RAD_SENSOR, length * 0.6, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RAD_SENSOR, length * 0.8, DeviceHandlerIF::GET_READ); -#endif return thisSequence->checkSequence(); } @@ -189,390 +173,155 @@ ReturnValue_t pst::pstTest(FixedTimeslotTaskIF *thisSequence) { ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg) { /* Length of a communication cycle */ uint32_t length = thisSequence->getPeriodMs(); - bool enableAside = true; - bool enableBside = true; - if (cfg.scheduleAcsBoard) { - if (enableAside) { - // A side - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_READ); - } - if (enableBside) { - // B side - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_READ); - } - } // SUS: 16 ms - bool addSus0 = true; - bool addSus1 = true; - bool addSus2 = true; - bool addSus3 = true; - bool addSus4 = true; - bool addSus5 = true; - bool addSus6 = true; - bool addSus7 = true; - bool addSus8 = true; - bool addSus9 = true; - bool addSus10 = true; - bool addSus11 = true; - if (cfg.scheduleSus) { - if (addSus0) { - /* Write setup */ - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, - DeviceHandlerIF::GET_READ); + /* Write setup */ + thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, + DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, - length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); - } - if (addSus1) { - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, - length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); - } - if (addSus2) { - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, - length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); - } - if (addSus3) { - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, - length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); - } - if (addSus4) { - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, - length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); - } - if (addSus5) { - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, - length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); - } - if (addSus6) { - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, - length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); - } - if (addSus7) { - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, - length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); - } - if (addSus8) { - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, - length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); - } - if (addSus9) { - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, - length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); - } - if (addSus10) { - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, - length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); - } - if (addSus11) { - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, - length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); - } + thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, + length * config::spiSched::SCHED_BLOCK_1_PERIOD, + DeviceHandlerIF::GET_READ); } if (cfg.scheduleStr) { @@ -583,83 +332,250 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_READ); } + bool enableAside = true; + bool enableBside = true; + if (cfg.scheduleAcsBoard) { + if (enableAside) { + // A side + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_READ); + } + if (enableBside) { + // B side + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_READ); + } + if (enableAside) { + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_READ); + } + if (enableBside) { + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, + length * config::spiSched::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, + length * config::spiSched::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_READ); + } + } + if (cfg.scheduleImtq) { // This is the MTM measurement cycle - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_1_PERIOD, imtq::ComStep::DHB_OP); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_1_PERIOD, imtq::ComStep::START_MEASURE_SEND); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_1_PERIOD, imtq::ComStep::START_MEASURE_GET); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD, imtq::ComStep::READ_MEASURE_SEND); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD, imtq::ComStep::READ_MEASURE_GET); } - thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::acs::SCHED_BLOCK_4_PERIOD, 0); + thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::spiSched::SCHED_BLOCK_4_PERIOD, + 0); if (cfg.scheduleImtq) { // This is the torquing cycle. - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD, + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_5_PERIOD, imtq::ComStep::START_ACTUATE_SEND); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD, + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_5_PERIOD, imtq::ComStep::START_ACTUATE_GET); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD, + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_6_PERIOD, imtq::ComStep::READ_ACTUATE_SEND); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD, + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_6_PERIOD, imtq::ComStep::READ_ACTUATE_GET); } if (cfg.scheduleRws) { // this is the torquing cycle - thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD, + thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD, + thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD, + thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD, + thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD, + thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD, + thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD, + thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD, + thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD, + thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD, + thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD, + thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD, + thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_7_PERIOD, + thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_7_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_7_PERIOD, + thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_7_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_7_PERIOD, + thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_7_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_7_PERIOD, + thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_7_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_7_PERIOD, + thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_7_PERIOD, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_7_PERIOD, + thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_7_PERIOD, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_7_PERIOD, + thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_7_PERIOD, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_7_PERIOD, + thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_7_PERIOD, DeviceHandlerIF::GET_READ); } - thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::acs::SCHED_BLOCK_RTD_PERIOD, 0); + thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::spiSched::SCHED_BLOCK_RTD_PERIOD, + 0); +#if OBSW_ADD_PL_PCDU == 1 + thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, + DeviceHandlerIF::GET_READ); +#endif + +#if OBSW_ADD_RAD_SENSORS == 1 + /* Radiation sensor */ + thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, + DeviceHandlerIF::GET_READ); +#endif return returnvalue::OK; } diff --git a/mission/devices/CMakeLists.txt b/mission/devices/CMakeLists.txt index ce02c688..cb6c066a 100644 --- a/mission/devices/CMakeLists.txt +++ b/mission/devices/CMakeLists.txt @@ -14,7 +14,10 @@ target_sources( ImtqHandler.cpp HeaterHandler.cpp RadiationSensorHandler.cpp - GyroADIS1650XHandler.cpp + GyrAdis1650XHandler.cpp + GyrL3gCustomHandler.cpp + MgmRm3100CustomHandler.cpp + MgmLis3CustomHandler.cpp RwHandler.cpp max1227.cpp SusHandler.cpp diff --git a/mission/devices/GyrAdis1650XHandler.cpp b/mission/devices/GyrAdis1650XHandler.cpp new file mode 100644 index 00000000..73e9a0cd --- /dev/null +++ b/mission/devices/GyrAdis1650XHandler.cpp @@ -0,0 +1,226 @@ +#include "mission/devices/GyrAdis1650XHandler.h" + +#include "fsfw/action/HasActionsIF.h" +#include "fsfw/datapool/PoolReadGuard.h" +#include "mission/devices/devicedefinitions/acsPolling.h" + +GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie, adis1650x::Type type) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie), + adisType(type), + primaryDataset(this), + configDataset(this), + breakCountdown() { + adisRequest.type = adisType; +} + +void GyrAdis1650XHandler::doStartUp() { + if (internalState != InternalState::STARTUP) { + internalState = InternalState::STARTUP; + commandExecuted = false; + } + // Initial 310 ms start up time after power-up + if (internalState == InternalState::STARTUP) { + if (not commandExecuted) { + warningSwitch = true; + breakCountdown.setTimeout(adis1650x::START_UP_TIME); + commandExecuted = true; + } + if (breakCountdown.hasTimedOut()) { + updatePeriodicReply(true, adis1650x::REPLY); + if (goToNormalMode) { + setMode(MODE_NORMAL); + } else { + setMode(MODE_ON); + } + internalState = InternalState::NONE; + } + } +} + +void GyrAdis1650XHandler::doShutDown() { + if (internalState != InternalState::SHUTDOWN) { + commandExecuted = false; + primaryDataset.setValidity(false, true); + internalState = InternalState::SHUTDOWN; + } + if (internalState == InternalState::SHUTDOWN and commandExecuted) { + updatePeriodicReply(false, adis1650x::REPLY); + internalState = InternalState::NONE; + commandExecuted = false; + setMode(MODE_OFF); + } +} + +ReturnValue_t GyrAdis1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = adis1650x::REQUEST; + return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL); +} + +ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + switch (internalState) { + case (InternalState::STARTUP): { + *id = adis1650x::REQUEST; + return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL); + } + case (InternalState::SHUTDOWN): { + *id = adis1650x::REQUEST; + acs::Adis1650XRequest *request = reinterpret_cast(cmdBuf.data()); + request->mode = acs::SimpleSensorMode::OFF; + request->type = adisType; + return returnvalue::OK; + } + default: { + return NOTHING_TO_SEND; + } + } + return returnvalue::OK; +} + +ReturnValue_t GyrAdis1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, + size_t commandDataLen) { + return NOTHING_TO_SEND; +} + +void GyrAdis1650XHandler::fillCommandAndReplyMap() { + insertInCommandMap(adis1650x::REQUEST); + insertInReplyMap(adis1650x::REPLY, 5, nullptr, 0, true); +} + +ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize, + DeviceCommandId_t *foundId, size_t *foundLen) { + if (breakCountdown.isBusy() or getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON or + getMode() == _MODE_POWER_DOWN) { + return IGNORE_FULL_PACKET; + } + if (remainingSize != sizeof(acs::Adis1650XReply)) { + *foundLen = remainingSize; + return returnvalue::FAILED; + } + *foundId = adis1650x::REPLY; + *foundLen = remainingSize; + if (internalState == InternalState::SHUTDOWN) { + commandExecuted = true; + } + return returnvalue::OK; +} + +ReturnValue_t GyrAdis1650XHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + using namespace adis1650x; + const acs::Adis1650XReply *reply = reinterpret_cast(packet); + if (internalState == InternalState::STARTUP and reply->cfgWasSet) { + switch (adisType) { + case (adis1650x::Type::ADIS16507): { + if (reply->cfg.prodId != adis1650x::PROD_ID_16507) { + sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID " + << reply->cfg.prodId << "for ADIS type 16507" << std::endl; + return returnvalue::FAILED; + } + break; + } + case (adis1650x::Type::ADIS16505): { + if (reply->cfg.prodId != adis1650x::PROD_ID_16505) { + sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID " + << reply->cfg.prodId << "for ADIS type 16505" << std::endl; + return returnvalue::FAILED; + } + break; + } + } + PoolReadGuard rg(&configDataset); + configDataset.setValidity(true, true); + configDataset.mscCtrlReg = reply->cfg.mscCtrlReg; + configDataset.rangMdl = reply->cfg.rangMdl; + configDataset.diagStatReg = reply->cfg.diagStat; + configDataset.filterSetting = reply->cfg.filterSetting; + configDataset.decRateReg = reply->cfg.decRateReg; + commandExecuted = true; + } + if (reply->dataWasSet) { + { + PoolReadGuard rg(&configDataset); + configDataset.diagStatReg = reply->cfg.diagStat; + } + auto sensitivity = reply->data.sensitivity; + auto accelSensitivity = reply->data.accelScaling; + PoolReadGuard pg(&primaryDataset); + primaryDataset.setValidity(true, true); + primaryDataset.angVelocX = static_cast(reply->data.angVelocities[0]) * sensitivity; + primaryDataset.angVelocY = static_cast(reply->data.angVelocities[1]) * sensitivity; + primaryDataset.angVelocZ = static_cast(reply->data.angVelocities[2]) * sensitivity; + // TODO: Check whether we need to divide by INT16_MAX + primaryDataset.accelX = static_cast(reply->data.accelerations[0]) * accelSensitivity; + primaryDataset.accelY = static_cast(reply->data.accelerations[1]) * accelSensitivity; + primaryDataset.accelZ = static_cast(reply->data.accelerations[2]) * accelSensitivity; + primaryDataset.temperature.value = static_cast(reply->data.temperatureRaw) * 0.1; + } + return returnvalue::OK; +} + +LocalPoolDataSetBase *GyrAdis1650XHandler::getDataSetHandle(sid_t sid) { + if (sid.ownerSetId == adis1650x::ADIS_DATASET_ID) { + return &primaryDataset; + } else if (sid.ownerSetId == adis1650x::ADIS_CFG_DATASET_ID) { + return &configDataset; + } + return nullptr; +} + +uint32_t GyrAdis1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; } + +void GyrAdis1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne, + uint8_t valueTwo) { + uint8_t secondReg = startReg + 1; + startReg |= adis1650x::WRITE_MASK; + secondReg |= adis1650x::WRITE_MASK; + cmdBuf[0] = startReg; + cmdBuf[1] = valueOne; + cmdBuf[2] = secondReg; + cmdBuf[3] = valueTwo; + this->rawPacketLen = 4; + this->rawPacket = cmdBuf.data(); +} + +ReturnValue_t GyrAdis1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(adis1650x::ANG_VELOC_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::TEMPERATURE, new PoolEntry({0.0})); + + localDataPoolMap.emplace(adis1650x::DIAG_STAT_REGISTER, new PoolEntry()); + localDataPoolMap.emplace(adis1650x::FILTER_SETTINGS, new PoolEntry()); + localDataPoolMap.emplace(adis1650x::RANG_MDL, &rangMdl); + localDataPoolMap.emplace(adis1650x::MSC_CTRL_REGISTER, new PoolEntry()); + localDataPoolMap.emplace(adis1650x::DEC_RATE_REGISTER, new PoolEntry()); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0)); + return returnvalue::OK; +} + +adis1650x::BurstModes GyrAdis1650XHandler::getBurstMode() { + using namespace adis1650x; + configDataset.mscCtrlReg.read(); + uint16_t currentCtrlReg = configDataset.mscCtrlReg.value; + configDataset.mscCtrlReg.commit(); + return adis1650x::burstModeFromMscCtrl(currentCtrlReg); +} + +void GyrAdis1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; } + +void GyrAdis1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} + +ReturnValue_t GyrAdis1650XHandler::preparePeriodicRequest(acs::SimpleSensorMode mode) { + adisRequest.mode = mode; + rawPacket = reinterpret_cast(&adisRequest); + rawPacketLen = sizeof(acs::Adis1650XRequest); + return returnvalue::OK; +} diff --git a/mission/devices/GyroADIS1650XHandler.h b/mission/devices/GyrAdis1650XHandler.h similarity index 70% rename from mission/devices/GyroADIS1650XHandler.h rename to mission/devices/GyrAdis1650XHandler.h index d348baa2..020dcd6e 100644 --- a/mission/devices/GyroADIS1650XHandler.h +++ b/mission/devices/GyrAdis1650XHandler.h @@ -1,27 +1,24 @@ #ifndef MISSION_DEVICES_GYROADIS16507HANDLER_H_ #define MISSION_DEVICES_GYROADIS16507HANDLER_H_ +#include +#include + #include "FSFWConfig.h" #include "OBSWConfig.h" -#include "devicedefinitions/GyroADIS1650XDefinitions.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" #include "fsfw/globalfunctions/PeriodicOperationDivider.h" -#ifdef FSFW_OSAL_LINUX -class SpiComIF; -class SpiCookie; -#endif - /** * @brief Device handle for the ADIS16507 Gyroscope by Analog Devices * @details * Flight manual: * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/ADIS16507_Gyro */ -class GyroADIS1650XHandler : public DeviceHandlerBase { +class GyrAdis1650XHandler : public DeviceHandlerBase { public: - GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, - ADIS1650X::Type type); + GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, + adis1650x::Type type); void enablePeriodicPrintouts(bool enable, uint8_t divider); void setToGoToNormalModeImmediately(); @@ -40,42 +37,31 @@ class GyroADIS1650XHandler : public DeviceHandlerBase { uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; private: - std::array commandBuffer; - ADIS1650X::Type adisType; + std::array cmdBuf; + acs::Adis1650XRequest adisRequest; + adis1650x::Type adisType; AdisGyroPrimaryDataset primaryDataset; AdisGyroConfigDataset configDataset; - double sensitivity = ADIS1650X::SENSITIVITY_UNSET; + double sensitivity = adis1650x::SENSITIVITY_UNSET; bool goToNormalMode = false; bool warningSwitch = true; - enum class InternalState { STARTUP, CONFIG, IDLE }; - - enum class BurstModes { - BURST_16_BURST_SEL_0, - BURST_16_BURST_SEL_1, - BURST_32_BURST_SEL_0, - BURST_32_BURST_SEL_1 - }; + enum class InternalState { NONE, STARTUP, SHUTDOWN }; InternalState internalState = InternalState::STARTUP; bool commandExecuted = false; PoolEntry rangMdl = PoolEntry(); - void prepareReadCommand(uint8_t *regList, size_t len); - - BurstModes getBurstMode(); - -#ifdef FSFW_OSAL_LINUX - static ReturnValue_t spiSendCallback(SpiComIF *comIf, SpiCookie *cookie, const uint8_t *sendData, - size_t sendLen, void *args); -#endif + adis1650x::BurstModes getBurstMode(); Countdown breakCountdown; void prepareWriteCommand(uint8_t startReg, uint8_t valueOne, uint8_t valueTwo); + ReturnValue_t preparePeriodicRequest(acs::SimpleSensorMode mode); ReturnValue_t handleSensorData(const uint8_t *packet); diff --git a/mission/devices/GyrL3gCustomHandler.cpp b/mission/devices/GyrL3gCustomHandler.cpp new file mode 100644 index 00000000..d3624bdf --- /dev/null +++ b/mission/devices/GyrL3gCustomHandler.cpp @@ -0,0 +1,191 @@ + +#include + +#include + +#include "fsfw/datapool/PoolReadGuard.h" +#include "mission/devices/devicedefinitions/acsPolling.h" + +GyrL3gCustomHandler::GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie, uint32_t transitionDelayMs) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie), + transitionDelayMs(transitionDelayMs), + dataset(this) {} + +GyrL3gCustomHandler::~GyrL3gCustomHandler() = default; + +void GyrL3gCustomHandler::doStartUp() { + if (internalState != InternalState::STARTUP) { + internalState = InternalState::STARTUP; + updatePeriodicReply(true, l3gd20h::REPLY); + commandExecuted = false; + } + + if (internalState == InternalState::STARTUP) { + if (commandExecuted) { + if (goNormalModeImmediately) { + setMode(MODE_NORMAL); + } else { + setMode(_MODE_TO_ON); + } + internalState = InternalState::NONE; + commandExecuted = false; + } + } +} + +void GyrL3gCustomHandler::doShutDown() { + if (internalState != InternalState::SHUTDOWN) { + internalState = InternalState::SHUTDOWN; + dataset.setValidity(false, true); + commandExecuted = false; + } + if (internalState == InternalState::SHUTDOWN and commandExecuted) { + internalState = InternalState::NONE; + updatePeriodicReply(false, l3gd20h::REPLY); + commandExecuted = false; + setMode(MODE_OFF); + } +} + +ReturnValue_t GyrL3gCustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + switch (internalState) { + case (InternalState::NONE): + case (InternalState::NORMAL): { + return NOTHING_TO_SEND; + } + case (InternalState::STARTUP): { + *id = l3gd20h::REQUEST; + gyrRequest.ctrlRegs[0] = l3gd20h::CTRL_REG_1_VAL; + gyrRequest.ctrlRegs[1] = l3gd20h::CTRL_REG_2_VAL; + gyrRequest.ctrlRegs[2] = l3gd20h::CTRL_REG_3_VAL; + gyrRequest.ctrlRegs[3] = l3gd20h::CTRL_REG_4_VAL; + gyrRequest.ctrlRegs[4] = l3gd20h::CTRL_REG_5_VAL; + return doPeriodicRequest(acs::SimpleSensorMode::NORMAL); + } + case (InternalState::SHUTDOWN): { + *id = l3gd20h::REQUEST; + return doPeriodicRequest(acs::SimpleSensorMode::OFF); + } + default: +#if FSFW_CPP_OSTREAM_ENABLED == 1 + /* Might be a configuration error. */ + sif::warning << "GyroL3GD20Handler::buildTransitionDeviceCommand: " + "Unknown internal state!" + << std::endl; +#else + sif::printDebug( + "GyroL3GD20Handler::buildTransitionDeviceCommand: " + "Unknown internal state!\n"); +#endif + return returnvalue::OK; + } + return returnvalue::OK; +} + +ReturnValue_t GyrL3gCustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = l3gd20h::REQUEST; + return doPeriodicRequest(acs::SimpleSensorMode::NORMAL); +} + +ReturnValue_t GyrL3gCustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, + size_t commandDataLen) { + switch (deviceCommand) { + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return returnvalue::OK; +} + +ReturnValue_t GyrL3gCustomHandler::scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) { + if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON or getMode() == _MODE_POWER_DOWN) { + return IGNORE_FULL_PACKET; + } + if (len != sizeof(acs::GyroL3gReply)) { + *foundLen = len; + return returnvalue::FAILED; + } + *foundId = l3gd20h::REPLY; + *foundLen = len; + if (internalState == InternalState::SHUTDOWN) { + commandExecuted = true; + } + return returnvalue::OK; +} + +ReturnValue_t GyrL3gCustomHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + const acs::GyroL3gReply *reply = reinterpret_cast(packet); + if (reply->cfgWasSet) { + if (internalState == InternalState::STARTUP) { + commandExecuted = true; + } + PoolReadGuard pg(&dataset); + dataset.setValidity(true, true); + dataset.angVelocX = static_cast(reply->angVelocities[0]) * reply->sensitivity; + dataset.angVelocY = static_cast(reply->angVelocities[1]) * reply->sensitivity; + dataset.angVelocZ = static_cast(reply->angVelocities[2]) * reply->sensitivity; + if (std::abs(dataset.angVelocX.value) > absLimitX) { + dataset.angVelocX.setValid(false); + } + if (std::abs(dataset.angVelocY.value) > absLimitY) { + dataset.angVelocY.setValid(false); + } + if (std::abs(dataset.angVelocZ.value) > absLimitZ) { + dataset.angVelocZ.setValid(false); + } + dataset.temperature = 25.0 - reply->tempOffsetRaw; + } + return returnvalue::OK; +} + +uint32_t GyrL3gCustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { + return this->transitionDelayMs; +} + +void GyrL3gCustomHandler::setToGoToNormalMode(bool enable) { this->goNormalModeImmediately = true; } + +ReturnValue_t GyrL3gCustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry({0.0})); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(dataset.getSid(), false, 10.0)); + return returnvalue::OK; +} + +void GyrL3gCustomHandler::fillCommandAndReplyMap() { + insertInCommandMap(l3gd20h::REQUEST); + insertInReplyMap(l3gd20h::REPLY, 5, nullptr, 0, true); +} + +void GyrL3gCustomHandler::modeChanged() { internalState = InternalState::NONE; } + +void GyrL3gCustomHandler::setAbsoluteLimits(float limitX, float limitY, float limitZ) { + this->absLimitX = limitX; + this->absLimitY = limitY; + this->absLimitZ = limitZ; +} + +void GyrL3gCustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} + +LocalPoolDataSetBase *GyrL3gCustomHandler::getDataSetHandle(sid_t sid) { + if (sid == dataset.getSid()) { + return &dataset; + } + return nullptr; +} + +ReturnValue_t GyrL3gCustomHandler::doPeriodicRequest(acs::SimpleSensorMode mode) { + gyrRequest.mode = mode; + rawPacket = reinterpret_cast(&gyrRequest); + rawPacketLen = sizeof(acs::GyroL3gRequest); + return returnvalue::OK; +} diff --git a/mission/devices/GyrL3gCustomHandler.h b/mission/devices/GyrL3gCustomHandler.h new file mode 100644 index 00000000..5f840cfc --- /dev/null +++ b/mission/devices/GyrL3gCustomHandler.h @@ -0,0 +1,91 @@ +#ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_ +#define MISSION_DEVICES_GYROL3GD20HANDLER_H_ + +#include +#include +#include +#include + +/** + * @brief Device Handler for the L3GD20H gyroscope sensor + * (https://www.st.com/en/mems-and-sensors/l3gd20h.html) + * @details + * Advanced documentation: + * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/L3GD20H_Gyro + * + * Data is read big endian with the smallest possible range of 245 degrees per second. + */ +class GyrL3gCustomHandler : public DeviceHandlerBase { + public: + GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, + uint32_t transitionDelayMs); + virtual ~GyrL3gCustomHandler(); + + void enablePeriodicPrintouts(bool enable, uint8_t divider); + + /** + * Set the absolute limit for the values on the axis in degrees per second. + * The dataset values will be marked as invalid if that limit is exceeded + * @param xLimit + * @param yLimit + * @param zLimit + */ + void setAbsoluteLimits(float limitX, float limitY, float limitZ); + + /** + * @brief Configure device handler to go to normal mode immediately + */ + void setToGoToNormalMode(bool enable); + + protected: + /* DeviceHandlerBase overrides */ + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override; + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) override; + virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + + void fillCommandAndReplyMap() override; + void modeChanged() override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; + + private: + uint32_t transitionDelayMs = 0; + GyroPrimaryDataset dataset; + + float absLimitX = l3gd20h::RANGE_DPS_00; + float absLimitY = l3gd20h::RANGE_DPS_00; + float absLimitZ = l3gd20h::RANGE_DPS_00; + + enum class InternalState { NONE, STARTUP, SHUTDOWN, NORMAL }; + InternalState internalState = InternalState::NONE; + bool commandExecuted = false; + + uint8_t statusReg = 0; + bool goNormalModeImmediately = false; + + uint8_t ctrlReg1Value = l3gd20h::CTRL_REG_1_VAL; + uint8_t ctrlReg2Value = l3gd20h::CTRL_REG_2_VAL; + uint8_t ctrlReg3Value = l3gd20h::CTRL_REG_3_VAL; + uint8_t ctrlReg4Value = l3gd20h::CTRL_REG_4_VAL; + uint8_t ctrlReg5Value = l3gd20h::CTRL_REG_5_VAL; + + acs::GyroL3gRequest gyrRequest{}; + + // Set default value + float sensitivity = l3gd20h::SENSITIVITY_00; + + bool periodicPrintout = false; + PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); + + ReturnValue_t doPeriodicRequest(acs::SimpleSensorMode mode); +}; + +#endif /* MISSION_DEVICES_GYROL3GD20HANDLER_H_ */ diff --git a/mission/devices/GyroADIS1650XHandler.cpp b/mission/devices/GyroADIS1650XHandler.cpp deleted file mode 100644 index daff1b06..00000000 --- a/mission/devices/GyroADIS1650XHandler.cpp +++ /dev/null @@ -1,524 +0,0 @@ -#include "GyroADIS1650XHandler.h" - -#include -#include - -#include "fsfw/FSFW.h" - -#ifdef FSFW_OSAL_LINUX -#include -#include - -#include "fsfw_hal/linux/UnixFileGuard.h" -#include "fsfw_hal/linux/spi/SpiComIF.h" -#include "fsfw_hal/linux/spi/SpiCookie.h" -#include "fsfw_hal/linux/utility.h" -#endif - -GyroADIS1650XHandler::GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, - CookieIF *comCookie, ADIS1650X::Type type) - : DeviceHandlerBase(objectId, deviceCommunication, comCookie), - adisType(type), - primaryDataset(this), - configDataset(this), - breakCountdown() { -#ifdef FSFW_OSAL_LINUX - SpiCookie *cookie = dynamic_cast(comCookie); - if (cookie != nullptr) { - cookie->setCallbackMode(&spiSendCallback, this); - } -#endif -} - -void GyroADIS1650XHandler::doStartUp() { - // Initial 310 ms start up time after power-up - if (internalState == InternalState::STARTUP) { - if (not commandExecuted) { - warningSwitch = true; - breakCountdown.setTimeout(ADIS1650X::START_UP_TIME); - commandExecuted = true; - } - if (breakCountdown.hasTimedOut()) { - internalState = InternalState::CONFIG; - commandExecuted = false; - } - } - - // Read all configuration registers first - if (internalState == InternalState::CONFIG) { - if (commandExecuted) { - commandExecuted = false; - internalState = InternalState::IDLE; - } - } - - if (internalState == InternalState::IDLE) { - if (goToNormalMode) { - setMode(MODE_NORMAL); - } else { - setMode(MODE_ON); - } - } -} - -void GyroADIS1650XHandler::doShutDown() { - commandExecuted = false; - internalState = InternalState::STARTUP; - setMode(_MODE_POWER_DOWN); -} - -ReturnValue_t GyroADIS1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { - *id = ADIS1650X::READ_SENSOR_DATA; - return buildCommandFromCommand(*id, nullptr, 0); -} - -ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { - switch (internalState) { - case (InternalState::CONFIG): { - *id = ADIS1650X::READ_OUT_CONFIG; - buildCommandFromCommand(*id, nullptr, 0); - break; - } - case (InternalState::STARTUP): { - return NOTHING_TO_SEND; - break; - } - default: { - // Might be a configuration error - sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: " - "Unknown internal state!" - << std::endl; - return returnvalue::OK; - } - } - return returnvalue::OK; -} - -ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, - const uint8_t *commandData, - size_t commandDataLen) { - switch (deviceCommand) { - case (ADIS1650X::READ_OUT_CONFIG): { - this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE; - uint8_t regList[6] = {}; - regList[0] = ADIS1650X::DIAG_STAT_REG; - regList[1] = ADIS1650X::FILTER_CTRL_REG; - regList[2] = ADIS1650X::RANG_MDL_REG; - regList[3] = ADIS1650X::MSC_CTRL_REG; - regList[4] = ADIS1650X::DEC_RATE_REG; - regList[5] = ADIS1650X::PROD_ID_REG; - prepareReadCommand(regList, sizeof(regList)); - this->rawPacket = commandBuffer.data(); - break; - } - case (ADIS1650X::READ_SENSOR_DATA): { - if (breakCountdown.isBusy()) { - // A glob command is pending and sensor data can't be read currently - return NO_REPLY_EXPECTED; - } - std::memcpy(commandBuffer.data(), ADIS1650X::BURST_READ_ENABLE.data(), - ADIS1650X::BURST_READ_ENABLE.size()); - std::memset(commandBuffer.data() + 2, 0, 10 * 2); - this->rawPacketLen = ADIS1650X::SENSOR_READOUT_SIZE; - this->rawPacket = commandBuffer.data(); - break; - } - case (ADIS1650X::SELF_TEST_SENSORS): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SENSOR_SELF_TEST, 0x00); - breakCountdown.setTimeout(ADIS1650X::SELF_TEST_BREAK); - break; - } - case (ADIS1650X::SELF_TEST_MEMORY): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_TEST, 0x00); - breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_TEST_BREAK); - break; - } - case (ADIS1650X::UPDATE_NV_CONFIGURATION): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_UPDATE, 0x00); - breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_UPDATE_BREAK); - break; - } - case (ADIS1650X::RESET_SENSOR_CONFIGURATION): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FACTORY_CALIBRATION, 0x00); - breakCountdown.setTimeout(ADIS1650X::FACTORY_CALIBRATION_BREAK); - break; - } - case (ADIS1650X::SW_RESET): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SOFTWARE_RESET, 0x00); - breakCountdown.setTimeout(ADIS1650X::SW_RESET_BREAK); - break; - } - case (ADIS1650X::PRINT_CURRENT_CONFIGURATION): { -#if OBSW_VERBOSE_LEVEL >= 1 - PoolReadGuard pg(&configDataset); - sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4) - << std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl; - sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x" - << configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x" - << configDataset.filterSetting.value << " | DEC_RATE: 0x" - << configDataset.decRateReg.value << std::setfill(' ') << std::endl; -#endif /* OBSW_VERBOSE_LEVEL >= 1 */ - } - } - return returnvalue::OK; -} - -void GyroADIS1650XHandler::fillCommandAndReplyMap() { - insertInCommandAndReplyMap(ADIS1650X::READ_SENSOR_DATA, 1, &primaryDataset); - insertInCommandAndReplyMap(ADIS1650X::READ_OUT_CONFIG, 1, &configDataset); - insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_SENSORS, 1); - insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_MEMORY, 1); - insertInCommandAndReplyMap(ADIS1650X::UPDATE_NV_CONFIGURATION, 1); - insertInCommandAndReplyMap(ADIS1650X::RESET_SENSOR_CONFIGURATION, 1); - insertInCommandAndReplyMap(ADIS1650X::SW_RESET, 1); - insertInCommandAndReplyMap(ADIS1650X::PRINT_CURRENT_CONFIGURATION, 1); -} - -ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize, - DeviceCommandId_t *foundId, size_t *foundLen) { - // For SPI, the ID will always be the one of the last sent command - *foundId = this->getPendingCommand(); - *foundLen = this->rawPacketLen; - - return returnvalue::OK; -} - -ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id, - const uint8_t *packet) { - using namespace ADIS1650X; - switch (id) { - case (ADIS1650X::READ_OUT_CONFIG): { - uint16_t readProdId = packet[12] << 8 | packet[13]; - if (((adisType == ADIS1650X::Type::ADIS16507) and (readProdId != ADIS1650X::PROD_ID_16507)) or - ((adisType == ADIS1650X::Type::ADIS16505) and (readProdId != ADIS1650X::PROD_ID_16505))) { -#if OBSW_VERBOSE_LEVEL >= 1 - if (warningSwitch) { - sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID " - << readProdId << std::endl; - } - warningSwitch = false; -#endif - return returnvalue::FAILED; - } - PoolReadGuard rg(&configDataset); - configDataset.diagStatReg.value = packet[2] << 8 | packet[3]; - configDataset.filterSetting.value = packet[4] << 8 | packet[5]; - uint16_t rangMdlRaw = packet[6] << 8 | packet[7]; - ADIS1650X::RangMdlBitfield bitfield = - static_cast((rangMdlRaw >> 2) & 0b11); - switch (bitfield) { - case (ADIS1650X::RangMdlBitfield::RANGE_125_1BMLZ): { - sensitivity = SENSITIVITY_1BMLZ; - break; - } - case (ADIS1650X::RangMdlBitfield::RANGE_500_2BMLZ): { - sensitivity = SENSITIVITY_2BMLZ; - break; - } - case (ADIS1650X::RangMdlBitfield::RANGE_2000_3BMLZ): { - sensitivity = SENSITIVITY_3BMLZ; - break; - } - case (RangMdlBitfield::RESERVED): { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl; -#endif - break; - } - } - configDataset.rangMdl.value = rangMdlRaw; - configDataset.mscCtrlReg.value = packet[8] << 8 | packet[9]; - configDataset.decRateReg.value = packet[10] << 8 | packet[11]; - configDataset.setValidity(true, true); - if (internalState == InternalState::CONFIG) { - commandExecuted = true; - } - break; - } - case (ADIS1650X::READ_SENSOR_DATA): { - return handleSensorData(packet); - } - } - return returnvalue::OK; -} - -ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) { - BurstModes burstMode = getBurstMode(); - switch (burstMode) { - case (BurstModes::BURST_16_BURST_SEL_1): - case (BurstModes::BURST_32_BURST_SEL_1): { - sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Analysis with BURST_SEL1" - " not implemented!" - << std::endl; - return returnvalue::OK; - } - case (BurstModes::BURST_16_BURST_SEL_0): { - uint16_t checksum = packet[20] << 8 | packet[21]; - // Now verify the read checksum with the expected checksum according to datasheet p. 20 - uint16_t calcChecksum = 0; - for (size_t idx = 2; idx < 20; idx++) { - calcChecksum += packet[idx]; - } - if (checksum != calcChecksum) { -#if OBSW_VERBOSE_LEVEL >= 1 - sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: " - "Invalid checksum detected!" - << std::endl; -#endif - return returnvalue::FAILED; - } - - ReturnValue_t result = configDataset.diagStatReg.read(); - if (result == returnvalue::OK) { - configDataset.diagStatReg.value = packet[2] << 8 | packet[3]; - configDataset.diagStatReg.setValid(true); - } - configDataset.diagStatReg.commit(); - - { - PoolReadGuard pg(&primaryDataset); - int16_t angVelocXRaw = packet[4] << 8 | packet[5]; - primaryDataset.angVelocX.value = static_cast(angVelocXRaw) * sensitivity; - int16_t angVelocYRaw = packet[6] << 8 | packet[7]; - primaryDataset.angVelocY.value = static_cast(angVelocYRaw) * sensitivity; - int16_t angVelocZRaw = packet[8] << 8 | packet[9]; - primaryDataset.angVelocZ.value = static_cast(angVelocZRaw) * sensitivity; - - float accelScaling = 0; - if (adisType == ADIS1650X::Type::ADIS16507) { - accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16507; - } else if (adisType == ADIS1650X::Type::ADIS16505) { - accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16505; - } else { - sif::warning << "GyroADIS1650XHandler::handleSensorData: " - "Unknown ADIS type" - << std::endl; - } - int16_t accelXRaw = packet[10] << 8 | packet[11]; - primaryDataset.accelX.value = static_cast(accelXRaw) / INT16_MAX * accelScaling; - int16_t accelYRaw = packet[12] << 8 | packet[13]; - primaryDataset.accelY.value = static_cast(accelYRaw) / INT16_MAX * accelScaling; - int16_t accelZRaw = packet[14] << 8 | packet[15]; - primaryDataset.accelZ.value = static_cast(accelZRaw) / INT16_MAX * accelScaling; - - int16_t temperatureRaw = packet[16] << 8 | packet[17]; - primaryDataset.temperature.value = static_cast(temperatureRaw) * 0.1; - // Ignore data counter for now - primaryDataset.setValidity(true, true); - } - - if (periodicPrintout) { - if (debugDivider.checkAndIncrement()) { - sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl; - sif::info << "X: " << primaryDataset.angVelocX.value << std::endl; - sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl; - sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl; - sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl; - sif::info << "X: " << primaryDataset.accelX.value << std::endl; - sif::info << "Y: " << primaryDataset.accelY.value << std::endl; - sif::info << "Z: " << primaryDataset.accelZ.value << std::endl; - } - } - - break; - } - case (BurstModes::BURST_32_BURST_SEL_0): { - break; - } - } - return returnvalue::OK; -} - -uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; } - -void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne, - uint8_t valueTwo) { - uint8_t secondReg = startReg + 1; - startReg |= ADIS1650X::WRITE_MASK; - secondReg |= ADIS1650X::WRITE_MASK; - commandBuffer[0] = startReg; - commandBuffer[1] = valueOne; - commandBuffer[2] = secondReg; - commandBuffer[3] = valueTwo; - this->rawPacketLen = 4; - this->rawPacket = commandBuffer.data(); -} - -void GyroADIS1650XHandler::prepareReadCommand(uint8_t *regList, size_t len) { - for (size_t idx = 0; idx < len; idx++) { - commandBuffer[idx * 2] = regList[idx]; - commandBuffer[idx * 2 + 1] = 0x00; - } - commandBuffer[len * 2] = 0x00; - commandBuffer[len * 2 + 1] = 0x00; -} - -ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, - LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry({0.0})); - - localDataPoolMap.emplace(ADIS1650X::DIAG_STAT_REGISTER, new PoolEntry()); - localDataPoolMap.emplace(ADIS1650X::FILTER_SETTINGS, new PoolEntry()); - localDataPoolMap.emplace(ADIS1650X::RANG_MDL, &rangMdl); - localDataPoolMap.emplace(ADIS1650X::MSC_CTRL_REGISTER, new PoolEntry()); - localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry()); - poolManager.subscribeForRegularPeriodicPacket( - subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0)); - return returnvalue::OK; -} - -GyroADIS1650XHandler::BurstModes GyroADIS1650XHandler::getBurstMode() { - configDataset.mscCtrlReg.read(); - uint16_t currentCtrlReg = configDataset.mscCtrlReg.value; - configDataset.mscCtrlReg.commit(); - if ((currentCtrlReg & ADIS1650X::BURST_32_BIT) == ADIS1650X::BURST_32_BIT) { - if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) { - return BurstModes::BURST_32_BURST_SEL_1; - } else { - return BurstModes::BURST_32_BURST_SEL_0; - } - } else { - if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) { - return BurstModes::BURST_16_BURST_SEL_1; - } else { - return BurstModes::BURST_16_BURST_SEL_0; - } - } -} - -#ifdef FSFW_OSAL_LINUX - -ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie, - const uint8_t *sendData, size_t sendLen, - void *args) { - GyroADIS1650XHandler *handler = reinterpret_cast(args); - if (handler == nullptr) { - sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!" - << std::endl; - return returnvalue::FAILED; - } - DeviceCommandId_t currentCommand = handler->getPendingCommand(); - switch (currentCommand) { - case (ADIS1650X::READ_SENSOR_DATA): { - return comIf->performRegularSendOperation(cookie, sendData, sendLen); - } - case (ADIS1650X::READ_OUT_CONFIG): - default: { - ReturnValue_t result = returnvalue::OK; - int retval = 0; - // Prepare transfer - int fileDescriptor = 0; - std::string device = comIf->getSpiDev(); - UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); - if (fileHelper.getOpenResult() != returnvalue::OK) { - return SpiComIF::OPENING_FILE_FAILED; - } - spi::SpiModes spiMode = spi::SpiModes::MODE_0; - uint32_t spiSpeed = 0; - cookie->getSpiParameters(spiMode, spiSpeed, nullptr); - comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); - cookie->assignWriteBuffer(sendData); - cookie->setTransferSize(2); - - gpioId_t gpioId = cookie->getChipSelectPin(); - 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) { -#if OBSW_VERBOSE_LEVEL >= 1 - sif::warning << "GyroADIS16507Handler::spiSendCallback: " - "Mutex or GPIO interface invalid" - << std::endl; - return returnvalue::FAILED; -#endif - } - - if (gpioId != gpio::NO_GPIO) { - result = mutex->lockMutex(timeoutType, timeoutMs); - if (result != returnvalue::OK) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl; -#endif - return result; - } - } - - size_t idx = 0; - spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle(); - uint64_t origTx = transferStruct->tx_buf; - uint64_t origRx = transferStruct->rx_buf; - while (idx < sendLen) { - // Pull SPI CS low. For now, no support for active high given - if (gpioId != gpio::NO_GPIO) { - gpioIF.pullLow(gpioId); - } - - // Execute transfer - // Initiate a full duplex SPI transfer. - retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle()); - if (retval < 0) { - utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); - result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED; - } -#if FSFW_HAL_SPI_WIRETAPPING == 1 - comIf->performSpiWiretapping(cookie); -#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ - - if (gpioId != gpio::NO_GPIO) { - gpioIF.pullHigh(gpioId); - } - - idx += 2; - if (idx < sendLen) { - usleep(ADIS1650X::STALL_TIME_MICROSECONDS); - } - - transferStruct->tx_buf += 2; - transferStruct->rx_buf += 2; - } - transferStruct->tx_buf = origTx; - transferStruct->rx_buf = origRx; - if (gpioId != gpio::NO_GPIO) { - mutex->unlockMutex(); - } - } - } - return returnvalue::OK; -} - -void GyroADIS1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; } - -void GyroADIS1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { - periodicPrintout = enable; - debugDivider.setDivider(divider); -} - -#endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */ diff --git a/mission/devices/HeaterHandler.cpp b/mission/devices/HeaterHandler.cpp index 2cde7f1e..150c4778 100644 --- a/mission/devices/HeaterHandler.cpp +++ b/mission/devices/HeaterHandler.cpp @@ -30,8 +30,8 @@ HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, H if (mainLineSwitcher == nullptr) { throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid PowerSwitchIF"); } - heaterHealthAndStateMutex = MutexFactory::instance()->createMutex(); - if (heaterHealthAndStateMutex == nullptr) { + handlerLock = MutexFactory::instance()->createMutex(); + if (handlerLock == nullptr) { throw std::runtime_error("HeaterHandler::HeaterHandler: Creating Mutex failed"); } auto mqArgs = MqArgs(setObjectId_, static_cast(this)); @@ -144,7 +144,7 @@ ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t if (action == SwitchAction::SET_SWITCH_ON) { HasHealthIF::HealthState health; { - MutexGuard mg(heaterHealthAndStateMutex); + MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); health = heater.healthDevice->getHealth(); } if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY or @@ -270,7 +270,7 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) { } else { triggerEvent(HEATER_WENT_ON, heaterIdx, 0); { - MutexGuard mg(heaterHealthAndStateMutex); + MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); heater.switchState = ON; } } @@ -320,7 +320,7 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) { triggerEvent(GPIO_PULL_LOW_FAILED, result); } else { { - MutexGuard mg(heaterHealthAndStateMutex); + MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); heater.switchState = OFF; } triggerEvent(HEATER_WENT_OFF, heaterIdx, 0); @@ -346,7 +346,7 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) { } HeaterHandler::SwitchState HeaterHandler::checkSwitchState(heater::Switchers switchNr) const { - MutexGuard mg(heaterHealthAndStateMutex); + MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); return heaterVec.at(switchNr).switchState; } @@ -396,7 +396,7 @@ object_id_t HeaterHandler::getObjectId() const { return SystemObject::getObjectI ReturnValue_t HeaterHandler::getAllSwitchStates(std::array& statesBuf) { { - MutexGuard mg(heaterHealthAndStateMutex); + MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); if (mg.getLockResult() != returnvalue::OK) { return returnvalue::FAILED; } @@ -409,7 +409,7 @@ ReturnValue_t HeaterHandler::getAllSwitchStates(std::array& stat bool HeaterHandler::allSwitchesOff() { bool allSwitchesOrd = false; - MutexGuard mg(heaterHealthAndStateMutex); + MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); /* Or all switches. As soon one switch is on, allSwitchesOrd will be true */ for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) { allSwitchesOrd = allSwitchesOrd || heaterVec.at(switchNr).switchState; @@ -442,7 +442,7 @@ uint32_t HeaterHandler::getSwitchDelayMs(void) const { return 2000; } HasHealthIF::HealthState HeaterHandler::getHealth(heater::Switchers heater) { auto* healthDev = heaterVec.at(heater).healthDevice; if (healthDev != nullptr) { - MutexGuard mg(heaterHealthAndStateMutex); + MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); return healthDev->getHealth(); } return HasHealthIF::HealthState::FAULTY; diff --git a/mission/devices/HeaterHandler.h b/mission/devices/HeaterHandler.h index ffc38440..42f42abf 100644 --- a/mission/devices/HeaterHandler.h +++ b/mission/devices/HeaterHandler.h @@ -136,7 +136,10 @@ class HeaterHandler : public ExecutableObjectIF, HeaterMap heaterVec = {}; - MutexIF* heaterHealthAndStateMutex = nullptr; + MutexIF* handlerLock = nullptr; + static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING; + static constexpr uint32_t LOCK_TIMEOUT = 20; + static constexpr char LOCK_CTX[] = "HeaterHandler"; HeaterHelper helper; ModeHelper modeHelper; diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index aa935ce5..46a2c53a 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -53,6 +53,13 @@ ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) { uint8_t dhbOpCode = DeviceHandlerIF::PERFORM_OPERATION; + auto actuateStep = [&]() { + if (ignoreActForRestOfComSteps) { + requestStep = imtq::RequestType::DO_NOTHING; + } else { + requestStep = imtq::RequestType::ACTUATE; + } + }; switch (static_cast(opCode)) { case (imtq::ComStep::DHB_OP): { break; @@ -78,22 +85,38 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) { break; } case (imtq::ComStep::START_ACTUATE_SEND): { - requestStep = imtq::RequestType::ACTUATE; + if (manualTorqueCmdActive) { + if (manuallyCommandedTorqueDuration.isBusy()) { + ignoreActForRestOfComSteps = true; + requestStep = imtq::RequestType::DO_NOTHING; + } else { + manualTorqueCmdActive = false; + PoolReadGuard pg(&dipoleSet); + dipoleSet.dipoles[0] = 0; + dipoleSet.dipoles[1] = 0; + dipoleSet.dipoles[2] = 0; + dipoleSet.currentTorqueDurationMs = 0; + requestStep = imtq::RequestType::ACTUATE; + ignoreActForRestOfComSteps = false; + } + } else { + requestStep = imtq::RequestType::ACTUATE; + } dhbOpCode = DeviceHandlerIF::SEND_WRITE; break; } case (imtq::ComStep::START_ACTUATE_GET): { - requestStep = imtq::RequestType::ACTUATE; + actuateStep(); dhbOpCode = DeviceHandlerIF::GET_WRITE; break; } case (imtq::ComStep::READ_ACTUATE_SEND): { - requestStep = imtq::RequestType::ACTUATE; + actuateStep(); dhbOpCode = DeviceHandlerIF::SEND_READ; break; } case (imtq::ComStep::READ_ACTUATE_GET): { - requestStep = imtq::RequestType::ACTUATE; + actuateStep(); dhbOpCode = DeviceHandlerIF::GET_READ; break; } @@ -108,18 +131,37 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) { ImtqHandler::~ImtqHandler() = default; void ImtqHandler::doStartUp() { - updatePeriodicReply(true, imtq::cmdIds::REPLY); - if (goToNormalMode) { - setMode(MODE_NORMAL); - } else { - setMode(_MODE_TO_ON); + if (internalState != InternalState::STARTUP) { + commandExecuted = false; + updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE); + updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE); + internalState = InternalState::STARTUP; + } + if (internalState == InternalState::STARTUP) { + if (commandExecuted) { + if (goToNormalMode) { + setMode(MODE_NORMAL); + } else { + setMode(_MODE_TO_ON); + } + commandExecuted = false; + } } } void ImtqHandler::doShutDown() { - updatePeriodicReply(false, imtq::cmdIds::REPLY); + updatePeriodicReply(false, imtq::cmdIds::REPLY_NO_TORQUE); + updatePeriodicReply(false, imtq::cmdIds::REPLY_WITH_TORQUE); specialRequestActive = false; firstReplyCycle = true; + internalState = InternalState::NONE; + commandExecuted = false; + statusSet.setValidity(false, true); + rawMtmNoTorque.setValidity(false, true); + rawMtmWithTorque.setValidity(false, true); + hkDatasetNoTorque.setValidity(false, true); + hkDatasetWithTorque.setValidity(false, true); + calMtmMeasurementSet.setValidity(false, true); setMode(_MODE_POWER_DOWN); } @@ -133,11 +175,24 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { *id = imtq::cmdIds::START_ACTUATION_DIPOLE; return buildCommandFromCommand(*id, nullptr, 0); } + default: { + *id = imtq::cmdIds::REQUEST; + request.request = imtq::RequestType::DO_NOTHING; + request.specialRequest = imtq::SpecialRequest::NONE; + expectedReply = DeviceHandlerIF::NO_COMMAND_ID; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(imtq::Request); + return returnvalue::OK; + } } return NOTHING_TO_SEND; } ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { + if (internalState == InternalState::STARTUP) { + *id = imtq::cmdIds::REQUEST; + return buildCommandFromCommand(*id, nullptr, 0); + } return NOTHING_TO_SEND; } @@ -145,11 +200,12 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma const uint8_t* commandData, size_t commandDataLen) { auto genericSpecialRequest = [&](imtq::SpecialRequest specialRequest) { - ImtqRequest request(commandBuffer, sizeof(commandBuffer)); - request.setMeasureRequest(specialRequest); + request.request = imtq::RequestType::MEASURE_NO_ACTUATION; + request.specialRequest = specialRequest; + expectedReply = imtq::cmdIds::REPLY_NO_TORQUE; specialRequestActive = true; - rawPacket = commandBuffer; - rawPacketLen = ImtqRequest::REQUEST_LEN; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(imtq::Request); }; switch (deviceCommand) { case (imtq::cmdIds::POS_X_SELF_TEST): { @@ -181,44 +237,50 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma return returnvalue::OK; } case (imtq::cmdIds::REQUEST): { - ImtqRequest request(commandBuffer, sizeof(commandBuffer)); - request.setMeasureRequest(imtq::SpecialRequest::NONE); - rawPacket = commandBuffer; - rawPacketLen = ImtqRequest::REQUEST_LEN; + request.request = imtq::RequestType::MEASURE_NO_ACTUATION; + request.specialRequest = imtq::SpecialRequest::NONE; + expectedReply = imtq::cmdIds::REPLY_NO_TORQUE; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(imtq::Request); return returnvalue::OK; } case (imtq::cmdIds::START_ACTUATION_DIPOLE): { - /* IMTQ expects low byte first */ - // commandBuffer[0] = imtq::CC::START_ACTUATION_DIPOLE; if (commandData != nullptr && commandDataLen < 8) { return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } - ImtqRequest request(commandBuffer, sizeof(commandBuffer)); - // Commands override anything which was set in the software - if (commandData != nullptr) { - // Read set dipole values from local pool + { + // Do this in any case to read values which might be commanded by the ACS controller. PoolReadGuard pg(&dipoleSet); - - int16_t xDipole = 0, yDipole = 0, zDipole = 0; - uint16_t torqueDuration = 0; - dipoleSet.xDipole = xDipole; - dipoleSet.yDipole = yDipole; - dipoleSet.zDipole = zDipole; - dipoleSet.currentTorqueDurationMs = torqueDuration; + // Commands override anything which was set in the software + if (commandData != nullptr) { + dipoleSet.setValidityBufferGeneration(false); + ReturnValue_t result = dipoleSet.deSerialize(&commandData, &commandDataLen, + SerializeIF::Endianness::NETWORK); + dipoleSet.setValidityBufferGeneration(true); + if (result != returnvalue::OK) { + return result; + } + manualTorqueCmdActive = true; + manuallyCommandedTorqueDuration.setTimeout(dipoleSet.currentTorqueDurationMs.value); + } } - request.setActuateRequest(dipoleSet.xDipole.value, dipoleSet.yDipole.value, - dipoleSet.zDipole.value, dipoleSet.currentTorqueDurationMs.value); + expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE; + request.request = imtq::RequestType::ACTUATE; + request.specialRequest = imtq::SpecialRequest::NONE; + std::memcpy(request.dipoles, dipoleSet.dipoles.value, sizeof(request.dipoles)); + request.torqueDuration = dipoleSet.currentTorqueDurationMs.value; if (ACTUATION_WIRETAPPING) { - sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value - << ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value + sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.dipoles[0] + << ", y = " << dipoleSet.dipoles[1] << ", z = " << dipoleSet.dipoles[2] << ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl; } - MutexGuard mg(torquer::lazyLock()); + MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT, + torquer::LOCK_CTX); torquer::TORQUEING = true; torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value); - rawPacket = commandBuffer; - rawPacketLen = ImtqRequest::REQUEST_LEN; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(imtq::Request); return returnvalue::OK; } default: @@ -230,7 +292,8 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma void ImtqHandler::fillCommandAndReplyMap() { insertInCommandMap(imtq::cmdIds::REQUEST); insertInCommandMap(imtq::cmdIds::START_ACTUATION_DIPOLE); - insertInReplyMap(imtq::cmdIds::REPLY, 5, nullptr, 0, true); + insertInReplyMap(imtq::cmdIds::REPLY_NO_TORQUE, 5, nullptr, 0, true); + insertInReplyMap(imtq::cmdIds::REPLY_WITH_TORQUE, 20, nullptr, 0, true); insertInCommandMap(imtq::cmdIds::POS_X_SELF_TEST); insertInCommandMap(imtq::cmdIds::NEG_X_SELF_TEST); insertInCommandMap(imtq::cmdIds::POS_Y_SELF_TEST); @@ -242,12 +305,12 @@ void ImtqHandler::fillCommandAndReplyMap() { ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { - if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) { + if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON or getMode() == _MODE_POWER_DOWN) { return IGNORE_FULL_PACKET; } if (remainingSize > 0) { *foundLen = remainingSize; - *foundId = imtq::cmdIds::REPLY; + *foundId = expectedReply; return returnvalue::OK; } return returnvalue::FAILED; @@ -257,14 +320,20 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint ReturnValue_t result; ReturnValue_t status = returnvalue::OK; if (getMode() != MODE_NORMAL) { - // Ignore replies during transitions. + if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) { + ImtqRepliesDefault replies(packet); + if (replies.devWasConfigured() and internalState == InternalState::STARTUP) { + commandExecuted = true; + } + } return returnvalue::OK; } - // arrayprinter::print(packet, ImtqReplies::BASE_LEN); - if (requestStep == imtq::RequestType::MEASURE_NO_ACTUATION) { - requestStep = imtq::RequestType::ACTUATE; + if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) { // sif::debug << "handle measure" << std::endl; ImtqRepliesDefault replies(packet); + if (replies.devWasConfigured() and internalState == InternalState::STARTUP) { + commandExecuted = true; + } if (specialRequestActive) { if (replies.wasSpecialRequestRead()) { uint8_t* specialRequest = replies.getSpecialRequest(); @@ -307,29 +376,29 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint } if (not replies.wasGetRawMgmMeasurementRead() and not firstReplyCycle) { - sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl; + sif::warning << "IMTQ: Possible timing issue, raw MGM measurement was not read" << std::endl; } uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement(); result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement); if (result == returnvalue::OK) { - fillRawMtmDataset(rawMgmMeasurement); + fillRawMtmDataset(rawMtmNoTorque, rawMgmMeasurement); } else { status = result; } if (not replies.wasCalibMgmMeasurementRead() and not firstReplyCycle) { - sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl; + sif::warning << "IMTQ: Possible timing issue, calib MGM measurement was not read" + << std::endl; } uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement(); result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement); if (result == returnvalue::OK) { - fillRawMtmDataset(calibMgmMeasurement); + fillCalibratedMtmDataset(calibMgmMeasurement); } else { status = result; } - } else { + } else if (expectedReply == imtq::cmdIds::REPLY_WITH_TORQUE) { // sif::debug << "handle measure with torque" << std::endl; - requestStep = imtq::RequestType::MEASURE_NO_ACTUATION; ImtqRepliesWithTorque replies(packet); if (replies.wasDipoleActuationRead()) { parseStatusByte(imtq::CC::START_ACTUATION_DIPOLE, replies.getDipoleActuation()); @@ -345,7 +414,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement(); result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement); if (result == returnvalue::OK) { - fillRawMtmDataset(rawMgmMeasurement); + fillRawMtmDataset(rawMtmWithTorque, rawMgmMeasurement); } else { status = result; } @@ -361,7 +430,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint } else { status = result; } - fillEngHkDataset(hkDatasetNoTorque, engHkReply); + fillEngHkDataset(hkDatasetWithTorque, engHkReply); if (firstReplyCycle) { firstReplyCycle = false; } @@ -374,6 +443,8 @@ LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) { return &hkDatasetNoTorque; } else if (sid == dipoleSet.getSid()) { return &dipoleSet; + } else if (sid == statusSet.getSid()) { + return &statusSet; } else if (sid == hkDatasetWithTorque.getSid()) { return &hkDatasetWithTorque; } else if (sid == rawMtmWithTorque.getSid()) { @@ -395,7 +466,7 @@ LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) { } else if (sid == negZselfTestDataset.getSid()) { return &negZselfTestDataset; } else { - sif::error << "IMTQHandler::getDataSetHandle: Invalid sid" << std::endl; + sif::error << "ImtqHandler::getDataSetHandle: Invalid SID" << std::endl; return nullptr; } } @@ -409,21 +480,26 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat localDataPoolMap.emplace(imtq::STATUS_BYTE_CONF, &statusConfig); localDataPoolMap.emplace(imtq::STATUS_BYTE_ERROR, &statusError); localDataPoolMap.emplace(imtq::STATUS_BYTE_UPTIME, &statusUptime); + + // ENG HK No Torque localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV, new PoolEntry({0})); localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV, new PoolEntry({0})); localDataPoolMap.emplace(imtq::DIGITAL_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(imtq::ANALOG_CURRENT, new PoolEntry({0})); - localDataPoolMap.emplace(imtq::COIL_X_CURRENT, new PoolEntry({0})); - localDataPoolMap.emplace(imtq::COIL_Y_CURRENT, new PoolEntry({0})); - localDataPoolMap.emplace(imtq::COIL_Z_CURRENT, new PoolEntry({0})); - localDataPoolMap.emplace(imtq::COIL_X_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(imtq::COIL_Y_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(imtq::COIL_Z_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::COIL_CURRENTS, &coilCurrentsMilliampsNoTorque); + localDataPoolMap.emplace(imtq::COIL_TEMPERATURES, &coilTempsNoTorque); 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); + // ENG HK With Torque + localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::DIGITAL_CURRENT_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::ANALOG_CURRENT_WT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::COIL_CURRENTS_WT, &coilCurrentsMilliampsWithTorque); + localDataPoolMap.emplace(imtq::COIL_TEMPERATURES_WT, &coilTempsWithTorque); + localDataPoolMap.emplace(imtq::MCU_TEMPERATURE_WT, new PoolEntry({0})); + + localDataPoolMap.emplace(imtq::DIPOLES_ID, &dipolesPoolEntry); localDataPoolMap.emplace(imtq::CURRENT_TORQUE_DURATION, &torqueDurationEntry); /** Entries of calibrated MTM measurement dataset */ @@ -431,8 +507,11 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat localDataPoolMap.emplace(imtq::ACTUATION_CAL_STATUS, new PoolEntry({0})); /** Entries of raw MTM measurement dataset */ - localDataPoolMap.emplace(imtq::MTM_RAW, new PoolEntry(3)); - localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::MTM_RAW, &mtmRawNoTorque); + localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, &actStatusNoTorque); + + localDataPoolMap.emplace(imtq::MTM_RAW_WT, &mtmRawWithTorque); + localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS_WT, &actStatusWithTorque); /** INIT measurements for positive X axis test */ localDataPoolMap.emplace(imtq::INIT_POS_X_ERR, new PoolEntry({0})); @@ -708,6 +787,10 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat subdp::DiagnosticsHkPeriodicParams(rawMtmWithTorque.getSid(), false, 10.0)); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(statusSet.getSid(), false, 10.0)); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(dipoleSet.getSid(), false, 10.0)); return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager); } @@ -731,7 +814,7 @@ ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) { } ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t* packet) { - uint8_t cmdErrorField = *(packet + 1) & 0xF; + uint8_t cmdErrorField = packet[1] & 0xF; if (cmdErrorField == 0) { return returnvalue::OK; } @@ -754,16 +837,20 @@ ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t* << " has invalid parameter" << std::endl; return imtq::PARAMETER_INVALID; case 5: - sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << " unavailable" << std::endl; + sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << command << " unavailable" + << std::endl; return imtq::CC_UNAVAILABLE; case 7: - sif::error << "IMTQ::parseStatusByte: IMTQ replied internal processing error" << std::endl; + sif::error << "IMTQ::parseStatusByte: Internal processing error for command 0x" + << std::setw(2) << command << std::endl; return imtq::INTERNAL_PROCESSING_ERROR; default: - sif::error << "IMTQ::parseStatusByte: CMD Error field contains unknown error code 0x" - << static_cast(cmdErrorField) << std::endl; + sif::error << "IMTQ::parseStatusByte: CMD error field for command 0x" << std::setw(2) + << command << " contains unknown error code 0x" << static_cast(cmdErrorField) + << std::endl; return imtq::CMD_ERR_UNKNOWN; } + sif::error << std::dec; } void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet) { @@ -777,20 +864,20 @@ void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* pa offset += 2; hkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - hkDataset.coilXCurrentmA = + hkDataset.coilCurrentsMilliamps[0] = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - hkDataset.coilYCurrentmA = + hkDataset.coilCurrentsMilliamps[1] = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - hkDataset.coilZCurrentmA = + hkDataset.coilCurrentsMilliamps[2] = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - hkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); + hkDataset.coilTemperatures[0] = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; - hkDataset.coilYTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); + hkDataset.coilTemperatures[1] = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; - hkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); + hkDataset.coilTemperatures[2] = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; size_t dummy = 2; SerializeAdapter::deSerialize(&hkDataset.mcuTemperature.value, packet + offset, &dummy, @@ -804,12 +891,15 @@ void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* pa sif::info << "IMTQ analog voltage: " << hkDataset.analogVoltageMv << " mV" << std::endl; sif::info << "IMTQ digital current: " << hkDataset.digitalCurrentmA << " mA" << std::endl; sif::info << "IMTQ analog current: " << hkDataset.analogCurrentmA << " mA" << std::endl; - sif::info << "IMTQ coil X current: " << hkDataset.coilXCurrentmA << " mA" << std::endl; - sif::info << "IMTQ coil Y current: " << hkDataset.coilYCurrentmA << " mA" << std::endl; - sif::info << "IMTQ coil Z current: " << hkDataset.coilZCurrentmA << " mA" << std::endl; - sif::info << "IMTQ coil X temperature: " << hkDataset.coilXTemperature << " °C" << std::endl; - sif::info << "IMTQ coil Y temperature: " << hkDataset.coilYTemperature << " °C" << std::endl; - sif::info << "IMTQ coil Z temperature: " << hkDataset.coilZTemperature << " °C" << std::endl; + sif::info << "IMTQ coil X current: " << hkDataset.coilCurrentsMilliamps[0] << " mA" + << std::endl; + sif::info << "IMTQ coil Y current: " << hkDataset.coilCurrentsMilliamps[1] << " mA" + << std::endl; + sif::info << "IMTQ coil Z current: " << hkDataset.coilCurrentsMilliamps[2] << " mA" + << std::endl; + sif::info << "IMTQ coil X temperature: " << hkDataset.coilTemperatures[0] << " °C" << std::endl; + sif::info << "IMTQ coil Y temperature: " << hkDataset.coilTemperatures[1] << " °C" << std::endl; + sif::info << "IMTQ coil Z temperature: " << hkDataset.coilTemperatures[2] << " °C" << std::endl; sif::info << "IMTQ coil MCU temperature: " << hkDataset.mcuTemperature << " °C" << std::endl; #endif } @@ -847,8 +937,11 @@ void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) { } } -void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) { - PoolReadGuard rg(&rawMtmNoTorque); +void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet) { + PoolReadGuard rg(&set); + if (rg.getReadResult() != returnvalue::OK) { + sif::error << "ImtqHandler::fillRawMtmDataset: Read failure" << std::endl; + } unsigned int offset = 2; size_t deSerLen = 16; const uint8_t* dataStart = packet + offset; @@ -876,18 +969,19 @@ void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) { if (res != returnvalue::OK) { return; } - rawMtmNoTorque.mtmRawNt[0] = xRaw * 7.5; - rawMtmNoTorque.mtmRawNt[1] = yRaw * 7.5; - rawMtmNoTorque.mtmRawNt[2] = zRaw * 7.5; - rawMtmNoTorque.coilActuationStatus = static_cast(coilActStatus); - rawMtmNoTorque.setValidity(true, true); + set.mtmRawNt[0] = static_cast(xRaw) * 7.5; + set.mtmRawNt[1] = static_cast(yRaw) * 7.5; + set.mtmRawNt[2] = static_cast(zRaw) * 7.5; + set.coilActuationStatus = static_cast(coilActStatus); + set.setValidity(true, true); if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 - sif::info << "IMTQ raw MTM measurement X: " << rawMtmNoTorque.mtmRawNt[0] << " nT" << std::endl; - sif::info << "IMTQ raw MTM measurement Y: " << rawMtmNoTorque.mtmRawNt[1] << " nT" << std::endl; - sif::info << "IMTQ raw MTM measurement Z: " << rawMtmNoTorque.mtmRawNt[2] << " nT" << std::endl; + sif::info << "Set ID: " << set.getSid().ownerSetId << std::endl; + sif::info << "IMTQ raw MTM measurement X: " << set.mtmRawNt[0] << " nT" << std::endl; + sif::info << "IMTQ raw MTM measurement Y: " << set.mtmRawNt[1] << " nT" << std::endl; + sif::info << "IMTQ raw MTM measurement Z: " << set.mtmRawNt[2] << " nT" << std::endl; sif::info << "IMTQ coil actuation status during MTM measurement: " - << (unsigned int)rawMtmNoTorque.coilActuationStatus.value << std::endl; + << (unsigned int)set.coilActuationStatus.value << std::endl; #endif } } diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h index a674f85b..e337f40c 100644 --- a/mission/devices/ImtqHandler.h +++ b/mission/devices/ImtqHandler.h @@ -83,6 +83,11 @@ class ImtqHandler : public DeviceHandlerBase { //! link between IMTQ and OBC. static const Event INVALID_ERROR_BYTE = MAKE_EVENT(8, severity::LOW); + enum class InternalState { NONE, STARTUP, SHUTDOWN } internalState = InternalState::NONE; + bool commandExecuted = false; + + imtq::Request request{}; + imtq::StatusDataset statusSet; imtq::DipoleActuationSet dipoleSet; imtq::RawMtmMeasurementNoTorque rawMtmNoTorque; @@ -98,6 +103,9 @@ class ImtqHandler : public DeviceHandlerBase { imtq::NegYSelfTestSet negYselfTestDataset; imtq::PosZSelfTestSet posZselfTestDataset; imtq::NegZSelfTestSet negZselfTestDataset; + bool manualTorqueCmdActive = false; + bool ignoreActForRestOfComSteps = false; + Countdown manuallyCommandedTorqueDuration = Countdown(); NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED; @@ -107,13 +115,20 @@ class ImtqHandler : public DeviceHandlerBase { PoolEntry statusUptime = PoolEntry({0}); PoolEntry mgmCalEntry = PoolEntry(3); - PoolEntry dipoleXEntry = PoolEntry(0, false); - PoolEntry dipoleYEntry = PoolEntry(0, false); - PoolEntry dipoleZEntry = PoolEntry(0, false); - PoolEntry torqueDurationEntry = PoolEntry(0, false); + PoolEntry dipolesPoolEntry = PoolEntry({0, 0, 0}, false); + PoolEntry torqueDurationEntry = PoolEntry({0}, false); + PoolEntry coilCurrentsMilliampsNoTorque = PoolEntry(3); + PoolEntry coilCurrentsMilliampsWithTorque = PoolEntry(3); + PoolEntry coilTempsNoTorque = PoolEntry(3); + PoolEntry coilTempsWithTorque = PoolEntry(3); + PoolEntry mtmRawNoTorque = PoolEntry(3); + PoolEntry actStatusNoTorque = PoolEntry(1); + PoolEntry mtmRawWithTorque = PoolEntry(3); + PoolEntry actStatusWithTorque = PoolEntry(1); + power::Switch_t switcher = power::NO_SWITCH; - uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE]; + DeviceCommandId_t expectedReply = DeviceHandlerIF::NO_COMMAND_ID; bool goToNormalMode = false; bool debugMode = false; bool specialRequestActive = false; @@ -159,7 +174,7 @@ class ImtqHandler : public DeviceHandlerBase { * @param packet Pointer to the reply data requested with the GET_RAW_MTM_MEASUREMENTS * command. */ - void fillRawMtmDataset(const uint8_t* packet); + void fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet); /** * @brief This function handles all self test results. This comprises parsing the error byte diff --git a/mission/devices/LegacySusHandler.cpp b/mission/devices/LegacySusHandler.cpp new file mode 100644 index 00000000..52a43de5 --- /dev/null +++ b/mission/devices/LegacySusHandler.cpp @@ -0,0 +1,233 @@ +#include +#include +#include + +#include "OBSWConfig.h" + +LegacySusHandler::LegacySusHandler(object_id_t objectId, uint8_t susIdx, object_id_t comIF, + CookieIF *comCookie) + : DeviceHandlerBase(objectId, comIF, comCookie), divider(5), dataset(this), susIdx(susIdx) {} + +LegacySusHandler::~LegacySusHandler() {} + +void LegacySusHandler::doStartUp() { + if (comState == ComStates::IDLE) { + comState = ComStates::WRITE_SETUP; + commandExecuted = false; + } + if (comState == ComStates::WRITE_SETUP) { + if (commandExecuted) { + if (goToNormalModeImmediately) { + setMode(MODE_NORMAL); + } else { + setMode(_MODE_TO_ON); + } + commandExecuted = false; + if (clkMode == ClkModes::INT_CLOCKED) { + comState = ComStates::START_INT_CLOCKED_CONVERSIONS; + } else { + comState = ComStates::EXT_CLOCKED_CONVERSIONS; + } + } + } +} + +void LegacySusHandler::doShutDown() { + setMode(_MODE_POWER_DOWN); + comState = ComStates::IDLE; +} + +ReturnValue_t LegacySusHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + switch (comState) { + case (ComStates::IDLE): { + break; + } + case (ComStates::WRITE_SETUP): { + *id = susMax1227::WRITE_SETUP; + return buildCommandFromCommand(*id, nullptr, 0); + } + case (ComStates::EXT_CLOCKED_CONVERSIONS): { + *id = susMax1227::READ_EXT_TIMED_CONVERSIONS; + return buildCommandFromCommand(*id, nullptr, 0); + } + case (ComStates::START_INT_CLOCKED_CONVERSIONS): { + *id = susMax1227::START_INT_TIMED_CONVERSIONS; + comState = ComStates::READ_INT_CLOCKED_CONVERSIONS; + return buildCommandFromCommand(*id, nullptr, 0); + } + case (ComStates::READ_INT_CLOCKED_CONVERSIONS): { + *id = susMax1227::READ_INT_TIMED_CONVERSIONS; + comState = ComStates::START_INT_CLOCKED_CONVERSIONS; + return buildCommandFromCommand(*id, nullptr, 0); + } + case (ComStates::EXT_CLOCKED_TEMP): { + *id = susMax1227::READ_EXT_TIMED_TEMPS; + return buildCommandFromCommand(*id, nullptr, 0); + } + } + return NOTHING_TO_SEND; +} + +ReturnValue_t LegacySusHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + if (comState == ComStates::WRITE_SETUP) { + *id = susMax1227::WRITE_SETUP; + return buildCommandFromCommand(*id, nullptr, 0); + } + return NOTHING_TO_SEND; +} + +ReturnValue_t LegacySusHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, + size_t commandDataLen) { + using namespace max1227; + switch (deviceCommand) { + case (susMax1227::WRITE_SETUP): { + if (clkMode == ClkModes::INT_CLOCKED) { + cmdBuffer[0] = susMax1227::SETUP_INT_CLOKED; + } else { + cmdBuffer[0] = susMax1227::SETUP_EXT_CLOCKED; + } + + rawPacket = cmdBuffer; + rawPacketLen = 1; + break; + } + case (susMax1227::START_INT_TIMED_CONVERSIONS): { + std::memset(cmdBuffer, 0, sizeof(cmdBuffer)); + cmdBuffer[0] = max1227::buildResetByte(true); + cmdBuffer[1] = susMax1227::CONVERSION; + rawPacket = cmdBuffer; + rawPacketLen = 2; + break; + } + case (susMax1227::READ_INT_TIMED_CONVERSIONS): { + std::memset(cmdBuffer, 0, sizeof(cmdBuffer)); + rawPacket = cmdBuffer; + rawPacketLen = susMax1227::SIZE_READ_INT_CONVERSIONS; + break; + } + case (susMax1227::READ_EXT_TIMED_CONVERSIONS): { + std::memset(cmdBuffer, 0, sizeof(cmdBuffer)); + rawPacket = cmdBuffer; + for (uint8_t idx = 0; idx < 6; idx++) { + cmdBuffer[idx * 2] = buildConvByte(ScanModes::N_ONCE, idx, false); + cmdBuffer[idx * 2 + 1] = 0; + } + cmdBuffer[12] = 0x00; + rawPacketLen = susMax1227::SIZE_READ_EXT_CONVERSIONS; + break; + } + case (susMax1227::READ_EXT_TIMED_TEMPS): { + cmdBuffer[0] = buildConvByte(ScanModes::N_ONCE, 0, true); + std::memset(cmdBuffer + 1, 0, 24); + rawPacket = cmdBuffer; + rawPacketLen = 25; + break; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return returnvalue::OK; +} + +void LegacySusHandler::fillCommandAndReplyMap() { + insertInCommandAndReplyMap(susMax1227::WRITE_SETUP, 1); + insertInCommandAndReplyMap(susMax1227::START_INT_TIMED_CONVERSIONS, 1); + insertInCommandAndReplyMap(susMax1227::READ_INT_TIMED_CONVERSIONS, 1, &dataset, + susMax1227::SIZE_READ_INT_CONVERSIONS); + insertInCommandAndReplyMap(susMax1227::READ_EXT_TIMED_CONVERSIONS, 1, &dataset, + susMax1227::SIZE_READ_EXT_CONVERSIONS); + insertInCommandAndReplyMap(susMax1227::READ_EXT_TIMED_TEMPS, 1); +} + +ReturnValue_t LegacySusHandler::scanForReply(const uint8_t *start, size_t remainingSize, + DeviceCommandId_t *foundId, size_t *foundLen) { + *foundId = this->getPendingCommand(); + *foundLen = remainingSize; + return returnvalue::OK; +} + +ReturnValue_t LegacySusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { + switch (id) { + case susMax1227::WRITE_SETUP: { + if (getMode() == _MODE_START_UP) { + commandExecuted = true; + } + return returnvalue::OK; + } + case susMax1227::START_INT_TIMED_CONVERSIONS: { + return returnvalue::OK; + } + case susMax1227::READ_INT_TIMED_CONVERSIONS: { + PoolReadGuard readSet(&dataset); + dataset.tempC = max1227::getTemperature(((packet[0] & 0x0f) << 8) | packet[1]); + for (uint8_t idx = 0; idx < 6; idx++) { + dataset.channels[idx] = packet[idx * 2 + 2] << 8 | packet[idx * 2 + 3]; + } + dataset.setValidity(true, true); + printDataset(); + break; + } + case (susMax1227::READ_EXT_TIMED_CONVERSIONS): { + PoolReadGuard readSet(&dataset); + for (uint8_t idx = 0; idx < 6; idx++) { + dataset.channels[idx] = packet[idx * 2 + 1] << 8 | packet[idx * 2 + 2]; + } + dataset.channels.setValid(true); + // Read temperature in next read cycle + if (clkMode == ClkModes::EXT_CLOCKED_WITH_TEMP) { + comState = ComStates::EXT_CLOCKED_TEMP; + } + printDataset(); + break; + } + case (susMax1227::READ_EXT_TIMED_TEMPS): { + PoolReadGuard readSet(&dataset); + dataset.tempC = max1227::getTemperature(((packet[23] & 0x0f) << 8) | packet[24]); + dataset.tempC.setValid(true); + comState = ComStates::EXT_CLOCKED_CONVERSIONS; + break; + } + default: { + sif::debug << "SusHandler::interpretDeviceReply: Unknown reply id" << std::endl; + return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; + } + } + return returnvalue::OK; +} + +uint32_t LegacySusHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 3000; } + +ReturnValue_t LegacySusHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(susMax1227::TEMPERATURE_C, &tempC); + localDataPoolMap.emplace(susMax1227::CHANNEL_VEC, &channelVec); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(dataset.getSid(), false, 5.0)); + return returnvalue::OK; +} + +void LegacySusHandler::setToGoToNormalMode(bool enable) { + this->goToNormalModeImmediately = enable; +} + +void LegacySusHandler::printDataset() { + if (periodicPrintout) { + if (divider.checkAndIncrement()) { + sif::info << "SUS ADC " << static_cast(susIdx) << " hex [" << std::setfill('0') + << std::hex; + for (uint8_t idx = 0; idx < 6; idx++) { + sif::info << std::setw(3) << dataset.channels[idx]; + if (idx < 6 - 1) { + sif::info << ","; + } + } + sif::info << "] | T[C] " << std::dec << dataset.tempC.value << std::endl; + } + } +} + +void LegacySusHandler::enablePeriodicPrintout(bool enable, uint8_t divider) { + this->periodicPrintout = enable; + this->divider.setDivider(divider); +} diff --git a/mission/devices/LegacySusHandler.h b/mission/devices/LegacySusHandler.h new file mode 100644 index 00000000..5a16d180 --- /dev/null +++ b/mission/devices/LegacySusHandler.h @@ -0,0 +1,92 @@ +#ifndef MISSION_DEVICES_LEGACYSUSHANDLER_H_ +#define MISSION_DEVICES_LEGACYSUSHANDLER_H_ + +#include +#include + +#include "events/subsystemIdRanges.h" +#include "fsfw/globalfunctions/PeriodicOperationDivider.h" +#include "mission/devices/max1227.h" +#include "returnvalues/classIds.h" + +/** + * @brief This is the device handler class for the SUS sensor based on the MAX1227 ADC. + * + * @details + * Datasheet of MAX1227: https://datasheets.maximintegrated.com/en/ds/MAX1227-MAX1231.pdf + * Details about the SUS electronic can be found at + * https://egit.irs.uni-stuttgart.de/eive/eive_dokumente/src/branch/master/400_Raumsegment/443_SunSensorDocumentation/release + * + * @note When adding a SusHandler to the polling sequence table make sure to add a slot with + * the executionStep FIRST_WRITE. Otherwise the communication sequence will never be + * started. + * + * @author J. Meier + */ +class LegacySusHandler : public DeviceHandlerBase { + public: + enum ClkModes { INT_CLOCKED, EXT_CLOCKED, EXT_CLOCKED_WITH_TEMP }; + + static const uint8_t FIRST_WRITE = 7; + + LegacySusHandler(object_id_t objectId, uint8_t susIdx, object_id_t comIF, CookieIF* comCookie); + virtual ~LegacySusHandler(); + + void enablePeriodicPrintout(bool enable, uint8_t divider); + + void setToGoToNormalMode(bool enable); + + protected: + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override; + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t* id) override; + void fillCommandAndReplyMap() override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, + size_t* foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) override; + uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, + LocalDataPoolManager& poolManager) override; + + private: + static const uint8_t INTERFACE_ID = CLASS_ID::SUS_HANDLER; + + static const ReturnValue_t ERROR_UNLOCK_MUTEX = MAKE_RETURN_CODE(0xA0); + static const ReturnValue_t ERROR_LOCK_MUTEX = MAKE_RETURN_CODE(0xA1); + + enum class ComStates { + IDLE, + WRITE_SETUP, + EXT_CLOCKED_CONVERSIONS, + EXT_CLOCKED_TEMP, + START_INT_CLOCKED_CONVERSIONS, + READ_INT_CLOCKED_CONVERSIONS + }; + + bool periodicPrintout = false; + PeriodicOperationDivider divider; + bool goToNormalModeImmediately = false; + bool commandExecuted = false; + + susMax1227::SusDataset dataset; + // Read temperature in each alternating communication step when using + // externally clocked mode + ClkModes clkMode = ClkModes::INT_CLOCKED; + PoolEntry tempC = PoolEntry({0.0}); + PoolEntry channelVec = PoolEntry({0, 0, 0, 0, 0, 0}); + + uint8_t susIdx = 0; + uint8_t cmdBuffer[susMax1227::MAX_CMD_SIZE]; + ComStates comState = ComStates::IDLE; + + MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; + uint32_t timeoutMs = 20; + void printDataset(); + + MutexIF* spiMutex = nullptr; +}; + +#endif /* MISSION_DEVICES_LEGACYSUSHANDLER_H_ */ diff --git a/mission/devices/MgmLis3CustomHandler.cpp b/mission/devices/MgmLis3CustomHandler.cpp new file mode 100644 index 00000000..151d321d --- /dev/null +++ b/mission/devices/MgmLis3CustomHandler.cpp @@ -0,0 +1,150 @@ +#include + +#include + +#include "fsfw/datapool/PoolReadGuard.h" + +MgmLis3CustomHandler::MgmLis3CustomHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie, uint32_t transitionDelay) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie), + dataset(this), + transitionDelay(transitionDelay) {} + +MgmLis3CustomHandler::~MgmLis3CustomHandler() = default; + +void MgmLis3CustomHandler::doStartUp() { + if (internalState != InternalState::STARTUP) { + commandExecuted = false; + updatePeriodicReply(true, REPLY); + internalState = InternalState::STARTUP; + } + if (internalState == InternalState::STARTUP) { + if (commandExecuted) { + setMode(MODE_NORMAL); + internalState = InternalState::NONE; + commandExecuted = false; + } + } +} + +void MgmLis3CustomHandler::doShutDown() { + if (internalState != InternalState::SHUTDOWN) { + dataset.setValidity(false, true); + internalState = InternalState::SHUTDOWN; + commandExecuted = false; + } + if (internalState == InternalState::SHUTDOWN and commandExecuted) { + updatePeriodicReply(false, REPLY); + commandExecuted = false; + internalState = InternalState::NONE; + setMode(MODE_OFF); + } +} + +ReturnValue_t MgmLis3CustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + if (internalState == InternalState::STARTUP) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::NORMAL); + } else if (internalState == InternalState::SHUTDOWN) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::OFF); + } + return NOTHING_TO_SEND; +} + +ReturnValue_t MgmLis3CustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::NORMAL); +} + +ReturnValue_t MgmLis3CustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, + size_t commandDataLen) { + return NOTHING_TO_SEND; +} + +ReturnValue_t MgmLis3CustomHandler::scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) { + if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) { + return IGNORE_FULL_PACKET; + } + if (len != sizeof(acs::MgmLis3Reply)) { + *foundLen = len; + return returnvalue::FAILED; + } + *foundId = REPLY; + *foundLen = len; + if (internalState == InternalState::SHUTDOWN) { + commandExecuted = true; + } + return returnvalue::OK; +} +ReturnValue_t MgmLis3CustomHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + const auto *reply = reinterpret_cast(packet); + if (reply->dataWasSet) { + if (internalState == InternalState::STARTUP) { + commandExecuted = true; + } + PoolReadGuard pg(&dataset); + for (uint8_t idx = 0; idx < 3; idx++) { + dataset.fieldStrengths[idx] = + reply->mgmValuesRaw[idx] * reply->sensitivity * mgmLis3::GAUSS_TO_MICROTESLA_FACTOR; + } + + dataset.setValidity(true, true); + if (std::abs(dataset.fieldStrengths[0]) > absLimitX or + std::abs(dataset.fieldStrengths[1]) > absLimitY or + std::abs(dataset.fieldStrengths[2]) > absLimitZ) { + dataset.fieldStrengths.setValid(false); + } + dataset.temperature = 25.0 + ((static_cast(reply->temperatureRaw)) / 8.0); + } + return returnvalue::OK; +} + +void MgmLis3CustomHandler::fillCommandAndReplyMap() { + insertInCommandMap(REQUEST); + insertInReplyMap(REPLY, 5, nullptr, 0, true); +} + +void MgmLis3CustomHandler::setToGoToNormalMode(bool enable) { this->goToNormalMode = enable; } + +uint32_t MgmLis3CustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { + return transitionDelay; +} + +void MgmLis3CustomHandler::modeChanged(void) { internalState = InternalState::NONE; } + +ReturnValue_t MgmLis3CustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(mgmLis3::FIELD_STRENGTHS, &mgmXYZ); + localDataPoolMap.emplace(mgmLis3::TEMPERATURE_CELCIUS, &temperature); + poolManager.subscribeForRegularPeriodicPacket({dataset.getSid(), false, 10.0}); + return returnvalue::OK; +} + +void MgmLis3CustomHandler::setAbsoluteLimits(float xLimit, float yLimit, float zLimit) { + this->absLimitX = xLimit; + this->absLimitY = yLimit; + this->absLimitZ = zLimit; +} + +void MgmLis3CustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} + +ReturnValue_t MgmLis3CustomHandler::prepareRequest(acs::SimpleSensorMode mode) { + request.mode = mode; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(acs::MgmLis3Request); + return returnvalue::OK; +} + +LocalPoolDataSetBase *MgmLis3CustomHandler::getDataSetHandle(sid_t sid) { + if (sid == dataset.getSid()) { + return &dataset; + } + return nullptr; +} diff --git a/mission/devices/MgmLis3CustomHandler.h b/mission/devices/MgmLis3CustomHandler.h new file mode 100644 index 00000000..15c87e0f --- /dev/null +++ b/mission/devices/MgmLis3CustomHandler.h @@ -0,0 +1,103 @@ +#ifndef MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_ +#define MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_ + +#include + +#include "fsfw/devicehandlers/DeviceHandlerBase.h" +#include "fsfw/globalfunctions/PeriodicOperationDivider.h" +#include "mission/devices/devicedefinitions/acsPolling.h" + +class PeriodicOperationDivider; + +/** + * @brief Device handler object for the LIS3MDL 3-axis magnetometer + * by STMicroeletronics + * @details + * Datasheet can be found online by googling LIS3MDL. + * Flight manual: + * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/LIS3MDL_MGM + * @author L. Loidold, R. Mueller + */ +class MgmLis3CustomHandler : public DeviceHandlerBase { + public: + static constexpr DeviceCommandId_t REQUEST = 0x70; + static constexpr DeviceCommandId_t REPLY = 0x77; + + static const uint8_t INTERFACE_ID = CLASS_ID::MGM_LIS3MDL; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MGM_LIS3MDL; + + MgmLis3CustomHandler(uint32_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, + uint32_t transitionDelay); + virtual ~MgmLis3CustomHandler(); + + void enablePeriodicPrintouts(bool enable, uint8_t divider); + /** + * Set the absolute limit for the values on the axis in microtesla. The dataset values will + * be marked as invalid if that limit is exceeded + * @param xLimit + * @param yLimit + * @param zLimit + */ + void setAbsoluteLimits(float xLimit, float yLimit, float zLimit); + void setToGoToNormalMode(bool enable); + + protected: + /** DeviceHandlerBase overrides */ + void doShutDown() override; + void doStartUp() override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) override; + /** + * This implementation is tailored towards space applications and will flag values larger + * than 100 microtesla on X,Y and 150 microtesla on Z as invalid + * @param id + * @param packet + * @return + */ + virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + void fillCommandAndReplyMap() override; + void modeChanged(void) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; + + private: + mgmLis3::MgmPrimaryDataset dataset; + acs::MgmLis3Request request{}; + + uint32_t transitionDelay; + + float absLimitX = 100; + float absLimitY = 100; + float absLimitZ = 150; + + uint8_t statusRegister = 0; + bool goToNormalMode = false; + + enum class InternalState { + NONE, + STARTUP, + SHUTDOWN, + }; + + InternalState internalState = InternalState::NONE; + bool commandExecuted = false; + + PoolEntry mgmXYZ = PoolEntry(3); + PoolEntry temperature = PoolEntry(); + /*------------------------------------------------------------------------*/ + /* Device specific commands and variables */ + /*------------------------------------------------------------------------*/ + + bool periodicPrintout = false; + PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); + + ReturnValue_t prepareRequest(acs::SimpleSensorMode mode); +}; + +#endif /* MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_ */ diff --git a/mission/devices/MgmRm3100CustomHandler.cpp b/mission/devices/MgmRm3100CustomHandler.cpp new file mode 100644 index 00000000..f6f86950 --- /dev/null +++ b/mission/devices/MgmRm3100CustomHandler.cpp @@ -0,0 +1,139 @@ +#include + +#include "fsfw/datapool/PoolReadGuard.h" +#include "fsfw/devicehandlers/DeviceHandlerMessage.h" +#include "fsfw/globalfunctions/bitutility.h" +#include "fsfw/objectmanager/SystemObjectIF.h" +#include "fsfw/returnvalues/returnvalue.h" + +MgmRm3100CustomHandler::MgmRm3100CustomHandler(object_id_t objectId, + object_id_t deviceCommunication, CookieIF *comCookie, + uint32_t transitionDelay) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie), + primaryDataset(this), + transitionDelay(transitionDelay) {} + +MgmRm3100CustomHandler::~MgmRm3100CustomHandler() = default; + +void MgmRm3100CustomHandler::doStartUp() { + if (internalState != InternalState::STARTUP) { + commandExecuted = false; + updatePeriodicReply(true, REPLY); + internalState = InternalState::STARTUP; + } + if (internalState == InternalState::STARTUP) { + if (commandExecuted) { + commandExecuted = false; + internalState = InternalState::NONE; + setMode(MODE_NORMAL); + } + } +} + +void MgmRm3100CustomHandler::doShutDown() { + if (internalState != InternalState::SHUTDOWN) { + commandExecuted = false; + primaryDataset.setValidity(false, true); + internalState = InternalState::SHUTDOWN; + } + if (internalState == InternalState::SHUTDOWN and commandExecuted) { + updatePeriodicReply(false, REPLY); + setMode(MODE_OFF); + commandExecuted = false; + } +} + +ReturnValue_t MgmRm3100CustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + if (internalState == InternalState::STARTUP) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::NORMAL); + } else if (internalState == InternalState::SHUTDOWN) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::OFF); + } + return NOTHING_TO_SEND; +} + +ReturnValue_t MgmRm3100CustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, + size_t commandDataLen) { + return NOTHING_TO_SEND; +} + +ReturnValue_t MgmRm3100CustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::NORMAL); +} + +ReturnValue_t MgmRm3100CustomHandler::scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) { + if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) { + return IGNORE_FULL_PACKET; + } + if (len != sizeof(acs::MgmRm3100Reply)) { + *foundLen = len; + return returnvalue::FAILED; + } + *foundId = REPLY; + *foundLen = len; + if (internalState == InternalState::SHUTDOWN) { + commandExecuted = true; + } + return returnvalue::OK; +} + +ReturnValue_t MgmRm3100CustomHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + const auto *reply = reinterpret_cast(packet); + if (reply->dataWasRead) { + if (internalState == InternalState::STARTUP) { + commandExecuted = true; + } + + PoolReadGuard pg(&primaryDataset); + primaryDataset.setValidity(true, true); + for (uint8_t idx = 0; idx < 3; idx++) { + primaryDataset.fieldStrengths[idx] = reply->mgmValuesRaw[idx] * reply->scaleFactors[idx]; + } + } + return returnvalue::OK; +} + +void MgmRm3100CustomHandler::fillCommandAndReplyMap() { + insertInCommandMap(REQUEST); + insertInReplyMap(REPLY, 5, nullptr, 0, true); +} + +void MgmRm3100CustomHandler::modeChanged() { internalState = InternalState::NONE; } + +ReturnValue_t MgmRm3100CustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(mgmRm3100::FIELD_STRENGTHS, &mgmXYZ); + poolManager.subscribeForRegularPeriodicPacket({primaryDataset.getSid(), false, 10.0}); + return returnvalue::OK; +} + +uint32_t MgmRm3100CustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { + return this->transitionDelay; +} + +void MgmRm3100CustomHandler::setToGoToNormalMode(bool enable) { goToNormalModeAtStartup = enable; } + +void MgmRm3100CustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} + +ReturnValue_t MgmRm3100CustomHandler::prepareRequest(acs::SimpleSensorMode mode) { + request.mode = mode; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(acs::MgmRm3100Request); + return returnvalue::OK; +} + +LocalPoolDataSetBase *MgmRm3100CustomHandler::getDataSetHandle(sid_t sid) { + if (sid == primaryDataset.getSid()) { + return &primaryDataset; + } + return nullptr; +} diff --git a/mission/devices/MgmRm3100CustomHandler.h b/mission/devices/MgmRm3100CustomHandler.h new file mode 100644 index 00000000..4c0c98b3 --- /dev/null +++ b/mission/devices/MgmRm3100CustomHandler.h @@ -0,0 +1,97 @@ +#ifndef MISSION_DEVICES_MGMRM3100CUSTOMHANDLER_H_ +#define MISSION_DEVICES_MGMRM3100CUSTOMHANDLER_H_ + +#include + +#include "fsfw/devicehandlers/DeviceHandlerBase.h" +#include "fsfw/globalfunctions/PeriodicOperationDivider.h" +#include "mission/devices/devicedefinitions/acsPolling.h" + +/** + * @brief Device Handler for the RM3100 geomagnetic magnetometer sensor + * (https://www.pnicorp.com/rm3100/) + * @details + * Flight manual: + * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/RM3100_MGM + */ +class MgmRm3100CustomHandler : public DeviceHandlerBase { + public: + static constexpr DeviceCommandId_t REQUEST = 0x70; + static constexpr DeviceCommandId_t REPLY = 0x77; + + static const uint8_t INTERFACE_ID = CLASS_ID::MGM_RM3100; + + //! [EXPORT] : [COMMENT] P1: TMRC value which was set, P2: 0 + static constexpr Event TMRC_SET = + event::makeEvent(SUBSYSTEM_ID::MGM_RM3100, 0x00, severity::INFO); + + //! [EXPORT] : [COMMENT] Cycle counter set. P1: First two bytes new Cycle Count X + //! P1: Second two bytes new Cycle Count Y + //! P2: New cycle count Z + static constexpr Event cycleCountersSet = + event::makeEvent(SUBSYSTEM_ID::MGM_RM3100, 0x01, severity::INFO); + + MgmRm3100CustomHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, + uint32_t transitionDelay); + virtual ~MgmRm3100CustomHandler(); + + void enablePeriodicPrintouts(bool enable, uint8_t divider); + /** + * Configure device handler to go to normal mode after startup immediately + * @param enable + */ + void setToGoToNormalMode(bool enable); + + protected: + /* DeviceHandlerBase overrides */ + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override; + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + + void fillCommandAndReplyMap() override; + void modeChanged(void) override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; + + private: + enum class InternalState { NONE, STARTUP, SHUTDOWN }; + InternalState internalState = InternalState::NONE; + bool commandExecuted = false; + mgmRm3100::Rm3100PrimaryDataset primaryDataset; + acs::MgmRm3100Request request{}; + + // uint8_t cmmRegValue = mgmRm3100::CMM_VALUE; + // uint8_t tmrcRegValue = mgmRm3100::TMRC_DEFAULT_VALUE; + // uint16_t cycleCountRegValueX = mgmRm3100::CYCLE_COUNT_VALUE; + // uint16_t cycleCountRegValueY = mgmRm3100::CYCLE_COUNT_VALUE; + // uint16_t cycleCountRegValueZ = mgmRm3100::CYCLE_COUNT_VALUE; + float scaleFactorX = 1.0 / mgmRm3100::DEFAULT_GAIN; + float scaleFactorY = 1.0 / mgmRm3100::DEFAULT_GAIN; + float scaleFactorZ = 1.0 / mgmRm3100::DEFAULT_GAIN; + + bool goToNormalModeAtStartup = false; + uint32_t transitionDelay; + PoolEntry mgmXYZ = PoolEntry(3); + bool periodicPrintout = false; + + ReturnValue_t handleCycleCountConfigCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, size_t commandDataLen); + ReturnValue_t handleCycleCommand(bool oneCycleValue, const uint8_t *commandData, + size_t commandDataLen); + + ReturnValue_t handleTmrcConfigCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen); + + ReturnValue_t prepareRequest(acs::SimpleSensorMode mode); + PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); +}; + +#endif /* MISSION_DEVICEHANDLING_MgmRm3100CustomHandler_H_ */ diff --git a/mission/devices/PcduHandler.cpp b/mission/devices/PcduHandler.cpp index 46106796..f5873fcf 100644 --- a/mission/devices/PcduHandler.cpp +++ b/mission/devices/PcduHandler.cpp @@ -18,7 +18,7 @@ PCDUHandler::PCDUHandler(object_id_t setObjectId, size_t cmdQueueSize) auto mqArgs = MqArgs(setObjectId, static_cast(this)); commandQueue = QueueFactory::instance()->createMessageQueue( cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); - pwrMutex = MutexFactory::instance()->createMutex(); + pwrLock = MutexFactory::instance()->createMutex(); } PCDUHandler::~PCDUHandler() {} @@ -41,7 +41,7 @@ ReturnValue_t PCDUHandler::performOperation(uint8_t counter) { if (pg.getReadResult() == returnvalue::OK) { if (switcherSet.p60Dock5VStack.value != switchState) { triggerEvent(power::SWITCH_HAS_CHANGED, switchState, pcdu::Switches::P60_DOCK_5V_STACK); - MutexGuard mg(pwrMutex); + MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); switchStates[pcdu::P60_DOCK_5V_STACK] = switchState; } switcherSet.p60Dock5VStack.setValid(true); @@ -179,7 +179,7 @@ void PCDUHandler::updatePdu2SwitchStates() { switcherSet.pdu2Switches[idx] = pdu2CoreHk.outputEnables[idx]; } switcherSet.pdu2Switches.setValid(true); - MutexGuard mg(pwrMutex); + MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH0_Q7S, pdu2CoreHk.outputEnables[Channels::Q7S]); checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH1_PL_PCDU_BATT_0_14V8, @@ -216,7 +216,7 @@ void PCDUHandler::updatePdu1SwitchStates() { switcherSet.pdu1Switches[idx] = pdu1CoreHk.outputEnables[idx]; } switcherSet.pdu1Switches.setValid(true); - MutexGuard mg(pwrMutex); + MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH0_TCS_BOARD_3V3, pdu1CoreHk.outputEnables[Channels::TCS_BOARD_3V3]); checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH1_SYRLINKS_12V, @@ -402,9 +402,11 @@ ReturnValue_t PCDUHandler::getSwitchState(uint8_t switchNr) const { sif::debug << "PCDUHandler::getSwitchState: Invalid switch number" << std::endl; return returnvalue::FAILED; } - pwrMutex->lockMutex(); - uint8_t currentState = switchStates[switchNr]; - pwrMutex->unlockMutex(); + uint8_t currentState = 0; + { + MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + currentState = switchStates[switchNr]; + } if (currentState == 1) { return PowerSwitchIF::SWITCH_ON; } else { diff --git a/mission/devices/PcduHandler.h b/mission/devices/PcduHandler.h index d1f3996b..45bbd392 100644 --- a/mission/devices/PcduHandler.h +++ b/mission/devices/PcduHandler.h @@ -51,7 +51,10 @@ class PCDUHandler : public PowerSwitchIF, private: uint32_t pstIntervalMs = 0; - MutexIF* pwrMutex = nullptr; + MutexIF* pwrLock = nullptr; + static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING; + static constexpr uint32_t LOCK_TIMEOUT = 20; + static constexpr char LOCK_CTX[] = "PcduHandler"; /** Housekeeping manager. Handles updates of local pool variables. */ LocalDataPoolManager poolManager; diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index 6d6032b3..7f82b202 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -366,9 +366,8 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) { statusSet.setValidity(true, true); if (statusSet.state == rws::STATE_ERROR) { - // This requires the commanding of the init reaction wheel controller command to recover - // from error state which must be handled by the FDIR instance. - triggerEvent(rws::ERROR_STATE, statusSet.state.value, 0); + // Trigger FDIR reaction, first recovery, then faulty if it doesnt fix the issue. + triggerEvent(DeviceHandlerIF::DEVICE_WANTS_HARD_REBOOT, statusSet.state.value, 0); sif::error << "RwHandler::handleGetRwStatusReply: Reaction wheel in error state" << std::endl; } diff --git a/mission/devices/ScexDeviceHandler.cpp b/mission/devices/ScexDeviceHandler.cpp index e2061a6b..9f41c8a0 100644 --- a/mission/devices/ScexDeviceHandler.cpp +++ b/mission/devices/ScexDeviceHandler.cpp @@ -319,11 +319,14 @@ void ScexDeviceHandler::performOperationHook() { auto mntPrefix = sdcMan.getCurrentMountPrefix(); if (mntPrefix != nullptr) { std::filesystem::path fullFilePath = mntPrefix; + std::error_code e; fullFilePath /= "scex"; - bool fileExists = std::filesystem::exists(fullFilePath); - + bool fileExists = std::filesystem::exists(fullFilePath, e); if (not fileExists) { - std::filesystem::create_directory(fullFilePath); + bool created = std::filesystem::create_directory(fullFilePath, e); + if (not created) { + sif::error << "Could not create SCEX directory: " << e << std::endl; + } } } uint32_t remainingMillis = finishCountdown.getRemainingMillis(); diff --git a/mission/devices/SolarArrayDeploymentHandler.cpp b/mission/devices/SolarArrayDeploymentHandler.cpp index 77c826fd..e4356630 100644 --- a/mission/devices/SolarArrayDeploymentHandler.cpp +++ b/mission/devices/SolarArrayDeploymentHandler.cpp @@ -43,15 +43,16 @@ ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCod #endif if (opDivider.checkAndIncrement()) { auto activeSdc = sdcMan.getActiveSdCard(); + std::error_code e; if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_0 and sdcMan.isSdCardUsable(activeSdc.value())) { - if (exists(SD_0_DEPL_FILE)) { + if (exists(SD_0_DEPL_FILE, e)) { // perform autonomous deployment handling performAutonomousDepl(sd::SdCard::SLOT_0, dryRunStringInFile(SD_0_DEPL_FILE)); } } else if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_1 and sdcMan.isSdCardUsable(activeSdc.value())) { - if (exists(SD_1_DEPL_FILE)) { + if (exists(SD_1_DEPL_FILE, e)) { // perform autonomous deployment handling performAutonomousDepl(sd::SdCard::SLOT_1, dryRunStringInFile(SD_1_DEPL_FILE)); } @@ -137,15 +138,16 @@ ReturnValue_t SolarArrayDeploymentHandler::performAutonomousDepl(sd::SdCard sdCa of << "phase: init\n"; of << "secs_since_start: 0\n"; }; + std::error_code e; if (sdCard == sd::SdCard::SLOT_0) { - if (not exists(SD_0_DEPLY_INFO)) { + if (not exists(SD_0_DEPLY_INFO, e)) { initFile(SD_0_DEPLY_INFO); } if (not autonomousDeplForFile(sd::SdCard::SLOT_0, SD_0_DEPLY_INFO, dryRun)) { initFile(SD_0_DEPLY_INFO); } } else if (sdCard == sd::SdCard::SLOT_1) { - if (not exists(SD_1_DEPLY_INFO)) { + if (not exists(SD_1_DEPLY_INFO, e)) { initFile(SD_1_DEPLY_INFO); } if (not autonomousDeplForFile(sd::SdCard::SLOT_1, SD_1_DEPLY_INFO, dryRun)) { diff --git a/mission/devices/SusHandler.cpp b/mission/devices/SusHandler.cpp index 0b053e99..886c7cec 100644 --- a/mission/devices/SusHandler.cpp +++ b/mission/devices/SusHandler.cpp @@ -1,231 +1,128 @@ #include "SusHandler.h" -#include -#include +#include -#include "OBSWConfig.h" +#include -SusHandler::SusHandler(object_id_t objectId, uint8_t susIdx, object_id_t comIF, CookieIF *comCookie) - : DeviceHandlerBase(objectId, comIF, comCookie), divider(5), dataset(this), susIdx(susIdx) {} +#include "fsfw/datapool/PoolReadGuard.h" -SusHandler::~SusHandler() {} +SusHandler::SusHandler(object_id_t objectId, uint8_t susIdx, object_id_t deviceCommunication, + CookieIF *comCookie) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie), dataset(this), susIdx(susIdx) {} + +SusHandler::~SusHandler() = default; void SusHandler::doStartUp() { - if (comState == ComStates::IDLE) { - comState = ComStates::WRITE_SETUP; + if (internalState != InternalState::STARTUP) { commandExecuted = false; + updatePeriodicReply(true, REPLY); + internalState = InternalState::STARTUP; } - if (comState == ComStates::WRITE_SETUP) { + if (internalState == InternalState::STARTUP) { if (commandExecuted) { - if (goToNormalModeImmediately) { - setMode(MODE_NORMAL); - } else { - setMode(_MODE_TO_ON); - } + setMode(MODE_NORMAL); + internalState = InternalState::NONE; commandExecuted = false; - if (clkMode == ClkModes::INT_CLOCKED) { - comState = ComStates::START_INT_CLOCKED_CONVERSIONS; - } else { - comState = ComStates::EXT_CLOCKED_CONVERSIONS; - } } } } void SusHandler::doShutDown() { - setMode(_MODE_POWER_DOWN); - comState = ComStates::IDLE; -} - -ReturnValue_t SusHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { - switch (comState) { - case (ComStates::IDLE): { - break; - } - case (ComStates::WRITE_SETUP): { - *id = SUS::WRITE_SETUP; - return buildCommandFromCommand(*id, nullptr, 0); - } - case (ComStates::EXT_CLOCKED_CONVERSIONS): { - *id = SUS::READ_EXT_TIMED_CONVERSIONS; - return buildCommandFromCommand(*id, nullptr, 0); - } - case (ComStates::START_INT_CLOCKED_CONVERSIONS): { - *id = SUS::START_INT_TIMED_CONVERSIONS; - comState = ComStates::READ_INT_CLOCKED_CONVERSIONS; - return buildCommandFromCommand(*id, nullptr, 0); - } - case (ComStates::READ_INT_CLOCKED_CONVERSIONS): { - *id = SUS::READ_INT_TIMED_CONVERSIONS; - comState = ComStates::START_INT_CLOCKED_CONVERSIONS; - return buildCommandFromCommand(*id, nullptr, 0); - } - case (ComStates::EXT_CLOCKED_TEMP): { - *id = SUS::READ_EXT_TIMED_TEMPS; - return buildCommandFromCommand(*id, nullptr, 0); - } + if (internalState != InternalState::SHUTDOWN) { + dataset.setValidity(false, true); + internalState = InternalState::SHUTDOWN; + commandExecuted = false; + } + if (internalState == InternalState::SHUTDOWN and commandExecuted) { + updatePeriodicReply(false, REPLY); + commandExecuted = false; + internalState = InternalState::NONE; + setMode(MODE_OFF); } - return NOTHING_TO_SEND; } ReturnValue_t SusHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { - if (comState == ComStates::WRITE_SETUP) { - *id = SUS::WRITE_SETUP; - return buildCommandFromCommand(*id, nullptr, 0); + if (internalState == InternalState::STARTUP) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::NORMAL); + } else if (internalState == InternalState::SHUTDOWN) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::OFF); } return NOTHING_TO_SEND; } +ReturnValue_t SusHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::NORMAL); +} + ReturnValue_t SusHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - using namespace max1227; - switch (deviceCommand) { - case (SUS::WRITE_SETUP): { - if (clkMode == ClkModes::INT_CLOCKED) { - cmdBuffer[0] = SUS::SETUP_INT_CLOKED; - } else { - cmdBuffer[0] = SUS::SETUP_EXT_CLOCKED; - } + return NOTHING_TO_SEND; +} - rawPacket = cmdBuffer; - rawPacketLen = 1; - break; +ReturnValue_t SusHandler::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) { + if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) { + return IGNORE_FULL_PACKET; + } + if (len != sizeof(acs::SusReply)) { + *foundLen = len; + return returnvalue::FAILED; + } + *foundId = REPLY; + *foundLen = len; + if (internalState == InternalState::SHUTDOWN) { + commandExecuted = true; + } + return returnvalue::OK; +} +ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { + const auto *reply = reinterpret_cast(packet); + if (reply->dataWasSet) { + if (internalState == InternalState::STARTUP) { + commandExecuted = true; } - case (SUS::START_INT_TIMED_CONVERSIONS): { - std::memset(cmdBuffer, 0, sizeof(cmdBuffer)); - cmdBuffer[0] = max1227::buildResetByte(true); - cmdBuffer[1] = SUS::CONVERSION; - rawPacket = cmdBuffer; - rawPacketLen = 2; - break; - } - case (SUS::READ_INT_TIMED_CONVERSIONS): { - std::memset(cmdBuffer, 0, sizeof(cmdBuffer)); - rawPacket = cmdBuffer; - rawPacketLen = SUS::SIZE_READ_INT_CONVERSIONS; - break; - } - case (SUS::READ_EXT_TIMED_CONVERSIONS): { - std::memset(cmdBuffer, 0, sizeof(cmdBuffer)); - rawPacket = cmdBuffer; - for (uint8_t idx = 0; idx < 6; idx++) { - cmdBuffer[idx * 2] = buildConvByte(ScanModes::N_ONCE, idx, false); - cmdBuffer[idx * 2 + 1] = 0; - } - cmdBuffer[12] = 0x00; - rawPacketLen = SUS::SIZE_READ_EXT_CONVERSIONS; - break; - } - case (SUS::READ_EXT_TIMED_TEMPS): { - cmdBuffer[0] = buildConvByte(ScanModes::N_ONCE, 0, true); - std::memset(cmdBuffer + 1, 0, 24); - rawPacket = cmdBuffer; - rawPacketLen = 25; - break; - } - default: - return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + PoolReadGuard pg(&dataset); + dataset.setValidity(true, true); + dataset.tempC = max1227::getTemperature(reply->tempRaw); + std::memcpy(dataset.channels.value, reply->channelsRaw, sizeof(reply->channelsRaw)); } return returnvalue::OK; } void SusHandler::fillCommandAndReplyMap() { - insertInCommandAndReplyMap(SUS::WRITE_SETUP, 1); - insertInCommandAndReplyMap(SUS::START_INT_TIMED_CONVERSIONS, 1); - insertInCommandAndReplyMap(SUS::READ_INT_TIMED_CONVERSIONS, 1, &dataset, - SUS::SIZE_READ_INT_CONVERSIONS); - insertInCommandAndReplyMap(SUS::READ_EXT_TIMED_CONVERSIONS, 1, &dataset, - SUS::SIZE_READ_EXT_CONVERSIONS); - insertInCommandAndReplyMap(SUS::READ_EXT_TIMED_TEMPS, 1); + insertInCommandMap(REQUEST); + insertInReplyMap(REPLY, 5, nullptr, 0, true); } -ReturnValue_t SusHandler::scanForReply(const uint8_t *start, size_t remainingSize, - DeviceCommandId_t *foundId, size_t *foundLen) { - *foundId = this->getPendingCommand(); - *foundLen = remainingSize; - return returnvalue::OK; -} +void SusHandler::setToGoToNormalMode(bool enable) { this->goToNormalMode = enable; } -ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - switch (id) { - case SUS::WRITE_SETUP: { - if (getMode() == _MODE_START_UP) { - commandExecuted = true; - } - return returnvalue::OK; - } - case SUS::START_INT_TIMED_CONVERSIONS: { - return returnvalue::OK; - } - case SUS::READ_INT_TIMED_CONVERSIONS: { - PoolReadGuard readSet(&dataset); - dataset.temperatureCelcius = max1227::getTemperature(((packet[0] & 0x0f) << 8) | packet[1]); - for (uint8_t idx = 0; idx < 6; idx++) { - dataset.channels[idx] = packet[idx * 2 + 2] << 8 | packet[idx * 2 + 3]; - } - dataset.setValidity(true, true); - printDataset(); - break; - } - case (SUS::READ_EXT_TIMED_CONVERSIONS): { - PoolReadGuard readSet(&dataset); - for (uint8_t idx = 0; idx < 6; idx++) { - dataset.channels[idx] = packet[idx * 2 + 1] << 8 | packet[idx * 2 + 2]; - } - dataset.channels.setValid(true); - // Read temperature in next read cycle - if (clkMode == ClkModes::EXT_CLOCKED_WITH_TEMP) { - comState = ComStates::EXT_CLOCKED_TEMP; - } - printDataset(); - break; - } - case (SUS::READ_EXT_TIMED_TEMPS): { - PoolReadGuard readSet(&dataset); - dataset.temperatureCelcius = max1227::getTemperature(((packet[23] & 0x0f) << 8) | packet[24]); - dataset.temperatureCelcius.setValid(true); - comState = ComStates::EXT_CLOCKED_CONVERSIONS; - break; - } - default: { - sif::debug << "SusHandler::interpretDeviceReply: Unknown reply id" << std::endl; - return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; - } - } - return returnvalue::OK; -} +uint32_t SusHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { return transitionDelay; } -uint32_t SusHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 3000; } +void SusHandler::modeChanged(void) { internalState = InternalState::NONE; } ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(SUS::TEMPERATURE_C, &tempC); - localDataPoolMap.emplace(SUS::CHANNEL_VEC, &channelVec); + localDataPoolMap.emplace(susMax1227::TEMPERATURE_C, &tempC); + localDataPoolMap.emplace(susMax1227::CHANNEL_VEC, &channelVec); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(dataset.getSid(), false, 5.0)); return returnvalue::OK; } -void SusHandler::setToGoToNormalMode(bool enable) { this->goToNormalModeImmediately = enable; } +ReturnValue_t SusHandler::prepareRequest(acs::SimpleSensorMode mode) { + request.mode = mode; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(acs::SusRequest); + return returnvalue::OK; +} -void SusHandler::printDataset() { - if (periodicPrintout) { - if (divider.checkAndIncrement()) { - sif::info << "SUS ADC " << static_cast(susIdx) << " hex [" << std::setfill('0') - << std::hex; - for (uint8_t idx = 0; idx < 6; idx++) { - sif::info << std::setw(3) << dataset.channels[idx]; - if (idx < 6 - 1) { - sif::info << ","; - } - } - sif::info << "] | T[C] " << std::dec << dataset.temperatureCelcius.value << std::endl; - } +LocalPoolDataSetBase *SusHandler::getDataSetHandle(sid_t sid) { + if (sid == dataset.getSid()) { + return &dataset; } -} - -void SusHandler::enablePeriodicPrintout(bool enable, uint8_t divider) { - this->periodicPrintout = enable; - this->divider.setDivider(divider); + return nullptr; } diff --git a/mission/devices/SusHandler.h b/mission/devices/SusHandler.h index 5645c47d..6eb781db 100644 --- a/mission/devices/SusHandler.h +++ b/mission/devices/SusHandler.h @@ -1,92 +1,68 @@ -#ifndef MISSION_DEVICES_SUSHANDLER_H_ -#define MISSION_DEVICES_SUSHANDLER_H_ +#ifndef MISSION_DEVICES_SusHandler_H_ +#define MISSION_DEVICES_SusHandler_H_ -#include +#include +#include -#include "devicedefinitions/SusDefinitions.h" -#include "events/subsystemIdRanges.h" +#include "fsfw/devicehandlers/DeviceHandlerBase.h" #include "fsfw/globalfunctions/PeriodicOperationDivider.h" -#include "mission/devices/max1227.h" -#include "returnvalues/classIds.h" +#include "mission/devices/devicedefinitions/acsPolling.h" +#include "mission/devices/devicedefinitions/susMax1227Helpers.h" -/** - * @brief This is the device handler class for the SUS sensor based on the MAX1227 ADC. - * - * @details - * Datasheet of MAX1227: https://datasheets.maximintegrated.com/en/ds/MAX1227-MAX1231.pdf - * Details about the SUS electronic can be found at - * https://egit.irs.uni-stuttgart.de/eive/eive_dokumente/src/branch/master/400_Raumsegment/443_SunSensorDocumentation/release - * - * @note When adding a SusHandler to the polling sequence table make sure to add a slot with - * the executionStep FIRST_WRITE. Otherwise the communication sequence will never be - * started. - * - * @author J. Meier - */ class SusHandler : public DeviceHandlerBase { public: - enum ClkModes { INT_CLOCKED, EXT_CLOCKED, EXT_CLOCKED_WITH_TEMP }; + static constexpr DeviceCommandId_t REQUEST = 0x70; + static constexpr DeviceCommandId_t REPLY = 0x77; - static const uint8_t FIRST_WRITE = 7; + static const uint8_t INTERFACE_ID = CLASS_ID::SUS_HANDLER; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SUS_BOARD_ASS; - SusHandler(object_id_t objectId, uint8_t susIdx, object_id_t comIF, CookieIF* comCookie); + SusHandler(uint32_t objectId, uint8_t susIdx, object_id_t deviceCommunication, + CookieIF *comCookie); virtual ~SusHandler(); - void enablePeriodicPrintout(bool enable, uint8_t divider); - + void enablePeriodicPrintouts(bool enable, uint8_t divider); void setToGoToNormalMode(bool enable); protected: - void doStartUp() override; + /** DeviceHandlerBase overrides */ void doShutDown() override; - ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override; - ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t* id) override; - void fillCommandAndReplyMap() override; - ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, + void doStartUp() override; + uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) override; - ReturnValue_t scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, - size_t* foundLen) override; - ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) override; - uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; - ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, - LocalDataPoolManager& poolManager) override; + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + void fillCommandAndReplyMap() override; + void modeChanged(void) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; private: - static const uint8_t INTERFACE_ID = CLASS_ID::SUS_HANDLER; + susMax1227::SusDataset dataset; + acs::SusRequest request{}; + uint8_t susIdx; - static const ReturnValue_t ERROR_UNLOCK_MUTEX = MAKE_RETURN_CODE(0xA0); - static const ReturnValue_t ERROR_LOCK_MUTEX = MAKE_RETURN_CODE(0xA1); + uint32_t transitionDelay = 1000; + bool goToNormalMode = false; - enum class ComStates { - IDLE, - WRITE_SETUP, - EXT_CLOCKED_CONVERSIONS, - EXT_CLOCKED_TEMP, - START_INT_CLOCKED_CONVERSIONS, - READ_INT_CLOCKED_CONVERSIONS - }; - - bool periodicPrintout = false; - PeriodicOperationDivider divider; - bool goToNormalModeImmediately = false; - bool commandExecuted = false; - - SUS::SusDataset dataset; - // Read temperature in each alternating communication step when using - // externally clocked mode - ClkModes clkMode = ClkModes::INT_CLOCKED; PoolEntry tempC = PoolEntry({0.0}); PoolEntry channelVec = PoolEntry({0, 0, 0, 0, 0, 0}); - uint8_t susIdx = 0; - uint8_t cmdBuffer[SUS::MAX_CMD_SIZE]; - ComStates comState = ComStates::IDLE; + enum class InternalState { + NONE, + STARTUP, + SHUTDOWN, + }; - MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; - uint32_t timeoutMs = 20; - void printDataset(); + InternalState internalState = InternalState::NONE; + bool commandExecuted = false; - MutexIF* spiMutex = nullptr; + ReturnValue_t prepareRequest(acs::SimpleSensorMode mode); }; -#endif /* MISSION_DEVICES_SUSHANDLER_H_ */ +#endif /* MISSION_DEVICES_SusHandler_H_ */ diff --git a/mission/devices/devicedefinitions/CMakeLists.txt b/mission/devices/devicedefinitions/CMakeLists.txt index b2394b2a..eab4d4cb 100644 --- a/mission/devices/devicedefinitions/CMakeLists.txt +++ b/mission/devices/devicedefinitions/CMakeLists.txt @@ -1,2 +1,2 @@ target_sources(${LIB_EIVE_MISSION} PRIVATE ScexDefinitions.cpp rwHelpers.cpp - imtqHelpers.cpp) + imtqHelpers.cpp gyroAdisHelpers.cpp) diff --git a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h index 4c4543d0..43783c82 100644 --- a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h +++ b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h @@ -6,7 +6,7 @@ #include -namespace L3GD20H { +namespace l3gd20h { /* Actual size is 15 but we round up a bit */ static constexpr size_t MAX_BUFFER_SIZE = 16; @@ -104,27 +104,27 @@ static constexpr uint32_t GYRO_DATASET_ID = READ_REGS; enum GyroPoolIds : lp_id_t { ANG_VELOC_X, ANG_VELOC_Y, ANG_VELOC_Z, TEMPERATURE }; -} // namespace L3GD20H +} // namespace l3gd20h class GyroPrimaryDataset : public StaticLocalDataSet<5> { public: /** Constructor for data users like controllers */ GyroPrimaryDataset(object_id_t mgmId) - : StaticLocalDataSet(sid_t(mgmId, L3GD20H::GYRO_DATASET_ID)) { + : StaticLocalDataSet(sid_t(mgmId, l3gd20h::GYRO_DATASET_ID)) { setAllVariablesReadOnly(); } /* Angular velocities in degrees per second (DPS) */ - lp_var_t angVelocX = lp_var_t(sid.objectId, L3GD20H::ANG_VELOC_X, this); - lp_var_t angVelocY = lp_var_t(sid.objectId, L3GD20H::ANG_VELOC_Y, this); - lp_var_t angVelocZ = lp_var_t(sid.objectId, L3GD20H::ANG_VELOC_Z, this); - lp_var_t temperature = lp_var_t(sid.objectId, L3GD20H::TEMPERATURE, this); + lp_var_t angVelocX = lp_var_t(sid.objectId, l3gd20h::ANG_VELOC_X, this); + lp_var_t angVelocY = lp_var_t(sid.objectId, l3gd20h::ANG_VELOC_Y, this); + lp_var_t angVelocZ = lp_var_t(sid.objectId, l3gd20h::ANG_VELOC_Z, this); + lp_var_t temperature = lp_var_t(sid.objectId, l3gd20h::TEMPERATURE, this); private: friend class GyroHandlerL3GD20H; /** Constructor for the data creator */ GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner) - : StaticLocalDataSet(hkOwner, L3GD20H::GYRO_DATASET_ID) {} + : StaticLocalDataSet(hkOwner, l3gd20h::GYRO_DATASET_ID) {} }; #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */ diff --git a/mission/devices/devicedefinitions/acsPolling.h b/mission/devices/devicedefinitions/acsPolling.h new file mode 100644 index 00000000..612512a7 --- /dev/null +++ b/mission/devices/devicedefinitions/acsPolling.h @@ -0,0 +1,92 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ + +#include "fsfw/thermal/tcsDefinitions.h" +#include "gyroAdisHelpers.h" + +namespace acs { + +enum SimpleSensorMode { NORMAL = 0, OFF = 1 }; + +struct Adis1650XRequest { + SimpleSensorMode mode; + adis1650x::Type type; +}; + +struct Adis1650XConfig { + uint16_t diagStat; + uint16_t filterSetting; + uint16_t rangMdl; + uint16_t mscCtrlReg; + uint16_t decRateReg; + uint16_t prodId; +}; + +struct Adis1650XData { + double sensitivity = 0.0; + // Angular velocities in all axes (X, Y and Z) + int16_t angVelocities[3]{}; + double accelScaling = 0.0; + // Accelerations in all axes + int16_t accelerations[3]{}; + int16_t temperatureRaw = thermal::INVALID_TEMPERATURE; +}; + +struct Adis1650XReply { + bool cfgWasSet = false; + Adis1650XConfig cfg; + bool dataWasSet = false; + Adis1650XData data; +}; + +struct GyroL3gRequest { + SimpleSensorMode mode = SimpleSensorMode::OFF; + uint8_t ctrlRegs[5]{}; +}; + +struct GyroL3gReply { + bool cfgWasSet = false; + uint8_t statusReg; + // Angular velocities in all axes (X, Y and Z) + int16_t angVelocities[3]{}; + int8_t tempOffsetRaw = 0; + uint8_t ctrlRegs[5]{}; + float sensitivity = 0.0; +}; + +struct MgmRm3100Request { + SimpleSensorMode mode = SimpleSensorMode::OFF; +}; + +struct MgmRm3100Reply { + bool dataWasRead = false; + float scaleFactors[3]{}; + int32_t mgmValuesRaw[3]{}; +}; + +struct MgmLis3Request { + SimpleSensorMode mode = SimpleSensorMode::OFF; +}; + +struct MgmLis3Reply { + bool dataWasSet = false; + float sensitivity = 0.0; + int16_t mgmValuesRaw[3]{}; + bool temperatureWasSet = false; + int16_t temperatureRaw = thermal::INVALID_TEMPERATURE; +}; + +struct SusRequest { + SimpleSensorMode mode = SimpleSensorMode::OFF; +}; + +struct SusReply { + bool cfgWasSet = false; + bool dataWasSet = false; + uint16_t tempRaw = 0; + uint16_t channelsRaw[6]{}; +}; + +} // namespace acs + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ */ diff --git a/mission/devices/devicedefinitions/gyroAdisHelpers.cpp b/mission/devices/devicedefinitions/gyroAdisHelpers.cpp new file mode 100644 index 00000000..0f41b058 --- /dev/null +++ b/mission/devices/devicedefinitions/gyroAdisHelpers.cpp @@ -0,0 +1,54 @@ +#include "gyroAdisHelpers.h" + +size_t adis1650x::prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, + size_t maxLen) { + if (len * 2 + 2 > maxLen) { + return 0; + } + for (size_t idx = 0; idx < len; idx++) { + outBuf[idx * 2] = regList[idx]; + outBuf[idx * 2 + 1] = 0x00; + } + outBuf[len * 2] = 0x00; + outBuf[len * 2 + 1] = 0x00; + return len * 2 + 2; +} + +adis1650x::BurstModes adis1650x::burstModeFromMscCtrl(uint16_t mscCtrl) { + if ((mscCtrl & adis1650x::BURST_32_BIT) == adis1650x::BURST_32_BIT) { + if ((mscCtrl & adis1650x::BURST_SEL_BIT) == adis1650x::BURST_SEL_BIT) { + return BurstModes::BURST_32_BURST_SEL_1; + } else { + return BurstModes::BURST_32_BURST_SEL_0; + } + } else { + if ((mscCtrl & adis1650x::BURST_SEL_BIT) == adis1650x::BURST_SEL_BIT) { + return BurstModes::BURST_16_BURST_SEL_1; + } else { + return BurstModes::BURST_16_BURST_SEL_0; + } + } +} + +double adis1650x::rangMdlToSensitivity(uint16_t rangMdl) { + adis1650x::RangMdlBitfield bitfield = + static_cast((rangMdl >> 2) & 0b11); + switch (bitfield) { + case (adis1650x::RangMdlBitfield::RANGE_125_1BMLZ): { + return SENSITIVITY_1BMLZ; + } + case (adis1650x::RangMdlBitfield::RANGE_500_2BMLZ): { + return SENSITIVITY_2BMLZ; + } + case (adis1650x::RangMdlBitfield::RANGE_2000_3BMLZ): { + return SENSITIVITY_3BMLZ; + } + case (RangMdlBitfield::RESERVED): + default: { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl; +#endif + return 0.0; + } + } +} diff --git a/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h b/mission/devices/devicedefinitions/gyroAdisHelpers.h similarity index 72% rename from mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h rename to mission/devices/devicedefinitions/gyroAdisHelpers.h index 3cfda2d4..09f0aa1e 100644 --- a/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h +++ b/mission/devices/devicedefinitions/gyroAdisHelpers.h @@ -6,9 +6,20 @@ #include "fsfw/datapoollocal/StaticLocalDataSet.h" #include "fsfw/devicehandlers/DeviceHandlerIF.h" -namespace ADIS1650X { +namespace adis1650x { -enum class Type { ADIS16505, ADIS16507 }; +enum class BurstModes { + BURST_16_BURST_SEL_0, + BURST_16_BURST_SEL_1, + BURST_32_BURST_SEL_0, + BURST_32_BURST_SEL_1 +}; + +size_t prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, size_t maxLen); +BurstModes burstModeFromMscCtrl(uint16_t mscCtrl); +double rangMdlToSensitivity(uint16_t rangMdl); + +enum class Type : uint8_t { ADIS16505, ADIS16507 }; static constexpr size_t MAXIMUM_REPLY_SIZE = 64; static constexpr uint8_t WRITE_MASK = 0b1000'0000; @@ -67,6 +78,9 @@ static constexpr DeviceCommandId_t RESET_SENSOR_CONFIGURATION = 30; static constexpr DeviceCommandId_t SW_RESET = 31; static constexpr DeviceCommandId_t PRINT_CURRENT_CONFIGURATION = 32; +static constexpr DeviceCommandId_t REQUEST = 0x70; +static constexpr DeviceCommandId_t REPLY = 0x77; + static constexpr uint16_t BURST_32_BIT = 1 << 9; static constexpr uint16_t BURST_SEL_BIT = 1 << 8; static constexpr uint16_t LIN_ACCEL_COMPENSATION_BIT = 1 << 7; @@ -111,57 +125,57 @@ enum FilterSettings : uint8_t { SIXTYFOUR_TAPS = 6 }; -} // namespace ADIS1650X +} // namespace adis1650x class AdisGyroPrimaryDataset : public StaticLocalDataSet<8> { public: /** Constructor for data users like controllers */ AdisGyroPrimaryDataset(object_id_t adisId) - : StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_DATASET_ID)) { + : StaticLocalDataSet(sid_t(adisId, adis1650x::ADIS_DATASET_ID)) { setAllVariablesReadOnly(); } /* Angular velocities in degrees per second (DPS) */ - lp_var_t angVelocX = lp_var_t(sid.objectId, ADIS1650X::ANG_VELOC_X, this); - lp_var_t angVelocY = lp_var_t(sid.objectId, ADIS1650X::ANG_VELOC_Y, this); - lp_var_t angVelocZ = lp_var_t(sid.objectId, ADIS1650X::ANG_VELOC_Z, this); - lp_var_t accelX = lp_var_t(sid.objectId, ADIS1650X::ACCELERATION_X, this); - lp_var_t accelY = lp_var_t(sid.objectId, ADIS1650X::ACCELERATION_Y, this); - lp_var_t accelZ = lp_var_t(sid.objectId, ADIS1650X::ACCELERATION_Z, this); - lp_var_t temperature = lp_var_t(sid.objectId, ADIS1650X::TEMPERATURE, this); + lp_var_t angVelocX = lp_var_t(sid.objectId, adis1650x::ANG_VELOC_X, this); + lp_var_t angVelocY = lp_var_t(sid.objectId, adis1650x::ANG_VELOC_Y, this); + lp_var_t angVelocZ = lp_var_t(sid.objectId, adis1650x::ANG_VELOC_Z, this); + lp_var_t accelX = lp_var_t(sid.objectId, adis1650x::ACCELERATION_X, this); + lp_var_t accelY = lp_var_t(sid.objectId, adis1650x::ACCELERATION_Y, this); + lp_var_t accelZ = lp_var_t(sid.objectId, adis1650x::ACCELERATION_Z, this); + lp_var_t temperature = lp_var_t(sid.objectId, adis1650x::TEMPERATURE, this); private: - friend class GyroADIS1650XHandler; + friend class GyrAdis1650XHandler; friend class GyroAdisDummy; /** Constructor for the data creator */ AdisGyroPrimaryDataset(HasLocalDataPoolIF* hkOwner) - : StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_DATASET_ID) {} + : StaticLocalDataSet(hkOwner, adis1650x::ADIS_DATASET_ID) {} }; class AdisGyroConfigDataset : public StaticLocalDataSet<5> { public: /** Constructor for data users like controllers */ AdisGyroConfigDataset(object_id_t adisId) - : StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_CFG_DATASET_ID)) { + : StaticLocalDataSet(sid_t(adisId, adis1650x::ADIS_CFG_DATASET_ID)) { setAllVariablesReadOnly(); } lp_var_t diagStatReg = - lp_var_t(sid.objectId, ADIS1650X::DIAG_STAT_REGISTER, this); + lp_var_t(sid.objectId, adis1650x::DIAG_STAT_REGISTER, this); lp_var_t filterSetting = - lp_var_t(sid.objectId, ADIS1650X::FILTER_SETTINGS, this); - lp_var_t rangMdl = lp_var_t(sid.objectId, ADIS1650X::RANG_MDL, this); + lp_var_t(sid.objectId, adis1650x::FILTER_SETTINGS, this); + lp_var_t rangMdl = lp_var_t(sid.objectId, adis1650x::RANG_MDL, this); lp_var_t mscCtrlReg = - lp_var_t(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER, this); + lp_var_t(sid.objectId, adis1650x::MSC_CTRL_REGISTER, this); lp_var_t decRateReg = - lp_var_t(sid.objectId, ADIS1650X::DEC_RATE_REGISTER, this); + lp_var_t(sid.objectId, adis1650x::DEC_RATE_REGISTER, this); private: - friend class GyroADIS1650XHandler; + friend class GyrAdis1650XHandler; /** Constructor for the data creator */ AdisGyroConfigDataset(HasLocalDataPoolIF* hkOwner) - : StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_CFG_DATASET_ID) {} + : StaticLocalDataSet(hkOwner, adis1650x::ADIS_CFG_DATASET_ID) {} }; #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ */ diff --git a/mission/devices/devicedefinitions/imtqHelpers.h b/mission/devices/devicedefinitions/imtqHelpers.h index 125b80c9..87b69f8b 100644 --- a/mission/devices/devicedefinitions/imtqHelpers.h +++ b/mission/devices/devicedefinitions/imtqHelpers.h @@ -10,19 +10,7 @@ class ImtqHandler; namespace imtq { -enum ComStep : uint8_t { - DHB_OP = 0, - START_MEASURE_SEND = 1, - START_MEASURE_GET = 2, - READ_MEASURE_SEND = 3, - READ_MEASURE_GET = 4, - START_ACTUATE_SEND = 5, - START_ACTUATE_GET = 6, - READ_ACTUATE_SEND = 7, - READ_ACTUATE_GET = 8, -}; - -enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE }; +enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE, DO_NOTHING }; enum class SpecialRequest : uint8_t { NONE = 0, @@ -35,6 +23,26 @@ enum class SpecialRequest : uint8_t { GET_SELF_TEST_RESULT = 7 }; +struct Request { + imtq::RequestType request = imtq::RequestType::MEASURE_NO_ACTUATION; + imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE; + uint8_t integrationTimeSel = 3; + int16_t dipoles[3]{}; + uint16_t torqueDuration = 0; +}; + +enum ComStep : uint8_t { + DHB_OP = 0, + START_MEASURE_SEND = 1, + START_MEASURE_GET = 2, + READ_MEASURE_SEND = 3, + READ_MEASURE_GET = 4, + START_ACTUATE_SEND = 5, + START_ACTUATE_GET = 6, + READ_ACTUATE_SEND = 7, + READ_ACTUATE_GET = 8, +}; + static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER; static constexpr ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0); @@ -53,7 +61,8 @@ static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(0xA7); namespace cmdIds { static constexpr DeviceCommandId_t REQUEST = 0x70; -static constexpr DeviceCommandId_t REPLY = 0x71; +static constexpr DeviceCommandId_t REPLY_NO_TORQUE = 0x71; +static constexpr DeviceCommandId_t REPLY_WITH_TORQUE = 0x72; static const DeviceCommandId_t START_ACTUATION_DIPOLE = 0x2; static const DeviceCommandId_t POS_X_SELF_TEST = 0x7; static const DeviceCommandId_t NEG_X_SELF_TEST = 0x8; @@ -195,20 +204,28 @@ enum PoolIds : lp_id_t { ANALOG_VOLTAGE_MV, DIGITAL_CURRENT, ANALOG_CURRENT, - COIL_X_CURRENT, - COIL_Y_CURRENT, - COIL_Z_CURRENT, - COIL_X_TEMPERATURE, - COIL_Y_TEMPERATURE, - COIL_Z_TEMPERATURE, + COIL_CURRENTS, + COIL_TEMPERATURES, MCU_TEMPERATURE, + + DIGITAL_VOLTAGE_MV_WT, + ANALOG_VOLTAGE_MV_WT, + DIGITAL_CURRENT_WT, + ANALOG_CURRENT_WT, + COIL_CURRENTS_WT, + COIL_TEMPERATURES_WT, + MCU_TEMPERATURE_WT, + MGM_CAL_NT, ACTUATION_CAL_STATUS, + MTM_RAW, ACTUATION_RAW_STATUS, - DIPOLES_X, - DIPOLES_Y, - DIPOLES_Z, + + MTM_RAW_WT, + ACTUATION_RAW_STATUS_WT, + + DIPOLES_ID, CURRENT_TORQUE_DURATION, INIT_POS_X_ERR, @@ -476,34 +493,56 @@ class StatusDataset : public StaticLocalDataSet<4> { class HkDataset : public StaticLocalDataSet { public: - HkDataset(HasLocalDataPoolIF* owner, uint32_t setId) : StaticLocalDataSet(owner, setId) {} + HkDataset(HasLocalDataPoolIF* owner, uint32_t setId, std::array pids) + : StaticLocalDataSet(owner, setId), + digitalVoltageMv(sid.objectId, pids[0], this), + analogVoltageMv(sid.objectId, pids[1], this), + digitalCurrentmA(sid.objectId, pids[2], this), + analogCurrentmA(sid.objectId, pids[3], this), + coilCurrentsMilliamps(sid.objectId, pids[4], this), + /** All temperatures in [C] for X, Y, Z */ + coilTemperatures(sid.objectId, pids[5], this), + mcuTemperature(sid.objectId, pids[6], this) {} - HkDataset(object_id_t objectId, uint32_t setId) : StaticLocalDataSet(sid_t(objectId, setId)) {} + HkDataset(object_id_t objectId, uint32_t setId, std::array pids) + : StaticLocalDataSet(sid_t(objectId, setId)), + digitalVoltageMv(sid.objectId, pids[0], this), + analogVoltageMv(sid.objectId, pids[1], this), + digitalCurrentmA(sid.objectId, pids[2], this), + analogCurrentmA(sid.objectId, pids[3], this), + coilCurrentsMilliamps(sid.objectId, pids[4], this), + /** All temperatures in [C] for X, Y, Z */ + coilTemperatures(sid.objectId, pids[5], this), + mcuTemperature(sid.objectId, pids[6], this) {} // Engineering HK variables - 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); - lp_var_t digitalCurrentmA = lp_var_t(sid.objectId, DIGITAL_CURRENT, this); - lp_var_t analogCurrentmA = lp_var_t(sid.objectId, ANALOG_CURRENT, this); - lp_var_t coilXCurrentmA = lp_var_t(sid.objectId, COIL_X_CURRENT, this); - lp_var_t coilYCurrentmA = lp_var_t(sid.objectId, COIL_Y_CURRENT, this); - lp_var_t coilZCurrentmA = lp_var_t(sid.objectId, COIL_Z_CURRENT, this); - /** All temperatures in [�C] */ - lp_var_t coilXTemperature = lp_var_t(sid.objectId, COIL_X_TEMPERATURE, this); - lp_var_t coilYTemperature = lp_var_t(sid.objectId, COIL_Y_TEMPERATURE, this); - lp_var_t coilZTemperature = lp_var_t(sid.objectId, COIL_Z_TEMPERATURE, this); - lp_var_t mcuTemperature = lp_var_t(sid.objectId, MCU_TEMPERATURE, this); + lp_var_t digitalVoltageMv; + lp_var_t analogVoltageMv; + lp_var_t digitalCurrentmA; + lp_var_t analogCurrentmA; + lp_vec_t coilCurrentsMilliamps; + /** All temperatures in [C] for X, Y, Z */ + lp_vec_t coilTemperatures; + lp_var_t mcuTemperature; + + private: }; class HkDatasetNoTorque : public HkDataset { public: - HkDatasetNoTorque(HasLocalDataPoolIF* owner) : HkDataset(owner, imtq::SetIds::ENG_HK_NO_TORQUE) {} + HkDatasetNoTorque(HasLocalDataPoolIF* owner) + : HkDataset(owner, imtq::SetIds::ENG_HK_NO_TORQUE, + {DIGITAL_VOLTAGE_MV, ANALOG_VOLTAGE_MV, DIGITAL_CURRENT, ANALOG_CURRENT, + COIL_CURRENTS, COIL_TEMPERATURES, MCU_TEMPERATURE}) {} }; class HkDatasetWithTorque : public HkDataset { public: HkDatasetWithTorque(HasLocalDataPoolIF* owner) - : HkDataset(owner, imtq::SetIds::ENG_HK_SET_WITH_TORQUE) {} + : HkDataset(owner, imtq::SetIds::ENG_HK_SET_WITH_TORQUE, + {DIGITAL_VOLTAGE_MV_WT, ANALOG_VOLTAGE_MV_WT, DIGITAL_CURRENT_WT, + ANALOG_CURRENT_WT, COIL_CURRENTS_WT, COIL_TEMPERATURES_WT, MCU_TEMPERATURE_WT}) { + } }; /** * @@ -529,32 +568,39 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet { public: - RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId) - : StaticLocalDataSet(owner, setId) {} + RawMtmMeasurementSet(object_id_t objectId, uint32_t setId, std::array pids) + : StaticLocalDataSet(sid_t(objectId, setId)), + mtmRawNt(sid.objectId, pids.at(0), this), + coilActuationStatus(sid.objectId, pids.at(1), this) {} - RawMtmMeasurementSet(object_id_t objectId, uint32_t setId) - : StaticLocalDataSet(sid_t(objectId, setId)) {} + RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId, std::array pids) + : StaticLocalDataSet(owner, setId), + mtmRawNt(sid.objectId, pids.at(0), this), + coilActuationStatus(sid.objectId, pids.at(1), this) {} /** The unit of all measurements is nT */ - lp_vec_t mtmRawNt = lp_vec_t(sid.objectId, MTM_RAW, this); + lp_vec_t mtmRawNt; /** 1 if coils were actuating during measurement otherwise 0 */ - lp_var_t coilActuationStatus = - lp_var_t(sid.objectId, ACTUATION_RAW_STATUS, this); + lp_var_t coilActuationStatus; }; class RawMtmMeasurementNoTorque : public RawMtmMeasurementSet { public: RawMtmMeasurementNoTorque(HasLocalDataPoolIF* owner) - : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_NO_TORQUE) {} + : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_NO_TORQUE, + {PoolIds::MTM_RAW, PoolIds::ACTUATION_RAW_STATUS}) {} RawMtmMeasurementNoTorque(object_id_t objectId) - : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_NO_TORQUE) {} + : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_NO_TORQUE, + {PoolIds::MTM_RAW, PoolIds::ACTUATION_RAW_STATUS}) {} }; class RawMtmMeasurementWithTorque : public RawMtmMeasurementSet { public: RawMtmMeasurementWithTorque(HasLocalDataPoolIF* owner) - : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_WITH_TORQUE) {} + : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_WITH_TORQUE, + {PoolIds::MTM_RAW_WT, PoolIds::ACTUATION_RAW_STATUS_WT}) {} RawMtmMeasurementWithTorque(object_id_t objectId) - : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_WITH_TORQUE) {} + : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_WITH_TORQUE, + {PoolIds::MTM_RAW_WT, PoolIds::ACTUATION_RAW_STATUS_WT}) {} }; /** @@ -608,28 +654,16 @@ class DipoleActuationSet : public StaticLocalDataSet<4> { void setDipoles(int16_t xDipole_, int16_t yDipole_, int16_t zDipole_, uint16_t currentTorqueDurationMs_) { - if (xDipole.value != xDipole_) { - xDipole = xDipole_; - } - if (yDipole.value != yDipole_) { - yDipole = yDipole_; - } - if (zDipole.value != zDipole_) { - zDipole = zDipole_; - } + dipoles[0] = xDipole_; + dipoles[1] = yDipole_; + dipoles[2] = zDipole_; currentTorqueDurationMs = currentTorqueDurationMs_; } - void getDipoles(int16_t& xDipole_, int16_t& yDipole_, int16_t& zDipole_) { - xDipole_ = xDipole.value; - yDipole_ = yDipole.value; - zDipole_ = zDipole.value; - } + const int16_t* getDipoles() const { return dipoles.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_vec_t dipoles = lp_vec_t(sid.objectId, DIPOLES_ID, this); lp_var_t currentTorqueDurationMs = lp_var_t(sid.objectId, CURRENT_TORQUE_DURATION, this); }; @@ -1098,80 +1132,21 @@ class NegZSelfTestSet : public StaticLocalDataSet { } // namespace imtq -struct ImtqRequest { - friend class ImtqHandler; - - public: - static constexpr size_t REQUEST_LEN = 10; - - ImtqRequest(const uint8_t* data, size_t maxSize) - : rawData(const_cast(data)), maxSize(maxSize) {} - - imtq::RequestType getRequestType() const { return static_cast(rawData[0]); } - - void setMeasureRequest(imtq::SpecialRequest specialRequest) { - rawData[0] = static_cast(imtq::RequestType::MEASURE_NO_ACTUATION); - rawData[1] = static_cast(specialRequest); - } - - void setActuateRequest(int16_t dipoleX, int16_t dipoleY, int16_t dipoleZ, - uint16_t torqueDuration) { - size_t dummy = 0; - rawData[0] = static_cast(imtq::RequestType::ACTUATE); - rawData[1] = static_cast(imtq::SpecialRequest::NONE); - uint8_t* serPtr = rawData + 2; - SerializeAdapter::serialize(&dipoleX, &serPtr, &dummy, maxSize, - SerializeIF::Endianness::MACHINE); - SerializeAdapter::serialize(&dipoleY, &serPtr, &dummy, maxSize, - SerializeIF::Endianness::MACHINE); - SerializeAdapter::serialize(&dipoleZ, &serPtr, &dummy, maxSize, - SerializeIF::Endianness::MACHINE); - SerializeAdapter::serialize(&torqueDuration, &serPtr, &dummy, maxSize, - SerializeIF::Endianness::MACHINE); - } - - uint8_t* startOfActuateDataPtr() { return rawData + 2; } - - int16_t* getDipoles() { return reinterpret_cast(rawData + 2); } - - uint16_t getTorqueDuration() { - uint8_t* data = rawData + 2 + 6; - uint16_t value = 0; - size_t dummy = 0; - SerializeAdapter::deSerialize(&value, data, &dummy, SerializeIF::Endianness::MACHINE); - return value; - } - - void setSpecialRequest(imtq::SpecialRequest specialRequest) { - rawData[1] = static_cast(specialRequest); - } - - imtq::SpecialRequest getSpecialRequest() const { - return static_cast(rawData[1]); - } - - private: - ImtqRequest(uint8_t* rawData, size_t maxLen) : rawData(rawData) { - if (rawData != nullptr) { - rawData[0] = static_cast(imtq::RequestType::MEASURE_NO_ACTUATION); - } - } - uint8_t* rawData; - size_t maxSize = 0; -}; - struct ImtqRepliesDefault { friend class ImtqPollingTask; public: static constexpr size_t BASE_LEN = - imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::SYSTEM_STATE + 1 + + 1 + imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::SYSTEM_STATE + 1 + +imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::RAW_MTM_MEASUREMENT + 1 + imtq::replySize::ENG_HK_DATA_REPLY + 1 + imtq::replySize::CAL_MTM_MEASUREMENT + 1; ImtqRepliesDefault(const uint8_t* rawData) : rawData(const_cast(rawData)) { initPointers(); } + void setConfigured() { rawData[0] = true; } + bool devWasConfigured() const { return rawData[0]; } + uint8_t* getCalibMgmMeasurement() const { return calibMgmMeasurement + 1; } bool wasCalibMgmMeasurementRead() const { return calibMgmMeasurement[0]; }; @@ -1193,7 +1168,7 @@ struct ImtqRepliesDefault { private: void initPointers() { - swReset = rawData; + swReset = rawData + 1; systemState = swReset + imtq::replySize::DEFAULT_MIN_LEN + 1; startMtmMeasurement = systemState + imtq::replySize::SYSTEM_STATE + 1; rawMgmMeasurement = startMtmMeasurement + imtq::replySize::DEFAULT_MIN_LEN + 1; @@ -1201,6 +1176,7 @@ struct ImtqRepliesDefault { calibMgmMeasurement = engHk + imtq::replySize::ENG_HK_DATA_REPLY + 1; specialRequestReply = calibMgmMeasurement + imtq::replySize::CAL_MTM_MEASUREMENT + 1; } + uint8_t* rawData; uint8_t* swReset; uint8_t* systemState; diff --git a/mission/devices/devicedefinitions/payloadPcduDefinitions.h b/mission/devices/devicedefinitions/payloadPcduDefinitions.h index b1adae5f..a5c3effd 100644 --- a/mission/devices/devicedefinitions/payloadPcduDefinitions.h +++ b/mission/devices/devicedefinitions/payloadPcduDefinitions.h @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -10,7 +11,6 @@ #include "OBSWConfig.h" #include "mission/devices/max1227.h" -#include "mission/memory/NVMParameterBase.h" namespace plpcdu { diff --git a/mission/devices/devicedefinitions/SusDefinitions.h b/mission/devices/devicedefinitions/susMax1227Helpers.h similarity index 95% rename from mission/devices/devicedefinitions/SusDefinitions.h rename to mission/devices/devicedefinitions/susMax1227Helpers.h index 59ff5d24..292db6e4 100644 --- a/mission/devices/devicedefinitions/SusDefinitions.h +++ b/mission/devices/devicedefinitions/susMax1227Helpers.h @@ -6,7 +6,7 @@ #include -namespace SUS { +namespace susMax1227 { static const DeviceCommandId_t NONE = 0x0; // Set when no command is pending @@ -69,9 +69,9 @@ class SusDataset : public StaticLocalDataSet { SusDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, SUS_DATA_SET_ID)) {} - lp_var_t temperatureCelcius = lp_var_t(sid.objectId, TEMPERATURE_C, this); + lp_var_t tempC = lp_var_t(sid.objectId, TEMPERATURE_C, this); lp_vec_t channels = lp_vec_t(sid.objectId, CHANNEL_VEC, this); }; -} // namespace SUS +} // namespace susMax1227 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_SUS_H_ */ diff --git a/mission/memory/CMakeLists.txt b/mission/memory/CMakeLists.txt index 132f0551..8c2da887 100644 --- a/mission/memory/CMakeLists.txt +++ b/mission/memory/CMakeLists.txt @@ -1 +1 @@ -target_sources(${LIB_EIVE_MISSION} PRIVATE NVMParameterBase.cpp) +target_sources(${LIB_EIVE_MISSION} PRIVATE NvmParameterBase.cpp) diff --git a/mission/memory/NVMParameterBase.cpp b/mission/memory/NvmParameterBase.cpp similarity index 82% rename from mission/memory/NVMParameterBase.cpp rename to mission/memory/NvmParameterBase.cpp index 368dd772..9b89440f 100644 --- a/mission/memory/NVMParameterBase.cpp +++ b/mission/memory/NvmParameterBase.cpp @@ -1,4 +1,4 @@ -#include "NVMParameterBase.h" +#include #include @@ -10,13 +10,14 @@ NVMParameterBase::NVMParameterBase(std::string fullName) : fullName(fullName) {} NVMParameterBase::NVMParameterBase() {} ReturnValue_t NVMParameterBase::readJsonFile() { - if (std::filesystem::exists(fullName)) { + std::error_code e; + if (std::filesystem::exists(fullName, e)) { // Read JSON file content into object std::ifstream i(fullName); try { i >> json; - } catch (nlohmann::json::exception& e) { - sif::warning << "Reading JSON file failed with error " << e.what() << std::endl; + } catch (nlohmann::json::exception& nlohmannE) { + sif::warning << "Reading JSON file failed with error " << nlohmannE.what() << std::endl; return returnvalue::FAILED; } return returnvalue::OK; @@ -39,7 +40,10 @@ void NVMParameterBase::setFullName(std::string fullName) { this->fullName = full std::string NVMParameterBase::getFullName() const { return fullName; } -bool NVMParameterBase::getJsonFileExists() { return std::filesystem::exists(fullName); } +bool NVMParameterBase::getJsonFileExists() { + std::error_code e; + return std::filesystem::exists(fullName, e); +} void NVMParameterBase::printKeys() const { sif::info << "Printing keys for JSON file " << fullName << std::endl; diff --git a/mission/memory/NVMParameterBase.h b/mission/memory/NvmParameterBase.h similarity index 100% rename from mission/memory/NVMParameterBase.h rename to mission/memory/NvmParameterBase.h diff --git a/mission/persistentTmStoreDefs.h b/mission/persistentTmStoreDefs.h new file mode 100644 index 00000000..2498536d --- /dev/null +++ b/mission/persistentTmStoreDefs.h @@ -0,0 +1,43 @@ +#ifndef MISSION_PERSISTENTTMSTOREDEFS_H_ +#define MISSION_PERSISTENTTMSTOREDEFS_H_ + +#include + +#include "eive/eventSubsystemIds.h" + +namespace persTmStore { + +struct PersistentTmStores { + PersistentTmStoreWithTmQueue* okStore; + PersistentTmStoreWithTmQueue* notOkStore; + PersistentTmStoreWithTmQueue* miscStore; + PersistentTmStoreWithTmQueue* hkStore; + PersistentTmStoreWithTmQueue* cfdpStore; +}; + +static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PERSISTENT_TM_STORE; + +//! [EXPORT] : [COMMENT] +//! P1: Result code of TM packet parser. +//! P2: Timestamp of possibly corrupt file as a unix timestamp. +static constexpr Event POSSIBLE_FILE_CORRUPTION = event::makeEvent(SUBSYSTEM_ID, 0, severity::LOW); +//! [EXPORT] : [COMMENT] File in store too large. P1: Detected file size +//! P2: Allowed file size +static constexpr Event FILE_TOO_LARGE = event::makeEvent(SUBSYSTEM_ID, 1, severity::LOW); +static constexpr Event BUSY_DUMPING_EVENT = event::makeEvent(SUBSYSTEM_ID, 2, severity::INFO); +//! [EXPORT] : [COMMENT] Dump was cancelled. P1: Object ID of store. +static constexpr Event DUMP_WAS_CANCELLED = event::makeEvent(SUBSYSTEM_ID, 3, severity::LOW); + +//! [EXPORT] : [COMMENT] P1: Number of dumped packets. P2: Total dumped bytes. +static constexpr Event DUMP_OK_STORE_DONE = event::makeEvent(SUBSYSTEM_ID, 5, severity::INFO); +//! [EXPORT] : [COMMENT] P1: Number of dumped packets. P2: Total dumped bytes. +static constexpr Event DUMP_NOK_STORE_DONE = event::makeEvent(SUBSYSTEM_ID, 6, severity::INFO); +//! [EXPORT] : [COMMENT] P1: Number of dumped packets. P2: Total dumped bytes. +static constexpr Event DUMP_MISC_STORE_DONE = event::makeEvent(SUBSYSTEM_ID, 7, severity::INFO); +//! [EXPORT] : [COMMENT] P1: Number of dumped packets. P2: Total dumped bytes. +static constexpr Event DUMP_HK_STORE_DONE = event::makeEvent(SUBSYSTEM_ID, 8, severity::INFO); +//! [EXPORT] : [COMMENT] P1: Number of dumped packets. P2: Total dumped bytes. +static constexpr Event DUMP_CFDP_STORE_DONE = event::makeEvent(SUBSYSTEM_ID, 9, severity::INFO); +}; // namespace persTmStore + +#endif /* MISSION_PERSISTENTTMSTOREDEFS_H_ */ diff --git a/mission/system/fdir/CMakeLists.txt b/mission/system/fdir/CMakeLists.txt index 37b2c290..34a7e125 100644 --- a/mission/system/fdir/CMakeLists.txt +++ b/mission/system/fdir/CMakeLists.txt @@ -1,3 +1,4 @@ target_sources( - ${LIB_EIVE_MISSION} PRIVATE AcsBoardFdir.cpp RtdFdir.cpp SusFdir.cpp - SyrlinksFdir.cpp GomspacePowerFdir.cpp) + ${LIB_EIVE_MISSION} + PRIVATE AcsBoardFdir.cpp RtdFdir.cpp StrFdir.cpp SusFdir.cpp SyrlinksFdir.cpp + GomspacePowerFdir.cpp) diff --git a/mission/system/fdir/StrFdir.cpp b/mission/system/fdir/StrFdir.cpp new file mode 100644 index 00000000..7d0947a9 --- /dev/null +++ b/mission/system/fdir/StrFdir.cpp @@ -0,0 +1,14 @@ +#include "StrFdir.h" + +#include "mission/acsDefs.h" + +StrFdir::StrFdir(object_id_t strObject) + : DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {} + +ReturnValue_t StrFdir::eventReceived(EventMessage* event) { + if (event->getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) { + setFaulty(event->getEvent()); + return returnvalue::OK; + } + return DeviceHandlerFailureIsolation::eventReceived(event); +} diff --git a/mission/system/fdir/StrFdir.h b/mission/system/fdir/StrFdir.h new file mode 100644 index 00000000..20476e1a --- /dev/null +++ b/mission/system/fdir/StrFdir.h @@ -0,0 +1,12 @@ +#ifndef MISSION_SYSTEM_FDIR_STRFDIR_H_ +#define MISSION_SYSTEM_FDIR_STRFDIR_H_ + +#include + +class StrFdir : public DeviceHandlerFailureIsolation { + public: + StrFdir(object_id_t strObject); + ReturnValue_t eventReceived(EventMessage* event) override; +}; + +#endif /* MISSION_SYSTEM_FDIR_STRFDIR_H_ */ diff --git a/mission/system/fdir/SyrlinksFdir.cpp b/mission/system/fdir/SyrlinksFdir.cpp index 524a94f6..2bdbd21d 100644 --- a/mission/system/fdir/SyrlinksFdir.cpp +++ b/mission/system/fdir/SyrlinksFdir.cpp @@ -47,8 +47,8 @@ ReturnValue_t SyrlinksFdir::eventReceived(EventMessage* event) { } // else if (missedReplyCount.incrementAndCheck()) { - // handleRecovery(event->getEvent()); - triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0); + handleRecovery(event->getEvent()); + // triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0); } break; case StorageManagerIF::GET_DATA_FAILED: @@ -80,7 +80,7 @@ ReturnValue_t SyrlinksFdir::eventReceived(EventMessage* event) { break; case Fuse::POWER_BELOW_LOW_LIMIT: // Device might got stuck during boot, retry. - // handleRecovery(event->getEvent()); + handleRecovery(event->getEvent()); triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0); break; //****Thermal***** diff --git a/mission/system/objects/AcsBoardAssembly.cpp b/mission/system/objects/AcsBoardAssembly.cpp index bcc2eb7a..4939c0eb 100644 --- a/mission/system/objects/AcsBoardAssembly.cpp +++ b/mission/system/objects/AcsBoardAssembly.cpp @@ -55,6 +55,12 @@ ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE); modeTable[ModeTableIdx::GPS].setMode(MODE_OFF); modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE); + if (recoveryState == RecoveryState::RECOVERY_IDLE) { + result = checkAndHandleHealthStates(mode, submode); + if (result == NEED_TO_CHANGE_HEALTH) { + return returnvalue::OK; + } + } if (recoveryState != RecoveryState::RECOVERY_STARTED) { if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) { result = handleNormalOrOnModeCmd(mode, submode); @@ -274,4 +280,36 @@ void AcsBoardAssembly::refreshHelperModes() { } } -ReturnValue_t AcsBoardAssembly::initialize() { return AssemblyBase::initialize(); } +ReturnValue_t AcsBoardAssembly::initialize() { + for (const auto& child : childrenMap) { + updateChildModeByObjId(child.first, MODE_OFF, 0); + } + return AssemblyBase::initialize(); +} + +ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode, + Submode_t deviceSubmode) { + using namespace returnvalue; + ReturnValue_t status = returnvalue::OK; + auto overwriteHealthForOneDev = [&](object_id_t dev) { + HealthState health = healthHelper.healthTable->getHealth(dev); + if (health == FAULTY or health == PERMANENT_FAULTY) { + overwriteDeviceHealth(dev, health); + status = NEED_TO_CHANGE_HEALTH; + } else if (health == EXTERNAL_CONTROL) { + modeHelper.setForced(true); + } + }; + if (deviceSubmode == duallane::DUAL_MODE) { + overwriteHealthForOneDev(helper.mgm0Lis3IdSideA); + overwriteHealthForOneDev(helper.mgm1Rm3100IdSideA); + overwriteHealthForOneDev(helper.mgm2Lis3IdSideB); + overwriteHealthForOneDev(helper.mgm3Rm3100IdSideB); + overwriteHealthForOneDev(helper.gyro0AdisIdSideA); + overwriteHealthForOneDev(helper.gyro1L3gIdSideA); + overwriteHealthForOneDev(helper.gyro2AdisIdSideB); + overwriteHealthForOneDev(helper.gyro3L3gIdSideB); + overwriteHealthForOneDev(helper.gpsId); + } + return status; +} diff --git a/mission/system/objects/AcsBoardAssembly.h b/mission/system/objects/AcsBoardAssembly.h index 0c25396f..a07431fe 100644 --- a/mission/system/objects/AcsBoardAssembly.h +++ b/mission/system/objects/AcsBoardAssembly.h @@ -121,6 +121,7 @@ class AcsBoardAssembly : public DualLaneAssemblyBase { ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override; ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode); + ReturnValue_t checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode); void refreshHelperModes(); }; diff --git a/mission/system/objects/AcsSubsystem.cpp b/mission/system/objects/AcsSubsystem.cpp index c812394c..873d88cf 100644 --- a/mission/system/objects/AcsSubsystem.cpp +++ b/mission/system/objects/AcsSubsystem.cpp @@ -8,92 +8,7 @@ AcsSubsystem::AcsSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables) - : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) { - auto mqArgs = MqArgs(getObjectId(), static_cast(this)); - eventQueue = - QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs); -} - -ReturnValue_t AcsSubsystem::initialize() { - EventManagerIF* manager = ObjectManager::instance()->get(objects::EVENT_MANAGER); - if (manager == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "AcsSubsystem::initialize: Invalid event manager" << std::endl; -#endif - return ObjectManagerIF::CHILD_INIT_FAILED; - } - ReturnValue_t result = manager->registerListener(eventQueue->getId()); - if (result != returnvalue::OK) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "AcsSubsystem::registerListener: Failed to register as " - "listener" - << std::endl; -#endif - return ObjectManagerIF::CHILD_INIT_FAILED; - ; - } - result = - manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::SAFE_RATE_VIOLATION)); - if (result != returnvalue::OK) { - sif::error << "AcsSubsystem: Subscribing for acs::SAFE_RATE_VIOLATION failed" << std::endl; - } - result = - manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::SAFE_RATE_RECOVERY)); - if (result != returnvalue::OK) { - sif::error << "AcsSubsystem: Subscribing for acs::SAFE_RATE_RECOVERY failed" << std::endl; - } - result = - manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::MULTIPLE_RW_INVALID)); - if (result != returnvalue::OK) { - sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl; - } - result = manager->subscribeToEvent(eventQueue->getId(), - event::getEventId(acs::MEKF_INVALID_MODE_VIOLATION)); - if (result != returnvalue::OK) { - sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl; - } - return Subsystem::initialize(); -} - -void AcsSubsystem::performChildOperation() { - handleEventMessages(); - return Subsystem::performChildOperation(); -} - -void AcsSubsystem::handleEventMessages() { - EventMessage event; - for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK; - result = eventQueue->receiveMessage(&event)) { - ReturnValue_t status; - switch (event.getMessageId()) { - case EventMessage::EVENT_MESSAGE: - if (event.getEvent() == acs::SAFE_RATE_VIOLATION) { - CommandMessage msg; - ModeMessage::setCmdModeMessage(msg, acs::AcsMode::DETUMBLE, 0); - status = commandQueue->sendMessage(commandQueue->getId(), &msg); - if (result != returnvalue::OK) { - sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl; - } - } - if (event.getEvent() == acs::SAFE_RATE_RECOVERY || - event.getEvent() == acs::MULTIPLE_RW_INVALID || - event.getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) { - CommandMessage msg; - ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0); - status = commandQueue->sendMessage(commandQueue->getId(), &msg); - if (status != returnvalue::OK) { - sif::error << "AcsSubsystem: sending SAFE mode cmd to self has failed" << std::endl; - } - } - break; - default: - sif::debug << "AcsSubsystem::performChildOperation: Did not subscribe " - "to this event message" - << std::endl; - break; - } - } -} + : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {} void AcsSubsystem::announceMode(bool recursive) { const char* modeStr = acs::getModeStr(static_cast(mode)); diff --git a/mission/system/objects/AcsSubsystem.h b/mission/system/objects/AcsSubsystem.h index c6c77fef..fc6b238e 100644 --- a/mission/system/objects/AcsSubsystem.h +++ b/mission/system/objects/AcsSubsystem.h @@ -8,13 +8,7 @@ class AcsSubsystem : public Subsystem { AcsSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables); private: - ReturnValue_t initialize() override; - void performChildOperation() override; void announceMode(bool recursive) override; - - void handleEventMessages(); - - MessageQueueIF* eventQueue = nullptr; }; #endif /* MISSION_SYSTEM_ACSSUBSYSTEM_H_ */ diff --git a/mission/system/objects/CMakeLists.txt b/mission/system/objects/CMakeLists.txt index 0290a311..dc4cd80c 100644 --- a/mission/system/objects/CMakeLists.txt +++ b/mission/system/objects/CMakeLists.txt @@ -7,10 +7,13 @@ target_sources( TcsSubsystem.cpp PayloadSubsystem.cpp AcsBoardAssembly.cpp + ImtqAssembly.cpp + SyrlinksAssembly.cpp Stack5VHandler.cpp SusAssembly.cpp RwAssembly.cpp DualLanePowerStateMachine.cpp + StrAssembly.cpp PowerStateMachineBase.cpp DualLaneAssemblyBase.cpp TcsBoardAssembly.cpp) diff --git a/mission/system/objects/ComSubsystem.cpp b/mission/system/objects/ComSubsystem.cpp index 9df7e72b..b9ed0a05 100644 --- a/mission/system/objects/ComSubsystem.cpp +++ b/mission/system/objects/ComSubsystem.cpp @@ -30,6 +30,7 @@ void ComSubsystem::performChildOperation() { if (countdownActive) { checkTransmitterCountdown(); } + Subsystem::performChildOperation(); } @@ -184,7 +185,7 @@ void ComSubsystem::startRxAndTxLowRateSeq() { void ComSubsystem::checkTransmitterCountdown() { if (transmitterCountdown.hasTimedOut()) { - triggerEvent(TX_TIMER_EXPIRED, transmitterTimeout); + triggerEvent(TX_TIMER_EXPIRED, transmitterTimeout); startTransition(com::Submode::RX_ONLY, SUBMODE_NONE); countdownActive = false; } diff --git a/mission/system/objects/ComSubsystem.h b/mission/system/objects/ComSubsystem.h index 831b4cec..854a877b 100644 --- a/mission/system/objects/ComSubsystem.h +++ b/mission/system/objects/ComSubsystem.h @@ -1,24 +1,23 @@ #ifndef MISSION_SYSTEM_COMSUBSYSTEM_H_ #define MISSION_SYSTEM_COMSUBSYSTEM_H_ +#include #include #include #include #include -#include #include "mission/comDefs.h" class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF { public: + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::COM_SUBSYSTEM; - static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::COM_SUBSYSTEM; - - //! [EXPORT] : [COMMENT] The transmit timer to protect the Syrlinks expired - //! P1: The current timer value - static const Event TX_TIMER_EXPIRED = MAKE_EVENT(1, severity::INFO); - //! [EXPORT] : [COMMENT] Transmitter will be turned on due to detection of bitlock - static const Event BIT_LOCK_TX_ON = MAKE_EVENT(2, severity::INFO); + //! [EXPORT] : [COMMENT] The transmit timer to protect the Syrlinks expired + //! P1: The current timer value + static const Event TX_TIMER_EXPIRED = MAKE_EVENT(1, severity::INFO); + //! [EXPORT] : [COMMENT] Transmitter will be turned on due to detection of bitlock + static const Event BIT_LOCK_TX_ON = MAKE_EVENT(2, severity::INFO); /** * @brief Constructor @@ -27,7 +26,8 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF { * @param maxNumberOfSequences * @param maxNumberOfTables * @param transmitterTimeout Maximum time the transmitter of the syrlinks - * will be enabled + * will + * be enabled */ ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables, uint32_t transmitterTimeout); diff --git a/mission/system/objects/DualLaneAssemblyBase.h b/mission/system/objects/DualLaneAssemblyBase.h index eadfb77f..a8a2f521 100644 --- a/mission/system/objects/DualLaneAssemblyBase.h +++ b/mission/system/objects/DualLaneAssemblyBase.h @@ -70,6 +70,7 @@ class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF { virtual void handleChildrenLostMode(ReturnValue_t result) override; virtual void handleModeTransitionFailed(ReturnValue_t result) override; virtual void handleModeReached() override; + ReturnValue_t checkAndHandleHealthState(Mode_t deviceMode, Submode_t deviceSubmode); MessageQueueId_t getEventReceptionQueue() override; @@ -95,7 +96,6 @@ inline void DualLaneAssemblyBase::initModeTableEntry( entry.setObject(id); entry.setMode(MODE_OFF); entry.setSubmode(SUBMODE_NONE); - entry.setInheritSubmode(false); modeTable.insert(entry); } diff --git a/mission/system/objects/EiveSystem.cpp b/mission/system/objects/EiveSystem.cpp index 26707bf1..bb2a229d 100644 --- a/mission/system/objects/EiveSystem.cpp +++ b/mission/system/objects/EiveSystem.cpp @@ -1,10 +1,17 @@ #include "EiveSystem.h" +#include +#include +#include #include EiveSystem::EiveSystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables) - : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {} + : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) { + auto mqArgs = MqArgs(getObjectId(), static_cast(this)); + eventQueue = + QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs); +} void EiveSystem::announceMode(bool recursive) { const char* modeStr = "UNKNOWN"; @@ -17,10 +24,6 @@ void EiveSystem::announceMode(bool recursive) { modeStr = "SAFE"; break; } - case (acs::AcsMode::DETUMBLE): { - modeStr = "DETUBMLE"; - break; - } case (acs::AcsMode::PTG_IDLE): { modeStr = "POINTING IDLE"; break; @@ -41,3 +44,44 @@ void EiveSystem::announceMode(bool recursive) { sif::info << "EIVE system is now in " << modeStr << " mode" << std::endl; return Subsystem::announceMode(recursive); } + +void EiveSystem::performChildOperation() { + handleEventMessages(); + return Subsystem::performChildOperation(); +} + +ReturnValue_t EiveSystem::initialize() { + EventManagerIF* manager = ObjectManager::instance()->get(objects::EVENT_MANAGER); + if (manager == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "AcsSubsystem::initialize: Invalid event manager" << std::endl; +#endif + return ObjectManagerIF::CHILD_INIT_FAILED; + } + ReturnValue_t result = manager->registerListener(eventQueue->getId()); + if (result != returnvalue::OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "AcsSubsystem::registerListener: Failed to register as " + "listener" + << std::endl; +#endif + return ObjectManagerIF::CHILD_INIT_FAILED; + } + return Subsystem::initialize(); +} + +void EiveSystem::handleEventMessages() { + EventMessage event; + for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK; + result = eventQueue->receiveMessage(&event)) { + switch (event.getMessageId()) { + case EventMessage::EVENT_MESSAGE: + break; + default: + sif::debug << "AcsSubsystem::performChildOperation: Did not subscribe " + "to this event message" + << std::endl; + break; + } + } +} diff --git a/mission/system/objects/EiveSystem.h b/mission/system/objects/EiveSystem.h index 59acf82e..0d75b31a 100644 --- a/mission/system/objects/EiveSystem.h +++ b/mission/system/objects/EiveSystem.h @@ -8,7 +8,12 @@ class EiveSystem : public Subsystem { EiveSystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables); private: + ReturnValue_t initialize() override; + void performChildOperation() override; void announceMode(bool recursive) override; + void handleEventMessages(); + + MessageQueueIF* eventQueue = nullptr; }; #endif /* MISSION_SYSTEM_EIVESYSTEM_H_ */ diff --git a/mission/system/objects/ImtqAssembly.cpp b/mission/system/objects/ImtqAssembly.cpp new file mode 100644 index 00000000..14d90b75 --- /dev/null +++ b/mission/system/objects/ImtqAssembly.cpp @@ -0,0 +1,54 @@ +#include "ImtqAssembly.h" + +#include + +using namespace returnvalue; + +ImtqAssembly::ImtqAssembly(object_id_t objectId) : AssemblyBase(objectId) { + ModeListEntry entry; + entry.setObject(objects::IMTQ_HANDLER); + entry.setMode(MODE_OFF); + entry.setSubmode(SUBMODE_NONE); + commandTable.insert(entry); +} + +ReturnValue_t ImtqAssembly::commandChildren(Mode_t mode, Submode_t submode) { + commandTable[0].setMode(mode); + commandTable[0].setSubmode(submode); + if (recoveryState == RECOVERY_IDLE) { + ReturnValue_t result = checkAndHandleHealthState(mode, submode); + if (result == NEED_TO_CHANGE_HEALTH) { + return OK; + } + } + HybridIterator iter(commandTable.begin(), commandTable.end()); + executeTable(iter); + return returnvalue::OK; +} + +ReturnValue_t ImtqAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) { + if (childrenMap[objects::IMTQ_HANDLER].mode != wantedMode) { + return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; + } + return returnvalue::OK; +} + +ReturnValue_t ImtqAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) { + if (mode == MODE_ON or mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_OFF) { + return returnvalue::OK; + } + return returnvalue::FAILED; +} + +ReturnValue_t ImtqAssembly::checkAndHandleHealthState(Mode_t deviceMode, Submode_t deviceSubmode) { + HealthState health = healthHelper.healthTable->getHealth(objects::IMTQ_HANDLER); + if (health == FAULTY or health == PERMANENT_FAULTY) { + overwriteDeviceHealth(objects::IMTQ_HANDLER, health); + return NEED_TO_CHANGE_HEALTH; + } else if (health == EXTERNAL_CONTROL) { + modeHelper.setForced(true); + } + return OK; +} + +void ImtqAssembly::handleChildrenLostMode(ReturnValue_t result) { startTransition(mode, submode); } diff --git a/mission/system/objects/ImtqAssembly.h b/mission/system/objects/ImtqAssembly.h new file mode 100644 index 00000000..59061701 --- /dev/null +++ b/mission/system/objects/ImtqAssembly.h @@ -0,0 +1,18 @@ +#pragma once + +#include + +class ImtqAssembly : public AssemblyBase { + public: + ImtqAssembly(object_id_t objectId); + + private: + FixedArrayList commandTable; + + ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override; + ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override; + ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override; + void handleChildrenLostMode(ReturnValue_t result) override; + + ReturnValue_t checkAndHandleHealthState(Mode_t deviceMode, Submode_t deviceSubmode); +}; diff --git a/mission/system/objects/RwAssembly.cpp b/mission/system/objects/RwAssembly.cpp index 886ec5ce..ce90f4f5 100644 --- a/mission/system/objects/RwAssembly.cpp +++ b/mission/system/objects/RwAssembly.cpp @@ -8,7 +8,6 @@ RwAssembly::RwAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, power:: entry.setObject(helper.rwIds[idx]); entry.setMode(MODE_OFF); entry.setSubmode(SUBMODE_NONE); - entry.setInheritSubmode(false); modeTable.insert(entry); } } diff --git a/mission/system/objects/Stack5VHandler.cpp b/mission/system/objects/Stack5VHandler.cpp index 1a3141c4..6c3c94d9 100644 --- a/mission/system/objects/Stack5VHandler.cpp +++ b/mission/system/objects/Stack5VHandler.cpp @@ -5,7 +5,7 @@ Stack5VHandler::Stack5VHandler(PowerSwitchIF& switcher) : switcher(switcher) { } ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander, bool updateStates) { - MutexGuard mg(stackLock); + MutexGuard mg(stackLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); if (updateStates) { updateInternalStates(); } @@ -27,7 +27,7 @@ ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander, bool updateSt } ReturnValue_t Stack5VHandler::deviceToOff(StackCommander commander, bool updateStates) { - MutexGuard mg(stackLock); + MutexGuard mg(stackLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); if (updateStates) { updateInternalStates(); } @@ -55,12 +55,12 @@ ReturnValue_t Stack5VHandler::deviceToOff(StackCommander commander, bool updateS } bool Stack5VHandler::isSwitchOn() { - MutexGuard mg(stackLock); + MutexGuard mg(stackLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); return updateInternalStates(); } void Stack5VHandler::update() { - MutexGuard mg(stackLock); + MutexGuard mg(stackLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); updateInternalStates(); } diff --git a/mission/system/objects/Stack5VHandler.h b/mission/system/objects/Stack5VHandler.h index 46fc963a..364d744b 100644 --- a/mission/system/objects/Stack5VHandler.h +++ b/mission/system/objects/Stack5VHandler.h @@ -21,7 +21,11 @@ class Stack5VHandler { void update(); private: + static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING; + static constexpr uint32_t LOCK_TIMEOUT = 20; + MutexIF* stackLock; + static constexpr char LOCK_CTX[] = "Stack5VHandler"; PowerSwitchIF& switcher; bool switchIsOn = false; bool targetState = false; diff --git a/mission/system/objects/StrAssembly.cpp b/mission/system/objects/StrAssembly.cpp new file mode 100644 index 00000000..10ca5759 --- /dev/null +++ b/mission/system/objects/StrAssembly.cpp @@ -0,0 +1,30 @@ +#include "StrAssembly.h" + +#include + +StrAssembly::StrAssembly(object_id_t objectId) : AssemblyBase(objectId) { + ModeListEntry entry; + entry.setObject(objects::STAR_TRACKER); + entry.setMode(MODE_OFF); + entry.setSubmode(SUBMODE_NONE); + commandTable.insert(entry); +} + +ReturnValue_t StrAssembly::commandChildren(Mode_t mode, Submode_t submode) { + commandTable[0].setMode(mode); + commandTable[0].setSubmode(submode); + HybridIterator iter(commandTable.begin(), commandTable.end()); + executeTable(iter); + return returnvalue::OK; +} + +ReturnValue_t StrAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) { + if (childrenMap[objects::STAR_TRACKER].mode != wantedMode) { + return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; + } + return returnvalue::OK; +} + +ReturnValue_t StrAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) { + return returnvalue::OK; +} diff --git a/mission/system/objects/StrAssembly.h b/mission/system/objects/StrAssembly.h new file mode 100644 index 00000000..417a2432 --- /dev/null +++ b/mission/system/objects/StrAssembly.h @@ -0,0 +1,18 @@ +#ifndef MISSION_SYSTEM_OBJECTS_STRASSEMBLY_H_ +#define MISSION_SYSTEM_OBJECTS_STRASSEMBLY_H_ + +#include "fsfw/devicehandlers/AssemblyBase.h" + +class StrAssembly : public AssemblyBase { + public: + StrAssembly(object_id_t objectId); + + private: + FixedArrayList commandTable; + + ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override; + ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override; + ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override; +}; + +#endif /* MISSION_SYSTEM_OBJECTS_STRASSEMBLY_H_ */ diff --git a/mission/system/objects/SusAssembly.cpp b/mission/system/objects/SusAssembly.cpp index 3f75a369..9008e7e0 100644 --- a/mission/system/objects/SusAssembly.cpp +++ b/mission/system/objects/SusAssembly.cpp @@ -24,6 +24,12 @@ ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) { modeTable[idx].setMode(MODE_OFF); modeTable[idx].setSubmode(SUBMODE_NONE); } + if (recoveryState == RecoveryState::RECOVERY_IDLE) { + result = checkAndHandleHealthStates(mode, submode); + if (result == NEED_TO_CHANGE_HEALTH) { + return returnvalue::OK; + } + } if (recoveryState != RecoveryState::RECOVERY_STARTED) { if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) { result = handleNormalOrOnModeCmd(mode, submode); @@ -120,7 +126,12 @@ ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wan return returnvalue::OK; } -ReturnValue_t SusAssembly::initialize() { return AssemblyBase::initialize(); } +ReturnValue_t SusAssembly::initialize() { + for (const auto& child : childrenMap) { + updateChildModeByObjId(child.first, MODE_OFF, 0); + } + return AssemblyBase::initialize(); +} bool SusAssembly::isUseable(object_id_t object, Mode_t mode) { if (healthHelper.healthTable->isFaulty(object)) { @@ -143,3 +154,23 @@ void SusAssembly::refreshHelperModes() { helper.susModes[idx] = childrenMap[helper.susIds[idx]].mode; } } + +ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode) { + using namespace returnvalue; + ReturnValue_t status = returnvalue::OK; + auto overwriteHealthForOneDev = [&](object_id_t dev) { + HealthState health = healthHelper.healthTable->getHealth(dev); + if (health == FAULTY or health == PERMANENT_FAULTY) { + overwriteDeviceHealth(dev, health); + status = NEED_TO_CHANGE_HEALTH; + } else if (health == EXTERNAL_CONTROL) { + modeHelper.setForced(true); + } + }; + if (deviceSubmode == duallane::DUAL_MODE) { + for (uint8_t idx = 0; idx < 12; idx++) { + overwriteHealthForOneDev(helper.susIds[idx]); + } + } + return status; +} diff --git a/mission/system/objects/SusAssembly.h b/mission/system/objects/SusAssembly.h index e95803df..e993bee4 100644 --- a/mission/system/objects/SusAssembly.h +++ b/mission/system/objects/SusAssembly.h @@ -66,6 +66,7 @@ class SusAssembly : public DualLaneAssemblyBase { void powerStateMachine(Mode_t mode, Submode_t submode); ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode); void refreshHelperModes(); + ReturnValue_t checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode); }; #endif /* MISSION_SYSTEM_SUSASSEMBLY_H_ */ diff --git a/mission/system/objects/SyrlinksAssembly.cpp b/mission/system/objects/SyrlinksAssembly.cpp new file mode 100644 index 00000000..b5e50924 --- /dev/null +++ b/mission/system/objects/SyrlinksAssembly.cpp @@ -0,0 +1,57 @@ +#include "SyrlinksAssembly.h" + +#include + +using namespace returnvalue; + +SyrlinksAssembly::SyrlinksAssembly(object_id_t objectId) : AssemblyBase(objectId) { + ModeListEntry entry; + entry.setObject(objects::SYRLINKS_HANDLER); + entry.setMode(MODE_OFF); + entry.setSubmode(SUBMODE_NONE); + commandTable.insert(entry); +} + +ReturnValue_t SyrlinksAssembly::commandChildren(Mode_t mode, Submode_t submode) { + commandTable[0].setMode(mode); + commandTable[0].setSubmode(submode); + HybridIterator iter(commandTable.begin(), commandTable.end()); + if (recoveryState == RECOVERY_IDLE) { + ReturnValue_t result = checkAndHandleHealthState(mode, submode); + if (result == NEED_TO_CHANGE_HEALTH) { + return OK; + } + } + executeTable(iter); + return returnvalue::OK; +} + +ReturnValue_t SyrlinksAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) { + if (childrenMap[objects::SYRLINKS_HANDLER].mode != wantedMode) { + return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; + } + return returnvalue::OK; +} + +ReturnValue_t SyrlinksAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) { + if (mode == MODE_ON or mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_OFF) { + return returnvalue::OK; + } + return returnvalue::FAILED; +} + +ReturnValue_t SyrlinksAssembly::checkAndHandleHealthState(Mode_t deviceMode, + Submode_t deviceSubmode) { + HealthState health = healthHelper.healthTable->getHealth(objects::SYRLINKS_HANDLER); + if (health == FAULTY or health == PERMANENT_FAULTY) { + overwriteDeviceHealth(objects::SYRLINKS_HANDLER, health); + return NEED_TO_CHANGE_HEALTH; + } else if (health == EXTERNAL_CONTROL) { + modeHelper.setForced(true); + } + return OK; +} + +void SyrlinksAssembly::handleChildrenLostMode(ReturnValue_t result) { + startTransition(mode, submode); +} diff --git a/mission/system/objects/SyrlinksAssembly.h b/mission/system/objects/SyrlinksAssembly.h new file mode 100644 index 00000000..314474d3 --- /dev/null +++ b/mission/system/objects/SyrlinksAssembly.h @@ -0,0 +1,20 @@ +#ifndef MISSION_SYSTEM_OBJECTS_SYRLINKSASSEMBLY_H_ +#define MISSION_SYSTEM_OBJECTS_SYRLINKSASSEMBLY_H_ +#include + +class SyrlinksAssembly : public AssemblyBase { + public: + SyrlinksAssembly(object_id_t objectId); + + private: + FixedArrayList commandTable; + + ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override; + ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override; + ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override; + void handleChildrenLostMode(ReturnValue_t result) override; + + ReturnValue_t checkAndHandleHealthState(Mode_t deviceMode, Submode_t deviceSubmode); +}; + +#endif /* MISSION_SYSTEM_OBJECTS_SYRLINKSASSEMBLY_H_ */ diff --git a/mission/system/objects/TcsBoardAssembly.cpp b/mission/system/objects/TcsBoardAssembly.cpp index 88075fdc..b32a00a9 100644 --- a/mission/system/objects/TcsBoardAssembly.cpp +++ b/mission/system/objects/TcsBoardAssembly.cpp @@ -12,7 +12,6 @@ TcsBoardAssembly::TcsBoardAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitc entry.setObject(helper.rtdInfos[idx].first); entry.setMode(MODE_OFF); entry.setSubmode(SUBMODE_NONE); - entry.setInheritSubmode(false); modeTable.insert(entry); } } @@ -41,6 +40,12 @@ ReturnValue_t TcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) modeTable[idx].setMode(MODE_OFF); modeTable[idx].setSubmode(SUBMODE_NONE); } + if (recoveryState == RecoveryState::RECOVERY_IDLE) { + result = checkAndHandleHealthStates(mode, submode); + if (result == NEED_TO_CHANGE_HEALTH) { + return returnvalue::OK; + } + } if (recoveryState != RecoveryState::RECOVERY_STARTED) { if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) { result = handleNormalOrOnModeCmd(mode, submode); @@ -62,10 +67,10 @@ ReturnValue_t TcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_ } catch (const std::out_of_range& e) { sif::error << "TcsBoardAssembly: Invalid children map: " << e.what() << std::endl; } - if (devsInWrongMode >= 3) { + if (devsInWrongMode == NUMBER_RTDS) { if (warningSwitch) { - sif::warning << "TcsBoardAssembly::checkChildrenStateOn: " << devsInWrongMode << " devices in" - << " wrong mode" << std::endl; + sif::warning << "TcsBoardAssembly::checkChildrenStateOn: All devices in wrong mode" + << std::endl; warningSwitch = false; } return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; @@ -180,9 +185,29 @@ void TcsBoardAssembly::handleModeReached() { } void TcsBoardAssembly::handleChildrenLostMode(ReturnValue_t result) { - // TODO: Maybe try a reboot once here? triggerEvent(CHILDREN_LOST_MODE, result); - return; + startTransition(mode, submode); +} + +ReturnValue_t TcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode, + Submode_t deviceSubmode) { + ReturnValue_t status = returnvalue::OK; + for (const auto& dev : helper.rtdInfos) { + HealthState health = healthHelper.healthTable->getHealth(dev.first); + if (health == HealthState::HEALTHY) { + return returnvalue::OK; + } + } + + for (const auto& dev : helper.rtdInfos) { + HealthState health = healthHelper.healthTable->getHealth(dev.first); + if (health == FAULTY or health == PERMANENT_FAULTY) { + status = NEED_TO_CHANGE_HEALTH; + } else if (health == EXTERNAL_CONTROL) { + modeHelper.setForced(true); + } + } + return status; } void TcsBoardAssembly::handleModeTransitionFailed(ReturnValue_t result) { diff --git a/mission/system/objects/TcsBoardAssembly.h b/mission/system/objects/TcsBoardAssembly.h index fb5f7d38..3c3fc0d6 100644 --- a/mission/system/objects/TcsBoardAssembly.h +++ b/mission/system/objects/TcsBoardAssembly.h @@ -52,6 +52,7 @@ class TcsBoardAssembly : public AssemblyBase, public ConfirmsFailuresIF { ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override; void startTransition(Mode_t mode, Submode_t submode) override; void handleModeReached() override; + ReturnValue_t checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode); // These two overrides prevent a transition of the whole assembly back to off just because // some devices are not working diff --git a/mission/system/tree/acsModeTree.cpp b/mission/system/tree/acsModeTree.cpp index 62fb76ed..9b0c573a 100644 --- a/mission/system/tree/acsModeTree.cpp +++ b/mission/system/tree/acsModeTree.cpp @@ -6,8 +6,11 @@ #include #include +#include + #include "eive/objects.h" #include "mission/acsDefs.h" +#include "mission/system/objects/definitions.h" #include "util.h" AcsSubsystem satsystem::acs::ACS_SUBSYSTEM(objects::ACS_SUBSYSTEM, 12, 24); @@ -17,7 +20,6 @@ namespace { 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 buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh); @@ -40,15 +42,6 @@ auto ACS_TABLE_OFF_TRANS_0 = auto ACS_TABLE_OFF_TRANS_1 = std::make_pair((acs::AcsMode::OFF << 24) | 3, FixedArrayList()); -auto ACS_SEQUENCE_DETUMBLE = - std::make_pair(acs::AcsMode::DETUMBLE, FixedArrayList()); -auto ACS_TABLE_DETUMBLE_TGT = - std::make_pair((acs::AcsMode::DETUMBLE << 24) | 1, FixedArrayList()); -auto ACS_TABLE_DETUMBLE_TRANS_0 = - std::make_pair((acs::AcsMode::DETUMBLE << 24) | 2, FixedArrayList()); -auto ACS_TABLE_DETUMBLE_TRANS_1 = - std::make_pair((acs::AcsMode::DETUMBLE << 24) | 3, FixedArrayList()); - auto ACS_SEQUENCE_SAFE = std::make_pair(acs::AcsMode::SAFE, FixedArrayList()); auto ACS_TABLE_SAFE_TGT = std::make_pair((acs::AcsMode::SAFE << 24) | 1, FixedArrayList()); @@ -100,30 +93,33 @@ Subsystem& satsystem::acs::init() { ModeListEntry entry; const char* ctxc = "satsystem::acs::init: generic target"; // Insert Helper Table - auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table, + bool allowAllSubmodes = false) { entry.setObject(obj); entry.setMode(mode); entry.setSubmode(submode); + if (allowAllSubmodes) { + entry.allowAllSubmodes(); + } check(table.insert(entry), "satsystem::acs::init: generic target"); }; // Build TARGET PT transition 0 - iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TRANS_0.second); - iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second); - iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second); - iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second); - iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TRANS_0.second); + iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second); + iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TRANS_0.second, true); + iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TRANS_0.second, true); + iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second); + iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second); check(ACS_SUBSYSTEM.addTable( TableEntry(ACS_TABLE_PTG_TRANS_0.first, &ACS_TABLE_PTG_TRANS_0.second)), ctxc); // Build SUS board transition - iht(objects::SUS_BOARD_ASS, NML, 0, SUS_BOARD_NML_TRANS.second); + iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, SUS_BOARD_NML_TRANS.second, true); check(ACS_SUBSYSTEM.addTable(TableEntry(SUS_BOARD_NML_TRANS.first, &SUS_BOARD_NML_TRANS.second)), ctxc); buildOffSequence(ACS_SUBSYSTEM, entry); buildSafeSequence(ACS_SUBSYSTEM, entry); - buildDetumbleSequence(ACS_SUBSYSTEM, entry); buildIdleSequence(ACS_SUBSYSTEM, entry); buildTargetPtSequence(ACS_SUBSYSTEM, entry); buildTargetPtGsSequence(ACS_SUBSYSTEM, entry); @@ -162,10 +158,10 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) { check(ss.addTable(TableEntry(ACS_TABLE_OFF_TRANS_0.first, &ACS_TABLE_OFF_TRANS_0.second)), ctxc); // Build OFF transition 1 - iht(objects::IMTQ_HANDLER, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); - iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); + iht(objects::IMTQ_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); + iht(objects::STR_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); iht(objects::ACS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); - iht(objects::RW_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); + iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_OFF_TRANS_1.first, &ACS_TABLE_OFF_TRANS_1.second)), ctxc); // Build OFF sequence @@ -182,10 +178,13 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { auto ctxc = context.c_str(); // Insert Helper Table auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, - ArrayList& sequence) { + ArrayList& sequence, bool allowAllSubmodes = false) { eh.setObject(obj); eh.setMode(mode); eh.setSubmode(submode); + if (allowAllSubmodes) { + eh.allowAllSubmodes(); + } check(sequence.insert(eh), ctxc); }; // Insert Helper Sequence @@ -196,31 +195,33 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { eh.setCheckSuccess(checkSuccess); check(sequence.insert(eh), ctxc); }; - // Build SAFE target - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::SAFE, 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); + // Build SAFE target. Allow detumble submode. + iht(objects::ACS_CONTROLLER, acs::AcsMode::SAFE, acs::SafeSubmode::DEFAULT, + ACS_TABLE_SAFE_TGT.second, true); + iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_SAFE_TGT.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second, true); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second, true); 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::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); + iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_SAFE_TRANS_0.second); + iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true); + iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true); + iht(objects::STR_ASSY, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second); + iht(objects::RW_ASSY, 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); // SUS board transition table is defined above // Build SAFE transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::SAFE, ACS_TABLE_SAFE_TRANS_1.second); + iht(objects::ACS_CONTROLLER, acs::AcsMode::SAFE, acs::SafeSubmode::DEFAULT, + 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, SUS_BOARD_NML_TRANS.first, 0, false); 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, @@ -228,70 +229,18 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { 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, NML, acs::AcsMode::DETUMBLE, 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); - - // SUS board transition table is defined above - - // 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, NML, acs::AcsMode::DETUMBLE, 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, SUS_BOARD_NML_TRANS.first, 0, false); - 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) { + ArrayList& sequence, bool allowAllSubmodes = false) { eh.setObject(obj); eh.setMode(mode); eh.setSubmode(submode); + if (allowAllSubmodes) { + eh.allowAllSubmodes(); + } check(sequence.insert(eh), ctxc); }; // Insert Helper Sequence @@ -303,30 +252,28 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; // Build IDLE target - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, 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); + iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_IDLE, 0, ACS_TABLE_IDLE_TGT.second); + iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second); + iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second); + iht(objects::STR_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second, true); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second, true); ss.addTable(&ACS_TABLE_IDLE_TGT.second, ACS_TABLE_IDLE_TGT.first, false, true); - // SUS board transition table is built above - // 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, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); + iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); + iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true); + iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true); + iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); + iht(objects::STR_ASSY, NML, 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, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TRANS_1.second); + iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_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, SUS_BOARD_NML_TRANS.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, true); ss.addSequence(&ACS_SEQUENCE_IDLE.second, ACS_SEQUENCE_IDLE.first, ACS_SEQUENCE_SAFE.first, false, @@ -338,10 +285,13 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) { auto ctxc = context.c_str(); // Insert Helper Table auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, - ArrayList& sequence) { + ArrayList& sequence, bool allowAllSubmodes = false) { eh.setObject(obj); eh.setMode(mode); eh.setSubmode(submode); + if (allowAllSubmodes) { + eh.allowAllSubmodes(); + } check(sequence.insert(eh), ctxc); }; // Insert Helper Sequence @@ -354,26 +304,24 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TGT.second); - iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); - iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); - iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); - iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); - iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); + iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET, 0, ACS_TABLE_PTG_TARGET_TGT.second); + iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second, true); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second, true); + iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); + iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); check(ss.addTable(&ACS_TABLE_PTG_TARGET_TGT.second, ACS_TABLE_PTG_TARGET_TGT.first, false, true), ctxc); - // SUS board transition table is built above // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TRANS_1.second); + iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET, 0, ACS_TABLE_PTG_TARGET_TRANS_1.second); check(ss.addTable(&ACS_TABLE_PTG_TARGET_TRANS_1.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, false, true), ctxc); // Build IDLE sequence ihs(ACS_SEQUENCE_PTG_TARGET.second, ACS_TABLE_PTG_TARGET_TGT.first, 0, true); - ihs(ACS_SEQUENCE_PTG_TARGET.second, SUS_BOARD_NML_TRANS.first, 0, true); ihs(ACS_SEQUENCE_PTG_TARGET.second, ACS_TABLE_PTG_TRANS_0.first, 0, true); ihs(ACS_SEQUENCE_PTG_TARGET.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, 0, true); check(ss.addSequence(&ACS_SEQUENCE_PTG_TARGET.second, ACS_SEQUENCE_PTG_TARGET.first, @@ -386,10 +334,13 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) { auto ctxc = context.c_str(); // Insert Helper Table auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, - ArrayList& sequence) { + ArrayList& sequence, bool allowAllSubmodes = false) { eh.setObject(obj); eh.setMode(mode); eh.setSubmode(submode); + if (allowAllSubmodes) { + eh.allowAllSubmodes(); + } check(sequence.insert(eh), ctxc); }; // Insert Helper Sequence @@ -402,20 +353,19 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, - ACS_TABLE_PTG_TARGET_NADIR_TGT.second); - iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); - iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); - iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); - iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); - iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); + iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_NADIR, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); + iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true); + iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); + iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TGT.first, &ACS_TABLE_PTG_TARGET_NADIR_TGT.second)), ctxc); // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_NADIR, + iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_NADIR, 0, ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first, &ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second)), @@ -437,10 +387,13 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) { auto ctxc = context.c_str(); // Insert Helper Table auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, - ArrayList& sequence) { + ArrayList& sequence, bool allowAllSubmodes = false) { eh.setObject(obj); eh.setMode(mode); eh.setSubmode(submode); + if (allowAllSubmodes) { + eh.allowAllSubmodes(); + } check(sequence.insert(eh), ctxc); }; // Insert Helper Sequence @@ -453,20 +406,19 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_GS, - ACS_TABLE_PTG_TARGET_GS_TGT.second); - iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); - iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); - iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); - iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); - iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); + iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET_GS, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); + iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second, true); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second, true); + iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); + iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); check(ss.addTable( TableEntry(ACS_TABLE_PTG_TARGET_GS_TGT.first, &ACS_TABLE_PTG_TARGET_GS_TGT.second)), ctxc); // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_GS, + iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET_GS, 0, ACS_TABLE_PTG_TARGET_GS_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_GS_TRANS_1.first, &ACS_TABLE_PTG_TARGET_GS_TRANS_1.second)), @@ -487,10 +439,13 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) { auto ctxc = context.c_str(); // Insert Helper Table auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, - ArrayList& sequence) { + ArrayList& sequence, bool allowAllSubmodes = false) { eh.setObject(obj); eh.setMode(mode); eh.setSubmode(submode); + if (allowAllSubmodes) { + eh.allowAllSubmodes(); + } check(sequence.insert(eh), ctxc); }; // Insert Helper Sequence @@ -503,20 +458,20 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_INERTIAL, + iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_INERTIAL, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); - iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); - iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); - iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); - iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); - iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); + iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); + iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second, true); + iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second, true); + iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); + iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TGT.first, &ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second)), ctxc); // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_INERTIAL, + iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_INERTIAL, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first, &ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second)), diff --git a/mission/system/tree/comModeTree.cpp b/mission/system/tree/comModeTree.cpp index 9dc85a78..dd3aa7a1 100644 --- a/mission/system/tree/comModeTree.cpp +++ b/mission/system/tree/comModeTree.cpp @@ -105,11 +105,11 @@ void buildRxOnlySequence(Subsystem& ss, ModeListEntry& eh) { // Build RX Only table. We could track the state of the CCSDS IP core handler // as well but I do not think this is necessary because enabling that should // not interfere with the Syrlinks Handler. - iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TGT.second); + iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TGT.second); check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TGT.first, &COM_TABLE_RX_ONLY_TGT.second)), ctxc); // Build RX Only transition 0 - iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TRANS_0.second); + iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TRANS_0.second); check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TRANS_0.first, &COM_TABLE_RX_ONLY_TRANS_0.second)), ctxc); @@ -147,7 +147,7 @@ void buildTxAndRxLowRateSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build RX and TX low datarate table. - iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE, + iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE, COM_TABLE_RX_AND_TX_LOW_RATE_TGT.second); iht(objects::CCSDS_HANDLER, ON, static_cast(::com::CcsdsSubmode::DATARATE_LOW), COM_TABLE_RX_AND_TX_LOW_RATE_TGT.second); @@ -163,7 +163,7 @@ void buildTxAndRxLowRateSequence(Subsystem& ss, ModeListEntry& eh) { ctxc); // Build TX and RX low transition 1 - iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE, + iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE, COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.second); check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.first, &COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.second)), @@ -199,7 +199,7 @@ void buildTxAndRxHighRateSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build RX and TX high datarate table. - iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE, + iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE, COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.second); iht(objects::CCSDS_HANDLER, ON, static_cast(::com::CcsdsSubmode::DATARATE_HIGH), COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.second); @@ -215,7 +215,7 @@ void buildTxAndRxHighRateSequence(Subsystem& ss, ModeListEntry& eh) { ctxc); // Build TX and RX high transition 1 - iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE, + iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE, COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.second); check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.first, &COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.second)), @@ -253,7 +253,7 @@ void buildTxAndRxDefaultRateSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build RX and TX default datarate table. - iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE, + iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE, COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.second); iht(objects::CCSDS_HANDLER, ON, static_cast(::com::CcsdsSubmode::DATARATE_DEFAULT), COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.second); @@ -269,7 +269,7 @@ void buildTxAndRxDefaultRateSequence(Subsystem& ss, ModeListEntry& eh) { ctxc); // Build TX and RX default transition 1 - iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE, + iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE, COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.second); check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.first, &COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.second)), diff --git a/mission/system/tree/comModeTree.h b/mission/system/tree/comModeTree.h index 9260e3ea..0ac7f9b3 100644 --- a/mission/system/tree/comModeTree.h +++ b/mission/system/tree/comModeTree.h @@ -11,8 +11,9 @@ extern ComSubsystem SUBSYSTEM; // The syrlinks must not transmitting longer then 15 minutes otherwise the // transceiver might be damaged due to overheating -// 15 minutes in milliseconds -static const uint32_t TRANSMITTER_TIMEOUT = 900000; +// This is the initial timeout of 2 minutes. The timeout needs to be incremented +// before each overpass +static const uint32_t TRANSMITTER_TIMEOUT = 120000; Subsystem& init(); } // namespace com diff --git a/mission/system/tree/system.cpp b/mission/system/tree/system.cpp index 5ec10a8c..dde32aef 100644 --- a/mission/system/tree/system.cpp +++ b/mission/system/tree/system.cpp @@ -35,6 +35,7 @@ void satsystem::init() { ModeListEntry entry; buildSafeSequence(EIVE_SYSTEM, entry); buildIdleSequence(EIVE_SYSTEM, entry); + EIVE_SYSTEM.setInitialMode(HasModesIF::MODE_OFF, 0); } EiveSystem satsystem::EIVE_SYSTEM = EiveSystem(objects::EIVE_SYSTEM, 12, 24); @@ -62,10 +63,14 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { std::string context = "satsystem::buildSafeSequence"; auto ctxc = context.c_str(); // Insert Helper Table - auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table, + bool allowAllSubmodes = false) { eh.setObject(obj); eh.setMode(mode); eh.setSubmode(submode); + if (allowAllSubmodes) { + eh.allowAllSubmodes(); + } check(table.insert(eh), ctxc); }; // Insert Helper Sequence @@ -77,9 +82,8 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; - // Do no track ACS for now because it might jump to detumble mode and back to safe as part of - // normal operations. - // iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TGT.second); + // Do no track submode to allow transitions to DETUMBLE submode. + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TGT.second, true); iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TGT.second); check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TGT.first, &EIVE_TABLE_SAFE_TGT.second)), ctxc); @@ -87,19 +91,14 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { // consecutive commanding of TCS and ACS can lead to SPI issues. iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second); iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_SAFE_TRANS_0.second); + iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TRANS_0.second); + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TRANS_0.second, true); check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_0.first, &EIVE_TABLE_SAFE_TRANS_0.second)), ctxc); - // Build SAFE transition 1 - iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TRANS_1.second); - iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TRANS_1.second); - check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_1.first, &EIVE_TABLE_SAFE_TRANS_1.second)), - ctxc); - // Build Safe sequence ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TGT.first, 0, false); ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TRANS_0.first, 0, false); - ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TRANS_1.first, 0, false); check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_SAFE.first, &EIVE_SEQUENCE_SAFE.second, EIVE_SEQUENCE_SAFE.first)), ctxc); @@ -127,21 +126,16 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TGT.second); check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TGT.first, &EIVE_TABLE_IDLE_TGT.second)), ctxc); - // Build SAFE transition 0 + // Build IDLE transition 0 iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_IDLE_TRANS_0.second); + iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_IDLE_TRANS_0.second); + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TRANS_0.second); check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_0.first, &EIVE_TABLE_IDLE_TRANS_0.second)), ctxc); - // Build SAFE transition 1 - iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_IDLE_TRANS_1.second); - iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TRANS_1.second); - check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_1.first, &EIVE_TABLE_IDLE_TRANS_1.second)), - ctxc); - - // Build Safe sequence + // Build IDLE sequence ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TGT.first, 0, false); ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TRANS_0.first, 0, false); - ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TRANS_1.first, 0, false); check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_IDLE.first, &EIVE_SEQUENCE_IDLE.second, EIVE_SEQUENCE_SAFE.first)), ctxc); diff --git a/mission/system/tree/tcsModeTree.cpp b/mission/system/tree/tcsModeTree.cpp index d411161a..b7188b17 100644 --- a/mission/system/tree/tcsModeTree.cpp +++ b/mission/system/tree/tcsModeTree.cpp @@ -19,8 +19,8 @@ static const auto NML = DeviceHandlerIF::MODE_NORMAL; auto TCS_SEQUENCE_OFF = std::make_pair(OFF, FixedArrayList()); auto TCS_TABLE_OFF_TGT = std::make_pair((OFF << 24) | 1, FixedArrayList()); -auto TCS_TABLE_OFF_TRANS_0 = std::make_pair((OFF << 24) | 2, FixedArrayList()); -auto TCS_TABLE_OFF_TRANS_1 = std::make_pair((OFF << 24) | 3, FixedArrayList()); +auto TCS_TABLE_OFF_TRANS_0 = std::make_pair((OFF << 24) | 2, FixedArrayList()); +auto TCS_TABLE_OFF_TRANS_1 = std::make_pair((OFF << 24) | 3, FixedArrayList()); auto TCS_SEQUENCE_NORMAL = std::make_pair(NML, FixedArrayList()); auto TCS_TABLE_NORMAL_TGT = std::make_pair((NML << 24) | 1, FixedArrayList()); @@ -59,16 +59,16 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) { // OFF target table is empty check(ss.addTable(TableEntry(TCS_TABLE_OFF_TGT.first, &TCS_TABLE_OFF_TGT.second)), ctxc); - iht(objects::TCS_BOARD_ASS, OFF, 0, TCS_TABLE_OFF_TRANS_0.second); - iht(objects::TMP1075_HANDLER_TCS_0, OFF, 0, TCS_TABLE_OFF_TRANS_0.second); - iht(objects::TMP1075_HANDLER_TCS_1, OFF, 0, TCS_TABLE_OFF_TRANS_0.second); - iht(objects::TMP1075_HANDLER_PLPCDU_0, OFF, 0, TCS_TABLE_OFF_TRANS_0.second); - // damaged - // iht(objects::TMP1075_HANDLER_PLPCDU_1, OFF, 0, TCS_TABLE_OFF_TRANS_0.second); - iht(objects::TMP1075_HANDLER_IF_BOARD, OFF, 0, TCS_TABLE_OFF_TRANS_0.second); + // Transition 1 + iht(objects::THERMAL_CONTROLLER, OFF, 0, TCS_TABLE_OFF_TRANS_0.second); check(ss.addTable(TableEntry(TCS_TABLE_OFF_TRANS_0.first, &TCS_TABLE_OFF_TRANS_0.second)), ctxc); - iht(objects::THERMAL_CONTROLLER, OFF, 0, TCS_TABLE_OFF_TRANS_0.second); + iht(objects::TCS_BOARD_ASS, OFF, 0, TCS_TABLE_OFF_TRANS_1.second); + iht(objects::TMP1075_HANDLER_TCS_0, OFF, 0, TCS_TABLE_OFF_TRANS_1.second); + iht(objects::TMP1075_HANDLER_TCS_1, OFF, 0, TCS_TABLE_OFF_TRANS_1.second); + iht(objects::TMP1075_HANDLER_PLPCDU_0, OFF, 0, TCS_TABLE_OFF_TRANS_1.second); + // TMP PL PCDU 1 is damaged + iht(objects::TMP1075_HANDLER_IF_BOARD, OFF, 0, TCS_TABLE_OFF_TRANS_1.second); check(ss.addTable(TableEntry(TCS_TABLE_OFF_TRANS_1.first, &TCS_TABLE_OFF_TRANS_1.second)), ctxc); ihs(TCS_SEQUENCE_OFF.second, TCS_TABLE_OFF_TGT.first, 0, false); @@ -98,20 +98,20 @@ void buildNormalSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; - // OFF target table is empty + // Normal target table is empty check(ss.addTable(TableEntry(TCS_TABLE_NORMAL_TGT.first, &TCS_TABLE_NORMAL_TGT.second)), ctxc); iht(objects::TCS_BOARD_ASS, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); iht(objects::TMP1075_HANDLER_TCS_0, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); iht(objects::TMP1075_HANDLER_TCS_1, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); iht(objects::TMP1075_HANDLER_PLPCDU_0, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); - // damaged - // iht(objects::TMP1075_HANDLER_PLPCDU_1, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); + // TMP PL PCDU 1 is damaged iht(objects::TMP1075_HANDLER_IF_BOARD, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); check(ss.addTable(TableEntry(TCS_TABLE_NORMAL_TRANS_0.first, &TCS_TABLE_NORMAL_TRANS_0.second)), ctxc); - iht(objects::THERMAL_CONTROLLER, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); + // Transition 1 + iht(objects::THERMAL_CONTROLLER, NML, 0, TCS_TABLE_NORMAL_TRANS_1.second); check(ss.addTable(TableEntry(TCS_TABLE_NORMAL_TRANS_1.first, &TCS_TABLE_NORMAL_TRANS_1.second)), ctxc); diff --git a/mission/tmtc/CMakeLists.txt b/mission/tmtc/CMakeLists.txt index c4c93ab6..b155e02e 100644 --- a/mission/tmtc/CMakeLists.txt +++ b/mission/tmtc/CMakeLists.txt @@ -1,10 +1,20 @@ target_sources( ${LIB_EIVE_MISSION} PRIVATE CcsdsIpCoreHandler.cpp + VirtualChannelWithQueue.cpp + PersistentTmStoreWithTmQueue.cpp + LiveTmTask.cpp VirtualChannel.cpp TmFunnelHandler.cpp TmFunnelBase.cpp CfdpTmFunnel.cpp + tmFilters.cpp + PusLiveDemux.cpp + PersistentSingleTmStoreTask.cpp + PersistentLogTmStoreTask.cpp + TmStoreTaskBase.cpp + PusPacketFilter.cpp + PusTmRouteByFilterHelper.cpp Service15TmStorage.cpp PersistentTmStore.cpp PusTmFunnel.cpp) diff --git a/mission/tmtc/CcsdsIpCoreHandler.cpp b/mission/tmtc/CcsdsIpCoreHandler.cpp index b462f236..838af4c0 100644 --- a/mission/tmtc/CcsdsIpCoreHandler.cpp +++ b/mission/tmtc/CcsdsIpCoreHandler.cpp @@ -12,11 +12,11 @@ #include "fsfw/serviceinterface/serviceInterfaceDefintions.h" #include "mission/devices/devicedefinitions/SyrlinksDefinitions.h" -CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId, - object_id_t tcDestination, PtmeConfig* ptmeConfig, +CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t tcDestination, + PtmeConfig& ptmeConfig, std::atomic_bool& linkState, GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData) : SystemObject(objectId), - ptmeId(ptmeId), + linkState(linkState), tcDestination(tcDestination), parameterHelper(this), actionHelper(this, nullptr), @@ -31,32 +31,15 @@ CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId, QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs); } -CcsdsIpCoreHandler::~CcsdsIpCoreHandler() {} +CcsdsIpCoreHandler::~CcsdsIpCoreHandler() = default; ReturnValue_t CcsdsIpCoreHandler::performOperation(uint8_t operationCode) { readCommandQueue(); - handleTelemetry(); - handleTelecommands(); return returnvalue::OK; } -void CcsdsIpCoreHandler::handleTelemetry() { - VirtualChannelMapIter iter; - for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) { - iter->second->performOperation(); - } -} - -void CcsdsIpCoreHandler::handleTelecommands() {} - ReturnValue_t CcsdsIpCoreHandler::initialize() { ReturnValue_t result = returnvalue::OK; - PtmeIF* ptme = ObjectManager::instance()->get(ptmeId); - if (ptme == nullptr) { - sif::warning << "Invalid PTME object" << std::endl; - return ObjectManagerIF::CHILD_INIT_FAILED; - } - AcceptsTelecommandsIF* tcDistributor = ObjectManager::instance()->get(tcDestination); if (tcDistributor == nullptr) { @@ -83,25 +66,15 @@ ReturnValue_t CcsdsIpCoreHandler::initialize() { return result; } - VirtualChannelMapIter iter; - for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) { - result = iter->second->initialize(); - if (result != returnvalue::OK) { - return result; - } - iter->second->setPtmeObject(ptme); - } - - result = ptmeConfig->initialize(); + result = ptmeConfig.initialize(); if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } #if OBSW_SYRLINKS_SIMULATED == 1 // Update data on rising edge - ptmeConfig->invertTxClock(false); - linkState = UP; - forwardLinkstate(); + ptmeConfig.invertTxClock(false); + linkState = LINK_UP; #endif /* OBSW_SYRLINKS_SIMULATED == 1*/ return result; @@ -134,44 +107,6 @@ void CcsdsIpCoreHandler::readCommandQueue(void) { MessageQueueId_t CcsdsIpCoreHandler::getCommandQueue() const { return commandQueue->getId(); } -void CcsdsIpCoreHandler::addVirtualChannel(VcId_t vcId, VirtualChannel* virtualChannel) { - if (vcId > config::NUMBER_OF_VIRTUAL_CHANNELS) { - sif::warning << "CcsdsHandler::addVirtualChannel: Invalid virtual channel ID" << std::endl; - return; - } - - if (virtualChannel == nullptr) { - 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 " - "virtual channel map" - << std::endl; - return; - } -} - -MessageQueueId_t CcsdsIpCoreHandler::getReportReceptionQueue(uint8_t virtualChannel) const { - if (virtualChannel < config::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 " - << static_cast(virtualChannel) << " not in virtual channel map" - << std::endl; - return MessageQueueIF::NO_QUEUE; - } - } else { - sif::debug << "CcsdsHandler::getReportReceptionQueue: Invalid virtual channel requested"; - } - return MessageQueueIF::NO_QUEUE; -} - ReturnValue_t CcsdsIpCoreHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier, ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues, @@ -182,7 +117,7 @@ ReturnValue_t CcsdsIpCoreHandler::getParameter(uint8_t domainId, uint8_t uniqueI uint32_t CcsdsIpCoreHandler::getIdentifier() const { return 0; } MessageQueueId_t CcsdsIpCoreHandler::getRequestQueue() const { - // Forward packets directly to TC distributor + // Forward packets directly to the CCSDS TC distributor return tcDistributorQueueId; } @@ -192,18 +127,18 @@ ReturnValue_t CcsdsIpCoreHandler::executeAction(ActionId_t actionId, MessageQueu switch (actionId) { case SET_LOW_RATE: { submode = static_cast(com::CcsdsSubmode::DATARATE_LOW); - result = ptmeConfig->setRate(RATE_100KBPS); + result = ptmeConfig.setRate(RATE_100KBPS); break; } case SET_HIGH_RATE: { submode = static_cast(com::CcsdsSubmode::DATARATE_HIGH); - result = ptmeConfig->setRate(RATE_500KBPS); + result = ptmeConfig.setRate(RATE_500KBPS); break; } case ARBITRARY_RATE: { uint32_t bitrate = 0; SerializeAdapter::deSerialize(&bitrate, &data, &size, SerializeIF::Endianness::BIG); - result = ptmeConfig->setRate(bitrate); + result = ptmeConfig.setRate(bitrate); break; } case EN_TRANSMITTER: { @@ -221,19 +156,19 @@ ReturnValue_t CcsdsIpCoreHandler::executeAction(ActionId_t actionId, MessageQueu return EXECUTION_FINISHED; } case ENABLE_TX_CLK_MANIPULATOR: { - result = ptmeConfig->configTxManipulator(true); + result = ptmeConfig.configTxManipulator(true); break; } case DISABLE_TX_CLK_MANIPULATOR: { - result = ptmeConfig->configTxManipulator(false); + result = ptmeConfig.configTxManipulator(false); break; } case UPDATE_ON_RISING_EDGE: { - result = ptmeConfig->invertTxClock(false); + result = ptmeConfig.invertTxClock(false); break; } case UPDATE_ON_FALLING_EDGE: { - result = ptmeConfig->invertTxClock(true); + result = ptmeConfig.invertTxClock(true); break; } default: @@ -245,20 +180,14 @@ ReturnValue_t CcsdsIpCoreHandler::executeAction(ActionId_t actionId, MessageQueu return EXECUTION_FINISHED; } -void CcsdsIpCoreHandler::forwardLinkstate() { - VirtualChannelMapIter iter; - for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) { - iter->second->setLinkState(linkState); - } -} +void CcsdsIpCoreHandler::updateLinkState() { linkState = LINK_UP; } void CcsdsIpCoreHandler::enableTransmit() { #ifndef TE0720_1CFA gpioIF->pullHigh(enTxClock); gpioIF->pullHigh(enTxData); #endif - linkState = UP; - forwardLinkstate(); + linkState = LINK_UP; } void CcsdsIpCoreHandler::getMode(Mode_t* mode, Submode_t* submode) { @@ -283,13 +212,13 @@ ReturnValue_t CcsdsIpCoreHandler::checkModeCommand(Mode_t mode, Submode_t submod void CcsdsIpCoreHandler::startTransition(Mode_t mode, Submode_t submode) { auto rateHigh = [&]() { - ReturnValue_t result = ptmeConfig->setRate(RATE_500KBPS); + ReturnValue_t result = ptmeConfig.setRate(RATE_500KBPS); if (result == returnvalue::OK) { this->mode = HasModesIF::MODE_ON; } }; auto rateLow = [&]() { - ReturnValue_t result = ptmeConfig->setRate(RATE_100KBPS); + ReturnValue_t result = ptmeConfig.setRate(RATE_100KBPS); if (result == returnvalue::OK) { this->mode = HasModesIF::MODE_ON; } @@ -325,8 +254,7 @@ void CcsdsIpCoreHandler::disableTransmit() { gpioIF->pullLow(enTxClock); gpioIF->pullLow(enTxData); #endif - linkState = DOWN; - forwardLinkstate(); + linkState = LINK_DOWN; } const char* CcsdsIpCoreHandler::getName() const { return "CCSDS Handler"; } diff --git a/mission/tmtc/CcsdsIpCoreHandler.h b/mission/tmtc/CcsdsIpCoreHandler.h index 356c2f8d..a147a13f 100644 --- a/mission/tmtc/CcsdsIpCoreHandler.h +++ b/mission/tmtc/CcsdsIpCoreHandler.h @@ -2,12 +2,12 @@ #define CCSDSHANDLER_H_ #include +#include #include #include #include "OBSWConfig.h" -#include "VirtualChannel.h" #include "eive/definitions.h" #include "fsfw/action/ActionHelper.h" #include "fsfw/action/HasActionsIF.h" @@ -39,11 +39,13 @@ class CcsdsIpCoreHandler : public SystemObject, public ModeTreeChildIF, public ModeTreeConnectionIF, public HasModesIF, - public AcceptsTelemetryIF, + // public AcceptsTelemetryIF, public AcceptsTelecommandsIF, public ReceivesParameterMessagesIF, public HasActionsIF { public: + static const bool LINK_UP = true; + static const bool LINK_DOWN = false; using VcId_t = uint8_t; /** @@ -58,8 +60,9 @@ class CcsdsIpCoreHandler : public SystemObject, * @param enTxClock GPIO ID of RS485 tx clock enable * @param enTxData GPIO ID of RS485 tx data enable */ - CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId, object_id_t tcDestination, - PtmeConfig* ptmeConfig, GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData); + CcsdsIpCoreHandler(object_id_t objectId, object_id_t tcDestination, PtmeConfig& ptmeConfig, + std::atomic_bool& linkState, GpioIF* gpioIF, gpioId_t enTxClock, + gpioId_t enTxData); ~CcsdsIpCoreHandler(); @@ -74,15 +77,6 @@ class CcsdsIpCoreHandler : public SystemObject, void startTransition(Mode_t mode, Submode_t submode) override; void announceMode(bool recursive) override; - /** - * @brief Function to add a virtual channel - * - * @param virtualChannelId ID of the virtual channel to add - * @param virtualChannel Pointer to virtual channel object - */ - void addVirtualChannel(VcId_t virtualChannelId, VirtualChannel* virtualChannel); - - MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel = 0) const override; ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueIdentifier, ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues, uint16_t startAtIndex); @@ -126,16 +120,9 @@ class CcsdsIpCoreHandler : public SystemObject, //! [EXPORT] : [COMMENT] Received action message with unknown action id static const ReturnValue_t COMMAND_NOT_IMPLEMENTED = MAKE_RETURN_CODE(0xA0); - static const bool UP = true; - static const bool DOWN = false; - - using VirtualChannelMap = std::unordered_map; - using VirtualChannelMapIter = VirtualChannelMap::iterator; - - VirtualChannelMap virtualChannelMap; - - // Object ID of PTME object - object_id_t ptmeId; + // using VirtualChannelMap = std::unordered_map; + // VirtualChannelMap virtualChannelMap; + std::atomic_bool& linkState; object_id_t tcDestination; @@ -150,7 +137,7 @@ class CcsdsIpCoreHandler : public SystemObject, MessageQueueId_t tcDistributorQueueId = MessageQueueIF::NO_QUEUE; - PtmeConfig* ptmeConfig = nullptr; + PtmeConfig& ptmeConfig; GpioIF* gpioIF = nullptr; // GPIO to enable RS485 transceiver for TX clock @@ -158,16 +145,13 @@ class CcsdsIpCoreHandler : public SystemObject, // GPIO to enable RS485 transceiver for TX data signal gpioId_t enTxData = gpio::NO_GPIO; - bool linkState = DOWN; - void readCommandQueue(void); void handleTelemetry(); - void handleTelecommands(); /** * @brief Forward link state to virtual channels. */ - void forwardLinkstate(); + void updateLinkState(); /** * @brief Starts transmit timer and enables transmitter. diff --git a/mission/tmtc/CfdpTmFunnel.cpp b/mission/tmtc/CfdpTmFunnel.cpp index 89d7c105..46915b82 100644 --- a/mission/tmtc/CfdpTmFunnel.cpp +++ b/mission/tmtc/CfdpTmFunnel.cpp @@ -4,8 +4,12 @@ #include "fsfw/tmtcpacket/ccsds/SpacePacketCreator.h" #include "fsfw/tmtcservices/TmTcMessage.h" -CfdpTmFunnel::CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, uint16_t cfdpInCcsdsApid) - : TmFunnelBase(cfg), cfdpInCcsdsApid(cfdpInCcsdsApid) {} +CfdpTmFunnel::CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, MessageQueueId_t fileStoreDest, + StorageManagerIF& ramToFileStore, uint16_t cfdpInCcsdsApid) + : TmFunnelBase(cfg), + fileStoreDest(fileStoreDest), + ramToFileStore(ramToFileStore), + cfdpInCcsdsApid(cfdpInCcsdsApid) {} const char* CfdpTmFunnel::getName() const { return "CFDP TM Funnel"; } @@ -75,36 +79,15 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) { tmStore.deleteData(msg.getStorageId()); msg.setStorageId(newStoreId); store_address_t origStoreId = newStoreId; - for (unsigned int idx = 0; idx < destinations.size(); idx++) { - const auto& dest = 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, packetLen); - if (result == returnvalue::OK) { - msg.setStorageId(storeId); - } else { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy or store " - "error" - << std::endl; -#endif - break; - } - } else { - msg.setStorageId(origStoreId); - } - } - result = tmQueue->sendMessage(dest.queueId, &msg); - if (result != returnvalue::OK) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler " << dest.name - << " failed" << std::endl; -#endif - tmStore.deleteData(msg.getStorageId()); - } + + store_address_t storageId; + result = ramToFileStore.addData(&storageId, newPacketData, packetLen); + TmTcMessage fileMsg(storageId); + if (result != returnvalue::OK) { + sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" << std::endl; + } else { + tmQueue->sendMessage(fileStoreDest, &fileMsg); } - return result; + + return demultiplexLivePackets(origStoreId, newPacketData, packetLen); } diff --git a/mission/tmtc/CfdpTmFunnel.h b/mission/tmtc/CfdpTmFunnel.h index 717573a0..a4d1bc70 100644 --- a/mission/tmtc/CfdpTmFunnel.h +++ b/mission/tmtc/CfdpTmFunnel.h @@ -12,7 +12,8 @@ class CfdpTmFunnel : public TmFunnelBase { public: - CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, uint16_t cfdpInCcsdsApid); + CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, MessageQueueId_t fileStoreDest, + StorageManagerIF& ramToFileStore, uint16_t cfdpInCcsdsApid); [[nodiscard]] const char* getName() const override; ReturnValue_t performOperation(uint8_t opCode); ReturnValue_t initialize() override; @@ -20,6 +21,8 @@ class CfdpTmFunnel : public TmFunnelBase { private: ReturnValue_t handlePacket(TmTcMessage& msg); + MessageQueueId_t fileStoreDest; + StorageManagerIF& ramToFileStore; uint16_t sourceSequenceCount = 0; uint16_t cfdpInCcsdsApid; }; diff --git a/mission/tmtc/DirectTmSinkIF.h b/mission/tmtc/DirectTmSinkIF.h new file mode 100644 index 00000000..11a17c79 --- /dev/null +++ b/mission/tmtc/DirectTmSinkIF.h @@ -0,0 +1,30 @@ +#ifndef MISSION_TMTC_DIRECTTMSINKIF_H_ +#define MISSION_TMTC_DIRECTTMSINKIF_H_ + +#include + +#include "eive/resultClassIds.h" +#include "fsfw/retval.h" + +class DirectTmSinkIF { + public: + virtual ~DirectTmSinkIF() = default; + + static constexpr uint8_t CLASS_ID = CLASS_ID::TM_SINK; + + static constexpr ReturnValue_t IS_BUSY = returnvalue::makeCode(CLASS_ID, 0); + + /** + * @brief Implements the functionality to write to a TM sink directly + * + * @param data Pointer to buffer holding the data to write + * @param size Number of bytes to write + * @return returnvalue::OK on success, returnvalue::FAILED on failure, IS_BUSY + * if the TM sink is busy. + */ + virtual ReturnValue_t write(const uint8_t* data, size_t size) = 0; + + virtual bool isBusy() const = 0; +}; + +#endif /* MISSION_TMTC_DIRECTTMSINKIF_H_ */ diff --git a/mission/tmtc/LiveTmTask.cpp b/mission/tmtc/LiveTmTask.cpp new file mode 100644 index 00000000..53a9f04a --- /dev/null +++ b/mission/tmtc/LiveTmTask.cpp @@ -0,0 +1,24 @@ +#include "LiveTmTask.h" + +#include +#include + +LiveTmTask::LiveTmTask(object_id_t objectId, PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel, + VirtualChannelWithQueue& channel) + : SystemObject(objectId), pusFunnel(pusFunnel), cfdpFunnel(cfdpFunnel), channel(channel) {} + +ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) { + while (true) { + // The funnel tasks are scheduled here directly as well. + ReturnValue_t result = channel.sendNextTm(); + if (result == MessageQueueIF::EMPTY) { + if (tmFunnelCd.hasTimedOut()) { + pusFunnel.performOperation(0); + cfdpFunnel.performOperation(0); + tmFunnelCd.resetTimer(); + } + // 40 ms IDLE delay. Might tweak this in the future. + TaskFactory::delayTask(40); + } + } +} diff --git a/mission/tmtc/LiveTmTask.h b/mission/tmtc/LiveTmTask.h new file mode 100644 index 00000000..a0ca6b83 --- /dev/null +++ b/mission/tmtc/LiveTmTask.h @@ -0,0 +1,25 @@ +#ifndef MISSION_TMTC_LIVETMTASK_H_ +#define MISSION_TMTC_LIVETMTASK_H_ + +#include +#include +#include +#include +#include +#include + +class LiveTmTask : public SystemObject, public ExecutableObjectIF { + public: + LiveTmTask(object_id_t objectId, PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel, + VirtualChannelWithQueue& channel); + + ReturnValue_t performOperation(uint8_t opCode) override; + + private: + Countdown tmFunnelCd = Countdown(100); + PusTmFunnel& pusFunnel; + CfdpTmFunnel& cfdpFunnel; + VirtualChannelWithQueue& channel; +}; + +#endif /* MISSION_TMTC_LIVETMTASK_H_ */ diff --git a/mission/tmtc/PersistentLogTmStoreTask.cpp b/mission/tmtc/PersistentLogTmStoreTask.cpp new file mode 100644 index 00000000..dc17ec58 --- /dev/null +++ b/mission/tmtc/PersistentLogTmStoreTask.cpp @@ -0,0 +1,48 @@ +#include "PersistentLogTmStoreTask.h" + +#include +#include + +PersistentLogTmStoreTask::PersistentLogTmStoreTask(object_id_t objectId, StorageManagerIF& ipcStore, + LogStores stores, VirtualChannel& channel, + SdCardMountedIF& sdcMan) + : TmStoreTaskBase(objectId, ipcStore, channel, sdcMan), + stores(stores), + okStoreContext(persTmStore::DUMP_OK_STORE_DONE), + notOkStoreContext(persTmStore::DUMP_NOK_STORE_DONE), + miscStoreContext(persTmStore::DUMP_MISC_STORE_DONE) {} + +ReturnValue_t PersistentLogTmStoreTask::performOperation(uint8_t opCode) { + while (true) { + if (not cyclicStoreCheck()) { + continue; + } + bool someonesBusy = false; + bool busy = false; + busy = handleOneStore(stores.okStore, okStoreContext); + if (busy) { + someonesBusy = true; + } + busy = handleOneStore(stores.notOkStore, notOkStoreContext); + if (busy) { + someonesBusy = true; + } + busy = handleOneStore(stores.miscStore, miscStoreContext); + if (busy) { + someonesBusy = true; + } + if (not someonesBusy) { + TaskFactory::delayTask(40); + } + } +} + +bool PersistentLogTmStoreTask::initStoresIfPossible() { + if (sdcMan.isSdCardUsable(std::nullopt)) { + stores.okStore.initializeTmStore(); + stores.miscStore.initializeTmStore(); + stores.notOkStore.initializeTmStore(); + return true; + } + return false; +} diff --git a/mission/tmtc/PersistentLogTmStoreTask.h b/mission/tmtc/PersistentLogTmStoreTask.h new file mode 100644 index 00000000..8cd74f20 --- /dev/null +++ b/mission/tmtc/PersistentLogTmStoreTask.h @@ -0,0 +1,39 @@ +#ifndef MISSION_TMTC_PERSISTENTLOGTMSTORETASK_H_ +#define MISSION_TMTC_PERSISTENTLOGTMSTORETASK_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct LogStores { + LogStores(PersistentTmStores& stores) + : okStore(*stores.okStore), notOkStore(*stores.notOkStore), miscStore(*stores.miscStore) {} + PersistentTmStoreWithTmQueue& okStore; + PersistentTmStoreWithTmQueue& notOkStore; + PersistentTmStoreWithTmQueue& miscStore; +}; + +class PersistentLogTmStoreTask : public TmStoreTaskBase, public ExecutableObjectIF { + public: + PersistentLogTmStoreTask(object_id_t objectId, StorageManagerIF& ipcStore, LogStores tmStore, + VirtualChannel& channel, SdCardMountedIF& sdcMan); + + ReturnValue_t performOperation(uint8_t opCode) override; + + private: + LogStores stores; + DumpContext okStoreContext; + DumpContext notOkStoreContext; + DumpContext miscStoreContext; + Countdown tcHandlingCd = Countdown(400); + + bool initStoresIfPossible(); +}; + +#endif /* MISSION_TMTC_PERSISTENTLOGTMSTORETASK_H_ */ diff --git a/mission/tmtc/PersistentSingleTmStoreTask.cpp b/mission/tmtc/PersistentSingleTmStoreTask.cpp new file mode 100644 index 00000000..3588b9fd --- /dev/null +++ b/mission/tmtc/PersistentSingleTmStoreTask.cpp @@ -0,0 +1,31 @@ +#include +#include +#include + +PersistentSingleTmStoreTask::PersistentSingleTmStoreTask( + object_id_t objectId, StorageManagerIF& ipcStore, PersistentTmStoreWithTmQueue& tmStore, + VirtualChannel& channel, Event eventIfDumpDone, SdCardMountedIF& sdcMan) + : TmStoreTaskBase(objectId, ipcStore, channel, sdcMan), + storeWithQueue(tmStore), + dumpContext(eventIfDumpDone) {} + +ReturnValue_t PersistentSingleTmStoreTask::performOperation(uint8_t opCode) { + while (true) { + // Delay done by the check + if (not cyclicStoreCheck()) { + continue; + } + bool busy = handleOneStore(storeWithQueue, dumpContext); + if (not busy) { + TaskFactory::delayTask(40); + } + } +} + +bool PersistentSingleTmStoreTask::initStoresIfPossible() { + if (sdcMan.isSdCardUsable(std::nullopt)) { + storeWithQueue.initializeTmStore(); + return true; + } + return false; +} diff --git a/mission/tmtc/PersistentSingleTmStoreTask.h b/mission/tmtc/PersistentSingleTmStoreTask.h new file mode 100644 index 00000000..b21ddf1d --- /dev/null +++ b/mission/tmtc/PersistentSingleTmStoreTask.h @@ -0,0 +1,26 @@ +#ifndef MISSION_TMTC_PERSISTENTSINGLETMSTORETASK_H_ +#define MISSION_TMTC_PERSISTENTSINGLETMSTORETASK_H_ + +#include +#include +#include +#include +#include + +class PersistentSingleTmStoreTask : public TmStoreTaskBase, public ExecutableObjectIF { + public: + PersistentSingleTmStoreTask(object_id_t objectId, StorageManagerIF& ipcStore, + PersistentTmStoreWithTmQueue& storeWithQueue, VirtualChannel& channel, + Event eventIfDumpDone, SdCardMountedIF& sdcMan); + + ReturnValue_t performOperation(uint8_t opCode) override; + + private: + PersistentTmStoreWithTmQueue& storeWithQueue; + DumpContext dumpContext; + Countdown tcHandlingCd = Countdown(400); + + bool initStoresIfPossible(); +}; + +#endif /* MISSION_TMTC_PERSISTENTSINGLETMSTORETASK_H_ */ diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index 6ceed609..445eee9d 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -1,6 +1,7 @@ #include "PersistentTmStore.h" #include +#include #include #include @@ -11,52 +12,26 @@ #include "fsfw/ipc/CommandMessage.h" #include "fsfw/ipc/QueueFactory.h" #include "fsfw/tmstorage/TmStoreMessage.h" +#include "mission/persistentTmStoreDefs.h" using namespace returnvalue; -PersistentTmStore::PersistentTmStore(object_id_t objectId, const char* baseDir, - std::string baseName, RolloverInterval intervalUnit, - uint32_t intervalCount, StorageManagerIF& tmStore, - SdCardMountedIF& sdcMan) - : SystemObject(objectId), - baseDir(baseDir), - baseName(std::move(baseName)), - sdcMan(sdcMan), - tmStore(tmStore) { +PersistentTmStore::PersistentTmStore(PersistentTmStoreArgs args) + : SystemObject(args.objectId), + tmStore(args.tmStore), + baseDir(args.baseDir), + baseName(std::move(args.baseName)), + sdcMan(args.sdcMan) { tcQueue = QueueFactory::instance()->createMessageQueue(); - calcDiffSeconds(intervalUnit, intervalCount); + calcDiffSeconds(args.intervalUnit, args.intervalCount); +} + +ReturnValue_t PersistentTmStore::cancelDump() { + state = State::IDLE; + return returnvalue::OK; } ReturnValue_t PersistentTmStore::assignAndOrCreateMostRecentFile() { - using namespace std::filesystem; - for (auto const& file : directory_iterator(basePath)) { - if (file.is_directory()) { - continue; - } - auto pathStr = file.path().string(); - if (pathStr.find(baseName) == std::string::npos) { - continue; - } - unsigned int underscorePos = pathStr.find_last_of('_'); - std::string stampStr = pathStr.substr(underscorePos + 1); - struct tm time {}; - if (nullptr == strptime(stampStr.c_str(), FILE_DATE_FORMAT, &time)) { - sif::error << "PersistentTmStore::assignOrCreateMostRecentFile: Error reading timestamp" - << std::endl; - // Delete the file and re-create it. - activeFile = std::nullopt; - std::filesystem::remove(file.path()); - break; - } - time_t fileEpoch = timegm(&time); - // There is still a file within the active time window, so re-use that file for new TMs to - // store. - if (fileEpoch + static_cast(rolloverDiffSeconds) > currentTv.tv_sec) { - activeFileTv.tv_sec = fileEpoch; - activeFile = file.path(); - break; - } - } if (not activeFile.has_value()) { return createMostRecentFile(std::nullopt); } @@ -64,12 +39,9 @@ ReturnValue_t PersistentTmStore::assignAndOrCreateMostRecentFile() { } ReturnValue_t PersistentTmStore::handleCommandQueue(StorageManagerIF& ipcStore, - TmFunnelBase& tmFunnel) { + Command_t& execCmd) { CommandMessage cmdMessage; ReturnValue_t result = tcQueue->receiveMessage(&cmdMessage); - if (result == MessageQueueIF::EMPTY) { - return returnvalue::OK; - } if (result != returnvalue::OK) { return result; } @@ -83,6 +55,7 @@ ReturnValue_t PersistentTmStore::handleCommandQueue(StorageManagerIF& ipcStore, size_t size = accessor.second.size(); SerializeAdapter::deSerialize(&deleteUpToUnixSeconds, accessor.second.data(), &size, SerializeIF::Endianness::NETWORK); + execCmd = cmd; deleteUpTo(deleteUpToUnixSeconds); } else if (cmd == TmStoreMessage::DOWNLINK_STORE_CONTENT_TIME) { Clock::getClock_timeval(¤tTv); @@ -91,56 +64,26 @@ ReturnValue_t PersistentTmStore::handleCommandQueue(StorageManagerIF& ipcStore, if (accessor.second.size() < 8) { return returnvalue::FAILED; } - uint32_t dumpFromUnixSeconds; - uint32_t dumpUntilUnixSeconds; + uint32_t dumpFromUnixSeconds = 0; + uint32_t dumpUntilUnixSeconds = 0; size_t size = 8; SerializeAdapter::deSerialize(&dumpFromUnixSeconds, accessor.second.data(), &size, SerializeIF::Endianness::NETWORK); SerializeAdapter::deSerialize(&dumpUntilUnixSeconds, accessor.second.data() + 4, &size, SerializeIF::Endianness::NETWORK); - dumpFromUpTo(dumpFromUnixSeconds, dumpUntilUnixSeconds, tmFunnel); + result = startDumpFromUpTo(dumpFromUnixSeconds, dumpUntilUnixSeconds); + if (result == BUSY_DUMPING) { + triggerEvent(persTmStore::BUSY_DUMPING_EVENT); + } else { + execCmd = cmd; + } } } - return returnvalue::OK; + return result; } -ReturnValue_t PersistentTmStore::passPacket(PusTmReader& reader) { - bool inApidList = false; - if (filter.apid) { - auto& apidFilter = filter.apid.value(); - if (std::find(apidFilter.begin(), apidFilter.end(), reader.getApid()) != apidFilter.end()) { - if (not filter.serviceSubservices and not filter.services) { - return storePacket(reader); - } - inApidList = true; - } - } - std::pair serviceSubservice; - serviceSubservice.first = reader.getService(); - serviceSubservice.second = reader.getSubService(); - if (filter.services) { - auto& serviceFilter = filter.services.value(); - if (std::find(serviceFilter.begin(), serviceFilter.end(), serviceSubservice.first) != - serviceFilter.end()) { - if (filter.apid and inApidList) { - return storePacket(reader); - } - } - } - if (filter.serviceSubservices) { - auto& serviceSubserviceFilter = filter.serviceSubservices.value(); - if (std::find(serviceSubserviceFilter.begin(), serviceSubserviceFilter.end(), - serviceSubservice) != serviceSubserviceFilter.end()) { - if (filter.apid and inApidList) { - return storePacket(reader); - } - } - } - return returnvalue::OK; -} - -void PersistentTmStore::dumpFrom(uint32_t fromUnixSeconds, TmFunnelBase& tmFunnel) { - return dumpFromUpTo(fromUnixSeconds, currentTv.tv_sec, tmFunnel); +ReturnValue_t PersistentTmStore::startDumpFrom(uint32_t fromUnixSeconds) { + return startDumpFromUpTo(fromUnixSeconds, currentTv.tv_sec); } ReturnValue_t PersistentTmStore::storePacket(PusTmReader& reader) { @@ -159,10 +102,18 @@ ReturnValue_t PersistentTmStore::storePacket(PusTmReader& reader) { bool createNewFile = false; std::optional suffix = std::nullopt; + std::error_code e; + size_t fileSize = file_size(activeFile.value(), e); + if (e) { + sif::error << "PersistentTmStore: Could not retrieve file size, " + "error " + << e.message() << std::endl; + return returnvalue::FAILED; + } if (currentTv.tv_sec > activeFileTv.tv_sec + static_cast(rolloverDiffSeconds)) { createNewFile = true; currentSameSecNumber = 0; - } else if (file_size(activeFile.value()) + reader.getFullPacketLen() > fileBuf.size()) { + } else if (fileSize + reader.getFullPacketLen() > fileBuf.size()) { createNewFile = true; if (currentSameSecNumber >= MAX_FILES_IN_ONE_SECOND) { currentSameSecNumber = 0; @@ -203,38 +154,14 @@ bool PersistentTmStore::updateBaseDir() { return false; } basePath = path(currentPrefix) / baseDir / baseName; - if (not exists(basePath)) { + std::error_code e; + if (not exists(basePath, e)) { create_directories(basePath); } baseDirUninitialized = false; return true; } -void PersistentTmStore::addApid(uint16_t apid) { - if (not filter.apid) { - filter.apid = std::vector({apid}); - return; - } - filter.apid.value().push_back(apid); -} - -void PersistentTmStore::addService(uint8_t service) { - if (not filter.services) { - filter.services = std::vector({service}); - return; - } - filter.services.value().push_back(service); -} - -void PersistentTmStore::addServiceSubservice(uint8_t service, uint8_t subservice) { - if (not filter.serviceSubservices) { - filter.serviceSubservices = - std::vector>({std::pair(service, subservice)}); - return; - } - filter.serviceSubservices.value().emplace_back(service, subservice); -} - void PersistentTmStore::deleteUpTo(uint32_t unixSeconds) { using namespace std::filesystem; for (auto const& file : directory_iterator(basePath)) { @@ -243,37 +170,132 @@ void PersistentTmStore::deleteUpTo(uint32_t unixSeconds) { } // Convert file time to the UNIX epoch struct tm fileTime {}; - if (pathToTm(file.path(), fileTime) != returnvalue::OK) { + if (pathToTime(file.path(), fileTime) != returnvalue::OK) { sif::error << "Time extraction for " << file << "failed" << std::endl; continue; } time_t fileEpoch = timegm(&fileTime); if (fileEpoch + rolloverDiffSeconds < unixSeconds) { - std::filesystem::remove(file.path()); + std::error_code e; + std::filesystem::remove(file.path(), e); } } } -void PersistentTmStore::dumpFromUpTo(uint32_t fromUnixSeconds, uint32_t upToUnixSeconds, - TmFunnelBase& funnel) { +ReturnValue_t PersistentTmStore::startDumpFromUpTo(uint32_t fromUnixSeconds, + uint32_t upToUnixSeconds) { using namespace std::filesystem; - for (auto const& file : directory_iterator(basePath)) { - if (file.is_directory()) { + if (state == State::DUMPING) { + return returnvalue::FAILED; + } + dumpParams.dirIter = directory_iterator(basePath); + if (dumpParams.dirIter == directory_iterator()) { + return returnvalue::FAILED; + } + dumpParams.fromUnixTime = fromUnixSeconds; + dumpParams.untilUnixTime = upToUnixSeconds; + state = State::DUMPING; + if (loadNextDumpFile() == DUMP_DONE) { + // State will be set inside the function loading the next file. + return DUMP_DONE; + } + return returnvalue::OK; +} + +ReturnValue_t PersistentTmStore::loadNextDumpFile() { + using namespace std::filesystem; + dumpParams.currentSize = 0; + std::error_code e; + for (; dumpParams.dirIter != directory_iterator(); dumpParams.dirIter++) { + dumpParams.dirEntry = *dumpParams.dirIter; + if (dumpParams.dirEntry.is_directory(e)) { continue; } + dumpParams.fileSize = std::filesystem::file_size(dumpParams.dirEntry.path(), e); + if (e) { + sif::error << "PersistentTmStore: Could not retrieve file size: " << e.message() << std::endl; + continue; + } + sif::debug << "Path: " << dumpParams.dirEntry.path() << std::endl; + + // Can't even read CCSDS header. + if (dumpParams.fileSize <= 6) { + continue; + } + if (dumpParams.fileSize > fileBuf.size()) { + sif::error << "PersistentTmStore: File too large, is deleted" << std::endl; + triggerEvent(persTmStore::FILE_TOO_LARGE, dumpParams.fileSize, fileBuf.size()); + std::filesystem::remove(dumpParams.dirEntry.path(), e); + continue; + } + const path& file = dumpParams.dirEntry.path(); struct tm fileTime {}; - if (pathToTm(file.path(), fileTime) != returnvalue::OK) { + if (pathToTime(file, fileTime) != returnvalue::OK) { sif::error << "Time extraction for file " << file << "failed" << std::endl; continue; } auto fileEpoch = static_cast(timegm(&fileTime)); - if ((fileEpoch > fromUnixSeconds) and (fileEpoch + rolloverDiffSeconds <= upToUnixSeconds)) { - fileToPackets(file, fileEpoch, funnel); + if ((fileEpoch > dumpParams.fromUnixTime) and + (fileEpoch + rolloverDiffSeconds <= dumpParams.untilUnixTime)) { + dumpParams.currentFileUnixStamp = fileEpoch; + std::ifstream ifile(file, std::ios::binary); + if (ifile.bad()) { + sif::error << "PersistentTmStore: File is bad" << std::endl; + continue; + } + ifile.read(reinterpret_cast(fileBuf.data()), + static_cast(dumpParams.fileSize)); + // Increment iterator for next cycle. + dumpParams.dirIter++; + break; } } + if (dumpParams.dirIter == directory_iterator()) { + state = State::IDLE; + return DUMP_DONE; + } + return returnvalue::OK; } -ReturnValue_t PersistentTmStore::pathToTm(const std::filesystem::path& path, struct tm& time) { +ReturnValue_t PersistentTmStore::dumpNextPacket(DirectTmSinkIF& tmSink, size_t& dumpedLen, + bool& fileHasSwapped) { + if (state == State::IDLE) { + return returnvalue::FAILED; + } + PusTmReader reader(&timeReader, fileBuf.data() + dumpParams.currentSize, + fileBuf.size() - dumpParams.currentSize); + // CRC check to fully ensure this is a valid TM + ReturnValue_t result = reader.parseDataWithCrcCheck(); + if (result == returnvalue::OK) { + result = tmSink.write(fileBuf.data() + dumpParams.currentSize, reader.getFullPacketLen()); + if (result == DirectTmSinkIF::IS_BUSY) { + return result; + } else if (result != returnvalue::OK) { + // TODO: Event? + sif::error << "PersistentTmStore: Writing to TM sink failed" << std::endl; + return result; + } + dumpParams.currentSize += reader.getFullPacketLen(); + dumpedLen = reader.getFullPacketLen(); + if (dumpParams.currentSize >= dumpParams.fileSize) { + fileHasSwapped = true; + return loadNextDumpFile(); + } + } else { + sif::error << "PersistentTmStore: Parsing of PUS TM failed with code " << result << std::endl; + triggerEvent(persTmStore::POSSIBLE_FILE_CORRUPTION, result, dumpParams.currentFileUnixStamp); + // Delete the file and load next. Could use better algorithm to partially + // restore the file dump, but for now do not trust the file. + dumpedLen = 0; + std::error_code e; + std::filesystem::remove(dumpParams.dirEntry.path().c_str(), e); + fileHasSwapped = true; + return loadNextDumpFile(); + } + return returnvalue::OK; +} + +ReturnValue_t PersistentTmStore::pathToTime(const std::filesystem::path& path, struct tm& time) { auto pathStr = path.string(); size_t splitChar = pathStr.find('_'); auto timeOnlyStr = pathStr.substr(splitChar + 1); @@ -283,39 +305,6 @@ ReturnValue_t PersistentTmStore::pathToTm(const std::filesystem::path& path, str return returnvalue::OK; } -void PersistentTmStore::fileToPackets(const std::filesystem::path& path, uint32_t unixStamp, - TmFunnelBase& funnel) { - store_address_t storeId; - TmTcMessage message; - size_t size = std::filesystem::file_size(path); - if (size < 6) { - // Can't even read the CCSDS header - return; - } - std::ifstream ifile(path, std::ios::binary); - ifile.read(reinterpret_cast(fileBuf.data()), static_cast(size)); - size_t currentIdx = 0; - while (currentIdx < size) { - PusTmReader reader(&timeReader, fileBuf.data(), fileBuf.size()); - // CRC check to fully ensure this is a valid TM - ReturnValue_t result = reader.parseDataWithCrcCheck(); - if (result == returnvalue::OK) { - result = tmStore.addData(&storeId, fileBuf.data() + currentIdx, reader.getFullPacketLen()); - if (result != returnvalue::OK) { - continue; - } - funnel.sendPacketToDestinations(storeId, message, fileBuf.data() + currentIdx, - reader.getFullPacketLen()); - currentIdx += reader.getFullPacketLen(); - } else { - sif::error << "Parsing of PUS TM failed with code " << result << std::endl; - triggerEvent(POSSIBLE_FILE_CORRUPTION, result, unixStamp); - // Stop for now, do not really know where to continue and we do not trust the file anymore. - break; - } - } -} - ReturnValue_t PersistentTmStore::createMostRecentFile(std::optional suffix) { using namespace std::filesystem; unsigned currentIdx = 0; @@ -360,3 +349,11 @@ ReturnValue_t PersistentTmStore::initializeTmStore() { updateBaseDir(); return assignAndOrCreateMostRecentFile(); } + +PersistentTmStore::State PersistentTmStore::getState() const { return state; } + +void PersistentTmStore::getStartAndEndTimeCurrentOrLastDump(uint32_t& startTime, + uint32_t& endTime) const { + startTime = dumpParams.fromUnixTime; + endTime = dumpParams.untilUnixTime; +} diff --git a/mission/tmtc/PersistentTmStore.h b/mission/tmtc/PersistentTmStore.h index d8bc9acf..a0910026 100644 --- a/mission/tmtc/PersistentTmStore.h +++ b/mission/tmtc/PersistentTmStore.h @@ -1,51 +1,76 @@ #ifndef MISSION_TMTC_TMSTOREBACKEND_H_ #define MISSION_TMTC_TMSTOREBACKEND_H_ +#include #include +#include #include #include #include #include #include +#include #include -#include "TmFunnelBase.h" #include "eive/eventSubsystemIds.h" - -struct PacketFilter { - std::optional> apid; - std::optional> services; - std::optional>> serviceSubservices; -}; +#include "eive/resultClassIds.h" enum class RolloverInterval { MINUTELY, HOURLY, DAILY }; +struct PersistentTmStoreArgs { + PersistentTmStoreArgs(object_id_t objectId, const char* baseDir, std::string baseName, + RolloverInterval intervalUnit, uint32_t intervalCount, + StorageManagerIF& tmStore, SdCardMountedIF& sdcMan) + : objectId(objectId), + baseDir(baseDir), + baseName(baseName), + intervalUnit(intervalUnit), + intervalCount(intervalCount), + tmStore(tmStore), + sdcMan(sdcMan) {} + + object_id_t objectId; + const char* baseDir; + std::string baseName; + RolloverInterval intervalUnit; + uint32_t intervalCount; + StorageManagerIF& tmStore; + SdCardMountedIF& sdcMan; +}; + class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { public: - static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PERSISTENT_TM_STORE; + enum class State { IDLE, DUMPING }; + static constexpr uint8_t INTERFACE_ID = CLASS_ID::PERSISTENT_TM_STORE; + static constexpr ReturnValue_t DUMP_DONE = returnvalue::makeCode(INTERFACE_ID, 0); + static constexpr ReturnValue_t BUSY_DUMPING = returnvalue::makeCode(INTERFACE_ID, 1); - //! [EXPORT] : [COMMENT] - //! P1: Result code of TM packet parser. - //! P2: Timestamp of possibly corrupt file as a unix timestamp. - static constexpr Event POSSIBLE_FILE_CORRUPTION = - event::makeEvent(SUBSYSTEM_ID, 0, severity::LOW); - PersistentTmStore(object_id_t objectId, const char* baseDir, std::string baseName, - RolloverInterval intervalUnit, uint32_t intervalCount, - StorageManagerIF& tmStore, SdCardMountedIF& sdcMan); + PersistentTmStore(PersistentTmStoreArgs args); ReturnValue_t initializeTmStore(); - ReturnValue_t handleCommandQueue(StorageManagerIF& ipcStore, TmFunnelBase& tmFunnel); - - void addApid(uint16_t apid); - void addService(uint8_t service); - void addServiceSubservice(uint8_t service, uint8_t subservice); + State getState() const; + ReturnValue_t handleCommandQueue(StorageManagerIF& ipcStore, Command_t& execCmd); void deleteUpTo(uint32_t unixSeconds); - void dumpFrom(uint32_t fromUnixSeconds, TmFunnelBase& tmFunnel); - void dumpFromUpTo(uint32_t fromUnixSeconds, uint32_t upToUnixSeconds, TmFunnelBase& tmFunnel); + ReturnValue_t startDumpFrom(uint32_t fromUnixSeconds); + ReturnValue_t startDumpFromUpTo(uint32_t fromUnixSeconds, uint32_t upToUnixSeconds); + /** + * + * @param tmSink + * @param dumpedLen + * @param fileHasSwapped + * @return DUMP_DONE if dump is finished, returnvalue::OK if dump of next packet was a success, + * and DirectTmSinkIF::IS_BUSY is TM sink is busy. + */ + ReturnValue_t dumpNextPacket(DirectTmSinkIF& tmSink, size_t& dumpedLen, bool& fileHasSwapped); - ReturnValue_t passPacket(PusTmReader& reader); + void getStartAndEndTimeCurrentOrLastDump(uint32_t& startTime, uint32_t& endTime) const; + ReturnValue_t storePacket(PusTmReader& reader); + ReturnValue_t cancelDump(); + + protected: + StorageManagerIF& tmStore; private: static constexpr uint8_t MAX_FILES_IN_ONE_SECOND = 10; @@ -53,8 +78,12 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { // ISO8601 timestamp. static constexpr char FILE_DATE_FORMAT[] = "%FT%H%M%SZ"; + //! [EXPORT] : [SKIP] + static constexpr ReturnValue_t INVALID_FILE_DETECTED_AND_DELETED = returnvalue::makeCode(2, 1); + MessageQueueIF* tcQueue; - PacketFilter filter; + State state = State::IDLE; + // PacketFilter filter; CdsShortTimeStamper timeReader; bool baseDirUninitialized = true; const char* baseDir; @@ -65,9 +94,19 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { std::array fileBuf{}; timeval currentTv; timeval activeFileTv{}; + + struct ActiveDumpParams { + uint32_t fromUnixTime = 0; + uint32_t untilUnixTime = 0; + uint32_t currentFileUnixStamp = 0; + std::filesystem::directory_iterator dirIter; + std::filesystem::directory_entry dirEntry; + size_t fileSize = 0; + size_t currentSize = 0; + }; + ActiveDumpParams dumpParams; std::optional activeFile; SdCardMountedIF& sdcMan; - StorageManagerIF& tmStore; /** * To get the queue where commands shall be sent. @@ -77,11 +116,11 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { void calcDiffSeconds(RolloverInterval intervalUnit, uint32_t intervalCount); ReturnValue_t createMostRecentFile(std::optional suffix); - static ReturnValue_t pathToTm(const std::filesystem::path& path, struct tm& time); - void fileToPackets(const std::filesystem::path& path, uint32_t unixStamp, TmFunnelBase& funnel); + static ReturnValue_t pathToTime(const std::filesystem::path& path, struct tm& time); + void fileToPackets(const std::filesystem::path& path, uint32_t unixStamp); + ReturnValue_t loadNextDumpFile(); bool updateBaseDir(); ReturnValue_t assignAndOrCreateMostRecentFile(); - ReturnValue_t storePacket(PusTmReader& reader); }; #endif /* MISSION_TMTC_TMSTOREBACKEND_H_ */ diff --git a/mission/tmtc/PersistentTmStoreWithTmQueue.cpp b/mission/tmtc/PersistentTmStoreWithTmQueue.cpp new file mode 100644 index 00000000..df4960d3 --- /dev/null +++ b/mission/tmtc/PersistentTmStoreWithTmQueue.cpp @@ -0,0 +1,33 @@ +#include "PersistentTmStoreWithTmQueue.h" + +#include +#include + +PersistentTmStoreWithTmQueue::PersistentTmStoreWithTmQueue(PersistentTmStoreArgs args, + const char* storeName, + uint32_t tmQueueDepth) + : PersistentTmStore(args), storeName(storeName) { + tmQueue = QueueFactory::instance()->createMessageQueue(tmQueueDepth); +} + +ReturnValue_t PersistentTmStoreWithTmQueue::handleNextTm() { + TmTcMessage msg; + ReturnValue_t result = tmQueue->receiveMessage(&msg); + if (result == MessageQueueIF::EMPTY) { + return result; + } + auto accessor = tmStore.getData(msg.getStorageId()); + if (accessor.first != returnvalue::OK) { + return result; + } + PusTmReader reader(accessor.second.data(), accessor.second.size()); + storePacket(reader); + return returnvalue::OK; +} + +const char* PersistentTmStoreWithTmQueue::getName() const { return storeName; } + +MessageQueueId_t PersistentTmStoreWithTmQueue::getReportReceptionQueue( + uint8_t virtualChannel) const { + return tmQueue->getId(); +} diff --git a/mission/tmtc/PersistentTmStoreWithTmQueue.h b/mission/tmtc/PersistentTmStoreWithTmQueue.h new file mode 100644 index 00000000..be1026f8 --- /dev/null +++ b/mission/tmtc/PersistentTmStoreWithTmQueue.h @@ -0,0 +1,20 @@ +#ifndef MISSION_TMTC_PERSISTENTTMSTOREWITHTMQUEUE_H_ +#define MISSION_TMTC_PERSISTENTTMSTOREWITHTMQUEUE_H_ +#include + +class PersistentTmStoreWithTmQueue : public PersistentTmStore, public AcceptsTelemetryIF { + public: + PersistentTmStoreWithTmQueue(PersistentTmStoreArgs args, const char* storeName, + uint32_t tmQueueDepth); + + ReturnValue_t handleNextTm(); + + [[nodiscard]] const char* getName() const override; + [[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override; + + private: + const char* storeName; + MessageQueueIF* tmQueue; +}; + +#endif /* MISSION_TMTC_PERSISTENTTMSTOREWITHTMQUEUE_H_ */ diff --git a/mission/tmtc/PusLiveDemux.cpp b/mission/tmtc/PusLiveDemux.cpp new file mode 100644 index 00000000..d8ae1126 --- /dev/null +++ b/mission/tmtc/PusLiveDemux.cpp @@ -0,0 +1,48 @@ +#include "PusLiveDemux.h" + +#include +#include + +PusLiveDemux::PusLiveDemux(MessageQueueIF& ownerQueue) : ownerQueue(ownerQueue) {} + +ReturnValue_t PusLiveDemux::demultiplexPackets(StorageManagerIF& tmStore, + store_address_t origStoreId, const uint8_t* tmData, + size_t tmSize) { + ReturnValue_t result = returnvalue::OK; + for (unsigned int idx = 0; idx < destinations.size(); idx++) { + const auto& dest = 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, tmData, tmSize); + if (result == returnvalue::OK) { + message.setStorageId(storeId); + } else { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" + << std::endl; +#endif + } + } else { + message.setStorageId(origStoreId); + } + } + result = ownerQueue.sendMessage(dest.queueId, &message); + if (result != returnvalue::OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "PusLiveDemux::handlePacket: Error sending TM to downlink handler " << dest.name + << std::endl; +#endif + tmStore.deleteData(message.getStorageId()); + } + } + return result; +} + +void PusLiveDemux::addDestination(const char* name, const AcceptsTelemetryIF& downlinkDestination, + uint8_t vcid) { + auto queueId = downlinkDestination.getReportReceptionQueue(vcid); + destinations.emplace_back(name, queueId, vcid); +} diff --git a/mission/tmtc/PusLiveDemux.h b/mission/tmtc/PusLiveDemux.h new file mode 100644 index 00000000..b9af04ff --- /dev/null +++ b/mission/tmtc/PusLiveDemux.h @@ -0,0 +1,35 @@ +#ifndef MISSION_TMTC_PUSLIVEDEMUX_H_ +#define MISSION_TMTC_PUSLIVEDEMUX_H_ + +#include +#include +#include +#include + +#include + +class PusLiveDemux { + public: + PusLiveDemux(MessageQueueIF& ownerQueue); + ReturnValue_t demultiplexPackets(StorageManagerIF& tmStore, store_address_t origStoreId, + const uint8_t* tmData, size_t tmSize); + + void addDestination(const char* name, const AcceptsTelemetryIF& downlinkDestination, + uint8_t vcid = 0); + + private: + struct Destination { + Destination(const char* name, MessageQueueId_t queueId, uint8_t virtualChannel) + : name(name), queueId(queueId), virtualChannel(virtualChannel) {} + + const char* name; + MessageQueueId_t queueId; + uint8_t virtualChannel = 0; + }; + + MessageQueueIF& ownerQueue; + TmTcMessage message; + std::vector destinations; +}; + +#endif /* MISSION_TMTC_PUSLIVEDEMUX_H_ */ diff --git a/mission/tmtc/PusPacketFilter.cpp b/mission/tmtc/PusPacketFilter.cpp new file mode 100644 index 00000000..51abc02b --- /dev/null +++ b/mission/tmtc/PusPacketFilter.cpp @@ -0,0 +1,64 @@ +#include + +#include + +PusPacketFilter::PusPacketFilter() {} + +void PusPacketFilter::addApid(uint16_t apid) { + if (not this->apid) { + this->apid = std::vector({apid}); + return; + } + this->apid.value().push_back(apid); +} + +void PusPacketFilter::addService(uint8_t service) { + if (not this->services) { + this->services = std::vector({service}); + return; + } + this->services.value().push_back(service); +} + +void PusPacketFilter::addServiceSubservice(uint8_t service, uint8_t subservice) { + if (not serviceSubservices) { + serviceSubservices = std::vector>({std::pair(service, subservice)}); + return; + } + serviceSubservices.value().emplace_back(service, subservice); +} + +bool PusPacketFilter::packetMatches(PusTmReader& reader) const { + bool inApidList = false; + if (apid) { + auto& apidFilter = apid.value(); + if (std::find(apidFilter.begin(), apidFilter.end(), reader.getApid()) != apidFilter.end()) { + if (not serviceSubservices and not services) { + return true; + } + inApidList = true; + } + } + std::pair serviceSubservice; + serviceSubservice.first = reader.getService(); + serviceSubservice.second = reader.getSubService(); + if (services) { + auto& serviceFilter = services.value(); + if (std::find(serviceFilter.begin(), serviceFilter.end(), serviceSubservice.first) != + serviceFilter.end()) { + if (apid and inApidList) { + return true; + } + } + } + if (serviceSubservices) { + auto& serviceSubserviceFilter = serviceSubservices.value(); + if (std::find(serviceSubserviceFilter.begin(), serviceSubserviceFilter.end(), + serviceSubservice) != serviceSubserviceFilter.end()) { + if (apid and inApidList) { + return true; + } + } + } + return false; +} diff --git a/mission/tmtc/PusPacketFilter.h b/mission/tmtc/PusPacketFilter.h new file mode 100644 index 00000000..78d9cc18 --- /dev/null +++ b/mission/tmtc/PusPacketFilter.h @@ -0,0 +1,24 @@ +#ifndef MISSION_TMTC_PUSPACKETFILTER_H_ +#define MISSION_TMTC_PUSPACKETFILTER_H_ + +#include + +#include +#include + +class PusPacketFilter { + public: + PusPacketFilter(); + + bool packetMatches(PusTmReader& reader) const; + void addApid(uint16_t apid); + void addService(uint8_t service); + void addServiceSubservice(uint8_t service, uint8_t subservice); + + private: + std::optional> apid; + std::optional> services; + std::optional>> serviceSubservices; +}; + +#endif /* MISSION_TMTC_PUSPACKETFILTER_H_ */ diff --git a/mission/tmtc/PusTmFunnel.cpp b/mission/tmtc/PusTmFunnel.cpp index 08a047a0..dbc8f1c4 100644 --- a/mission/tmtc/PusTmFunnel.cpp +++ b/mission/tmtc/PusTmFunnel.cpp @@ -10,72 +10,15 @@ #include "fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h" #include "tmtc/pusIds.h" -PusTmFunnel::PusTmFunnel(TmFunnelBase::FunnelCfg cfg, TimeReaderIF &timeReader, - SdCardMountedIF &sdcMan) - : TmFunnelBase(cfg), - timeReader(timeReader), - miscStore(objects::MISC_TM_STORE, "tm", "misc", RolloverInterval::HOURLY, 2, tmStore, sdcMan), - okStore(objects::OK_TM_STORE, "tm", "ok", RolloverInterval::MINUTELY, 30, tmStore, sdcMan), - notOkStore(objects::NOT_OK_TM_STORE, "tm", "nok", RolloverInterval::MINUTELY, 30, tmStore, - sdcMan), - hkStore(objects::HK_TM_STORE, "tm", "hk", RolloverInterval::MINUTELY, 15, tmStore, sdcMan), - sdcMan(sdcMan) { - Clock::getClock_timeval(¤tTv); - Clock::getUptime(&lastTvUpdate); - hkStore.addApid(config::EIVE_PUS_APID); - hkStore.addService(pus::PUS_SERVICE_3); - miscStore.addApid(config::EIVE_PUS_APID); - miscStore.addService(pus::PUS_SERVICE_17); - miscStore.addService(pus::PUS_SERVICE_2); - miscStore.addService(pus::PUS_SERVICE_200); - miscStore.addService(pus::PUS_SERVICE_201); - miscStore.addService(pus::PUS_SERVICE_9); - miscStore.addService(pus::PUS_SERVICE_20); - okStore.addApid(config::EIVE_PUS_APID); - okStore.addServiceSubservice(pus::PUS_SERVICE_5, - Service5EventReporting::Subservice::NORMAL_REPORT); - okStore.addService(pus::PUS_SERVICE_8); - okStore.addServiceSubservice(pus::PUS_SERVICE_1, 1); - okStore.addServiceSubservice(pus::PUS_SERVICE_1, 3); - okStore.addServiceSubservice(pus::PUS_SERVICE_1, 5); - okStore.addServiceSubservice(pus::PUS_SERVICE_1, 7); - notOkStore.addApid(config::EIVE_PUS_APID); - notOkStore.addServiceSubservice(pus::PUS_SERVICE_5, 2); - notOkStore.addServiceSubservice(pus::PUS_SERVICE_5, 3); - notOkStore.addServiceSubservice(pus::PUS_SERVICE_5, 4); - notOkStore.addServiceSubservice(pus::PUS_SERVICE_1, 2); - notOkStore.addServiceSubservice(pus::PUS_SERVICE_1, 4); - notOkStore.addServiceSubservice(pus::PUS_SERVICE_1, 6); - notOkStore.addServiceSubservice(pus::PUS_SERVICE_1, 8); -} +PusTmFunnel::PusTmFunnel(TmFunnelBase::FunnelCfg cfg, StorageManagerIF &ramToFileStore, + TimeReaderIF &timeReader, SdCardMountedIF &sdcMan) + : TmFunnelBase(cfg), ramToFileStore(ramToFileStore), timeReader(timeReader), sdcMan(sdcMan) {} PusTmFunnel::~PusTmFunnel() = default; ReturnValue_t PusTmFunnel::performOperation(uint8_t) { CommandMessage cmdMessage; ReturnValue_t result; - try { - result = okStore.handleCommandQueue(ipcStore, *this); - if (result != returnvalue::OK) { - sif::error << "PusTmFunnel::performOperation: Issue handling OK store command" << std::endl; - } - result = notOkStore.handleCommandQueue(ipcStore, *this); - if (result != returnvalue::OK) { - sif::error << "PusTmFunnel::performOperation: Issue handling NOT OK store command" - << std::endl; - } - result = hkStore.handleCommandQueue(ipcStore, *this); - if (result != returnvalue::OK) { - sif::error << "PusTmFunnel::performOperation: Issue handling HK store command" << std::endl; - } - result = miscStore.handleCommandQueue(ipcStore, *this); - if (result != returnvalue::OK) { - sif::error << "PusTmFunnel::performOperation: Issue handling MISC store command" << std::endl; - } - } catch (const std::bad_optional_access &e) { - sif::error << e.what() << "when handling TM store command" << std::endl; - } - TmTcMessage currentMessage; unsigned int count = 0; result = tmQueue->receiveMessage(¤tMessage); @@ -86,7 +29,7 @@ ReturnValue_t PusTmFunnel::performOperation(uint8_t) { break; } count++; - if (count == 500) { + if (count == 1000) { sif::error << "PusTmFunnel: Possible message storm detected" << std::endl; break; } @@ -119,38 +62,25 @@ ReturnValue_t PusTmFunnel::handleTmPacket(TmTcMessage &message) { sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT; packet.updateErrorControl(); - timeval currentUptime{}; - Clock::getUptime(¤tUptime); - if (currentUptime.tv_sec - lastTvUpdate.tv_sec > - static_cast(TV_UPDATE_INTERVAL_SECS)) { - Clock::getClock_timeval(¤tTv); - lastTvUpdate = currentUptime; + // Send to persistent TM store if the packet matches some filter. + MessageQueueId_t destination; + if (persistentTmMap.packetMatches(packet, destination)) { + store_address_t storageId; + result = ramToFileStore.addData(&storageId, packetData, size); + TmTcMessage msg(storageId); + if (result != returnvalue::OK) { + sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" << std::endl; + } else { + tmQueue->sendMessage(destination, &msg); + } } - bool sdcUsable = sdcMan.isSdCardUsable(std::nullopt); - initStoresIfPossible(sdcUsable); - if (sdcUsable) { - miscStore.passPacket(packet); - okStore.passPacket(packet); - notOkStore.passPacket(packet); - hkStore.passPacket(packet); - } - return sendPacketToDestinations(origStoreId, message, packetData, size); + // Send to registered live targets. + return demultiplexLivePackets(origStoreId, packetData, size); } const char *PusTmFunnel::getName() const { return "PUS TM Funnel"; } -void PusTmFunnel::initStoresIfPossible(bool sdCardUsable) { - if (not storesInitialized and sdCardUsable and sdcMan.getCurrentMountPrefix() != nullptr) { - miscStore.initializeTmStore(); - okStore.initializeTmStore(); - hkStore.initializeTmStore(); - notOkStore.initializeTmStore(); - storesInitialized = true; - } -} - -ReturnValue_t PusTmFunnel::initialize() { - initStoresIfPossible(sdcMan.isSdCardUsable(std::nullopt)); - return returnvalue::OK; +void PusTmFunnel::addPersistentTmStoreRouting(PusPacketFilter filter, MessageQueueId_t dest) { + persistentTmMap.addRouting(filter, dest); } diff --git a/mission/tmtc/PusTmFunnel.h b/mission/tmtc/PusTmFunnel.h index ab6a9480..ba6e8d3e 100644 --- a/mission/tmtc/PusTmFunnel.h +++ b/mission/tmtc/PusTmFunnel.h @@ -6,6 +6,7 @@ #include #include #include +#include #include #include @@ -23,30 +24,26 @@ */ class PusTmFunnel : public TmFunnelBase { public: - PusTmFunnel(TmFunnelBase::FunnelCfg cfg, TimeReaderIF &timeReader, SdCardMountedIF &sdcMan); + PusTmFunnel(TmFunnelBase::FunnelCfg cfg, StorageManagerIF &ramToFileStore, + TimeReaderIF &timeReader, SdCardMountedIF &sdcMan); [[nodiscard]] const char *getName() const override; ~PusTmFunnel() override; ReturnValue_t performOperation(uint8_t operationCode); + void addPersistentTmStoreRouting(PusPacketFilter filter, MessageQueueId_t dest); private: // Update TV stamp every 5 minutes static constexpr dur_millis_t TV_UPDATE_INTERVAL_SECS = 60 * 5; uint16_t sourceSequenceCount = 0; + StorageManagerIF &ramToFileStore; TimeReaderIF &timeReader; bool storesInitialized = false; - timeval currentTv{}; - timeval lastTvUpdate{}; - PersistentTmStore miscStore; - PersistentTmStore okStore; - PersistentTmStore notOkStore; - PersistentTmStore hkStore; SdCardMountedIF &sdcMan; + PusTmRouteByFilterHelper persistentTmMap; ReturnValue_t handleTmPacket(TmTcMessage &message); - void initStoresIfPossible(bool sdCardUsable); - ReturnValue_t initialize() override; }; #endif // FSFW_EXAMPLE_COMMON_PUSTMFUNNEL_H diff --git a/mission/tmtc/PusTmRouteByFilterHelper.cpp b/mission/tmtc/PusTmRouteByFilterHelper.cpp new file mode 100644 index 00000000..15adde05 --- /dev/null +++ b/mission/tmtc/PusTmRouteByFilterHelper.cpp @@ -0,0 +1,19 @@ +#include "PusTmRouteByFilterHelper.h" + +#include + +PusTmRouteByFilterHelper::PusTmRouteByFilterHelper() = default; + +bool PusTmRouteByFilterHelper::packetMatches(PusTmReader& reader, MessageQueueId_t& destination) { + for (const auto& filterAndDest : routerMap) { + if (filterAndDest.first.packetMatches(reader)) { + destination = filterAndDest.second; + return true; + } + } + return false; +} + +void PusTmRouteByFilterHelper::addRouting(PusPacketFilter filter, MessageQueueId_t destination) { + routerMap.emplace_back(std::move(filter), destination); +} diff --git a/mission/tmtc/PusTmRouteByFilterHelper.h b/mission/tmtc/PusTmRouteByFilterHelper.h new file mode 100644 index 00000000..92bb0f6f --- /dev/null +++ b/mission/tmtc/PusTmRouteByFilterHelper.h @@ -0,0 +1,29 @@ +#ifndef MISSION_TMTC_PUSTMROUTER_H_ +#define MISSION_TMTC_PUSTMROUTER_H_ + +#include +#include + +/** + * Simple composition of concrecte @PusPacketFilters which also maps them to + * a destination ID. + */ +class PusTmRouteByFilterHelper { + public: + PusTmRouteByFilterHelper(); + + /** + * Checks whether the packet matches any of the inserted filters and returns + * the destination if it does. Currently only supports one destination. + * @param reader + * @param destination + * @return + */ + bool packetMatches(PusTmReader& reader, MessageQueueId_t& destination); + void addRouting(PusPacketFilter filter, MessageQueueId_t destination); + + private: + std::vector> routerMap; +}; + +#endif /* MISSION_TMTC_PUSTMROUTER_H_ */ diff --git a/mission/tmtc/TmFunnelBase.cpp b/mission/tmtc/TmFunnelBase.cpp index 6f788fc7..eb480b03 100644 --- a/mission/tmtc/TmFunnelBase.cpp +++ b/mission/tmtc/TmFunnelBase.cpp @@ -5,8 +5,16 @@ #include "fsfw/ipc/QueueFactory.h" TmFunnelBase::TmFunnelBase(FunnelCfg cfg) - : SystemObject(cfg.objectId), tmStore(cfg.tmStore), ipcStore(cfg.ipcStore) { - tmQueue = QueueFactory::instance()->createMessageQueue(cfg.tmMsgDepth); + : SystemObject(cfg.objectId), + name(cfg.name), + tmStore(cfg.tmStore), + ipcStore(cfg.ipcStore), + tmQueue(QueueFactory::instance()->createMessageQueue(cfg.tmMsgDepth)), + liveDemux(*tmQueue) {} + +ReturnValue_t TmFunnelBase::demultiplexLivePackets(store_address_t origStoreId, + const uint8_t *tmData, size_t tmSize) { + return liveDemux.demultiplexPackets(tmStore, origStoreId, tmData, tmSize); } TmFunnelBase::~TmFunnelBase() { QueueFactory::instance()->deleteMessageQueue(tmQueue); } @@ -15,43 +23,7 @@ MessageQueueId_t TmFunnelBase::getReportReceptionQueue(uint8_t virtualChannel) c return tmQueue->getId(); } -void TmFunnelBase::addDestination(const char *name, const AcceptsTelemetryIF &downlinkDestination, - uint8_t vcid) { - auto queueId = downlinkDestination.getReportReceptionQueue(vcid); - destinations.emplace_back(name, queueId, vcid); -} - -ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId, - TmTcMessage &message, - const uint8_t *packetData, size_t size) { - ReturnValue_t result = returnvalue::OK; - for (unsigned int idx = 0; idx < destinations.size(); idx++) { - const auto &dest = 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: Store too full to create data copy" - << std::endl; -#endif - } - } else { - message.setStorageId(origStoreId); - } - } - result = tmQueue->sendMessage(dest.queueId, &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; +void TmFunnelBase::addLiveDestination(const char *name, + const AcceptsTelemetryIF &downlinkDestination, uint8_t vcid) { + liveDemux.addDestination(name, downlinkDestination, vcid); } diff --git a/mission/tmtc/TmFunnelBase.h b/mission/tmtc/TmFunnelBase.h index af65771f..51d16626 100644 --- a/mission/tmtc/TmFunnelBase.h +++ b/mission/tmtc/TmFunnelBase.h @@ -6,45 +6,41 @@ #include #include #include +#include #include class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject { public: struct FunnelCfg { - FunnelCfg(object_id_t objId, StorageManagerIF& tmStore, StorageManagerIF& ipcStore, - uint32_t tmMsgDepth) - : objectId(objId), tmStore(tmStore), ipcStore(ipcStore), tmMsgDepth(tmMsgDepth) {} + FunnelCfg(object_id_t objId, const char* name, StorageManagerIF& tmStore, + StorageManagerIF& ipcStore, uint32_t tmMsgDepth) + : objectId(objId), + name(name), + tmStore(tmStore), + ipcStore(ipcStore), + tmMsgDepth(tmMsgDepth) {} object_id_t objectId; + const char* name; StorageManagerIF& tmStore; StorageManagerIF& ipcStore; uint32_t tmMsgDepth; }; explicit TmFunnelBase(FunnelCfg cfg); - void addDestination(const char* name, const AcceptsTelemetryIF& downlinkDestination, - uint8_t vcid = 0); - ReturnValue_t sendPacketToDestinations(store_address_t origStoreId, TmTcMessage& message, - const uint8_t* packetData, size_t size); [[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override; + void addLiveDestination(const char* name, const AcceptsTelemetryIF& downlinkDestination, + uint8_t vcid = 0); + ReturnValue_t demultiplexLivePackets(store_address_t origStoreId, const uint8_t* tmData, + size_t tmSize); ~TmFunnelBase() override; protected: + const char* name; StorageManagerIF& tmStore; StorageManagerIF& ipcStore; - - struct Destination { - Destination(const char* name, MessageQueueId_t queueId, uint8_t virtualChannel) - : name(name), queueId(queueId), virtualChannel(virtualChannel) {} - - const char* name; - MessageQueueId_t queueId; - uint8_t virtualChannel = 0; - }; - - std::vector destinations; - MessageQueueIF* tmQueue = nullptr; + PusLiveDemux liveDemux; }; #endif /* MISSION_TMTC_TMFUNNELBASE_H_ */ diff --git a/mission/tmtc/TmStoreTaskBase.cpp b/mission/tmtc/TmStoreTaskBase.cpp new file mode 100644 index 00000000..d8b6bdcb --- /dev/null +++ b/mission/tmtc/TmStoreTaskBase.cpp @@ -0,0 +1,87 @@ +#include "TmStoreTaskBase.h" + +#include +#include +#include +#include + +#include "mission/persistentTmStoreDefs.h" + +TmStoreTaskBase::TmStoreTaskBase(object_id_t objectId, StorageManagerIF& ipcStore, + VirtualChannel& channel, SdCardMountedIF& sdcMan) + : SystemObject(objectId), ipcStore(ipcStore), channel(channel), sdcMan(sdcMan) {} + +bool TmStoreTaskBase::handleOneStore(PersistentTmStoreWithTmQueue& store, + DumpContext& dumpContext) { + ReturnValue_t result; + bool tmToStoreReceived = false; + bool tcRequestReceived = false; + bool dumpsPerformed = false; + // Store TM persistently + result = store.handleNextTm(); + if (result == returnvalue::OK) { + tmToStoreReceived = true; + } + // Dump TMs when applicable + if (store.getState() == PersistentTmStore::State::DUMPING) { + size_t dumpedLen = 0; + bool fileHasSwapped; + if (not channel.isBusy()) { + tmSinkBusyCd.resetTimer(); + result = store.dumpNextPacket(channel, dumpedLen, fileHasSwapped); + if ((result == PersistentTmStore::DUMP_DONE or result == returnvalue::OK) and dumpedLen > 0) { + dumpContext.dumpedBytes += dumpedLen; + dumpContext.numberOfDumpedPackets += 1; + } + if (result == PersistentTmStore::DUMP_DONE) { + uint32_t startTime; + uint32_t endTime; + store.getStartAndEndTimeCurrentOrLastDump(startTime, endTime); + triggerEvent(dumpContext.eventIfDone, dumpContext.numberOfDumpedPackets, + dumpContext.dumpedBytes); + dumpsPerformed = true; + } else if (result == returnvalue::OK) { + dumpsPerformed = true; + } + } + if (cancelDumpCd.hasTimedOut() or tmSinkBusyCd.hasTimedOut()) { + triggerEvent(persTmStore::DUMP_WAS_CANCELLED, store.getObjectId()); + store.cancelDump(); + } + } else { + Command_t execCmd; + // Handle TC requests, for example deletion or retrieval requests. + result = store.handleCommandQueue(ipcStore, execCmd); + if (result == returnvalue::OK) { + if (execCmd == TmStoreMessage::DOWNLINK_STORE_CONTENT_TIME) { + cancelDumpCd.resetTimer(); + tmSinkBusyCd.resetTimer(); + dumpContext.reset(); + } + tcRequestReceived = true; + } + } + if (tcRequestReceived or tmToStoreReceived or dumpsPerformed) { + return true; + } + return false; +} + +bool TmStoreTaskBase::cyclicStoreCheck() { + if (not storesInitialized) { + storesInitialized = initStoresIfPossible(); + if (not storesInitialized) { + TaskFactory::delayTask(400); + return false; + } + } else if (sdCardCheckCd.hasTimedOut()) { + if (not sdcMan.isSdCardUsable(std::nullopt)) { + // Might be due to imminent shutdown or SD card switch. + storesInitialized = false; + TaskFactory::delayTask(100); + return false; + } + sdCardCheckCd.resetTimer(); + } + return true; +} diff --git a/mission/tmtc/TmStoreTaskBase.h b/mission/tmtc/TmStoreTaskBase.h new file mode 100644 index 00000000..2d899be3 --- /dev/null +++ b/mission/tmtc/TmStoreTaskBase.h @@ -0,0 +1,51 @@ +#ifndef MISSION_TMTC_TMSTORETASKBASE_H_ +#define MISSION_TMTC_TMSTORETASKBASE_H_ + +#include +#include +#include + +class TmStoreTaskBase : public SystemObject { + public: + struct DumpContext { + DumpContext(Event eventIfDone) : eventIfDone(eventIfDone) {} + void reset() { + numberOfDumpedPackets = 0; + dumpedBytes = 0; + } + const Event eventIfDone; + uint32_t numberOfDumpedPackets = 0; + uint32_t dumpedBytes = 0; + }; + + TmStoreTaskBase(object_id_t objectId, StorageManagerIF& ipcStore, VirtualChannel& channel, + SdCardMountedIF& sdcMan); + + protected: + /** + * Handling for one store. Returns if anything was done. + * @param store + * @return + */ + bool handleOneStore(PersistentTmStoreWithTmQueue& store, DumpContext& dumpContext); + + /** + * Occasionally check whether SD card is okay to be used. If not, poll whether it is ready to + * be used again and re-initialize stores. Returns whether store is okay to be used. + */ + bool cyclicStoreCheck(); + + virtual bool initStoresIfPossible() = 0; + + StorageManagerIF& ipcStore; + Countdown sdCardCheckCd = Countdown(800); + // 20 minutes are allowed as maximum dump time. + Countdown cancelDumpCd = Countdown(60 * 20 * 1000); + // If the TM sink is busy for 1 minute for whatever reason, cancel the dump. + Countdown tmSinkBusyCd = Countdown(60 * 1000); + VirtualChannel& channel; + bool storesInitialized = false; + SdCardMountedIF& sdcMan; +}; + +#endif /* MISSION_TMTC_TMSTORETASKBASE_H_ */ diff --git a/mission/tmtc/VirtualChannel.cpp b/mission/tmtc/VirtualChannel.cpp index 6a24cc09..da6ce13f 100644 --- a/mission/tmtc/VirtualChannel.cpp +++ b/mission/tmtc/VirtualChannel.cpp @@ -1,75 +1,28 @@ #include "VirtualChannel.h" -#include "CcsdsIpCoreHandler.h" -#include "OBSWConfig.h" -#include "fsfw/ipc/QueueFactory.h" -#include "fsfw/objectmanager/ObjectManager.h" -#include "fsfw/serviceinterface/ServiceInterfaceStream.h" -#include "fsfw/tmtcservices/TmTcMessage.h" +VirtualChannel::VirtualChannel(object_id_t objectId, uint8_t vcId, const char* vcName, PtmeIF& ptme, + const std::atomic_bool& linkStateProvider) + : SystemObject(objectId), + ptme(ptme), + vcId(vcId), + vcName(vcName), + linkStateProvider(linkStateProvider) {} -VirtualChannel::VirtualChannel(uint8_t vcId, uint32_t tmQueueDepth, object_id_t ownerId) - : vcId(vcId) { - auto mqArgs = MqArgs(ownerId, reinterpret_cast(vcId)); - tmQueue = QueueFactory::instance()->createMessageQueue( - tmQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); - vcName = "VC " + vcId; +ReturnValue_t VirtualChannel::initialize() { return returnvalue::OK; } + +ReturnValue_t VirtualChannel::sendNextTm(const uint8_t* data, size_t size) { + return write(data, size); } -ReturnValue_t VirtualChannel::initialize() { - tmStore = ObjectManager::instance()->get(objects::TM_STORE); - if (tmStore == nullptr) { - sif::error << "VirtualChannel::initialize: Failed to get tm store" << std::endl; - return returnvalue::FAILED; +ReturnValue_t VirtualChannel::write(const uint8_t* data, size_t size) { + if (linkStateProvider.load()) { + return ptme.writeToVc(vcId, data, size); } return returnvalue::OK; } -ReturnValue_t VirtualChannel::performOperation() { - ReturnValue_t result = returnvalue::OK; - TmTcMessage message; - - unsigned int count = 0; - while (tmQueue->receiveMessage(&message) == returnvalue::OK) { - store_address_t storeId = message.getStorageId(); - const uint8_t* data = nullptr; - size_t size = 0; - result = tmStore->getData(storeId, &data, &size); - if (result != returnvalue::OK) { - sif::warning << "VirtualChannel::performOperation: Failed to read data from TM store" - << std::endl; - tmStore->deleteData(storeId); - return result; - } - - if (linkIsUp) { - result = ptme->writeToVc(vcId, data, size); - } - tmStore->deleteData(storeId); - if (result != returnvalue::OK) { - return result; - } - - count++; - if (count == 500) { - sif::error << "VirtualChannel: Possible message storm detected" << std::endl; - break; - } - } - return result; -} - -MessageQueueId_t VirtualChannel::getReportReceptionQueue(uint8_t virtualChannel) const { - return tmQueue->getId(); -} - -void VirtualChannel::setPtmeObject(PtmeIF* ptme_) { - if (ptme_ == nullptr) { - sif::warning << "VirtualChannel::setPtmeObject: Invalid ptme object" << std::endl; - return; - } - ptme = ptme_; -} - -void VirtualChannel::setLinkState(bool linkIsUp_) { linkIsUp = linkIsUp_; } +uint8_t VirtualChannel::getVcid() const { return vcId; } const char* VirtualChannel::getName() const { return vcName.c_str(); } + +bool VirtualChannel::isBusy() const { return ptme.isBusy(vcId); } diff --git a/mission/tmtc/VirtualChannel.h b/mission/tmtc/VirtualChannel.h index 96d7ba9d..983fa448 100644 --- a/mission/tmtc/VirtualChannel.h +++ b/mission/tmtc/VirtualChannel.h @@ -1,14 +1,11 @@ -#ifndef VIRTUALCHANNEL_H_ -#define VIRTUALCHANNEL_H_ +#pragma once -#include +#include #include +#include -#include "OBSWConfig.h" -#include "fsfw/returnvalues/returnvalue.h" -#include "fsfw/tmtcservices/AcceptsTelemetryIF.h" - -class StorageManagerIF; +#include +#include /** * @brief This class represents a virtual channel. Sending a tm message to an object of this class @@ -16,7 +13,7 @@ class StorageManagerIF; * * @author J. Meier */ -class VirtualChannel : public AcceptsTelemetryIF { +class VirtualChannel : public SystemObject, public VirtualChannelIF { public: /** * @brief Constructor @@ -24,35 +21,20 @@ class VirtualChannel : public AcceptsTelemetryIF { * @param vcId The virtual channel id assigned to this object * @param tmQueueDepth Queue depth of queue receiving telemetry from other objects */ - VirtualChannel(uint8_t vcId, uint32_t tmQueueDepth, object_id_t ownerId); + VirtualChannel(object_id_t objectId, uint8_t vcId, const char* vcName, PtmeIF& ptme, + const std::atomic_bool& linkStateProvider); - ReturnValue_t initialize(); - MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel = 0) const override; - ReturnValue_t performOperation(); + ReturnValue_t initialize() override; + ReturnValue_t sendNextTm(const uint8_t* data, size_t size); + bool isBusy() const override; + ReturnValue_t write(const uint8_t* data, size_t size) override; + uint8_t getVcid() const; - /** - * @brief Sets the PTME object which handles access to the PTME IP Core. - * - * @param ptme Pointer to ptme object - */ - void setPtmeObject(PtmeIF* ptme_); - - /** - * @brief Can be used by the owner to set the link state. Packets will be discarded if link - * to ground station is down. - */ - void setLinkState(bool linkIsUp_); - const char* getName() const override; + const char* getName() const; private: - PtmeIF* ptme = nullptr; - MessageQueueIF* tmQueue = nullptr; + PtmeIF& ptme; uint8_t vcId = 0; std::string vcName; - - bool linkIsUp = false; - - StorageManagerIF* tmStore = nullptr; + const std::atomic_bool& linkStateProvider; }; - -#endif /* VIRTUALCHANNEL_H_ */ diff --git a/mission/tmtc/VirtualChannelWithQueue.cpp b/mission/tmtc/VirtualChannelWithQueue.cpp new file mode 100644 index 00000000..884dbf89 --- /dev/null +++ b/mission/tmtc/VirtualChannelWithQueue.cpp @@ -0,0 +1,49 @@ +#include + +#include "CcsdsIpCoreHandler.h" +#include "OBSWConfig.h" +#include "fsfw/ipc/QueueFactory.h" +#include "fsfw/objectmanager/ObjectManager.h" +#include "fsfw/serviceinterface/ServiceInterfaceStream.h" +#include "fsfw/tmtcservices/TmTcMessage.h" + +VirtualChannelWithQueue::VirtualChannelWithQueue(object_id_t objectId, uint8_t vcId, + const char* vcName, PtmeIF& ptme, + const std::atomic_bool& linkStateProvider, + StorageManagerIF& tmStore, uint32_t tmQueueDepth) + : VirtualChannel(objectId, vcId, vcName, ptme, linkStateProvider), tmStore(tmStore) { + auto mqArgs = MqArgs(getObjectId(), reinterpret_cast(getVcid())); + tmQueue = QueueFactory::instance()->createMessageQueue( + tmQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); +} + +const char* VirtualChannelWithQueue::getName() const { return VirtualChannel::getName(); } + +ReturnValue_t VirtualChannelWithQueue::sendNextTm() { + TmTcMessage message; + ReturnValue_t result = tmQueue->receiveMessage(&message); + if (result == MessageQueueIF::EMPTY) { + return result; + } + store_address_t storeId = message.getStorageId(); + const uint8_t* data = nullptr; + size_t size = 0; + result = tmStore.getData(storeId, &data, &size); + if (result != returnvalue::OK) { + sif::warning << "VirtualChannel::performOperation: Failed to read data from TM store" + << std::endl; + tmStore.deleteData(storeId); + return result; + } + + write(data, size); + tmStore.deleteData(storeId); + if (result != returnvalue::OK) { + return result; + } + return returnvalue::OK; +} + +MessageQueueId_t VirtualChannelWithQueue::getReportReceptionQueue(uint8_t virtualChannel) const { + return tmQueue->getId(); +} diff --git a/mission/tmtc/VirtualChannelWithQueue.h b/mission/tmtc/VirtualChannelWithQueue.h new file mode 100644 index 00000000..0c060a06 --- /dev/null +++ b/mission/tmtc/VirtualChannelWithQueue.h @@ -0,0 +1,42 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include "OBSWConfig.h" +#include "fsfw/returnvalues/returnvalue.h" +#include "fsfw/tmtcservices/AcceptsTelemetryIF.h" + +class StorageManagerIF; + +/** + * @brief This class represents a virtual channel. Sending a tm message to an object of this class + * will forward the tm packet to the respective virtual channel of the PTME IP Core. + * + * @author J. Meier + */ +class VirtualChannelWithQueue : public VirtualChannel, public AcceptsTelemetryIF { + public: + /** + * @brief Constructor + * + * @param vcId The virtual channel id assigned to this object + * @param tmQueueDepth Queue depth of queue receiving telemetry from other objects + */ + VirtualChannelWithQueue(object_id_t objectId, uint8_t vcId, const char* vcName, PtmeIF& ptme, + const std::atomic_bool& linkStateProvider, StorageManagerIF& tmStore, + uint32_t tmQueueDepth); + + MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel = 0) const override; + [[nodiscard]] const char* getName() const override; + ReturnValue_t sendNextTm(); + + private: + MessageQueueIF* tmQueue = nullptr; + StorageManagerIF& tmStore; +}; diff --git a/mission/tmtc/tmFilters.cpp b/mission/tmtc/tmFilters.cpp new file mode 100644 index 00000000..944cd3be --- /dev/null +++ b/mission/tmtc/tmFilters.cpp @@ -0,0 +1,57 @@ +#include "tmFilters.h" + +#include +#include + +#include "eive/definitions.h" + +PusPacketFilter filters::hkFilter() { + PusPacketFilter hkFilter; + hkFilter.addApid(config::EIVE_PUS_APID); + hkFilter.addService(pus::PUS_SERVICE_3); + return hkFilter; +} + +PusPacketFilter filters::miscFilter() { + PusPacketFilter miscFilter; + miscFilter.addApid(config::EIVE_PUS_APID); + miscFilter.addService(pus::PUS_SERVICE_17); + miscFilter.addService(pus::PUS_SERVICE_2); + miscFilter.addService(pus::PUS_SERVICE_200); + miscFilter.addService(pus::PUS_SERVICE_201); + miscFilter.addService(pus::PUS_SERVICE_9); + miscFilter.addService(pus::PUS_SERVICE_20); + return miscFilter; +} + +PusPacketFilter filters::okFilter() { + PusPacketFilter okFilter; + okFilter.addApid(config::EIVE_PUS_APID); + okFilter.addServiceSubservice(pus::PUS_SERVICE_5, + Service5EventReporting::Subservice::NORMAL_REPORT); + okFilter.addService(pus::PUS_SERVICE_8); + okFilter.addServiceSubservice(pus::PUS_SERVICE_1, 1); + okFilter.addServiceSubservice(pus::PUS_SERVICE_1, 3); + okFilter.addServiceSubservice(pus::PUS_SERVICE_1, 5); + okFilter.addServiceSubservice(pus::PUS_SERVICE_1, 7); + return okFilter; +} + +PusPacketFilter filters::notOkFilter() { + PusPacketFilter notOkFilter; + notOkFilter.addApid(config::EIVE_PUS_APID); + notOkFilter.addServiceSubservice(pus::PUS_SERVICE_5, 2); + notOkFilter.addServiceSubservice(pus::PUS_SERVICE_5, 3); + notOkFilter.addServiceSubservice(pus::PUS_SERVICE_5, 4); + notOkFilter.addServiceSubservice(pus::PUS_SERVICE_1, 2); + notOkFilter.addServiceSubservice(pus::PUS_SERVICE_1, 4); + notOkFilter.addServiceSubservice(pus::PUS_SERVICE_1, 6); + notOkFilter.addServiceSubservice(pus::PUS_SERVICE_1, 8); + return notOkFilter; +} + +PusPacketFilter filters::cfdpFilter() { + PusPacketFilter cfdpFilter; + cfdpFilter.addApid(config::EIVE_CFDP_APID); + return cfdpFilter; +} diff --git a/mission/tmtc/tmFilters.h b/mission/tmtc/tmFilters.h new file mode 100644 index 00000000..a358c8f0 --- /dev/null +++ b/mission/tmtc/tmFilters.h @@ -0,0 +1,15 @@ +#ifndef MISSION_TMTC_FILTERS_H_ +#define MISSION_TMTC_FILTERS_H_ +#include + +namespace filters { + +PusPacketFilter cfdpFilter(); +PusPacketFilter hkFilter(); +PusPacketFilter miscFilter(); +PusPacketFilter okFilter(); +PusPacketFilter notOkFilter(); + +} // namespace filters + +#endif /* MISSION_TMTC_FILTERS_H_ */ diff --git a/mission/utility/GlobalConfigHandler.h b/mission/utility/GlobalConfigHandler.h index 80e141c6..bb3badf2 100644 --- a/mission/utility/GlobalConfigHandler.h +++ b/mission/utility/GlobalConfigHandler.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -22,7 +23,6 @@ #include "OBSWConfig.h" #include "fsfw/parameters/HasParametersIF.h" #include "fsfw/parameters/ParameterHelper.h" -#include "mission/memory/NVMParameterBase.h" static std::map PARAM_KEY_MAP = { {PARAM0, "Parameter0"}, diff --git a/scripts/auto-formatter.sh b/scripts/auto-formatter.sh index 958ba0ac..1bea10a4 100755 --- a/scripts/auto-formatter.sh +++ b/scripts/auto-formatter.sh @@ -4,6 +4,7 @@ if [[ ! -f README.md ]]; then fi folder_list=( + "./watchdog" "./mission" "./linux" "./bsp_q7s" diff --git a/test/DummyParameter.h b/test/DummyParameter.h index 410f0958..fe9296ba 100644 --- a/test/DummyParameter.h +++ b/test/DummyParameter.h @@ -1,11 +1,11 @@ #ifndef BSP_Q7S_CORE_NVMPARAMS_PARAMETERDEFINITIONS_H_ #define BSP_Q7S_CORE_NVMPARAMS_PARAMETERDEFINITIONS_H_ +#include + #include #include -#include "mission/memory/NVMParameterBase.h" - class DummyParameter : public NVMParameterBase { public: static constexpr char DUMMY_KEY_PARAM_1[] = "dummy1"; diff --git a/thirdparty/arcsec_star_tracker b/thirdparty/arcsec_star_tracker index 93e93965..42907c36 160000 --- a/thirdparty/arcsec_star_tracker +++ b/thirdparty/arcsec_star_tracker @@ -1 +1 @@ -Subproject commit 93e93965e2c6405170b62c523dea1990db02d2ad +Subproject commit 42907c36c58e7133d3d3cbefbf96c1a8e35b60b7 diff --git a/tmtc b/tmtc index 56630b05..e862df4d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 56630b05c2d49651823892876c80d0885b25b9b4 +Subproject commit e862df4d06a502b059315bc6254a3b513686b52e diff --git a/unittest/mocks/EventManagerMock.cpp b/unittest/mocks/EventManagerMock.cpp index 383c755b..acbf1a0b 100644 --- a/unittest/mocks/EventManagerMock.cpp +++ b/unittest/mocks/EventManagerMock.cpp @@ -2,7 +2,7 @@ #include -EventManagerMock::EventManagerMock() : EventManager(objects::EVENT_MANAGER) {} +EventManagerMock::EventManagerMock() : EventManager(objects::EVENT_MANAGER, 80) {} ReturnValue_t EventManagerMock::performOperation(uint8_t opCode) { ReturnValue_t result = returnvalue::OK; @@ -60,4 +60,4 @@ bool EventManagerMock::isEventInEventList(object_id_t object, EventId_t eventId, } } return false; -} \ No newline at end of file +} diff --git a/watchdog/CMakeLists.txt b/watchdog/CMakeLists.txt index ecb50627..f7c7330c 100644 --- a/watchdog/CMakeLists.txt +++ b/watchdog/CMakeLists.txt @@ -1,10 +1,5 @@ -target_sources(${WATCHDOG_NAME} PRIVATE - main.cpp - Watchdog.cpp -) +target_sources(${WATCHDOG_NAME} PRIVATE main.cpp Watchdog.cpp) -target_include_directories(${WATCHDOG_NAME} PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR} -) +target_include_directories(${WATCHDOG_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) install(TARGETS ${WATCHDOG_NAME} RUNTIME DESTINATION bin) diff --git a/watchdog/Watchdog.cpp b/watchdog/Watchdog.cpp index a14bae53..18d71609 100644 --- a/watchdog/Watchdog.cpp +++ b/watchdog/Watchdog.cpp @@ -1,258 +1,304 @@ #include "Watchdog.h" -#include "definitions.h" #include -#include -#include -#include -#include #include +#include +#include +#include +#include -#include -#include -#include +#include #include #include +#include +#include +#include +#include "definitions.h" -WatchdogTask::WatchdogTask (): fd(0) { - int result = 0; - // Only create the FIFO if it does not exist yet - if(not std::filesystem::exists(watchdog::FIFO_NAME)) { - // Permission 666 or rw-rw-rw- - mode_t mode = DEFFILEMODE; - result = mkfifo(watchdog::FIFO_NAME.c_str(), mode); - if(result != 0) { - std::cerr << "eive-watchdog: Could not created named pipe at " << - watchdog::FIFO_NAME << ", error " << errno << ": " << strerror(errno) << - std::endl; - throw std::runtime_error("eive-watchdog: FIFO creation failed"); - } -#if WATCHDOG_VERBOSE_LEVEL >= 1 - std::cout << "eive-watchdog: Pipe at " << watchdog::FIFO_NAME << - " created successfully" << std::endl; -#endif +WatchdogTask::WatchdogTask() : fd(0) { + int result = 0; + std::error_code e; + // Only create the FIFO if it does not exist yet + if (not std::filesystem::exists(watchdog::FIFO_NAME, e)) { + // Permission 666 or rw-rw-rw- + mode_t mode = DEFFILEMODE; + result = mkfifo(watchdog::FIFO_NAME.c_str(), mode); + if (result != 0) { + std::cerr << "Could not created named pipe at " << watchdog::FIFO_NAME << ", error " << errno + << ": " << strerror(errno) << std::endl; + throw std::runtime_error("eive-watchdog: FIFO creation failed"); } +#if WATCHDOG_VERBOSE_LEVEL >= 1 + std::cout << "Pipe at " << watchdog::FIFO_NAME << " created successfully" << std::endl; +#endif + } } -WatchdogTask::~WatchdogTask() { - -} +WatchdogTask::~WatchdogTask() {} int WatchdogTask::performOperation() { - // Open FIFO read only and non-blocking - fd = open(watchdog::FIFO_NAME.c_str(), O_RDONLY | O_NONBLOCK); - if(fd < 0) { - std::cerr << "eive-watchdog: Opening pipe " << watchdog::FIFO_NAME << - "read-only failed with " << errno << ": " << strerror(errno) << std::endl; - return -1; + // Open FIFO read only and non-blocking + fd = open(watchdog::FIFO_NAME.c_str(), O_RDONLY | O_NONBLOCK); + if (fd < 0) { + std::cerr << "Opening pipe " << watchdog::FIFO_NAME << "read-only failed with " << errno << ": " + << strerror(errno) << std::endl; + return -1; + } + // Clear FIFO by reading until it is empty. + while (true) { + ssize_t readBytes = read(fd, buf.data(), buf.size()); + if (readBytes < 0) { + std::cerr << "Read error of FIFO: " << strerror(errno) << std::endl; + } else if (readBytes == 0) { + break; } - state = States::RUNNING; + } + state = States::NOT_STARTED; - while(true) { - WatchdogTask::LoopResult loopResult = watchdogLoop(); - switch(loopResult) { - case(LoopResult::OK): { - performRunningOperation(); - break; - } - case(LoopResult::CANCEL_RQ): { - std::cout << "eive-watchdog: Received cancel request, closing watchdog.." << std::endl; - return 0; - } - case(LoopResult::SUSPEND_RQ): { - performSuspendOperation(); - break; - } - case(LoopResult::TIMEOUT): { - performNotRunningOperation(loopResult); - break; - } - case(LoopResult::HUNG_UP): { - performNotRunningOperation(loopResult); - break; - } - case(LoopResult::RESTART_RQ): { - if(state == States::SUSPENDED or state == States::FAULTY) { - performRunningOperation(); - } - break; - } - case(LoopResult::FAULT): { - using namespace std::chrono_literals; - // Configuration error - std::cerr << "Fault has occured in watchdog loop" << std::endl; - // Prevent spam - std::this_thread::sleep_for(2000ms); - - } - } + bool breakOuter = false; + while (true) { + watchdogLoop(); + while (not resultQueue.empty()) { + auto nextRequest = resultQueue.front(); + if (not stateMachine(nextRequest)) { + breakOuter = true; + resultQueue.pop(); + break; + } + resultQueue.pop(); } - if (close(fd) < 0) { - std::cerr << "eive-watchdog: Closing named pipe at " << watchdog::FIFO_NAME << - "failed, error " << errno << ": " << strerror(errno) << std::endl; + if (breakOuter) { + break; } - std::cout << "eive-watchdog: Finished" << std::endl; - return 0; + } + if (close(fd) < 0) { + std::cerr << "Closing named pipe at " << watchdog::FIFO_NAME << "failed, error " << errno + << ": " << strerror(errno) << std::endl; + } + std::cout << "Closing" << std::endl; + return 0; } -WatchdogTask::LoopResult WatchdogTask::watchdogLoop() { - using namespace std::chrono_literals; - struct pollfd waiter = {}; - waiter.fd = fd; - waiter.events = POLLIN; +void WatchdogTask::watchdogLoop() { + using namespace std::chrono_literals; + struct pollfd waiter = {}; + waiter.fd = fd; + waiter.events = POLLIN; - switch(state) { - case(States::SUSPENDED): { - // Sleep, then check whether a restart request was received - std::this_thread::sleep_for(1000ms); - break; + // Only poll one file descriptor with timeout + switch (poll(&waiter, 1, watchdog::TIMEOUT_MS)) { + case (0): { + resultQueue.push(LoopResult::TIMEOUT); + return; } - case(States::RUNNING): { - // Continue as usual - break; - } - case(States::NOT_STARTED): { - // This should not happen - std::cerr << "eive-watchdog: State is NOT_STARTED, configuration error" << std::endl; - break; - } - case(States::FAULTY): { - // TODO: Not sure what to do yet. Continue for now - break; - } - } - - // 10 seconds timeout, only poll one file descriptor - switch(poll(&waiter, 1, watchdog::TIMEOUT_MS)) { - case(0): { - return LoopResult::TIMEOUT; - } - case(1): { - return pollEvent(waiter); + case (1): { + pollEvent(waiter); + return; } default: { - std::cerr << "eive-watchdog: Unknown poll error at " << watchdog::FIFO_NAME << ", error " << - errno << ": " << strerror(errno) << std::endl; - break; + std::cerr << "Unknown poll error at " << watchdog::FIFO_NAME << ", error " << errno << ": " + << strerror(errno) << std::endl; + break; } - } - return LoopResult::OK; + } } -WatchdogTask::LoopResult WatchdogTask::pollEvent(struct pollfd& waiter) { - if (waiter.revents & POLLIN) { - ssize_t readLen = read(fd, buf.data(), buf.size()); - if (readLen < 0) { - std::cerr << "eive-watchdog: Read error on pipe " << watchdog::FIFO_NAME << - ", error " << errno << ": " << strerror(errno) << std::endl; - return LoopResult::OK; - } +void WatchdogTask::pollEvent(struct pollfd& waiter) { + if (waiter.revents & POLLIN) { + ssize_t readLen = read(fd, buf.data(), buf.size()); #if WATCHDOG_VERBOSE_LEVEL == 2 - std::cout << "Read " << readLen << " byte(s) on the pipe " << FIFO_NAME - << std::endl; + std::cout << "Read " << readLen << " byte(s) on the pipe " << watchdog::FIFO_NAME << std::endl; #endif - else if(readLen >= 1) { - return parseCommandByte(readLen); - } + if (readLen < 0) { + std::cerr << "Read error on pipe " << watchdog::FIFO_NAME << ", error " << errno << ": " + << strerror(errno) << std::endl; + resultQueue.push(LoopResult::OK); + } else if (readLen >= 1) { + parseCommands(readLen); + } - } - else if(waiter.revents & POLLERR) { - std::cerr << "eive-watchdog: Poll error error on pipe " << watchdog::FIFO_NAME << - std::endl; - return LoopResult::FAULT; - } - else if (waiter.revents & POLLHUP) { - // Writer closed its end - return LoopResult::HUNG_UP; - } - return LoopResult::FAULT; + } else if (waiter.revents & POLLERR) { + std::cerr << "Poll error error on pipe " << watchdog::FIFO_NAME << std::endl; + resultQueue.push(LoopResult::FAULT); + } else if (waiter.revents & POLLHUP) { + // Writer closed its end + resultQueue.push(LoopResult::HUNG_UP); + } } -WatchdogTask::LoopResult WatchdogTask::parseCommandByte(ssize_t readLen) { - for(ssize_t idx = 0; idx < readLen; idx++) { - char readChar = buf[idx]; - // Cancel request - if(readChar == watchdog::CANCEL_CHAR) { - return LoopResult::CANCEL_RQ; - } - // Begin request. Does not work if the operation was not suspended before - else if(readChar == watchdog::RESTART_CHAR) { - return LoopResult::RESTART_RQ; - } - // Suspend request - else if(readChar == watchdog::SUSPEND_CHAR) { - return LoopResult::SUSPEND_RQ; - } - // Everything else: All working as expected +void WatchdogTask::parseCommands(ssize_t readLen) { + for (ssize_t idx = 0; idx < readLen; idx++) { + char nextChar = buf[idx]; + // Cancel request + if (nextChar == watchdog::first::CANCEL_CHAR) { + resultQueue.push(LoopResult::CANCEL_REQ); + } else if (nextChar == watchdog::first::SUSPEND_CHAR) { + // Suspend request + resultQueue.push(LoopResult::SUSPEND_REQ); + } else if (nextChar == watchdog::first::START_CHAR) { + if (idx < readLen - 1 and static_cast(buf[idx + 1]) == watchdog::second::WATCH_FLAG) { + resultQueue.push(LoopResult::START_WITH_WATCH_REQ); + idx++; + continue; + } + resultQueue.push(LoopResult::START_REQ); + } else if (nextChar == watchdog::first::IDLE_CHAR) { + resultQueue.push(LoopResult::OK); } - return LoopResult::OK; + } + // Everything else: All working as expected } int WatchdogTask::performRunningOperation() { - if(state != States::RUNNING) { - state = States::RUNNING; + if (state != States::RUNNING) { + state = States::RUNNING; + } + if (notRunningStart.has_value()) { + notRunningStart = std::nullopt; + } + + if (not obswRunning) { + if (printNotRunningLatch) { + // Reset latch so user can see timeouts + printNotRunningLatch = false; } - if(not obswRunning) { - if(printNotRunningLatch) { - // Reset latch so user can see timeouts - printNotRunningLatch = false; - } - - obswRunning = true; - std::cout << "eive-watchdog: Running OBSW detected.." << std::endl; + obswRunning = true; + std::cout << "OBSW is running" << std::endl; #if WATCHDOG_CREATE_FILE_IF_RUNNING == 1 - std::cout << "eive-watchdog: Creating " << watchdog::RUNNING_FILE_NAME << std::endl; - if (not std::filesystem::exists(watchdog::RUNNING_FILE_NAME)) { - std::ofstream obswRunningFile(watchdog::RUNNING_FILE_NAME); - if(not obswRunningFile.good()) { - std::cerr << "Creating file " << watchdog::RUNNING_FILE_NAME << " failed" - << std::endl; - } - } -#endif + std::cout << "Creating " << watchdog::RUNNING_FILE_NAME << std::endl; + std::error_code e; + if (not std::filesystem::exists(watchdog::RUNNING_FILE_NAME, e)) { + std::ofstream obswRunningFile(watchdog::RUNNING_FILE_NAME); + if (not obswRunningFile.good()) { + std::cerr << "Creating file " << watchdog::RUNNING_FILE_NAME << " failed" << std::endl; + } } - return 0; +#endif + } + return 0; } int WatchdogTask::performNotRunningOperation(LoopResult type) { - // Latch prevents spam on console - if(not printNotRunningLatch) { - if(type == LoopResult::HUNG_UP) { - std::cout << "eive-watchdog: FIFO writer hung up!" << std::endl; - } - else { - std::cout << "eive-watchdog: The FIFO timed out!" << std::endl; - } - printNotRunningLatch = true; + // Latch prevents spam on console + if (not printNotRunningLatch) { + if (type == LoopResult::HUNG_UP) { + std::cout << "OBSW hung up" << std::endl; + } else { + std::cout << "The FIFO timed out, OBSW might not be running" << std::endl; } + printNotRunningLatch = true; + } - if(obswRunning) { + if (not notRunningStart.has_value()) { + notRunningStart = std::chrono::steady_clock::now(); + } + + if (obswRunning) { #if WATCHDOG_CREATE_FILE_IF_RUNNING == 1 - if (std::filesystem::exists(watchdog::RUNNING_FILE_NAME)) { - int result = std::remove(watchdog::RUNNING_FILE_NAME.c_str()); - if(result != 0) { - std::cerr << "Removing " << watchdog::RUNNING_FILE_NAME << " failed with code " << - errno << ": " << strerror(errno) << std::endl; - } - } + std::cout << "Removing " << watchdog::RUNNING_FILE_NAME << std::endl; + std::error_code e; + if (std::filesystem::exists(watchdog::RUNNING_FILE_NAME, e)) { + int result = std::remove(watchdog::RUNNING_FILE_NAME.c_str()); + if (result != 0) { + std::cerr << "Removing " << watchdog::RUNNING_FILE_NAME << " failed with code " << errno + << ": " << strerror(errno) << std::endl; + } + } #endif - obswRunning = false; + obswRunning = false; + } + + if (watchingObsw) { + auto timeNotRunning = std::chrono::steady_clock::now() - notRunningStart.value(); + if (std::chrono::duration_cast(timeNotRunning).count() > + watchdog::MAX_NOT_RUNNING_MS) { + std::cout << "Restarting OBSW with systemctl" << std::endl; + std::system("systemctl restart obsw"); } - if(type == LoopResult::HUNG_UP) { - using namespace std::chrono_literals; - // Prevent spam - std::this_thread::sleep_for(2000ms); - } - return 0; + } + if (type == LoopResult::HUNG_UP) { + using namespace std::chrono_literals; + // Prevent spam + std::this_thread::sleep_for(2000ms); + } + return 0; } -int WatchdogTask::performSuspendOperation() { - if(state == States::RUNNING or state == States::FAULTY) { - std::cout << "eive-watchdog: Suspending watchdog operations" << std::endl; - watchdogRunning = false; - state = States::SUSPENDED; +bool WatchdogTask::stateMachine(LoopResult loopResult) { + using namespace std::chrono_literals; + bool sleep = false; + switch (state) { + case (States::RUNNING): { + switch (loopResult) { + case (LoopResult::TIMEOUT): + case (LoopResult::HUNG_UP): { + performNotRunningOperation(loopResult); + break; + } + case (LoopResult::OK): { + performRunningOperation(); + break; + } + case (LoopResult::SUSPEND_REQ): { + if (state == States::RUNNING or state == States::FAULTY) { + std::cout << "Received suspend request, suspending watchdog operations" << std::endl; + state = States::SUSPENDED; + } + performSuspendOperation(); + sleep = true; + break; + } + case (LoopResult::CANCEL_REQ): { + std::cout << "Received cancel request, closing watchdog.." << std::endl; + return false; + } + } } - return 0; + case (States::FAULTY): + case (States::SUSPENDED): + case (States::NOT_STARTED): { + switch (loopResult) { + case (LoopResult::SUSPEND_REQ): { + // Ignore and also delay + sleep = true; + break; + } + case (LoopResult::START_REQ): + case (LoopResult::START_WITH_WATCH_REQ): { + if (state == States::NOT_STARTED or state == States::FAULTY) { + state = States::RUNNING; + } + if (loopResult == LoopResult::START_REQ) { + std::cout << "Start request without watch request received" << std::endl; + watchingObsw = false; + } else if (loopResult == LoopResult::START_WITH_WATCH_REQ) { + std::cout << "Start request with watch request received. Restarting OBSW if not " + "running for " + << watchdog::MAX_NOT_RUNNING_MS / 1000 << " seconds" << std::endl; + watchingObsw = true; + } + performRunningOperation(); + break; + } + default: { + sleep = true; + } + } + break; + } + } + if (loopResult == LoopResult::FAULT) { + // Configuration error + std::cerr << "Fault has occured in watchdog loop" << std::endl; + // Prevent spam + sleep = true; + } + if (sleep) { + std::this_thread::sleep_for(500ms); + } + return true; } + +int WatchdogTask::performSuspendOperation() { return 0; } diff --git a/watchdog/Watchdog.h b/watchdog/Watchdog.h index 5745c033..340a9f9d 100644 --- a/watchdog/Watchdog.h +++ b/watchdog/Watchdog.h @@ -2,49 +2,55 @@ #define WATCHDOG_WATCHDOG_H_ #include +#include #include +#include +#include #include class WatchdogTask { -public: - enum class States { - NOT_STARTED, - RUNNING, - SUSPENDED, - FAULTY - }; + public: + enum class States { NOT_STARTED, RUNNING, SUSPENDED, FAULTY }; - enum class LoopResult { - OK, - SUSPEND_RQ, - CANCEL_RQ, - RESTART_RQ, - TIMEOUT, - HUNG_UP, - FAULT - }; + enum class LoopResult { + OK, + START_REQ, + START_WITH_WATCH_REQ, + SUSPEND_REQ, + CANCEL_REQ, + TIMEOUT, + HUNG_UP, + FAULT + }; - WatchdogTask(); + WatchdogTask(); - virtual ~WatchdogTask(); + virtual ~WatchdogTask(); - int performOperation(); -private: - int fd = 0; + int performOperation(); - bool obswRunning = false; - bool watchdogRunning = false; - bool printNotRunningLatch = false; - std::array buf; - States state = States::NOT_STARTED; + private: + int fd = 0; - LoopResult watchdogLoop(); - LoopResult pollEvent(struct pollfd& waiter); - LoopResult parseCommandByte(ssize_t readLen); + bool obswRunning = false; + bool watchingObsw = false; + bool printNotRunningLatch = false; + std::array buf; + std::queue resultQueue; - int performRunningOperation(); - int performNotRunningOperation(LoopResult type); - int performSuspendOperation(); + std::optional> notRunningStart; + States state = States::NOT_STARTED; + + // Primary loop. Takes care of delaying, and reading from the communication pipe and translating + // messages to loop results. + void watchdogLoop(); + bool stateMachine(LoopResult result); + void pollEvent(struct pollfd& waiter); + void parseCommands(ssize_t readLen); + + int performRunningOperation(); + int performNotRunningOperation(LoopResult type); + int performSuspendOperation(); }; #endif /* WATCHDOG_WATCHDOG_H_ */ diff --git a/watchdog/definitions.h b/watchdog/definitions.h index bfb1ec13..5b68023a 100644 --- a/watchdog/definitions.h +++ b/watchdog/definitions.h @@ -5,17 +5,31 @@ namespace watchdog { +namespace first { + +// Start or restart character +static constexpr char START_CHAR = 'b'; // Suspend watchdog operations temporarily static constexpr char SUSPEND_CHAR = 's'; -// Resume watchdog operations -static constexpr char RESTART_CHAR = 'b'; // Causes the watchdog to close down static constexpr char CANCEL_CHAR = 'c'; +static constexpr char IDLE_CHAR = 'i'; + +} // namespace first + +namespace second { + +// Supplied with the start character. This will instruct the watchdog to actually watch +// the OBSW is runnng all the time. +static constexpr char WATCH_FLAG = 'w'; +} // namespace second static constexpr int TIMEOUT_MS = 5 * 1000; +// 2 minutes +static constexpr unsigned MAX_NOT_RUNNING_MS = 2 * 60 * 1000; const std::string FIFO_NAME = "/tmp/watchdog-pipe"; const std::string RUNNING_FILE_NAME = "/tmp/obsw-running"; -} +} // namespace watchdog #endif /* WATCHDOG_DEFINITIONS_H_ */ diff --git a/watchdog/main.cpp b/watchdog/main.cpp index ba75dc30..0173a299 100644 --- a/watchdog/main.cpp +++ b/watchdog/main.cpp @@ -1,24 +1,34 @@ -#include "Watchdog.h" +#include #include +#include + +#include "Watchdog.h" +#include "definitions.h" /** * @brief This watchdog application uses a FIFO to check whether the OBSW is still running. * It checks whether the OBSW writes to the the FIFO regularly. */ int main() { - std::cout << "eive-watchdog: Starting OBSW watchdog.." << std::endl; - try { - WatchdogTask watchdogTask; - int result = watchdogTask.performOperation(); - if(result != 0) { - return result; - } + std::cout << "Starting OBSW watchdog" << std::endl; + std::error_code e; + if (std::filesystem::exists(watchdog::RUNNING_FILE_NAME, e)) { + std::cout << "Removing " << watchdog::RUNNING_FILE_NAME << std::endl; + int result = std::remove(watchdog::RUNNING_FILE_NAME.c_str()); + if (result != 0) { + std::cerr << "file removal failure" << std::endl; } - catch(const std::runtime_error& e) { - std::cerr << "eive-watchdog: Run time exception " << e.what() << std::endl; - return -1; + } + try { + WatchdogTask watchdogTask; + int result = watchdogTask.performOperation(); + if (result != 0) { + return result; } - return 0; + } catch (const std::runtime_error& e) { + std::cerr << "Run time exception " << e.what() << std::endl; + return -1; + } + return 0; } -