diff --git a/CHANGELOG.md b/CHANGELOG.md index 6c3e4e50..44c98cb5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,365 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Changed + +- TCS: Remove OBC IF board thermal module, which is exactly identical to OBC module and therefore + obsolete. +- Swapped PL and PS I2C, BPX battery and MGT are connected to PS I2C now for firmware versions + equal or above v4. However, this software version is compatible to both v3 and v4 of the firmware. +- The firmware version variables are global statics inititalized early during the program + runtime now. This makes it possible to check the firmware version earlier. +- The TCS controller will now always command heaters OFF when being blind for thermal + components (no sensors available), irrespective of current switch state. +- Make OBSW compatible to prospective FW version v5.0.0, where the Q7 I2C devices were + moved to a PL I2C block and the TMP sensor devices were moved to the PS I2C0. +- Made `Xadc` code a little bit more robust against errors. + +## Fixed + +- TMP1075: Set dataset invalid on shutdown explicitely +- Small fixes for TMP1075 FDIR: Use strange and missed reply counters. +- TCS controller: Last heater (S-band heater) was skipped for transition completion + checks. +- TCS controller: A helper member to track the elapsed heater control cycles was not reset + properly, which could lead to switch transitions being completed immediately. This can + lead to weird bugs like heaters being commanded ON twice and can potentially lead to + other bugs. +- TMP1075: Devices did not go to OFF mode when being set faulty. +- Update PL PCDU 1 in TCS mode tree on the EM. +- TMP1075: Possibly ignored health commands. +- Bugfix in FSFW where certain packet types could only be sent with source data fields with a + maximum size of 255 bytes. +- TCS CTRL: Limit number of heater handler messages sent in case there are not sensors available + anymore. + +# Added + +- Two events for heaters being commanded ON and OFF by the TCS controller +- Upper limit for burn time of TCS heaters. Currently set to 1 hour for each heater. + This mechanism will only track the burn time for heaters which were commanded by the + TCS controller. +- TCS controller is now observable by introducing a new HK dataset which exposes some internal + fields related to TCS control. + +# [v6.0.0] 2023-07-02 + +- `q7s-package` version v3.2.0 +- Important bugfixes for PTME. See `q7s-package` CHANGELOG. + +## Changed + +- Added back PTME busy bit polling. This is necessary due to changes to the AXI APB interface + to the PTME core. + +## Fixed + +- For the live channel (VC0), telemetry was still only dumped if the transmitter is active. + Please note that this fix will lead to crashes for FW versions below v3.2. + However, it might not be an issue for the oldest firmware on the satellite (v2.5.1). + +# [v5.2.0] 2023-07-02 + +## Fixed + +- The firmware information event was not triggered even when possible because of an ordering + bug in the initializer function. +- Empty dumps (no TM in time range) will now correctly be completed immediately + +## Changed + +- PTME was always reset on submode changes. The reset will now only be performed if the actual data + rate changes. +- Add back ACS board code for the EM. Now that the radiation sensor was removed, the image switching + issue has disappeared and adding back the ACS board is worth it for the GPS timekeeping. + +# [v5.1.0] 2023-06-28 + +- `eive-tmtc` version v5.1.0 + +## Changed + +- Persistent TM store dumps are now performed in chronological order. +- Increase Syrlinks RX HK rate to 5.0 seconds during a pass. +- Various robustness improvements for the heater handler. The heater handler will now only + process the command queue if it is not busy with switch commanding which reduces the amount + of possible bugs. +- The heater handler is only able to process messages stricly sequentially now but is scheduled + twice in a 0.5 second slot so something like a consecutive heater ON or OFF command can still + be handled relatively quickly. + +## Added + +- Sequence counters for PUS and CFDP packets are now stored persistently across graceful reboots. +- The PUS packet message type counter will now be incremented properly for each PUS service. +- Internal error reporter set is now enabled by default and generated every 120 seconds. + +# [v5.0.0] 2023-06-26 + +v3.3.1 and all following version will now be moved to v5.0.0 with the additional changes listed +here. This was done because the firmware update (v4.0.0) is not working right now and it is not +known when and how it will be fixed. Because of that, all updates to make the SW work with the new +firmware, which are limited to a few files will be moved to a dev branch so regular development +compatible to the old firmware can continue. + +TLDR: This version is compatible to the old firmware and some changes which only work with the new +firmware have been reverted. + +## Changed + +- Added `sync` syscall in graceful shutdown handler +- Graceful shutdown is now performed by the reboot watchdog +- There is now a separate file for the total reboot counter. The reboot watchdog has its own local + counters to determine whether a reboot is necessary. + +# [v4.0.1] 2023-06-24 + +## Fixed + +- `PusLiveDemux` packet demultiplexing bugfix where the demultiplexing did not work when there was + only one destination available. + +# [v4.0.0] 2023-06-22 + +- `eive-tmtc` version v5.0.0 +- `q7s-package` version v3.1.1 + +## Fixed + +- Important bugfixes for PTME. See `q7s-package` CHANGELOG. +- TCS fixes: Set temperature values to invalid value for MAX31865 RTD handler, SUS handler + and STR handler. Also set dataset to invakid for RTD handler. +- Fixed H parameter in SUS converter from 1 mm to 2.5 mm. + +## Changed + +- Removed PTME busy/ready signals. Those were not used anyway because register reads are used now. +- APB bus access busy checking is not done anymore as this is performed by the bus itself now. +- Core controller will now announce version and image information event in addition to reboot + event in the `inititalize` function. +- Core controller will now try to request and announce the firmware version in addition to the + OBSW version as well. +- Added core controller action to read reboot mechansm information +- GNSS reset pin will now only be asserted for 5 ms instead of 100 ms. + +## Added + +- Added PL I2C reset pin. It is not used/connected for now but could be used for FDIR procedures to + restore the PL I2C. +- Core controller now announces firmware version as well when requesting a version info event + +# [v3.3.1] 2023-06-22 + +## Fixed + +- `PusLiveDemux` packet demultiplexing bugfix where the demultiplexing did not work when there was + only one destination available. + +## Fixed + +- Fixed H parameter in SUS converter from 1 mm to 2.5 mm. + +# [v3.3.0] 2023-06-21 + +Like v3.2.0 but without the custom FM changes related to VC0. + +# [v3.2.0] 2023-06-21 + +## Fixed + +- Fix sun vector calculation +- SUS total vector was not reset to being a zero vector during eclipse due to a wrong memcpy + length. + +## Changed + +- Reverted all changes related to VC0 handling to avoid FM bug possibly related to FPGA bug. + +# [v3.1.1] 2023-06-14 + +## Fixed + +- TMP1075 bugfix where negative temperatures could not be measured because of a two's-complement + conversion bug. + +# [v3.1.0] 2023-06-14 + +- `eive-tmtc` version v4.1.0 + +## Fixed + +- TCS heater switch enumeration naming was old/wrong and was not updated in sync with the object ID + update. This lead to the TCS controller commanding the wrong heaters. + +## Changed + +- Increase number of allowed parallel HK commands to 16 + +## Added + +- Added `CONFIG_SET`, `MAN_HEATER_ON` and `MAN_HEATER_OFF` support for the BPX battery handler + +# [v3.0.0] 2023-06-11 + +- `eive-tmtc` version v4.0.0 + +## Changed + +- Adapt EM configuration to include all GomSpace PCDU devices except the ACU. For the ACU + (which broke), a dummy will still be used. +- Event Manager queue depth is configurable now. +- Do not construct and schedule broken TMP1075 device anymore. +- Do not track payload modes in system mode tables. +- ACS modes derived from system modes. +- The CMake build generator will now search for the cross-compiler binaries in the environmental + variable named `CROSS_COMPILE_BIN_PATH` first when setting up the build system. This prevents + CMake from selecting wrong cross-compilers if multiple cross-compilers with the same name are used + on the same system. +- Add ACS board for EM by default now. +- Add support for MPSoC HK packet. +- Add support for MPSoC Flash Directory Content Report. +- Dynamically enable and disable HK packets for MPSoC on `ON` and `OFF` commands. +- Add support for MPSoC Flash Directory Content Report. +- Larger allowed path and file sizes for STR and PLOC MPSoC modules. +- More robust MPSoC flash read and write command data handling. +- Increase frequency of payload handlers from 0.8 seconds to 0.5 seconds. +- Disable missed deadlines per default. Not useful in orbit, and triggers all the time on the EM + build after a number of subsequent runs, without any apparent reason (deadlines are not actually + missed, thread usage displayed is nominal) +- TM store dumpes will not be cancelled anymore if the transmitter is off. The dump can be cancelled + with an OFF command, and the PTME is perfectly capable of dumping without the transmitter being + on. +- Transmitter state is not taken into account anymore for writing into the PTME. The PTME should + be perfectly capable of generating a valid CADU, even when the transmitter is not ON for any + reason. +- OFF mode is ignores in TM store for determining whether a store will be written. The modes will + only be used to cancel a transfer. +- Handling of multiple RWs in the ACS Controller is improved and can be changed by parameter + commands. +- The Directory Listing direct dumper now has a state machine to stagger the directory listing dump. + This is required because very large dumps will overload the queue capacities in the framework. +- The PUS Service 8 now has larger queue sizes to handle more action replies. The PUS Service 1 + also has a larger queue size to handle a lot of step replies now. +- Moved PDU `vcc` and `vbat` variable from auxiliary dataset to core dataset. +- Tweak TCP/IP configuration: Only the TCP server will be included on the EM. For the FM, no + TCP/IP servers will be included by default. + +## Added + +- Add the remaining system modes. +- PLOC MPSoC flash read command working. +- BPX battery handler is added for EM by default. +- ACU dummy HK sets +- IMTQ HK sets +- IMTQ dummy now handles power switch +- Added some new ACS parameters +- Enabled decimation filter for the ADIS GYRs +- Enabled second low-pass filter for L3GD20H GYRs + +## Fixed + +- CFDP low level protocol bugfix. Requires `fsfw` update and `tmtc` update. +- Compile fix if SCEX is compiled for the EM. +- Set up Rad Sensor chip select even for EM to avoid SPI bus issues. +- Correct ADIS Gyroscope type configuration for the EM, where the 16507 type is used instead of the + 16505 type. +- Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP + funnel. +- PLOC Supervisor handler now has a power switcher assigned to make PLOC power switching work + without an additional PCDU power switch command. +- The PLOC Supervisor handler now waits for the replies to the `SET_TIME` command to verify working + communication. +- The PLOC MPSoC now waits 10 cycles before going to on. These wait cycles are necessary because + the MPSoC is not ready to process commands without this additional boot time. +- Fixed correction for `GPS Altitude` in case the sensor data is out of the expected bonds. +- PLOC MPSoC special communication is now scheduled, which allows flash read and flash write + commands to work. +- Fixed the MPSoC flash write command. +- Added missing ACS parameter. +- HK TM store: The HK store dump success event was triggered for cancelled HK dumps. +- When a PUS parsing error occured while parsing a TM store file, the dump completion procedure + was always executed. +- Some smaller logic fixes in the TM store base class +- Fixed usage of C `abs` instead of C++ `std::abs`, which results in MTQ commands not being + scaled correctly between 1Am² and 0.2Am². +- TCS Heater Handler: Always trigger mode event if a heater goes `OFF` or `ON`. This event might + soon replace the `HEATER_WENT_ON` and `HEATER_WENT_OFF` events. +- Prevent spam of TCS controller heater unavailability event if all heaters are in external control. +- TCS heater switch info set contained invalid values because of a faulty `memcpy` in the TCS + controller. There is not crash risk but the heater states were invalid. +- STR datasets were not set to invalid on shutdown. +- Fixed usage of quaternion valid flag, which does not actually represent the validity of the + quaternion. +- Various fixes for the pointing modes of the `ACS Controller`. All modes should work now as + intended. +- The variance for the ADIS GYRs now represents the used `-3` version and not the `-1` version +- CFDP funnel did not route packets to live channel VC0 + +# [v2.0.5] 2023-05-11 + +- The dual lane assembly transition failed handler started new transitions towards the current mode + instead of the target mode. This means that if the dual lane assembly never reached the initial + submode (e.g. mode normal and submode dual side), it will transition back to the current mode, + which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent + recovery handling becomes stuck in the custom recovery sequence when swichting power back on. +- The dual lane custom recovery handling was adapted to always perform proper power switch handling + irrespective of current or target modes. + +# [v2.0.4] 2023-04-19 + +## Fixed + +- The dual lane assembly datasets were not marked invalid properly on OFF transitions. + +# [v2.0.3] 2023-04-17 + +- eive-tmtc: v3.1.1 + +## Fixed + +- Fixed shadowing within the `SafeCtrl`, which prevented actuator commands to be calculated during + eclipse phase. +- EM build idle mode fixes for RW dummy. + +## Added + +- Add `MGT_OVERHEATING` event and fallback to system SAFE mode if the MGT is overheating for + whatever reason. + +## Changed + +- Low-pass filters can no longer be executed if no actual data is available. + +# [v2.0.2] 2023-04-16 + +- Bump patch version to 2. + +# [v2.0.1] 2023-04-16 + +- eive-tmtc: v3.1.0 + +# [v2.0.0] 2023-04-16 + +This is the version which will fly on the satellite for the initial launch phase. + +- q7s-package: v2.5.0 +- eive-tmtc: v3.0.0 +- `wire` library is now on version v10.7 as well. + +## Added + +- Added `mv`, `cp` and `rm` action helpers for the core controller for common filesystem operations. +- Extended directory listing helpers. There is now a directory listing helper which dumps the + directory listing as an action data reply immediately. For smaller directory listings, this + allows a listing without requiring a separate file downlink (which also has not been implemented + yet) + +## Changed + +- The directory listing action commands now allow compressing of either the target output file + for the directory listing into file action command, or compression in the helper which dumps + the directory listing directly. + # [v1.45.0] 2023-04-14 - q7s-package: v2.5.0 diff --git a/CMakeLists.txt b/CMakeLists.txt index 2d96ca08..54b43f47 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,8 +9,8 @@ # ############################################################################## cmake_minimum_required(VERSION 3.13) -set(OBSW_VERSION_MAJOR 1) -set(OBSW_VERSION_MINOR 45) +set(OBSW_VERSION_MAJOR 6) +set(OBSW_VERSION_MINOR 0) set(OBSW_VERSION_REVISION 0) # set(CMAKE_VERBOSE TRUE) @@ -79,12 +79,19 @@ else() set(INIT_VAL 1) set(OBSW_STAR_TRACKER_GROUND_CONFIG 0) endif() + +set(OBSW_ADD_TMTC_TCP_SERVER + ${OBSW_Q7S_EM} + CACHE STRING "Add TCP TMTC Server") +set(OBSW_ADD_TMTC_UDP_SERVER + 0 + CACHE STRING "Add UDP TMTC Server") set(OBSW_ADD_MGT ${INIT_VAL} CACHE STRING "Add MGT module") set(OBSW_ADD_BPX_BATTERY_HANDLER - ${INIT_VAL} - CACHE STRING "Add MGT module") + 1 + CACHE STRING "Add BPX battery module") set(OBSW_ADD_STAR_TRACKER ${INIT_VAL} CACHE STRING "Add Startracker module") @@ -98,7 +105,7 @@ set(OBSW_ADD_THERMAL_TEMP_INSERTER ${OBSW_Q7S_EM} CACHE STRING "Add thermal sensor temperature inserter") set(OBSW_ADD_ACS_BOARD - ${INIT_VAL} + 1 CACHE STRING "Add ACS board module") set(OBSW_ADD_GPS_CTRL ${INIT_VAL} @@ -143,11 +150,14 @@ set(OBSW_ADD_SYRLINKS 1 CACHE STRING "Add Syrlinks module") set(OBSW_ADD_TMP_DEVICES - ${INIT_VAL} + 1 CACHE STRING "Add TMP devices") set(OBSW_ADD_GOMSPACE_PCDU - ${INIT_VAL} + 1 CACHE STRING "Add GomSpace PCDU modules") +set(OBSW_ADD_GOMSPACE_ACU + ${INIT_VAL} + CACHE STRING "Add GomSpace ACU submodule") set(OBSW_ADD_RW ${INIT_VAL} CACHE STRING "Add RW modules") diff --git a/README.md b/README.md index cd350386..ce57c33f 100644 --- a/README.md +++ b/README.md @@ -99,11 +99,21 @@ When using Windows, run theses steps in MSYS2. git submodule update --init ``` -3. Ensure that the cross-compiler is working with `arm-linux-gnueabihf-gcc --version` and that +3. Create two system variables to pass the system root path and the cross-compiler path to the + build system. You only need to do this once when setting up the build system. + Example for Unix: + + ```sh + export CROSS_COMPILE_BIN_PATH= + export ZYNQ_7020_SYSROOT= + ``` + +4. Ensure that the cross-compiler is working with + `${CROSS_COMPILE_BIN_PATH}/arm-linux-gnueabihf-gcc --version` and that the sysroot environmental variables have been set like specified in the [root filesystem chapter](#sysroot). -4. Run the CMake configuration to create the build system in a `build-Debug-Q7S` folder. +5. Run the CMake configuration to create the build system in a `build-Debug-Q7S` folder. Add `-G "MinGW Makefiles` in MinGW64 on Windows. ```sh @@ -112,7 +122,7 @@ When using Windows, run theses steps in MSYS2. cmake --build . -j ``` - You can also use provided shell scripts to perform these commands. + Please note that you can also use provided shell scripts to perform these commands. ```sh cp scripts/q7s-env.sh .. cp scripts/q7s-env-em.sh .. @@ -144,7 +154,7 @@ When using Windows, run theses steps in MSYS2. There are also different values for `-DTGT_BSP` to build for the Raspberry Pi or the Beagle Bone Black: `arm/raspberrypi` and `arm/beagleboneblack`. -5. Build the software with +6. Build the software with ```sh cd cmake-build-debug-q7s @@ -954,6 +964,12 @@ used by other software components to read the current chip and copy. This is a configuration scripts which runs after the Network Time Protocol has run. This script currently sets the static IP address `192.168.133.10` and starts the `can` interface. +## Initial boot delay + +You can create a file named `boot_delays_secs.txt` inside the home folder to delay the OBSW boot +for 6 seconds if the file is empty of for the number of seconds specified inside the file. This +can be helpful if something inside the OBSW leads to an immediate crash of the OBC. + ## PCDU Connect to serial console of P60 Dock diff --git a/automation/Jenkinsfile b/automation/Jenkinsfile index 167c28cc..12c4bdf8 100644 --- a/automation/Jenkinsfile +++ b/automation/Jenkinsfile @@ -22,7 +22,7 @@ pipeline { steps { dir(BUILDDIR_Q7S) { sh 'cmake -DTGT_BSP="arm/q7s" -DCMAKE_BUILD_TYPE=Debug ..' - sh 'cmake --build . -j6' + sh 'cmake --build . -j8' } } } @@ -30,7 +30,7 @@ pipeline { steps { dir(BUILDDIR_Q7S_EM) { sh 'cmake -DTGT_BSP="arm/q7s" -DEIVE_Q7S_EM=ON -DCMAKE_BUILD_TYPE=Debug ..' - sh 'cmake --build . -j6' + sh 'cmake --build . -j8' } } } @@ -38,7 +38,7 @@ pipeline { steps { dir(BUILDDIR_LINUX) { sh 'cmake ..' - sh 'cmake --build . -j6' + sh 'cmake --build . -j8' sh './eive-unittest' } } diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 3d618cdb..4873bc58 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -39,7 +39,7 @@ #include "devices/gpioIds.h" #include "fsfw_hal/linux/gpio/Gpio.h" #include "linux/payload/PlocMpsocHandler.h" -#include "linux/payload/PlocMpsocHelper.h" +#include "linux/payload/PlocMpsocSpecialComHelper.h" #include "linux/payload/PlocSupervisorHandler.h" #include "linux/payload/PlocSupvUartMan.h" #include "test/gpio/DummyGpioIF.h" @@ -62,10 +62,15 @@ void ObjectFactory::produce(void* args) { StorageManagerIF* tmStore; StorageManagerIF* ipcStore; PersistentTmStores persistentStores; + bool enableHkSets = false; +#if OBSW_ENABLE_PERIODIC_HK == 1 + enableHkSets = true; +#endif auto sdcMan = new DummySdCardManager("/tmp"); ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan, &ipcStore, - &tmStore, persistentStores); + &tmStore, persistentStores, 120, enableHkSets); + new TmFunnelHandler(objects::LIVE_TM_TASK, *pusFunnel, *cfdpFunnel); auto* dummyGpioIF = new DummyGpioIF(); auto* dummySwitcher = new DummyPowerSwitcher(objects::PCDU_HANDLER, 18, 0); std::vector switcherList; @@ -81,8 +86,8 @@ void ObjectFactory::produce(void* args) { auto mpsocCookie = new UartCookie(objects::PLOC_MPSOC_HANDLER, mpscoDev, uart::PLOC_MPSOC_BAUD, mpsoc::MAX_REPLY_SIZE, UartModes::NON_CANONICAL); mpsocCookie->setNoFixedSizeReply(); - auto plocMpsocHelper = new PlocMPSoCHelper(objects::PLOC_MPSOC_HELPER); - new PlocMPSoCHandler(objects::PLOC_MPSOC_HANDLER, objects::UART_COM_IF, mpsocCookie, + auto plocMpsocHelper = new PlocMpsocSpecialComHelper(objects::PLOC_MPSOC_HELPER); + new PlocMpsocHandler(objects::PLOC_MPSOC_HANDLER, objects::UART_COM_IF, mpsocCookie, plocMpsocHelper, Gpio(gpioIds::ENABLE_MPSOC_UART, dummyGpioIF), objects::PLOC_SUPERVISOR_HANDLER); #endif /* OBSW_ADD_PLOC_MPSOC == 1 */ @@ -100,7 +105,7 @@ void ObjectFactory::produce(void* args) { #endif dummy::DummyCfg cfg; - dummy::createDummies(cfg, *dummySwitcher, dummyGpioIF); + dummy::createDummies(cfg, *dummySwitcher, dummyGpioIF, enableHkSets); HeaterHandler* heaterHandler = nullptr; // new ThermalController(objects::THERMAL_CONTROLLER); @@ -108,7 +113,7 @@ void ObjectFactory::produce(void* args) { if (heaterHandler == nullptr) { sif::error << "HeaterHandler could not be created" << std::endl; } else { - ObjectFactory::createThermalController(*heaterHandler); + ObjectFactory::createThermalController(*heaterHandler, true); } new TestTask(objects::TEST_TASK); } diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index 4c1fdb55..b98926e5 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 290 translations. + * @brief Auto-generated event translation file. Contains 299 translations. * @details - * Generated on: 2023-04-14 20:23:17 + * Generated on: 2023-07-07 12:06:06 */ #include "translateEvents.h" @@ -197,6 +197,10 @@ const char *MPSOC_EXE_INVALID_APID_STRING = "MPSOC_EXE_INVALID_APID"; const char *MPSOC_HELPER_SEQ_CNT_MISMATCH_STRING = "MPSOC_HELPER_SEQ_CNT_MISMATCH"; const char *MPSOC_TM_SIZE_ERROR_STRING = "MPSOC_TM_SIZE_ERROR"; const char *MPSOC_TM_CRC_MISSMATCH_STRING = "MPSOC_TM_CRC_MISSMATCH"; +const char *MPSOC_FLASH_READ_PACKET_ERROR_STRING = "MPSOC_FLASH_READ_PACKET_ERROR"; +const char *MPSOC_FLASH_READ_FAILED_STRING = "MPSOC_FLASH_READ_FAILED"; +const char *MPSOC_FLASH_READ_SUCCESSFUL_STRING = "MPSOC_FLASH_READ_SUCCESSFUL"; +const char *MPSOC_READ_TIMEOUT_STRING = "MPSOC_READ_TIMEOUT"; const char *TRANSITION_BACK_TO_OFF_STRING = "TRANSITION_BACK_TO_OFF"; const char *NEG_V_OUT_OF_BOUNDS_STRING = "NEG_V_OUT_OF_BOUNDS"; const char *U_DRO_OUT_OF_BOUNDS_STRING = "U_DRO_OUT_OF_BOUNDS"; @@ -273,6 +277,7 @@ const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS"; const char *TRYING_I2C_RECOVERY_STRING = "TRYING_I2C_RECOVERY"; const char *I2C_REBOOT_STRING = "I2C_REBOOT"; const char *PDEC_REBOOT_STRING = "PDEC_REBOOT"; +const char *FIRMWARE_INFO_STRING = "FIRMWARE_INFO"; 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"; @@ -280,6 +285,10 @@ const char *OBC_OVERHEATING_STRING = "OBC_OVERHEATING"; const char *CAMERA_OVERHEATING_STRING = "CAMERA_OVERHEATING"; const char *PCDU_SYSTEM_OVERHEATING_STRING = "PCDU_SYSTEM_OVERHEATING"; const char *HEATER_NOT_OFF_FOR_OFF_MODE_STRING = "HEATER_NOT_OFF_FOR_OFF_MODE"; +const char *MGT_OVERHEATING_STRING = "MGT_OVERHEATING"; +const char *TCS_SWITCHING_HEATER_ON_STRING = "TCS_SWITCHING_HEATER_ON"; +const char *TCS_SWITCHING_HEATER_OFF_STRING = "TCS_SWITCHING_HEATER_OFF"; +const char *TCS_HEATER_MAX_BURN_TIME_REACHED_STRING = "TCS_HEATER_MAX_BURN_TIME_REACHED"; 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"; @@ -682,6 +691,14 @@ const char *translateEvents(Event event) { return MPSOC_TM_SIZE_ERROR_STRING; case (12613): return MPSOC_TM_CRC_MISSMATCH_STRING; + case (12614): + return MPSOC_FLASH_READ_PACKET_ERROR_STRING; + case (12615): + return MPSOC_FLASH_READ_FAILED_STRING; + case (12616): + return MPSOC_FLASH_READ_SUCCESSFUL_STRING; + case (12617): + return MPSOC_READ_TIMEOUT_STRING; case (12700): return TRANSITION_BACK_TO_OFF_STRING; case (12701): @@ -834,6 +851,8 @@ const char *translateEvents(Event event) { return I2C_REBOOT_STRING; case (14012): return PDEC_REBOOT_STRING; + case (14013): + return FIRMWARE_INFO_STRING; case (14100): return NO_VALID_SENSOR_TEMPERATURE_STRING; case (14101): @@ -848,6 +867,14 @@ const char *translateEvents(Event event) { return PCDU_SYSTEM_OVERHEATING_STRING; case (14107): return HEATER_NOT_OFF_FOR_OFF_MODE_STRING; + case (14108): + return MGT_OVERHEATING_STRING; + case (14109): + return TCS_SWITCHING_HEATER_ON_STRING; + case (14110): + return TCS_SWITCHING_HEATER_OFF_STRING; + case (14111): + return TCS_HEATER_MAX_BURN_TIME_REACHED_STRING; case (14201): return TX_TIMER_EXPIRED_STRING; case (14202): diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index b89eba11..458081f9 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 171 translations. - * Generated on: 2023-04-14 20:23:17 + * Generated on: 2023-07-07 12:06:06 */ #include "translateObjects.h" diff --git a/bsp_hosted/scheduling.cpp b/bsp_hosted/scheduling.cpp index 0a5f1c35..5fd53906 100644 --- a/bsp_hosted/scheduling.cpp +++ b/bsp_hosted/scheduling.cpp @@ -59,19 +59,15 @@ void scheduling::initTasks() { "DIST", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); ReturnValue_t result = tmtcDistributor->addComponent(objects::CCSDS_PACKET_DISTRIBUTOR); if (result != returnvalue::OK) { - sif::error << "adding CCSDS distributor failed" << std::endl; + sif::error << "Adding CCSDS distributor failed" << std::endl; } result = tmtcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR); if (result != returnvalue::OK) { - sif::error << "adding PUS distributor failed" << std::endl; - } - result = tmtcDistributor->addComponent(objects::TM_FUNNEL); - if (result != returnvalue::OK) { - sif::error << "adding TM funnel failed" << std::endl; + sif::error << "Adding PUS distributor failed" << std::endl; } result = tmtcDistributor->addComponent(objects::CFDP_DISTRIBUTOR); if (result != returnvalue::OK) { - sif::error << "adding CFDP distributor failed" << std::endl; + sif::error << "Adding CFDP distributor failed" << std::endl; } result = tmtcDistributor->addComponent(objects::UDP_TMTC_SERVER); if (result != returnvalue::OK) { @@ -95,6 +91,13 @@ void scheduling::initTasks() { sif::error << "Add component UDP Polling failed" << std::endl; } + PeriodicTaskIF* liveTmTask = factory->createPeriodicTask( + "LIVE_TM", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.4, nullptr, &RR_SCHEDULING); + result = liveTmTask->addComponent(objects::LIVE_TM_TASK); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("LIVE_TM", objects::LIVE_TM_TASK); + } + PeriodicTaskIF* pusHighPrio = factory->createPeriodicTask( "PUS_HIGH_PRIO", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusHighPrio->addComponent(objects::PUS_SERVICE_1_VERIFICATION); @@ -149,7 +152,7 @@ void scheduling::initTasks() { "THERMAL_CTL_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); result = thermalTask->addComponent(objects::CORE_CONTROLLER); if (result != returnvalue::OK) { - scheduling::printAddObjectError("Core controller dummy", objects::CORE_CONTROLLER); + scheduling::printAddObjectError("CORE_CTRL", objects::CORE_CONTROLLER); } result = thermalTask->addComponent(objects::THERMAL_CONTROLLER); if (result != returnvalue::OK) { @@ -217,6 +220,7 @@ void scheduling::initTasks() { tmtcDistributor->startTask(); udpPollingTask->startTask(); tcpPollingTask->startTask(); + liveTmTask->startTask(); pusHighPrio->startTask(); pusMedPrio->startTask(); diff --git a/bsp_q7s/CMakeLists.txt b/bsp_q7s/CMakeLists.txt index cf8fcacd..e3232363 100644 --- a/bsp_q7s/CMakeLists.txt +++ b/bsp_q7s/CMakeLists.txt @@ -7,7 +7,8 @@ target_link_libraries(${SIMPLE_OBSW_NAME} PUBLIC ${LIB_FSFW_NAME}) target_compile_definitions(${SIMPLE_OBSW_NAME} PRIVATE "Q7S_SIMPLE_MODE") add_subdirectory(simple) -target_sources(${OBSW_NAME} PUBLIC main.cpp obsw.cpp) +target_sources(${OBSW_NAME} PUBLIC main.cpp obsw.cpp scheduling.cpp + objectFactory.cpp) add_subdirectory(boardtest) diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index 3502a7bb..2555d7cd 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -22,6 +22,9 @@ #define OBSW_COMMAND_SAFE_MODE_AT_STARTUP 1 #define OBSW_ADD_GOMSPACE_PCDU @OBSW_ADD_GOMSPACE_PCDU@ +// This define is necessary because the EM setup has the P60 dock module, but no ACU on the P60 +// module because it broke. +#define OBSW_ADD_GOMSPACE_ACU @OBSW_ADD_GOMSPACE_ACU@ #define OBSW_ADD_MGT @OBSW_ADD_MGT@ #define OBSW_ADD_BPX_BATTERY_HANDLER @OBSW_ADD_BPX_BATTERY_HANDLER@ #define OBSW_ADD_STAR_TRACKER @OBSW_ADD_STAR_TRACKER@ @@ -64,12 +67,12 @@ // because UDP packets are not allowed in the VPN // This will cause the OBSW to initialize the TMTC bridge responsible for exchanging data with the // CCSDS IP Cores. -#define OBSW_ADD_TMTC_TCP_SERVER 1 -#define OBSW_ADD_TMTC_UDP_SERVER 1 +#define OBSW_ADD_TMTC_TCP_SERVER @OBSW_ADD_TMTC_TCP_SERVER@ +#define OBSW_ADD_TMTC_UDP_SERVER @OBSW_ADD_TMTC_UDP_SERVER@ // Can be used to switch device to NORMAL mode immediately #define OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP 0 -#define OBSW_PRINT_MISSED_DEADLINES 1 +#define OBSW_PRINT_MISSED_DEADLINES 0 #define OBSW_MPSOC_JTAG_BOOT 0 #define OBSW_STAR_TRACKER_GROUND_CONFIG @OBSW_STAR_TRACKER_GROUND_CONFIG@ diff --git a/bsp_q7s/boardconfig/busConf.h b/bsp_q7s/boardconfig/busConf.h index 4fd15258..dc3779a7 100644 --- a/bsp_q7s/boardconfig/busConf.h +++ b/bsp_q7s/boardconfig/busConf.h @@ -12,6 +12,9 @@ static constexpr char SPI_RW_DEV[] = "/dev/spi_rw"; static constexpr char I2C_PL_EIVE[] = "/dev/i2c_pl"; //! I2C bus using the I2C peripheral of the ARM processing system (PS) static constexpr char I2C_PS_EIVE[] = "/dev/i2c_ps"; +//! I2C bus using the first I2C peripheral of the ARM processing system (PS). +//! Named like this because it is used by default for the Q7 devices. +static constexpr char I2C_Q7_EIVE[] = "/dev/i2c_q7"; static constexpr char UART_GNSS_DEV[] = "/dev/gps0"; static constexpr char UART_PLOC_MPSOC_DEV[] = "/dev/ul_plmpsoc"; @@ -23,6 +26,7 @@ static constexpr char UART_SCEX_DEV[] = "/dev/scex"; static constexpr char UIO_PDEC_REGISTERS[] = "/dev/uio_pdec_regs"; static constexpr char UIO_PTME[] = "/dev/uio_ptme"; static constexpr char UIO_PDEC_CONFIG_MEMORY[] = "/dev/uio_pdec_cfg_mem"; +static constexpr char UIO_SYS_ROM[] = "/dev/uio_sys_rom"; static constexpr char UIO_PDEC_RAM[] = "/dev/uio_pdec_ram"; static constexpr char UIO_PDEC_IRQ[] = "/dev/uio_pdec_irq"; static constexpr int MAP_ID_PTME_CONFIG = 3; @@ -57,6 +61,7 @@ static constexpr char GYRO_0_ENABLE[] = "enable_gyro_0"; static constexpr char GYRO_2_ENABLE[] = "enable_gyro_2"; static constexpr char GNSS_SELECT[] = "gnss_mux_select"; static constexpr char GNSS_MUX_SELECT[] = "gnss_mux_select"; +static constexpr char PL_I2C_ARESETN[] = "pl_i2c_aresetn"; static constexpr char HEATER_0[] = "heater0"; static constexpr char HEATER_1[] = "heater1"; @@ -82,14 +87,12 @@ static constexpr char EN_RW_4[] = "enable_rw_4"; static constexpr char RAD_SENSOR_CHIP_SELECT[] = "rad_sensor_chip_select"; static constexpr char ENABLE_RADFET[] = "enable_radfet"; -static constexpr char PAPB_BUSY_SIGNAL_VC0[] = "papb_busy_signal_vc0"; + static constexpr char PAPB_EMPTY_SIGNAL_VC0[] = "papb_empty_signal_vc0"; -static constexpr char PAPB_BUSY_SIGNAL_VC1[] = "papb_busy_signal_vc1"; static constexpr char PAPB_EMPTY_SIGNAL_VC1[] = "papb_empty_signal_vc1"; -static constexpr char PAPB_BUSY_SIGNAL_VC2[] = "papb_busy_signal_vc2"; static constexpr char PAPB_EMPTY_SIGNAL_VC2[] = "papb_empty_signal_vc2"; -static constexpr char PAPB_BUSY_SIGNAL_VC3[] = "papb_busy_signal_vc3"; static constexpr char PAPB_EMPTY_SIGNAL_VC3[] = "papb_empty_signal_vc3"; + static constexpr char PTME_RESETN[] = "ptme_resetn"; static constexpr char RS485_EN_TX_CLOCK[] = "tx_clock_enable_ltc2872"; diff --git a/bsp_q7s/core/CMakeLists.txt b/bsp_q7s/core/CMakeLists.txt index b726885b..33550144 100644 --- a/bsp_q7s/core/CMakeLists.txt +++ b/bsp_q7s/core/CMakeLists.txt @@ -1,4 +1 @@ -target_sources(${OBSW_NAME} PRIVATE CoreController.cpp scheduling.cpp - ObjectFactory.cpp WatchdogHandler.cpp) - -target_sources(${SIMPLE_OBSW_NAME} PRIVATE scheduling.cpp) +target_sources(${OBSW_NAME} PRIVATE CoreController.cpp WatchdogHandler.cpp) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 49145f9f..00c7f248 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include "commonConfig.h" #include "fsfw/serviceinterface/ServiceInterface.h" @@ -22,6 +23,7 @@ #include #include +#include "bsp_q7s/boardconfig/busConf.h" #include "bsp_q7s/fs/SdCardManager.h" #include "bsp_q7s/memory/scratchApi.h" #include "bsp_q7s/xadc/Xadc.h" @@ -112,6 +114,9 @@ void CoreController::performControlOperation() { sdStateMachine(); performMountedSdCardOperations(); readHkData(); + if (dumpContext.active) { + dirListingDumpHandler(); + } if (shellCmdIsExecuting) { bool replyReceived = false; @@ -165,7 +170,6 @@ ReturnValue_t CoreController::initialize() { sdStateMachine(); - triggerEvent(core::REBOOT_SW, CURRENT_CHIP, CURRENT_COPY); EventManagerIF *eventManager = ObjectManager::instance()->get(objects::EVENT_MANAGER); if (eventManager == nullptr or eventQueue == nullptr) { @@ -182,7 +186,11 @@ ReturnValue_t CoreController::initialize() { if (result != returnvalue::OK) { sif::warning << "Subscribing for GPS GPS_FIX_CHANGE event failed" << std::endl; } - return returnvalue::OK; + triggerEvent(core::REBOOT_SW, CURRENT_CHIP, CURRENT_COPY); + announceCurrentImageInfo(); + announceVersionInfo(); + + return result; } ReturnValue_t CoreController::initializeAfterTaskCreation() { @@ -207,19 +215,7 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ using namespace core; switch (actionId) { case (ANNOUNCE_VERSION): { - uint32_t p1 = (common::OBSW_VERSION_MAJOR << 24) | (common::OBSW_VERSION_MINOR << 16) | - (common::OBSW_VERSION_REVISION << 8); - uint32_t p2 = 0; - if (strcmp("", common::OBSW_VERSION_CST_GIT_SHA1) != 0) { - p1 |= 1; - auto shaAsStr = std::string(common::OBSW_VERSION_CST_GIT_SHA1); - size_t posDash = shaAsStr.find("-"); - auto gitHash = shaAsStr.substr(posDash + 2, 4); - // Only copy first 4 letters of git hash - memcpy(&p2, gitHash.c_str(), 4); - } - - triggerEvent(VERSION_INFO, p1, p2); + announceVersionInfo(); return HasActionsIF::EXECUTION_FINISHED; } case (ANNOUNCE_BOOT_COUNTS): { @@ -227,38 +223,129 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ return HasActionsIF::EXECUTION_FINISHED; } case (ANNOUNCE_CURRENT_IMAGE): { - triggerEvent(CURRENT_IMAGE_INFO, CURRENT_CHIP, CURRENT_COPY); + announceCurrentImageInfo(); return HasActionsIF::EXECUTION_FINISHED; } case (LIST_DIRECTORY_INTO_FILE): { return actionListDirectoryIntoFile(actionId, commandedBy, data, size); } + case (LIST_DIRECTORY_DUMP_DIRECTLY): { + return actionListDirectoryDumpDirectly(actionId, commandedBy, data, size); + } + case (CP_HELPER): { + CpHelperParser parser(data, size); + ReturnValue_t result = parser.parse(); + if (result != returnvalue::OK) { + return result; + } + std::ostringstream oss("cp ", std::ostringstream::ate); + if (parser.isForceOptSet()) { + oss << "-f "; + } + if (parser.isRecursiveOptSet()) { + oss << "-r "; + } + auto &sourceTgt = parser.destTgtPair(); + oss << sourceTgt.sourceName << " " << sourceTgt.targetName; + sif::info << "CoreController: Performing copy command: " << oss.str() << std::endl; + int ret = std::system(oss.str().c_str()); + if (ret != 0) { + return returnvalue::FAILED; + } + return EXECUTION_FINISHED; + } + case (MV_HELPER): { + MvHelperParser parser(data, size); + ReturnValue_t result = parser.parse(); + if (result != returnvalue::OK) { + return result; + } + std::ostringstream oss("mv ", std::ostringstream::ate); + auto &sourceTgt = parser.destTgtPair(); + oss << sourceTgt.sourceName << " " << sourceTgt.targetName; + sif::info << "CoreController: Performing move command: " << oss.str() << std::endl; + int ret = std::system(oss.str().c_str()); + if (ret != 0) { + return returnvalue::FAILED; + } + return EXECUTION_FINISHED; + } + case (RM_HELPER): { + RmHelperParser parser(data, size); + ReturnValue_t result = parser.parse(); + if (result != returnvalue::OK) { + return result; + } + std::ostringstream oss("rm ", std::ostringstream::ate); + if (parser.isRecursiveOptSet() or parser.isForceOptSet()) { + oss << "-"; + } + if (parser.isRecursiveOptSet()) { + oss << "r"; + } + if (parser.isForceOptSet()) { + oss << "f"; + } + size_t removeTargetSize = 0; + const char *removeTgt = parser.getRemoveTarget(removeTargetSize); + oss << " " << removeTgt; + sif::info << "CoreController: Performing remove command: " << oss.str() << std::endl; + int ret = std::system(oss.str().c_str()); + if (ret != 0) { + return returnvalue::FAILED; + } + return EXECUTION_FINISHED; + } + case (MKDIR_HELPER): { + if (size < 1) { + return HasActionsIF::INVALID_PARAMETERS; + } + std::string createdDir = std::string(reinterpret_cast(data), size); + std::ostringstream oss("mkdir ", std::ostringstream::ate); + oss << createdDir; + sif::info << "CoreController: Performing directory creation: " << oss.str() << std::endl; + int ret = std::system(oss.str().c_str()); + if (ret != 0) { + return returnvalue::FAILED; + } + return EXECUTION_FINISHED; + } case (SWITCH_REBOOT_FILE_HANDLING): { if (size < 1) { return HasActionsIF::INVALID_PARAMETERS; } - std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_FILE; - // Disable the reboot file mechanism - parseRebootFile(path, rebootFile); + std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_WATCHDOG_FILE; + parseRebootWatchdogFile(path, rebootWatchdogFile); if (data[0] == 0) { - rebootFile.enabled = false; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.enabled = false; + rewriteRebootWatchdogFile(rebootWatchdogFile); } else if (data[0] == 1) { - rebootFile.enabled = true; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.enabled = true; + rewriteRebootWatchdogFile(rebootWatchdogFile); } else { return HasActionsIF::INVALID_PARAMETERS; } return HasActionsIF::EXECUTION_FINISHED; } + case (READ_REBOOT_MECHANISM_INFO): { + std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_WATCHDOG_FILE; + parseRebootWatchdogFile(path, rebootWatchdogFile); + RebootWatchdogPacket packet(rebootWatchdogFile); + ReturnValue_t result = actionHelper.reportData(commandedBy, actionId, &packet); + if (result != returnvalue::OK) { + return result; + } + return HasActionsIF::EXECUTION_FINISHED; + } case (RESET_REBOOT_COUNTERS): { if (size == 0) { - resetRebootCount(xsc::ALL_CHIP, xsc::ALL_COPY); + resetRebootWatchdogCounters(xsc::ALL_CHIP, xsc::ALL_COPY); } else if (size == 2) { if (data[0] > 1 or data[1] > 1) { return HasActionsIF::INVALID_PARAMETERS; } - resetRebootCount(static_cast(data[0]), static_cast(data[1])); + resetRebootWatchdogCounters(static_cast(data[0]), + static_cast(data[1])); } return HasActionsIF::EXECUTION_FINISHED; } @@ -353,11 +440,11 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ if (size < 1) { return HasActionsIF::INVALID_PARAMETERS; } - std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix() + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism - parseRebootFile(path, rebootFile); - rebootFile.maxCount = data[0]; - rewriteRebootFile(rebootFile); + parseRebootWatchdogFile(path, rebootWatchdogFile); + rebootWatchdogFile.maxCount = data[0]; + rewriteRebootWatchdogFile(rebootWatchdogFile); return HasActionsIF::EXECUTION_FINISHED; } case (XSC_REBOOT_OBC): { @@ -911,59 +998,152 @@ ReturnValue_t CoreController::initVersionFile() { return returnvalue::OK; } -ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId, - MessageQueueId_t commandedBy, - const uint8_t *data, size_t size) { - // TODO: Packet definition for clean deserialization - // 2 bytes for a and R flag, at least 5 bytes for minimum valid path /tmp with - // null termination, at least 7 bytes for minimum target file name /tmp/a with - // null termination. - if (size < 14) { - return HasActionsIF::INVALID_PARAMETERS; +ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionId, + MessageQueueId_t commandedBy, + const uint8_t *data, size_t size) { + core::ListDirectoryCmdBase parser(data, size); + ReturnValue_t result = parser.parse(); + if (result != returnvalue::OK) { + return result; } - // We could also make -l optional, but I can't think of a reason why to not use -l.. - // This flag specifies to run ls with -a - bool aFlag = data[0]; - data += 1; - // This flag specifies to run ls with -R - bool RFlag = data[1]; - data += 1; - - size_t remainingSize = size - 2; - // One larger for null termination, which prevents undefined behaviour if the sent - // strings are not 0 terminated properly - std::vector repoAndTargetFileBuffer(remainingSize + 1, 0); - std::memcpy(repoAndTargetFileBuffer.data(), data, remainingSize); - const char *currentCharPtr = reinterpret_cast(repoAndTargetFileBuffer.data()); - // Full target file name - std::string repoName(currentCharPtr); - size_t repoLength = repoName.length(); - // The other string needs to be at least one letter plus NULL termination to be valid at all - // The first string also needs to be NULL terminated, but the termination is not included - // in the string length, so this is subtracted from the remaining size as well - if (repoLength > remainingSize - 3) { - return HasActionsIF::INVALID_PARAMETERS; - } - // The file length will not include the NULL termination, so we skip it - currentCharPtr += repoLength + 1; - std::string targetFileName(currentCharPtr); - std::ostringstream oss; - oss << "ls -l"; - if (aFlag) { + std::ostringstream oss("ls -l", std::ostringstream::ate); + if (parser.aFlagSet()) { oss << "a"; } - if (RFlag) { + if (parser.rFlagSet()) { oss << "R"; } - oss << " " << repoName << " > " << targetFileName; - int result = std::system(oss.str().c_str()); - if (result != 0) { - utility::handleSystemError(result, "CoreController::actionListDirectoryIntoFile"); - actionHelper.finish(false, commandedBy, actionId); + size_t repoNameLen = 0; + const char *repoName = parser.getRepoName(repoNameLen); + + oss << " " << repoName << " > " << LIST_DIR_DUMP_WORK_FILE; + sif::info << "Executing " << oss.str() << " for direct dump"; + if (parser.compressionOptionSet()) { + sif::info << " with compression"; } - return returnvalue::OK; + sif::info << std::endl; + int ret = std::system(oss.str().c_str()); + if (ret != 0) { + utility::handleSystemError(result, "CoreController::actionListDirectoryDumpDirectly"); + return returnvalue::FAILED; + } + if (parser.compressionOptionSet()) { + std::string compressedName = LIST_DIR_DUMP_WORK_FILE + std::string(".gz"); + oss.str(""); + oss << "gzip " << LIST_DIR_DUMP_WORK_FILE; + ret = std::system(oss.str().c_str()); + if (ret != 0) { + utility::handleSystemError(result, "CoreController::actionListDirectoryDumpDirectly"); + return returnvalue::FAILED; + } + oss.str(""); + // Overwrite the work file with the compressed archive. + oss << "mv " << compressedName << " " << LIST_DIR_DUMP_WORK_FILE; + ret = std::system(oss.str().c_str()); + if (ret != 0) { + utility::handleSystemError(result, "CoreController::actionListDirectoryDumpDirectly"); + return returnvalue::FAILED; + } + } + dirListingBuf[8] = parser.compressionOptionSet(); + // First four bytes reserved for segment index. One byte for compression option information + std::strcpy(reinterpret_cast(dirListingBuf.data() + 2 * sizeof(uint32_t) + 1), repoName); + std::ifstream ifile(LIST_DIR_DUMP_WORK_FILE, std::ios::binary); + if (ifile.bad()) { + return returnvalue::FAILED; + } + std::error_code e; + dumpContext.totalFileSize = std::filesystem::file_size(LIST_DIR_DUMP_WORK_FILE, e); + dumpContext.segmentIdx = 0; + dumpContext.dumpedBytes = 0; + size_t nextDumpLen = 0; + size_t dummy = 0; + dumpContext.maxDumpLen = dirListingBuf.size() - 2 * sizeof(uint32_t) - 1 - repoNameLen - 1; + dumpContext.listingDataOffset = 2 * sizeof(uint32_t) + 1 + repoNameLen + 1; + uint32_t chunks = dumpContext.totalFileSize / dumpContext.maxDumpLen; + if (dumpContext.totalFileSize % dumpContext.maxDumpLen != 0) { + chunks++; + } + SerializeAdapter::serialize(&chunks, dirListingBuf.data() + sizeof(uint32_t), &dummy, + dirListingBuf.size() - sizeof(uint32_t), + SerializeIF::Endianness::NETWORK); + while (dumpContext.dumpedBytes < dumpContext.totalFileSize) { + ifile.seekg(dumpContext.dumpedBytes, std::ios::beg); + nextDumpLen = dumpContext.maxDumpLen; + if (dumpContext.totalFileSize - dumpContext.dumpedBytes < dumpContext.maxDumpLen) { + nextDumpLen = dumpContext.totalFileSize - dumpContext.dumpedBytes; + } + SerializeAdapter::serialize(&dumpContext.segmentIdx, dirListingBuf.data(), &dummy, + dirListingBuf.size(), SerializeIF::Endianness::NETWORK); + ifile.read(reinterpret_cast(dirListingBuf.data() + dumpContext.listingDataOffset), + nextDumpLen); + result = actionHelper.reportData(commandedBy, actionId, dirListingBuf.data(), + dumpContext.listingDataOffset + nextDumpLen); + if (result != returnvalue::OK) { + // Remove work file when we are done + std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); + return result; + } + dumpContext.segmentIdx++; + dumpContext.dumpedBytes += nextDumpLen; + // Dump takes multiple task cycles, so cache the dump state and continue dump the next cycles. + if (dumpContext.segmentIdx == 10) { + dumpContext.active = true; + dumpContext.firstDump = true; + dumpContext.commander = commandedBy; + dumpContext.actionId = actionId; + return returnvalue::OK; + } + } + // Remove work file when we are done + std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); + return EXECUTION_FINISHED; +} + +ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId, + MessageQueueId_t commandedBy, + const uint8_t *data, size_t size) { + core::ListDirectoryIntoFile parser(data, size); + ReturnValue_t result = parser.parse(); + if (result != returnvalue::OK) { + return result; + } + + std::ostringstream oss("ls -l", std::ostringstream::ate); + if (parser.aFlagSet()) { + oss << "a"; + } + if (parser.rFlagSet()) { + oss << "R"; + } + + size_t repoNameLen = 0; + const char *repoName = parser.getRepoName(repoNameLen); + size_t targetFileNameLen = 0; + const char *targetFileName = parser.getTargetName(targetFileNameLen); + oss << " " << repoName << " > " << targetFileName; + sif::info << "Executing list directory request, command: " << oss.str() << std::endl; + int ret = std::system(oss.str().c_str()); + if (ret != 0) { + utility::handleSystemError(result, "CoreController::actionListDirectoryIntoFile"); + return returnvalue::FAILED; + } + + // Compression will add a .gz ending. I don't have any issue with this, it makes it explicit + // that this is a compressed file. + if (parser.compressionOptionSet()) { + oss.str(""); + oss << "gzip " << targetFileName; + sif::info << "Compressing directory listing: " << oss.str() << std::endl; + ret = std::system(oss.str().c_str()); + if (ret != 0) { + utility::handleSystemError(result, "CoreController::actionListDirectoryIntoFile"); + return returnvalue::FAILED; + } + } + return EXECUTION_FINISHED; } ReturnValue_t CoreController::initBootCopyFile() { @@ -1022,45 +1202,7 @@ ReturnValue_t CoreController::actionXscReboot(const uint8_t *data, size_t size) auto tgtChip = static_cast(data[1]); auto tgtCopy = static_cast(data[2]); - // This function can not really fail - gracefulShutdownTasks(tgtChip, tgtCopy, protOpPerformed); - - switch (tgtChip) { - case (xsc::Chip::CHIP_0): { - switch (tgtCopy) { - case (xsc::Copy::COPY_0): { - xsc_boot_copy(XSC_LIBNOR_CHIP_0, XSC_LIBNOR_COPY_NOMINAL); - break; - } - case (xsc::Copy::COPY_1): { - xsc_boot_copy(XSC_LIBNOR_CHIP_0, XSC_LIBNOR_COPY_GOLD); - break; - } - default: { - break; - } - } - break; - } - case (xsc::Chip::CHIP_1): { - switch (tgtCopy) { - case (xsc::Copy::COPY_0): { - xsc_boot_copy(XSC_LIBNOR_CHIP_1, XSC_LIBNOR_COPY_NOMINAL); - break; - } - case (xsc::Copy::COPY_1): { - xsc_boot_copy(XSC_LIBNOR_CHIP_1, XSC_LIBNOR_COPY_GOLD); - break; - } - default: { - break; - } - } - break; - } - default: - break; - } + performGracefulShutdown(tgtChip, tgtCopy); return returnvalue::FAILED; } @@ -1073,14 +1215,23 @@ ReturnValue_t CoreController::actionReboot(const uint8_t *data, size_t size) { ReturnValue_t CoreController::gracefulShutdownTasks(xsc::Chip chip, xsc::Copy copy, bool &protOpPerformed) { + // Store both sequence counters persistently. + core::SAVE_CFDP_SEQUENCE_COUNT = true; + core::SAVE_PUS_SEQUENCE_COUNT = true; + sdcMan->setBlocking(true); sdcMan->markUnusable(); // Wait two seconds to ensure no one uses the SD cards TaskFactory::delayTask(2000); + + // Ensure that all writes/reads do finish. + sync(); + // Attempt graceful shutdown by unmounting and switching off SD cards sdcMan->switchOffSdCard(sd::SdCard::SLOT_0); sdcMan->switchOffSdCard(sd::SdCard::SLOT_1); - // If any boot copies are unprotected + // If any boot copies are unprotected. + // Actually this function only ensures that reboots to the own image are protected.. ReturnValue_t result = setBootCopyProtection(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, true, protOpPerformed, false); if (result == returnvalue::OK and protOpPerformed) { @@ -1369,7 +1520,8 @@ void CoreController::performMountedSdCardOperations() { if (not timeFileInitDone) { initClockFromTimeFile(); } - performRebootFileHandling(false); + performRebootWatchdogHandling(false); + performRebootCountersHandling(false); } backupTimeFileHandler(); }; @@ -1441,118 +1593,127 @@ ReturnValue_t CoreController::performSdCardCheck() { return returnvalue::OK; } -void CoreController::performRebootFileHandling(bool recreateFile) { +void CoreController::performRebootWatchdogHandling(bool recreateFile) { using namespace std; - std::string path = currMntPrefix + REBOOT_FILE; + std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; + std::string legacyPath = currMntPrefix + LEGACY_REBOOT_WATCHDOG_FILE; std::error_code e; + // TODO: Remove at some point in the future. + if (std::filesystem::exists(legacyPath, e)) { + // Old file might still exist, so copy it to new path + std::filesystem::copy(legacyPath, path, std::filesystem::copy_options::overwrite_existing, e); + if (e) { + sif::error << "File copy has failed: " << e.message() << std::endl; + } + } if (not std::filesystem::exists(path, e) or recreateFile) { #if OBSW_VERBOSE_LEVEL >= 1 - sif::info << "CoreController::performRebootFileHandling: Recreating reboot file" << std::endl; + sif::info << "CoreController::performRebootFileHandling: Recreating reboot watchdog file" + << std::endl; #endif - rebootFile.enabled = false; - rebootFile.img00Cnt = 0; - rebootFile.img01Cnt = 0; - rebootFile.img10Cnt = 0; - rebootFile.img11Cnt = 0; - rebootFile.lastChip = xsc::Chip::CHIP_0; - rebootFile.lastCopy = xsc::Copy::COPY_0; - rebootFile.img00Lock = false; - rebootFile.img01Lock = false; - rebootFile.img10Lock = false; - rebootFile.img11Lock = false; - rebootFile.mechanismNextChip = xsc::Chip::NO_CHIP; - rebootFile.mechanismNextCopy = xsc::Copy::NO_COPY; - rebootFile.bootFlag = false; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.enabled = false; + rebootWatchdogFile.img00Cnt = 0; + rebootWatchdogFile.img01Cnt = 0; + rebootWatchdogFile.img10Cnt = 0; + rebootWatchdogFile.img11Cnt = 0; + rebootWatchdogFile.lastChip = xsc::Chip::CHIP_0; + rebootWatchdogFile.lastCopy = xsc::Copy::COPY_0; + rebootWatchdogFile.img00Lock = false; + rebootWatchdogFile.img01Lock = false; + rebootWatchdogFile.img10Lock = false; + rebootWatchdogFile.img11Lock = false; + rebootWatchdogFile.mechanismNextChip = xsc::Chip::NO_CHIP; + rebootWatchdogFile.mechanismNextCopy = xsc::Copy::NO_COPY; + rebootWatchdogFile.bootFlag = false; + rewriteRebootWatchdogFile(rebootWatchdogFile); } else { - if (not parseRebootFile(path, rebootFile)) { - performRebootFileHandling(true); + if (not parseRebootWatchdogFile(path, rebootWatchdogFile)) { + performRebootWatchdogHandling(true); + return; } } if (CURRENT_CHIP == xsc::CHIP_0) { if (CURRENT_COPY == xsc::COPY_0) { - rebootFile.img00Cnt++; + rebootWatchdogFile.img00Cnt++; } else { - rebootFile.img01Cnt++; + rebootWatchdogFile.img01Cnt++; } } else { if (CURRENT_COPY == xsc::COPY_0) { - rebootFile.img10Cnt++; + rebootWatchdogFile.img10Cnt++; } else { - rebootFile.img11Cnt++; + rebootWatchdogFile.img11Cnt++; } } - if (rebootFile.bootFlag) { + if (rebootWatchdogFile.bootFlag) { // Trigger event to inform ground that a reboot was triggered - uint32_t p1 = rebootFile.lastChip << 16 | rebootFile.lastCopy; + uint32_t p1 = rebootWatchdogFile.lastChip << 16 | rebootWatchdogFile.lastCopy; triggerEvent(core::REBOOT_MECHANISM_TRIGGERED, p1, 0); // Clear the boot flag - rebootFile.bootFlag = false; + rebootWatchdogFile.bootFlag = false; } - announceBootCounts(); - - if (rebootFile.mechanismNextChip != xsc::NO_CHIP and - rebootFile.mechanismNextCopy != xsc::NO_COPY) { - if (CURRENT_CHIP != rebootFile.mechanismNextChip or - CURRENT_COPY != rebootFile.mechanismNextCopy) { - std::string infoString = std::to_string(rebootFile.mechanismNextChip) + " " + - std::to_string(rebootFile.mechanismNextCopy); + if (rebootWatchdogFile.mechanismNextChip != xsc::NO_CHIP and + rebootWatchdogFile.mechanismNextCopy != xsc::NO_COPY) { + if (CURRENT_CHIP != rebootWatchdogFile.mechanismNextChip or + CURRENT_COPY != rebootWatchdogFile.mechanismNextCopy) { + std::string infoString = std::to_string(rebootWatchdogFile.mechanismNextChip) + " " + + std::to_string(rebootWatchdogFile.mechanismNextCopy); sif::warning << "CoreController::performRebootFileHandling: Expected to be on image " << infoString << " but currently on other image. Locking " << infoString << std::endl; // Firmware or other component might be corrupt and we are on another image then the target // image specified by the mechanism. We can't really trust the target image anymore. // Lock it for now - if (rebootFile.mechanismNextChip == xsc::CHIP_0) { - if (rebootFile.mechanismNextCopy == xsc::COPY_0) { - rebootFile.img00Lock = true; + if (rebootWatchdogFile.mechanismNextChip == xsc::CHIP_0) { + if (rebootWatchdogFile.mechanismNextCopy == xsc::COPY_0) { + rebootWatchdogFile.img00Lock = true; } else { - rebootFile.img01Lock = true; + rebootWatchdogFile.img01Lock = true; } } else { - if (rebootFile.mechanismNextCopy == xsc::COPY_0) { - rebootFile.img10Lock = true; + if (rebootWatchdogFile.mechanismNextCopy == xsc::COPY_0) { + rebootWatchdogFile.img10Lock = true; } else { - rebootFile.img11Lock = true; + rebootWatchdogFile.img11Lock = true; } } } } - rebootFile.lastChip = CURRENT_CHIP; - rebootFile.lastCopy = CURRENT_COPY; + rebootWatchdogFile.lastChip = CURRENT_CHIP; + rebootWatchdogFile.lastCopy = CURRENT_COPY; // Only reboot if the reboot functionality is enabled. // The handler will still increment the boot counts - if (rebootFile.enabled and (*rebootFile.relevantBootCnt >= rebootFile.maxCount)) { + if (rebootWatchdogFile.enabled and + (*rebootWatchdogFile.relevantBootCnt >= rebootWatchdogFile.maxCount)) { // Reboot to other image bool doReboot = false; xsc::Chip tgtChip = xsc::NO_CHIP; xsc::Copy tgtCopy = xsc::NO_COPY; - determineAndExecuteReboot(rebootFile, doReboot, tgtChip, tgtCopy); + rebootWatchdogAlgorithm(rebootWatchdogFile, doReboot, tgtChip, tgtCopy); if (doReboot) { - rebootFile.bootFlag = true; + rebootWatchdogFile.bootFlag = true; #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "Boot counter for image " << CURRENT_CHIP << " " << CURRENT_COPY << " too high. Rebooting to " << tgtChip << " " << tgtCopy << std::endl; #endif - rebootFile.mechanismNextChip = tgtChip; - rebootFile.mechanismNextCopy = tgtCopy; - rewriteRebootFile(rebootFile); - xsc_boot_copy(static_cast(tgtChip), - static_cast(tgtCopy)); + rebootWatchdogFile.mechanismNextChip = tgtChip; + rebootWatchdogFile.mechanismNextCopy = tgtCopy; + rewriteRebootWatchdogFile(rebootWatchdogFile); + performGracefulShutdown(tgtChip, tgtCopy); } } else { - rebootFile.mechanismNextChip = xsc::NO_CHIP; - rebootFile.mechanismNextCopy = xsc::NO_COPY; + rebootWatchdogFile.mechanismNextChip = xsc::NO_CHIP; + rebootWatchdogFile.mechanismNextCopy = xsc::NO_COPY; } - rewriteRebootFile(rebootFile); + rewriteRebootWatchdogFile(rebootWatchdogFile); } -void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot, - xsc::Chip &tgtChip, xsc::Copy &tgtCopy) { +void CoreController::rebootWatchdogAlgorithm(RebootWatchdogFile &rf, bool &needsReboot, + xsc::Chip &tgtChip, xsc::Copy &tgtCopy) { tgtChip = xsc::CHIP_0; tgtCopy = xsc::COPY_0; needsReboot = false; @@ -1640,7 +1801,7 @@ void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot } } -bool CoreController::parseRebootFile(std::string path, RebootFile &rf) { +bool CoreController::parseRebootWatchdogFile(std::string path, RebootWatchdogFile &rf) { using namespace std; std::string selfMatch; if (CURRENT_CHIP == xsc::CHIP_0) { @@ -1822,68 +1983,174 @@ bool CoreController::parseRebootFile(std::string path, RebootFile &rf) { return true; } -void CoreController::resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy) { - std::string path = currMntPrefix + REBOOT_FILE; - // Disable the reboot file mechanism - parseRebootFile(path, rebootFile); +bool CoreController::parseRebootCountersFile(std::string path, RebootCountersFile &rf) { + using namespace std; + ifstream file(path); + string word; + string line; + uint8_t lineIdx = 0; + while (std::getline(file, line)) { + istringstream iss(line); + switch (lineIdx) { + case 0: { + iss >> word; + if (word.find("img00:") == string::npos) { + return false; + } + iss >> rf.img00Cnt; + + break; + } + case 1: { + iss >> word; + if (word.find("img01:") == string::npos) { + return false; + } + iss >> rf.img01Cnt; + + break; + } + case 2: { + iss >> word; + if (word.find("img10:") == string::npos) { + return false; + } + iss >> rf.img10Cnt; + + break; + } + case 3: { + iss >> word; + if (word.find("img11:") == string::npos) { + return false; + } + iss >> rf.img11Cnt; + break; + } + } + lineIdx++; + } + return true; +} + +void CoreController::resetRebootWatchdogCounters(xsc::Chip tgtChip, xsc::Copy tgtCopy) { + std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; + parseRebootWatchdogFile(path, rebootWatchdogFile); if (tgtChip == xsc::ALL_CHIP and tgtCopy == xsc::ALL_COPY) { - rebootFile.img00Cnt = 0; - rebootFile.img01Cnt = 0; - rebootFile.img10Cnt = 0; - rebootFile.img11Cnt = 0; + rebootWatchdogFile.img00Cnt = 0; + rebootWatchdogFile.img01Cnt = 0; + rebootWatchdogFile.img10Cnt = 0; + rebootWatchdogFile.img11Cnt = 0; } else { if (tgtChip == xsc::CHIP_0) { if (tgtCopy == xsc::COPY_0) { - rebootFile.img00Cnt = 0; + rebootWatchdogFile.img00Cnt = 0; } else { - rebootFile.img01Cnt = 0; + rebootWatchdogFile.img01Cnt = 0; } } else { if (tgtCopy == xsc::COPY_0) { - rebootFile.img10Cnt = 0; + rebootWatchdogFile.img10Cnt = 0; } else { - rebootFile.img11Cnt = 0; + rebootWatchdogFile.img11Cnt = 0; } } } - rewriteRebootFile(rebootFile); + rewriteRebootWatchdogFile(rebootWatchdogFile); } -void CoreController::rewriteRebootFile(RebootFile file) { - std::string path = currMntPrefix + REBOOT_FILE; +void CoreController::performRebootCountersHandling(bool recreateFile) { + std::string path = currMntPrefix + REBOOT_COUNTERS_FILE; + std::error_code e; + if (not std::filesystem::exists(path, e) or recreateFile) { +#if OBSW_VERBOSE_LEVEL >= 1 + sif::info << "CoreController::performRebootFileHandling: Recreating reboot counters file" + << std::endl; +#endif + rebootCountersFile.img00Cnt = 0; + rebootCountersFile.img01Cnt = 0; + rebootCountersFile.img10Cnt = 0; + rebootCountersFile.img11Cnt = 0; + rewriteRebootCountersFile(rebootCountersFile); + } else { + if (not parseRebootCountersFile(path, rebootCountersFile)) { + performRebootCountersHandling(true); + return; + } + } + + if (CURRENT_CHIP == xsc::CHIP_0) { + if (CURRENT_COPY == xsc::COPY_0) { + rebootCountersFile.img00Cnt++; + } else { + rebootCountersFile.img01Cnt++; + } + } else { + if (CURRENT_COPY == xsc::COPY_0) { + rebootCountersFile.img10Cnt++; + } else { + rebootCountersFile.img11Cnt++; + } + } + announceBootCounts(); + rewriteRebootCountersFile(rebootCountersFile); +} +void CoreController::rewriteRebootWatchdogFile(RebootWatchdogFile file) { + using namespace std::filesystem; + std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; + std::string legacyPath = currMntPrefix + LEGACY_REBOOT_WATCHDOG_FILE; + { + std::ofstream rebootFile(path); + if (rebootFile.is_open()) { + // Initiate reboot file first. Reboot handling will be on on initialization + rebootFile << "on: " << file.enabled << "\nmaxcnt: " << file.maxCount + << "\nimg00: " << file.img00Cnt << "\nimg01: " << file.img01Cnt + << "\nimg10: " << file.img10Cnt << "\nimg11: " << file.img11Cnt + << "\nimg00lock: " << file.img00Lock << "\nimg01lock: " << file.img01Lock + << "\nimg10lock: " << file.img10Lock << "\nimg11lock: " << file.img11Lock + << "\nbootflag: " << file.bootFlag << "\nlast: " << static_cast(file.lastChip) + << " " << static_cast(file.lastCopy) + << "\nnext: " << static_cast(file.mechanismNextChip) << " " + << static_cast(file.mechanismNextCopy) << "\n"; + } + } + std::error_code e; + // TODO: Remove at some point in the future when all images have been updated. + if (std::filesystem::exists(legacyPath)) { + // Keep those two files in sync + std::filesystem::copy(path, legacyPath, std::filesystem::copy_options::overwrite_existing, e); + if (e) { + sif::error << "File copy has failed: " << e.message() << std::endl; + } + } +} + +void CoreController::rewriteRebootCountersFile(RebootCountersFile file) { + std::string path = currMntPrefix + REBOOT_COUNTERS_FILE; std::ofstream rebootFile(path); if (rebootFile.is_open()) { - // Initiate reboot file first. Reboot handling will be on on initialization - rebootFile << "on: " << file.enabled << "\nmaxcnt: " << file.maxCount - << "\nimg00: " << file.img00Cnt << "\nimg01: " << file.img01Cnt - << "\nimg10: " << file.img10Cnt << "\nimg11: " << file.img11Cnt - << "\nimg00lock: " << file.img00Lock << "\nimg01lock: " << file.img01Lock - << "\nimg10lock: " << file.img10Lock << "\nimg11lock: " << file.img11Lock - << "\nbootflag: " << file.bootFlag << "\nlast: " << static_cast(file.lastChip) - << " " << static_cast(file.lastCopy) - << "\nnext: " << static_cast(file.mechanismNextChip) << " " - << static_cast(file.mechanismNextCopy) << "\n"; + rebootFile << "img00: " << file.img00Cnt << "\nimg01: " << file.img01Cnt + << "\nimg10: " << file.img10Cnt << "\nimg11: " << file.img11Cnt << "\n"; } } void CoreController::setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy) { - std::string path = currMntPrefix + REBOOT_FILE; - // Disable the reboot file mechanism - parseRebootFile(path, rebootFile); + std::string path = currMntPrefix + REBOOT_WATCHDOG_FILE; + parseRebootWatchdogFile(path, rebootWatchdogFile); if (tgtChip == xsc::CHIP_0) { if (tgtCopy == xsc::COPY_0) { - rebootFile.img00Lock = lock; + rebootWatchdogFile.img00Lock = lock; } else { - rebootFile.img01Lock = lock; + rebootWatchdogFile.img01Lock = lock; } } else { if (tgtCopy == xsc::COPY_0) { - rebootFile.img10Lock = lock; + rebootWatchdogFile.img10Lock = lock; } else { - rebootFile.img11Lock = lock; + rebootWatchdogFile.img11Lock = lock; } } - rewriteRebootFile(rebootFile); + rewriteRebootWatchdogFile(rebootWatchdogFile); } ReturnValue_t CoreController::backupTimeFileHandler() { @@ -2049,6 +2316,8 @@ ReturnValue_t CoreController::executeSwUpdate(SwUpdateSources sourceDir, const u if (not exists(archivePath, e)) { return HasFileSystemIF::FILE_DOES_NOT_EXIST; } + // TODO: Decompressing without limiting memory usage with xz is actually a bit risky.. + // But has not been an issue so far. ostringstream cmd("tar -xJf", ios::app); cmd << " " << archivePath << " -C " << prefixPath; int result = system(cmd.str().c_str()); @@ -2168,10 +2437,12 @@ bool CoreController::startSdStateMachine(sd::SdCard targetActiveSd, SdCfgMode mo } 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; + uint64_t totalBootCount = rebootCountersFile.img00Cnt + rebootCountersFile.img01Cnt + + rebootCountersFile.img10Cnt + rebootCountersFile.img11Cnt; + uint32_t individualBootCountsP1 = + (rebootCountersFile.img00Cnt << 16) | rebootCountersFile.img01Cnt; + uint32_t individualBootCountsP2 = + (rebootCountersFile.img10Cnt << 16) | rebootCountersFile.img11Cnt; triggerEvent(core::INDIVIDUAL_BOOT_COUNTS, individualBootCountsP1, individualBootCountsP2); triggerEvent(core::REBOOT_COUNTER, (totalBootCount >> 32) & 0xffffffff, totalBootCount & 0xffffffff); @@ -2181,6 +2452,124 @@ MessageQueueId_t CoreController::getCommandQueue() const { return ExtendedControllerBase::getCommandQueue(); } +void CoreController::dirListingDumpHandler() { + if (dumpContext.firstDump) { + dumpContext.firstDump = false; + return; + } + size_t nextDumpLen = 0; + size_t dummy = 0; + ReturnValue_t result; + std::error_code e; + std::ifstream ifile(LIST_DIR_DUMP_WORK_FILE, std::ios::binary); + if (ifile.bad()) { + return; + } + while (dumpContext.dumpedBytes < dumpContext.totalFileSize) { + ifile.seekg(dumpContext.dumpedBytes, std::ios::beg); + nextDumpLen = dumpContext.maxDumpLen; + if (dumpContext.totalFileSize - dumpContext.dumpedBytes < dumpContext.maxDumpLen) { + nextDumpLen = dumpContext.totalFileSize - dumpContext.dumpedBytes; + } + SerializeAdapter::serialize(&dumpContext.segmentIdx, dirListingBuf.data(), &dummy, + dirListingBuf.size(), SerializeIF::Endianness::NETWORK); + ifile.read(reinterpret_cast(dirListingBuf.data() + dumpContext.listingDataOffset), + nextDumpLen); + result = + actionHelper.reportData(dumpContext.commander, dumpContext.actionId, dirListingBuf.data(), + dumpContext.listingDataOffset + nextDumpLen); + if (result != returnvalue::OK) { + // Remove work file when we are done + std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); + dumpContext.active = false; + actionHelper.finish(false, dumpContext.commander, dumpContext.actionId, result); + return; + } + dumpContext.segmentIdx++; + dumpContext.dumpedBytes += nextDumpLen; + // Dump takes multiple task cycles, so cache the dump state and continue dump the next cycles. + if (dumpContext.segmentIdx == 10) { + break; + } + } + if (dumpContext.dumpedBytes >= dumpContext.totalFileSize) { + actionHelper.finish(true, dumpContext.commander, dumpContext.actionId, result); + dumpContext.active = false; + // Remove work file when we are done + std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); + } +} + +void CoreController::announceVersionInfo() { + using namespace core; + uint32_t p1 = (common::OBSW_VERSION_MAJOR << 24) | (common::OBSW_VERSION_MINOR << 16) | + (common::OBSW_VERSION_REVISION << 8); + uint32_t p2 = 0; + if (strcmp("", common::OBSW_VERSION_CST_GIT_SHA1) != 0) { + p1 |= 1; + auto shaAsStr = std::string(common::OBSW_VERSION_CST_GIT_SHA1); + size_t posDash = shaAsStr.find("-"); + auto gitHash = shaAsStr.substr(posDash + 2, 4); + // Only copy first 4 letters of git hash + memcpy(&p2, gitHash.c_str(), 4); + } + + triggerEvent(VERSION_INFO, p1, p2); + p1 = (core::FW_VERSION_MAJOR << 24) | (core::FW_VERSION_MINOR << 16) | + (core::FW_VERSION_REVISION << 8) | (core::FW_VERSION_HAS_SHA); + std::memcpy(&p2, core::FW_VERSION_GIT_SHA, 4); + triggerEvent(FIRMWARE_INFO, p1, p2); +} + +void CoreController::announceCurrentImageInfo() { + using namespace core; + triggerEvent(CURRENT_IMAGE_INFO, CURRENT_CHIP, CURRENT_COPY); +} + +ReturnValue_t CoreController::performGracefulShutdown(xsc::Chip tgtChip, xsc::Copy tgtCopy) { + bool protOpPerformed = false; + // This function can not really fail + gracefulShutdownTasks(tgtChip, tgtCopy, protOpPerformed); + + switch (tgtChip) { + case (xsc::Chip::CHIP_0): { + switch (tgtCopy) { + case (xsc::Copy::COPY_0): { + xsc_boot_copy(XSC_LIBNOR_CHIP_0, XSC_LIBNOR_COPY_NOMINAL); + break; + } + case (xsc::Copy::COPY_1): { + xsc_boot_copy(XSC_LIBNOR_CHIP_0, XSC_LIBNOR_COPY_GOLD); + break; + } + default: { + break; + } + } + break; + } + case (xsc::Chip::CHIP_1): { + switch (tgtCopy) { + case (xsc::Copy::COPY_0): { + xsc_boot_copy(XSC_LIBNOR_CHIP_1, XSC_LIBNOR_COPY_NOMINAL); + break; + } + case (xsc::Copy::COPY_1): { + xsc_boot_copy(XSC_LIBNOR_CHIP_1, XSC_LIBNOR_COPY_GOLD); + break; + } + default: { + break; + } + } + break; + } + default: + break; + } + return returnvalue::OK; +} + 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 c1f5e40d..b6bc9bfb 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -1,11 +1,13 @@ #ifndef BSP_Q7S_CORE_CORECONTROLLER_H_ #define BSP_Q7S_CORE_CORECONTROLLER_H_ +#include #include #include #include #include #include +#include #include #include #include @@ -13,7 +15,6 @@ #include #include -#include "CoreDefinitions.h" #include "OBSWConfig.h" #include "bsp_q7s/fs/SdCardManager.h" #include "events/subsystemIdRanges.h" @@ -23,7 +24,7 @@ class Timer; class SdCardManager; -struct RebootFile { +struct RebootWatchdogFile { static constexpr uint8_t DEFAULT_MAX_BOOT_CNT = 10; bool enabled = true; @@ -44,6 +45,93 @@ struct RebootFile { xsc::Copy mechanismNextCopy = xsc::Copy::NO_COPY; }; +class RebootWatchdogPacket : public SerialLinkedListAdapter { + public: + RebootWatchdogPacket(RebootWatchdogFile& rf) { + enabled = rf.enabled; + maxCount = rf.maxCount; + img00Count = rf.img00Cnt; + img01Count = rf.img01Cnt; + img10Count = rf.img10Cnt; + img11Count = rf.img11Cnt; + img00Lock = rf.img00Lock; + img01Lock = rf.img01Lock; + img10Lock = rf.img10Lock; + img11Lock = rf.img11Lock; + lastChip = static_cast(rf.lastChip); + lastCopy = static_cast(rf.lastCopy); + nextChip = static_cast(rf.mechanismNextChip); + nextCopy = static_cast(rf.mechanismNextCopy); + setLinks(); + } + + private: + void setLinks() { + setStart(&enabled); + enabled.setNext(&maxCount); + maxCount.setNext(&img00Count); + img00Count.setNext(&img01Count); + img01Count.setNext(&img10Count); + img10Count.setNext(&img11Count); + img11Count.setNext(&img00Lock); + img00Lock.setNext(&img01Lock); + img01Lock.setNext(&img10Lock); + img10Lock.setNext(&img11Lock); + img11Lock.setNext(&lastChip); + lastChip.setNext(&lastCopy); + lastCopy.setNext(&nextChip); + nextChip.setNext(&nextCopy); + setLast(&nextCopy); + } + + SerializeElement enabled = false; + SerializeElement maxCount = 0; + SerializeElement img00Count = 0; + SerializeElement img01Count = 0; + SerializeElement img10Count = 0; + SerializeElement img11Count = 0; + SerializeElement img00Lock = false; + SerializeElement img01Lock = false; + SerializeElement img10Lock = false; + SerializeElement img11Lock = false; + SerializeElement lastChip = 0; + SerializeElement lastCopy = 0; + SerializeElement nextChip = 0; + SerializeElement nextCopy = 0; +}; + +struct RebootCountersFile { + // 16 bit values so all boot counters fit into one event. + uint16_t img00Cnt = 0; + uint16_t img01Cnt = 0; + uint16_t img10Cnt = 0; + uint16_t img11Cnt = 0; +}; + +class RebootCountersPacket : public SerialLinkedListAdapter { + RebootCountersPacket(RebootCountersFile& rf) { + img00Count = rf.img00Cnt; + img01Count = rf.img01Cnt; + img10Count = rf.img10Cnt; + img11Count = rf.img11Cnt; + setLinks(); + } + + private: + void setLinks() { + setStart(&img00Count); + img00Count.setNext(&img01Count); + img01Count.setNext(&img10Count); + img10Count.setNext(&img11Count); + setLast(&img11Count); + } + + SerializeElement img00Count = 0; + SerializeElement img01Count = 0; + SerializeElement img10Count = 0; + SerializeElement img11Count = 0; +}; + class CoreController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: enum ParamId : uint8_t { PREF_SD = 0, NUM_IDS }; @@ -57,15 +145,21 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe const std::string VERSION_FILE = "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::VERSION_FILE_NAME); - const std::string REBOOT_FILE = - "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::REBOOT_FILE_NAME); + const std::string LEGACY_REBOOT_WATCHDOG_FILE = + "/" + std::string(core::CONF_FOLDER) + "/" + + std::string(core::LEGACY_REBOOT_WATCHDOG_FILE_NAME); + const std::string REBOOT_WATCHDOG_FILE = + "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::REBOOT_WATCHDOG_FILE_NAME); const std::string BACKUP_TIME_FILE = "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::TIME_FILE_NAME); + const std::string REBOOT_COUNTERS_FILE = + "/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::REBOOT_COUNTER_FILE_NAME); static constexpr char CHIP_0_COPY_0_MOUNT_DIR[] = "/tmp/mntupdate-xdi-qspi0-nom-rootfs"; static constexpr char CHIP_0_COPY_1_MOUNT_DIR[] = "/tmp/mntupdate-xdi-qspi0-gold-rootfs"; static constexpr char CHIP_1_COPY_0_MOUNT_DIR[] = "/tmp/mntupdate-xdi-qspi1-nom-rootfs"; static constexpr char CHIP_1_COPY_1_MOUNT_DIR[] = "/tmp/mntupdate-xdi-qspi1-gold-rootfs"; + static constexpr char LIST_DIR_DUMP_WORK_FILE[] = "/tmp/dir_listing.tmp"; static constexpr dur_millis_t INIT_SD_CARD_CHECK_TIMEOUT = 5000; static constexpr dur_millis_t DEFAULT_SD_CARD_CHECK_TIMEOUT = 60000; @@ -141,6 +235,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe static constexpr bool BLOCKING_SD_INIT = false; + uint32_t* mappedSysRomAddr = nullptr; SdCardManager* sdcMan = nullptr; MessageQueueIF* eventQueue = nullptr; @@ -176,7 +271,22 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe DeviceCommandId_t actionId; } sdCommandingInfo; - RebootFile rebootFile = {}; + struct DirListingDumpContext { + bool active; + bool firstDump; + size_t dumpedBytes; + size_t totalFileSize; + size_t listingDataOffset; + size_t maxDumpLen; + uint32_t segmentIdx; + MessageQueueId_t commander = MessageQueueIF::NO_QUEUE; + DeviceCommandId_t actionId; + }; + std::array dirListingBuf{}; + DirListingDumpContext dumpContext{}; + + RebootWatchdogFile rebootWatchdogFile = {}; + RebootCountersFile rebootCountersFile = {}; CommandExecutor cmdExecutor; SimpleRingBuffer cmdReplyBuf; @@ -246,10 +356,18 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe void currentStateSetter(sd::SdCard sdCard, sd::SdState newState); void executeNextExternalSdCommand(); void checkExternalSdCommandStatus(); - void performRebootFileHandling(bool recreateFile); + void performRebootWatchdogHandling(bool recreateFile); + void performRebootCountersHandling(bool recreateFile); ReturnValue_t actionListDirectoryIntoFile(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size); + ReturnValue_t actionListDirectoryDumpDirectly(ActionId_t actionId, MessageQueueId_t commandedBy, + const uint8_t* data, size_t size); + ReturnValue_t performGracefulShutdown(xsc::Chip targetChip, xsc::Copy targetCopy); + + ReturnValue_t actionListDirectoryCommonCommandCreator(const uint8_t* data, size_t size, + std::ostringstream& oss); + ReturnValue_t actionXscReboot(const uint8_t* data, size_t size); ReturnValue_t actionReboot(const uint8_t* data, size_t size); @@ -259,14 +377,19 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe int handleBootCopyProtAtIndex(xsc::Chip targetChip, xsc::Copy targetCopy, bool protect, bool& protOperationPerformed, bool selfChip, bool selfCopy, bool allChips, bool allCopies, uint8_t arrIdx); - void determineAndExecuteReboot(RebootFile& rf, bool& needsReboot, xsc::Chip& tgtChip, - xsc::Copy& tgtCopy); - void resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy); + void rebootWatchdogAlgorithm(RebootWatchdogFile& rf, bool& needsReboot, xsc::Chip& tgtChip, + xsc::Copy& tgtCopy); + void resetRebootWatchdogCounters(xsc::Chip tgtChip, xsc::Copy tgtCopy); void setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy); - bool parseRebootFile(std::string path, RebootFile& file); - void rewriteRebootFile(RebootFile file); + bool parseRebootWatchdogFile(std::string path, RebootWatchdogFile& file); + bool parseRebootCountersFile(std::string path, RebootCountersFile& file); + void rewriteRebootWatchdogFile(RebootWatchdogFile file); + void rewriteRebootCountersFile(RebootCountersFile file); void announceBootCounts(); + void announceVersionInfo(); + void announceCurrentImageInfo(); void readHkData(); + void dirListingDumpHandler(); bool isNumber(const std::string& s); }; diff --git a/bsp_q7s/core/CoreDefinitions.h b/bsp_q7s/core/defs.h similarity index 81% rename from bsp_q7s/core/CoreDefinitions.h rename to bsp_q7s/core/defs.h index 91896301..a82c1c56 100644 --- a/bsp_q7s/core/CoreDefinitions.h +++ b/bsp_q7s/core/defs.h @@ -1,10 +1,16 @@ -#ifndef BSP_Q7S_CORE_COREDEFINITIONS_H_ -#define BSP_Q7S_CORE_COREDEFINITIONS_H_ +#ifndef BSP_Q7S_CORE_DEFS_H_ +#define BSP_Q7S_CORE_DEFS_H_ #include namespace core { +extern uint8_t FW_VERSION_MAJOR; +extern uint8_t FW_VERSION_MINOR; +extern uint8_t FW_VERSION_REVISION; +extern bool FW_VERSION_HAS_SHA; +extern char FW_VERSION_GIT_SHA[4]; + static const uint8_t HK_SET_ENTRIES = 3; static const uint32_t HK_SET_ID = 5; @@ -36,4 +42,4 @@ class HkSet : public StaticLocalDataSet { } // namespace core -#endif /* BSP_Q7S_CORE_COREDEFINITIONS_H_ */ +#endif /* BSP_Q7S_CORE_DEFS_H_ */ diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 05a3fee0..a7b05e3d 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -10,8 +11,8 @@ #include "OBSWConfig.h" #include "bsp_q7s/core/CoreController.h" -#include "bsp_q7s/core/ObjectFactory.h" #include "busConf.h" +#include "common/config/devices/addresses.h" #include "devConf.h" #include "dummies/helperFactory.h" #include "eive/objects.h" @@ -35,27 +36,54 @@ void ObjectFactory::produce(void* args) { #endif PersistentTmStores stores; + readFirmwareVersion(); ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel, - *SdCardManager::instance(), &ipcStore, &tmStore, stores); + *SdCardManager::instance(), &ipcStore, &tmStore, stores, 200, + enableHkSets); LinuxLibgpioIF* gpioComIF = nullptr; SerialComIF* uartComIF = nullptr; SpiComIF* spiMainComIF = nullptr; I2cComIF* i2cComIF = nullptr; createCommunicationInterfaces(&gpioComIF, &uartComIF, &spiMainComIF, &i2cComIF); - /* Adding gpios for chip select decoding to the gpioComIf */ + // Adding GPIOs for chip select decoding and initializing them. q7s::gpioCallbacks::initSpiCsDecoder(gpioComIF); gpioCallbacks::disableAllDecoder(gpioComIF); + createPlI2cResetGpio(gpioComIF); // Hardware is usually not connected to EM, so we need to create dummies which replace lower // level components. dummy::DummyCfg dummyCfg; dummyCfg.addCoreCtrlCfg = false; + dummyCfg.addCamSwitcherDummy = false; #if OBSW_ADD_SYRLINKS == 1 dummyCfg.addSyrlinksDummies = false; #endif +#if OBSW_ADD_PLOC_SUPERVISOR == 1 || OBSW_ADD_PLOC_MPSOC == 1 + dummyCfg.addPlocDummies = false; +#endif +#if OBSW_ADD_TMP_DEVICES == 1 + std::vector> tmpDevsToAdd = {{ + {objects::TMP1075_HANDLER_PLPCDU_0, addresses::TMP1075_PLPCDU_0}, + {objects::TMP1075_HANDLER_PLPCDU_1, addresses::TMP1075_PLPCDU_1}, + {objects::TMP1075_HANDLER_IF_BOARD, addresses::TMP1075_IF_BOARD}, + }}; + createTmpComponents(tmpDevsToAdd); + dummy::Tmp1075Cfg tmpCfg{}; + tmpCfg.addTcsBrd0 = true; + tmpCfg.addTcsBrd1 = true; + tmpCfg.addPlPcdu0 = false; + tmpCfg.addPlPcdu1 = false; + tmpCfg.addIfBrd = false; + dummyCfg.tmp1075Cfg = tmpCfg; +#endif #if OBSW_ADD_GOMSPACE_PCDU == 1 dummyCfg.addPowerDummies = false; + // The ACU broke. + dummyCfg.addOnlyAcuDummy = true; +#endif +#if OBSW_ADD_BPX_BATTERY_HANDLER == 1 + dummyCfg.addBpxBattDummy = false; #endif #if OBSW_ADD_ACS_BOARD == 1 dummyCfg.addAcsBoardDummies = false; @@ -69,27 +97,16 @@ void ObjectFactory::produce(void* args) { #endif satsystem::EIVE_SYSTEM.setI2cRecoveryParams(pwrSwitcher); - dummy::createDummies(dummyCfg, *pwrSwitcher, gpioComIF); + dummy::createDummies(dummyCfg, *pwrSwitcher, gpioComIF, enableHkSets); new CoreController(objects::CORE_CONTROLLER, enableHkSets); - // Regular FM code, does not work for EM if the hardware is not connected - // createPcduComponents(gpioComIF, &pwrSwitcher); - // createPlPcduComponents(gpioComIF, spiMainComIF, pwrSwitcher); - // createSyrlinksComponents(pwrSwitcher); - // createSunSensorComponents(gpioComIF, spiMainComIF, pwrSwitcher, q7s::SPI_DEFAULT_DEV); - // createRtdComponents(q7s::SPI_DEFAULT_DEV, gpioComIF, pwrSwitcher, spiMainComIF); - // createTmpComponents(); - // createSolarArrayDeploymentComponents(); - // createPayloadComponents(gpioComIF); - // createHeaterComponents(gpioComIF, pwrSwitcher, healthTable); - - // TODO: Careful! Switching this on somehow messes with the communication with the ProASIC - // and will cause xsc_boot_copy commands to always boot to 0 0 - // createRadSensorComponent(gpioComIF); + // Initialize chip select to avoid SPI bus issues. + createRadSensorChipSelect(gpioComIF); #if OBSW_ADD_ACS_BOARD == 1 - createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher); + createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher, true, + adis1650x::Type::ADIS16507); #else // Still add all GPIOs for EM. GpioCookie* acsBoardGpios = new GpioCookie(); @@ -97,8 +114,12 @@ void ObjectFactory::produce(void* args) { gpioChecker(gpioComIF->addGpios(acsBoardGpios), "ACS Board"); #endif + const char* battAndImtqI2cDev = q7s::I2C_PL_EIVE; + if (core::FW_VERSION_MAJOR >= 4) { + battAndImtqI2cDev = q7s::I2C_PS_EIVE; + } #if OBSW_ADD_MGT == 1 - createImtqComponents(pwrSwitcher, enableHkSets); + createImtqComponents(pwrSwitcher, enableHkSets, battAndImtqI2cDev); #endif #if OBSW_ADD_SYRLINKS == 1 @@ -110,13 +131,15 @@ void ObjectFactory::produce(void* args) { #endif #if OBSW_ADD_BPX_BATTERY_HANDLER == 1 - createBpxBatteryComponent(enableHkSets); + createBpxBatteryComponent(enableHkSets, battAndImtqI2cDev); #endif #if OBSW_ADD_STAR_TRACKER == 1 createStrComponents(pwrSwitcher); #endif /* OBSW_ADD_STAR_TRACKER == 1 */ + createPayloadComponents(gpioComIF, *pwrSwitcher); + #if OBSW_ADD_CCSDS_IP_CORES == 1 CcsdsIpCoreHandler* ipCoreHandler = nullptr; CcsdsComponentArgs ccsdsArgs(*gpioComIF, *ipcStore, *tmStore, stores, *pusFunnel, *cfdpFunnel, @@ -125,6 +148,7 @@ void ObjectFactory::produce(void* args) { #if OBSW_TM_TO_PTME == 1 if (ccsdsArgs.liveDestination != nullptr) { pusFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); + cfdpFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); } #endif #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ @@ -134,11 +158,11 @@ void ObjectFactory::produce(void* args) { #endif /* OBSW_ADD_TEST_CODE == 1 */ #if OBSW_ADD_SCEX_DEVICE == 1 createScexComponents(q7s::UART_SCEX_DEV, pwrSwitcher, *SdCardManager::instance(), false, - pcdu::Switches::PDU1_CH5_SOLAR_CELL_EXP_5V); + power::Switches::PDU1_CH5_SOLAR_CELL_EXP_5V); #endif createAcsController(true, enableHkSets); HeaterHandler* heaterHandler; createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler); - createThermalController(*heaterHandler); - satsystem::init(); + createThermalController(*heaterHandler, true); + satsystem::init(true); } diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 5cb7c751..e43e3551 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -1,4 +1,6 @@ #include +#include +#include #include #include #include @@ -6,9 +8,9 @@ #include "OBSWConfig.h" #include "bsp_q7s/core/CoreController.h" -#include "bsp_q7s/core/ObjectFactory.h" #include "busConf.h" #include "devConf.h" +#include "devices/addresses.h" #include "eive/objects.h" #include "fsfw_hal/linux/gpio/LinuxLibgpioIF.h" #include "linux/ObjectFactory.h" @@ -31,8 +33,10 @@ void ObjectFactory::produce(void* args) { #endif PersistentTmStores stores; + readFirmwareVersion(); ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel, - *SdCardManager::instance(), &ipcStore, &tmStore, stores); + *SdCardManager::instance(), &ipcStore, &tmStore, stores, 200, + true); LinuxLibgpioIF* gpioComIF = nullptr; SerialComIF* uartComIF = nullptr; @@ -43,6 +47,7 @@ void ObjectFactory::produce(void* args) { /* Adding gpios for chip select decoding to the gpioComIf */ q7s::gpioCallbacks::initSpiCsDecoder(gpioComIF); gpioCallbacks::disableAllDecoder(gpioComIF); + createPlI2cResetGpio(gpioComIF); new CoreController(objects::CORE_CONTROLLER, enableHkSets); createPcduComponents(gpioComIF, &pwrSwitcher, enableHkSets); @@ -58,12 +63,22 @@ void ObjectFactory::produce(void* args) { #endif #if OBSW_ADD_ACS_BOARD == 1 - createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher, true); + createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher, true, + adis1650x::Type::ADIS16505); #endif HeaterHandler* heaterHandler; createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler); #if OBSW_ADD_TMP_DEVICES == 1 - createTmpComponents(); + std::vector> tmpDevsToAdd = {{ + {objects::TMP1075_HANDLER_TCS_0, addresses::TMP1075_TCS_0}, + {objects::TMP1075_HANDLER_TCS_1, addresses::TMP1075_TCS_1}, + {objects::TMP1075_HANDLER_PLPCDU_0, addresses::TMP1075_PLPCDU_0}, + // damaged + // {objects::TMP1075_HANDLER_PLPCDU_1, addresses::TMP1075_PLPCDU_1}, + {objects::TMP1075_HANDLER_IF_BOARD, addresses::TMP1075_IF_BOARD}, + }}; + + createTmpComponents(tmpDevsToAdd); #endif createSolarArrayDeploymentComponents(*pwrSwitcher, *gpioComIF); createPlPcduComponents(gpioComIF, spiMainComIF, pwrSwitcher, *stackHandler); @@ -73,13 +88,17 @@ void ObjectFactory::produce(void* args) { createRtdComponents(q7s::SPI_DEFAULT_DEV, gpioComIF, pwrSwitcher, spiMainComIF); createPayloadComponents(gpioComIF, *pwrSwitcher); + const char* battAndImtqI2cDev = q7s::I2C_PL_EIVE; + if (core::FW_VERSION_MAJOR >= 4) { + battAndImtqI2cDev = q7s::I2C_PS_EIVE; + } #if OBSW_ADD_MGT == 1 - createImtqComponents(pwrSwitcher, enableHkSets); + createImtqComponents(pwrSwitcher, enableHkSets, battAndImtqI2cDev); #endif createReactionWheelComponents(gpioComIF, pwrSwitcher); #if OBSW_ADD_BPX_BATTERY_HANDLER == 1 - createBpxBatteryComponent(enableHkSets); + createBpxBatteryComponent(enableHkSets, battAndImtqI2cDev); #endif #if OBSW_ADD_STAR_TRACKER == 1 @@ -94,6 +113,7 @@ void ObjectFactory::produce(void* args) { #if OBSW_TM_TO_PTME == 1 if (ccsdsArgs.liveDestination != nullptr) { pusFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); + cfdpFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); } #endif #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ @@ -108,7 +128,7 @@ void ObjectFactory::produce(void* args) { #endif /* OBSW_ADD_TEST_CODE == 1 */ createMiscComponents(); - createThermalController(*heaterHandler); + createThermalController(*heaterHandler, false); createAcsController(true, enableHkSets); - satsystem::init(); + satsystem::init(false); } diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/objectFactory.cpp similarity index 90% rename from bsp_q7s/core/ObjectFactory.cpp rename to bsp_q7s/objectFactory.cpp index 964d728b..e6ae6603 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/objectFactory.cpp @@ -1,7 +1,9 @@ -#include "ObjectFactory.h" +#include "objectFactory.h" #include #include +#include +#include #include #include #include @@ -10,10 +12,10 @@ #include #include #include -#include +#include #include #include -#include +#include #include #include #include @@ -31,6 +33,9 @@ #include #include +#include +#include + #include "OBSWConfig.h" #include "bsp_q7s/boardtest/Q7STestTask.h" #include "bsp_q7s/callbacks/gnssCallback.h" @@ -99,6 +104,7 @@ #include +#include "bsp_q7s/core/defs.h" #include "fsfw/datapoollocal/LocalDataPoolManager.h" #include "fsfw/tmtcpacket/pus/tm.h" #include "fsfw/tmtcservices/CommandingServiceBase.h" @@ -129,6 +135,11 @@ ResetArgs RESET_ARGS_GNSS; std::atomic_bool LINK_STATE = CcsdsIpCoreHandler::LINK_DOWN; std::atomic_bool PTME_LOCKED = false; std::atomic_uint16_t I2C_FATAL_ERRORS = 0; +uint8_t core::FW_VERSION_MAJOR = 0; +uint8_t core::FW_VERSION_MINOR = 0; +uint8_t core::FW_VERSION_REVISION = 0; +bool core::FW_VERSION_HAS_SHA = false; +char core::FW_VERSION_GIT_SHA[4] = {}; void Factory::setStaticFrameworkObjectIds() { PusServiceBase::PUS_DISTRIBUTOR = objects::PUS_PACKET_DISTRIBUTOR; @@ -150,28 +161,23 @@ void Factory::setStaticFrameworkObjectIds() { void ObjectFactory::setStatics() { Factory::setStaticFrameworkObjectIds(); } -void ObjectFactory::createTmpComponents() { - std::vector> tmpDevIds = {{ - {objects::TMP1075_HANDLER_TCS_0, addresses::TMP1075_TCS_0}, - {objects::TMP1075_HANDLER_TCS_1, addresses::TMP1075_TCS_1}, - {objects::TMP1075_HANDLER_PLPCDU_0, addresses::TMP1075_PLPCDU_0}, - // damaged - // {objects::TMP1075_HANDLER_PLPCDU_1, addresses::TMP1075_PLPCDU_1}, - {objects::TMP1075_HANDLER_IF_BOARD, addresses::TMP1075_IF_BOARD}, - }}; +void ObjectFactory::createTmpComponents( + std::vector> tmpDevsToAdd) { + const char* tmpI2cDev = q7s::I2C_PS_EIVE; + if (core::FW_VERSION_MAJOR == 4) { + tmpI2cDev = q7s::I2C_PL_EIVE; + } else if (core::FW_VERSION_MAJOR >= 5) { + tmpI2cDev = q7s::I2C_Q7_EIVE; + } std::vector tmpDevCookies; - for (size_t idx = 0; idx < tmpDevIds.size(); idx++) { + for (size_t idx = 0; idx < tmpDevsToAdd.size(); idx++) { tmpDevCookies.push_back( - new I2cCookie(tmpDevIds[idx].second, TMP1075::MAX_REPLY_LENGTH, q7s::I2C_PS_EIVE)); + new I2cCookie(tmpDevsToAdd[idx].second, TMP1075::MAX_REPLY_LENGTH, tmpI2cDev)); auto* tmpDevHandler = - new Tmp1075Handler(tmpDevIds[idx].first, objects::I2C_COM_IF, tmpDevCookies[idx]); - tmpDevHandler->setCustomFdir(new TmpDevFdir(tmpDevIds[idx].first)); + new Tmp1075Handler(tmpDevsToAdd[idx].first, objects::I2C_COM_IF, tmpDevCookies[idx]); + tmpDevHandler->setCustomFdir(new TmpDevFdir(tmpDevsToAdd[idx].first)); tmpDevHandler->connectModeTreeParent(satsystem::tcs::SUBSYSTEM); - // TODO: Remove this after TCS subsystem was added - // These devices are connected to the 3V3 stack and should be powered permanently. Therefore, - // we set them to normal mode immediately here. - tmpDevHandler->setModeNormal(); } } @@ -189,7 +195,6 @@ void ObjectFactory::createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, *i2cComIF = new I2cComIF(objects::I2C_COM_IF); *uartComIF = new SerialComIF(objects::UART_COM_IF); *spiMainComIF = new SpiComIF(objects::SPI_MAIN_COM_IF, q7s::SPI_DEFAULT_DEV, **gpioComIF); - //*spiRWComIF = new SpiComIF(objects::SPI_RW_COM_IF, q7s::SPI_RW_DEV, **gpioComIF); } void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher, @@ -197,7 +202,6 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI CspCookie* p60DockCspCookie = new CspCookie(P60Dock::MAX_REPLY_SIZE, addresses::P60DOCK, 500); CspCookie* pdu1CspCookie = new CspCookie(PDU::MAX_REPLY_SIZE, addresses::PDU1, 500); CspCookie* pdu2CspCookie = new CspCookie(PDU::MAX_REPLY_SIZE, addresses::PDU2, 500); - CspCookie* acuCspCookie = new CspCookie(ACU::MAX_REPLY_SIZE, addresses::ACU, 500); auto p60Fdir = new GomspacePowerFdir(objects::P60DOCK_HANDLER); P60DockHandler* p60dockhandler = new P60DockHandler(objects::P60DOCK_HANDLER, objects::CSP_COM_IF, @@ -211,9 +215,12 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI Pdu2Handler* pdu2handler = new Pdu2Handler(objects::PDU2_HANDLER, objects::CSP_COM_IF, pdu2CspCookie, pdu2Fdir, enableHkSets); +#if OBSW_ADD_GOMSPACE_ACU == 1 + CspCookie* acuCspCookie = new CspCookie(ACU::MAX_REPLY_SIZE, addresses::ACU, 500); auto acuFdir = new GomspacePowerFdir(objects::ACU_HANDLER); ACUHandler* acuhandler = new ACUHandler(objects::ACU_HANDLER, objects::CSP_COM_IF, acuCspCookie, acuFdir, enableHkSets); +#endif auto pcduHandler = new PcduHandler(objects::PCDU_HANDLER, 50); /** @@ -223,7 +230,9 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI p60dockhandler->setModeNormal(); pdu1handler->setModeNormal(); pdu2handler->setModeNormal(); +#if OBSW_ADD_GOMSPACE_ACU == 1 acuhandler->setModeNormal(); +#endif if (pwrSwitcher != nullptr) { *pwrSwitcher = pcduHandler; } @@ -237,20 +246,7 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI ReturnValue_t ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& stackHandler) { - using namespace gpio; - if (gpioComIF == nullptr) { - return returnvalue::FAILED; - } - GpioCookie* gpioCookieRadSensor = new GpioCookie; - std::stringstream consumer; - consumer << "0x" << std::hex << objects::RAD_SENSOR; - GpiodRegularByLineName* gpio = new GpiodRegularByLineName( - q7s::gpioNames::RAD_SENSOR_CHIP_SELECT, consumer.str(), Direction::OUT, Levels::HIGH); - gpioCookieRadSensor->addGpio(gpioIds::CS_RAD_SENSOR, gpio); - gpio = new GpiodRegularByLineName(q7s::gpioNames::ENABLE_RADFET, consumer.str(), Direction::OUT, - Levels::LOW); - gpioCookieRadSensor->addGpio(gpioIds::ENABLE_RADFET, gpio); - gpioChecker(gpioComIF->addGpios(gpioCookieRadSensor), "RAD sensor"); + createRadSensorChipSelect(gpioComIF); SpiCookie* spiCookieRadSensor = new SpiCookie(addresses::RAD_SENSOR, gpioIds::CS_RAD_SENSOR, RAD_SENSOR::READ_SIZE, @@ -364,7 +360,7 @@ void ObjectFactory::createAcsBoardGpios(GpioCookie& cookie) { void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, PowerSwitchIF& pwrSwitcher, - bool enableHkSets) { + bool enableHkSets, adis1650x::Type adisType) { using namespace gpio; GpioCookie* gpioCookieAcsBoard = new GpioCookie(); createAcsBoardGpios(*gpioCookieAcsBoard); @@ -450,9 +446,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* 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 GyrAdis1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK, - spiCookie, adis1650x::Type::ADIS16505); + auto adisHandler = new GyrAdis1650XHandler(objects::GYRO_0_ADIS_HANDLER, + objects::ACS_BOARD_POLLING_TASK, spiCookie, adisType); fdir = new AcsBoardFdir(objects::GYRO_0_ADIS_HANDLER); adisHandler->setCustomFdir(fdir); assemblyChildren[4] = adisHandler; @@ -485,9 +480,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* 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 GyrAdis1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK, - spiCookie, adis1650x::Type::ADIS16505); + adisHandler = new GyrAdis1650XHandler(objects::GYRO_2_ADIS_HANDLER, + objects::ACS_BOARD_POLLING_TASK, spiCookie, adisType); fdir = new AcsBoardFdir(objects::GYRO_2_ADIS_HANDLER); adisHandler->setCustomFdir(fdir); assemblyChildren[6] = adisHandler; @@ -517,7 +511,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* debugGps = true; #endif RESET_ARGS_GNSS.gpioComIF = gpioComIF; - RESET_ARGS_GNSS.waitPeriodMs = 100; + RESET_ARGS_GNSS.waitPeriodMs = 5; auto gpsCtrl = new GpsHyperionLinuxController(objects::GPS_CONTROLLER, objects::NO_OBJECT, enableHkSets, debugGps); gpsCtrl->setResetPinTriggerFunction(gps::triggerGpioResetPin, &RESET_ARGS_GNSS); @@ -635,8 +629,8 @@ void ObjectFactory::createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwit new SerialCookie(objects::PLOC_MPSOC_HANDLER, q7s::UART_PLOC_MPSOC_DEV, serial::PLOC_MPSOC_BAUD, mpsoc::MAX_REPLY_SIZE, UartModes::NON_CANONICAL); mpsocCookie->setNoFixedSizeReply(); - auto plocMpsocHelper = new PlocMPSoCHelper(objects::PLOC_MPSOC_HELPER); - auto* mpsocHandler = new PlocMPSoCHandler( + auto plocMpsocHelper = new PlocMpsocSpecialComHelper(objects::PLOC_MPSOC_HELPER); + auto* mpsocHandler = new PlocMpsocHandler( objects::PLOC_MPSOC_HANDLER, objects::UART_COM_IF, mpsocCookie, plocMpsocHelper, Gpio(gpioIds::ENABLE_MPSOC_UART, gpioComIF), objects::PLOC_SUPERVISOR_HANDLER); mpsocHandler->connectModeTreeParent(satsystem::payload::SUBSYSTEM); @@ -656,6 +650,7 @@ void ObjectFactory::createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwit auto* supvHandler = new PlocSupervisorHandler(objects::PLOC_SUPERVISOR_HANDLER, supervisorCookie, Gpio(gpioIds::ENABLE_SUPV_UART, gpioComIF), power::PDU1_CH6_PLOC_12V, *supvHelper); + supvHandler->setPowerSwitcher(&pwrSwitch); supvHandler->connectModeTreeParent(satsystem::payload::SUBSYSTEM); #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ static_cast(consumer); @@ -741,20 +736,12 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) { // GPIO definitions of signals connected to the virtual channel interfaces of the PTME IP Core GpioCookie* gpioCookiePtmeIp = new GpioCookie; GpiodRegularByLineName* gpio = nullptr; - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC0, "PAPB VC0"); - gpioCookiePtmeIp->addGpio(gpioIds::VC0_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC0, "PAPB VC0"); gpioCookiePtmeIp->addGpio(gpioIds::VC0_PAPB_EMPTY, gpio); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC1, "PAPB VC1"); - gpioCookiePtmeIp->addGpio(gpioIds::VC1_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC1, "PAPB VC1"); gpioCookiePtmeIp->addGpio(gpioIds::VC1_PAPB_EMPTY, gpio); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC2, "PAPB VC2"); - gpioCookiePtmeIp->addGpio(gpioIds::VC2_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC2, "PAPB VC2"); gpioCookiePtmeIp->addGpio(gpioIds::VC2_PAPB_EMPTY, gpio); - gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC3, "PAPB VC3"); - gpioCookiePtmeIp->addGpio(gpioIds::VC3_PAPB_BUSY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC3, "PAPB VC3"); gpioCookiePtmeIp->addGpio(gpioIds::VC3_PAPB_EMPTY, gpio); gpio = new GpiodRegularByLineName(q7s::gpioNames::PTME_RESETN, "PTME RESETN", @@ -763,18 +750,14 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) { gpioChecker(args.gpioComIF.addGpios(gpioCookiePtmeIp), "PTME PAPB VCs"); // Creating virtual channel interfaces - 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); + VirtualChannelIF* vc0 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC0_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC0); + VirtualChannelIF* vc1 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC1_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC1); + VirtualChannelIF* vc2 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC2_PAPB_EMPTY, + q7s::UIO_PTME, q7s::uiomapids::PTME_VC2); + VirtualChannelIF* vc3 = new PapbVcInterface(&args.gpioComIF, 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); @@ -816,7 +799,7 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) { // Core task which handles the HK store and takes care of dumping it as TM using a VC directly auto* hkStore = new PersistentSingleTmStoreTask( objects::HK_STORE_AND_TM_TASK, args.ipcStore, *args.stores.hkStore, *vc, - persTmStore::DUMP_HK_STORE_DONE, persTmStore::DUMP_HK_STORE_DONE, *SdCardManager::instance(), + persTmStore::DUMP_HK_STORE_DONE, persTmStore::DUMP_HK_CANCELLED, *SdCardManager::instance(), PTME_LOCKED); hkStore->connectModeTreeParent(satsystem::com::SUBSYSTEM); @@ -976,12 +959,13 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher) { starTracker->setCustomFdir(strFdir); } -void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enableHkSets) { +void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enableHkSets, + const char* i2cDev) { auto* imtqAssy = new ImtqAssembly(objects::IMTQ_ASSY); imtqAssy->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); new ImtqPollingTask(objects::IMTQ_POLLING, I2C_FATAL_ERRORS); - I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, imtq::MAX_REPLY_SIZE, q7s::I2C_PL_EIVE); + I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, imtq::MAX_REPLY_SIZE, i2cDev); auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::IMTQ_POLLING, imtqI2cCookie, power::Switches::PDU1_CH3_MGT_5V, enableHkSets); imtqHandler->enableThermalModule(ThermalStateCfg()); @@ -997,8 +981,8 @@ void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enable #endif } -void ObjectFactory::createBpxBatteryComponent(bool enableHkSets) { - I2cCookie* bpxI2cCookie = new I2cCookie(addresses::BPX_BATTERY, 100, q7s::I2C_PL_EIVE); +void ObjectFactory::createBpxBatteryComponent(bool enableHkSets, const char* i2cDev) { + I2cCookie* bpxI2cCookie = new I2cCookie(addresses::BPX_BATTERY, 100, i2cDev); BpxBatteryHandler* bpxHandler = new BpxBatteryHandler( objects::BPX_BATT_HANDLER, objects::I2C_COM_IF, bpxI2cCookie, enableHkSets); bpxHandler->setStartUpImmediately(); @@ -1019,3 +1003,67 @@ void ObjectFactory::testAcsBrdAss(AcsBoardAssembly* acsAss) { sif::warning << "Sending mode command failed" << std::endl; } } + +void ObjectFactory::createRadSensorChipSelect(LinuxLibgpioIF* gpioIF) { + using namespace gpio; + if (gpioIF == nullptr) { + return; + } + GpioCookie* gpioCookieRadSensor = new GpioCookie; + std::stringstream consumer; + consumer << "0x" << std::hex << objects::RAD_SENSOR; + GpiodRegularByLineName* gpio = new GpiodRegularByLineName( + q7s::gpioNames::RAD_SENSOR_CHIP_SELECT, consumer.str(), Direction::OUT, Levels::HIGH); + gpioCookieRadSensor->addGpio(gpioIds::CS_RAD_SENSOR, gpio); + gpio = new GpiodRegularByLineName(q7s::gpioNames::ENABLE_RADFET, consumer.str(), Direction::OUT, + Levels::LOW); + gpioCookieRadSensor->addGpio(gpioIds::ENABLE_RADFET, gpio); + gpioChecker(gpioIF->addGpios(gpioCookieRadSensor), "RAD sensor"); +} + +void ObjectFactory::createPlI2cResetGpio(LinuxLibgpioIF* gpioIF) { + using namespace gpio; + if (common::OBSW_VERSION_MAJOR >= 6 or common::OBSW_VERSION_MAJOR == 4) { + if (gpioIF == nullptr) { + return; + } + GpioCookie* gpioI2cResetnCookie = new GpioCookie; + GpiodRegularByLineName* gpioI2cResetn = new GpiodRegularByLineName( + q7s::gpioNames::PL_I2C_ARESETN, "PL_I2C_ARESETN", Direction::OUT, Levels::HIGH); + gpioI2cResetnCookie->addGpio(gpioIds::PL_I2C_ARESETN, gpioI2cResetn); + gpioChecker(gpioIF->addGpios(gpioI2cResetnCookie), "PL I2C ARESETN"); + // Reset I2C explicitely again. + gpioIF->pullLow(gpioIds::PL_I2C_ARESETN); + TaskFactory::delayTask(1); + gpioIF->pullHigh(gpioIds::PL_I2C_ARESETN); + } +} + +ReturnValue_t ObjectFactory::readFirmwareVersion() { + uint32_t* mappedSysRomAddr = nullptr; + // The SYS ROM FPGA block is only available in those versions. + if (not(common::OBSW_VERSION_MAJOR >= 6) or (common::OBSW_VERSION_MAJOR == 4)) { + return returnvalue::OK; + } + // This has to come before the version announce because it might be required for retrieving + // the firmware version. + UioMapper sysRomMapper(q7s::UIO_SYS_ROM); + ReturnValue_t result = + sysRomMapper.getMappedAdress(&mappedSysRomAddr, UioMapper::Permissions::READ_ONLY); + if (result != returnvalue::OK) { + sif::error << "Getting mapped SYS ROM UIO address failed" << std::endl; + return returnvalue::FAILED; + } + if (mappedSysRomAddr != nullptr) { + uint32_t firstEntry = *(reinterpret_cast(mappedSysRomAddr)); + uint32_t secondEntry = *(reinterpret_cast(mappedSysRomAddr) + 1); + core::FW_VERSION_MAJOR = (firstEntry >> 24) & 0xff; + core::FW_VERSION_MINOR = (firstEntry >> 16) & 0xff; + core::FW_VERSION_REVISION = (firstEntry >> 8) & 0xff; + bool hasGitSha = (firstEntry & 0x0b1); + if (hasGitSha) { + std::memcpy(core::FW_VERSION_GIT_SHA, &secondEntry, 4); + } + } + return returnvalue::OK; +} diff --git a/bsp_q7s/core/ObjectFactory.h b/bsp_q7s/objectFactory.h similarity index 87% rename from bsp_q7s/core/ObjectFactory.h rename to bsp_q7s/objectFactory.h index b5685389..491720ac 100644 --- a/bsp_q7s/core/ObjectFactory.h +++ b/bsp_q7s/objectFactory.h @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -57,24 +58,28 @@ void createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher bool enableHkSets); void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF, PowerSwitchIF* pwrSwitcher, Stack5VHandler& stackHandler); -void createTmpComponents(); +void createTmpComponents(std::vector> tmpDevsToAdd); +void createRadSensorChipSelect(LinuxLibgpioIF* gpioIF); ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler); void createAcsBoardGpios(GpioCookie& cookie); void createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, - PowerSwitchIF& pwrSwitcher, bool enableHkSets); + PowerSwitchIF& pwrSwitcher, bool enableHkSets, + adis1650x::Type adisType); void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTableIF* healthTable, HeaterHandler*& heaterHandler); -void createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enableHkSets); -void createBpxBatteryComponent(bool enableHkSets); +void createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enableHkSets, const char* i2cDev); +void createBpxBatteryComponent(bool enableHkSets, const char* i2cDev); void createStrComponents(PowerSwitchIF* pwrSwitcher); void createSolarArrayDeploymentComponents(PowerSwitchIF& pwrSwitcher, GpioIF& gpioIF); void createSyrlinksComponents(PowerSwitchIF* pwrSwitcher); void createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF& pwrSwitcher); void createReactionWheelComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF* pwrSwitcher); ReturnValue_t createCcsdsComponents(CcsdsComponentArgs& args); +ReturnValue_t readFirmwareVersion(); void createMiscComponents(); void createTestComponents(LinuxLibgpioIF* gpioComIF); +void createPlI2cResetGpio(LinuxLibgpioIF* gpioComIF); void testAcsBrdAss(AcsBoardAssembly* assAss); diff --git a/bsp_q7s/obsw.cpp b/bsp_q7s/obsw.cpp index dc547b03..fc49c369 100644 --- a/bsp_q7s/obsw.cpp +++ b/bsp_q7s/obsw.cpp @@ -11,13 +11,13 @@ #include "OBSWConfig.h" #include "bsp_q7s/core/WatchdogHandler.h" #include "commonConfig.h" -#include "core/scheduling.h" #include "fsfw/tasks/TaskFactory.h" #include "fsfw/version.h" #include "mission/acs/defs.h" #include "mission/com/defs.h" #include "mission/system/systemTree.h" #include "q7sConfig.h" +#include "scheduling.h" #include "watchdog/definitions.h" static constexpr int OBSW_ALREADY_RUNNING = -2; diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/scheduling.cpp similarity index 97% rename from bsp_q7s/core/scheduling.cpp rename to bsp_q7s/scheduling.cpp index f2cd8229..d7c179ad 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/scheduling.cpp @@ -9,7 +9,6 @@ #include #include "OBSWConfig.h" -#include "bsp_q7s/core/ObjectFactory.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/objectmanager/ObjectManagerIF.h" #include "fsfw/platform.h" @@ -21,6 +20,8 @@ #include "mission/pollingSeqTables.h" #include "mission/scheduling.h" #include "mission/utility/InitMission.h" +#include "objectFactory.h" +#include "q7sConfig.h" /* This is configured for linux without CR */ #ifdef PLATFORM_UNIX @@ -324,6 +325,10 @@ void scheduling::initTasks() { if (result != returnvalue::OK) { scheduling::printAddObjectError("HEATER_HANDLER", objects::HEATER_HANDLER); } + result = tcsSystemTask->addComponent(objects::HEATER_HANDLER); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("HEATER_HANDLER", objects::HEATER_HANDLER); + } #if OBSW_ADD_SYRLINKS == 1 PeriodicTaskIF* syrlinksCom = factory->createPeriodicTask( @@ -345,7 +350,6 @@ void scheduling::initTasks() { } #endif /* OBSW_ADD_STAR_TRACKER == 1 */ - // TODO: Use regular scheduler for this task #if OBSW_ADD_PLOC_MPSOC == 1 PeriodicTaskIF* mpsocHelperTask = factory->createPeriodicTask( "PLOC_MPSOC_HELPER", 0, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); @@ -366,7 +370,7 @@ void scheduling::initTasks() { #endif /* OBSW_ADD_PLOC_SUPERVISOR */ PeriodicTaskIF* plTask = factory->createPeriodicTask( - "PL_TASK", 25, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc, &RR_SCHEDULING); + "PL_TASK", 25, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.5, missedDeadlineFunc, &RR_SCHEDULING); plTask->addComponent(objects::CAM_SWITCHER); scheduling::addMpsocSupvHandlers(plTask); scheduling::scheduleScexDev(plTask); @@ -472,6 +476,9 @@ void scheduling::initTasks() { #if OBSW_ADD_PLOC_SUPERVISOR == 1 supvHelperTask->startTask(); #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ +#if OBSW_ADD_PLOC_MPSOC == 1 + mpsocHelperTask->startTask(); +#endif plTask->startTask(); #if OBSW_ADD_TEST_CODE == 1 @@ -525,7 +532,15 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction FixedTimeslotTaskIF* i2cPst = factory.createFixedTimeslotTask("I2C_PS_PST", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.6, missedDeadlineFunc, &RR_SCHEDULING); - result = pst::pstI2cProcessingSystem(i2cPst); + pst::TmpSchedConfig tmpSchedConf; +#if OBSW_Q7S_EM == 1 + tmpSchedConf.scheduleTmpDev0 = true; + tmpSchedConf.scheduleTmpDev1 = true; + tmpSchedConf.schedulePlPcduDev0 = true; + tmpSchedConf.schedulePlPcduDev1 = true; + tmpSchedConf.scheduleIfBoardDev = true; +#endif + result = pst::pstI2c(tmpSchedConf, i2cPst); if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { sif::warning << "scheduling::initTasks: I2C PST is empty" << std::endl; diff --git a/bsp_q7s/core/scheduling.h b/bsp_q7s/scheduling.h similarity index 100% rename from bsp_q7s/core/scheduling.h rename to bsp_q7s/scheduling.h diff --git a/bsp_q7s/xadc/Xadc.cpp b/bsp_q7s/xadc/Xadc.cpp index 34a4e159..da3fbf7f 100644 --- a/bsp_q7s/xadc/Xadc.cpp +++ b/bsp_q7s/xadc/Xadc.cpp @@ -129,7 +129,7 @@ ReturnValue_t Xadc::readValFromFile(const char* filename, T& val) { sif::warning << "Xadc::readValFromFile: Failed to open file " << filename << std::endl; return returnvalue::FAILED; } - char valstring[MAX_STR_LENGTH] = ""; + char valstring[MAX_STR_LENGTH]{}; char* returnVal = fgets(valstring, MAX_STR_LENGTH, fp); if (returnVal == nullptr) { sif::warning << "Xadc::readValFromFile: Failed to read string from file " << filename @@ -139,6 +139,11 @@ ReturnValue_t Xadc::readValFromFile(const char* filename, T& val) { } std::istringstream valSstream(valstring); valSstream >> val; + if (valSstream.bad()) { + sif::warning << "Xadc: Conversion of value to target type failed" << std::endl; + fclose(fp); + return returnvalue::FAILED; + } fclose(fp); return returnvalue::OK; } diff --git a/cmake/Zynq7020CrossCompileConfig.cmake b/cmake/Zynq7020CrossCompileConfig.cmake index be6702a1..5e269f1a 100644 --- a/cmake/Zynq7020CrossCompileConfig.cmake +++ b/cmake/Zynq7020CrossCompileConfig.cmake @@ -40,8 +40,8 @@ set(CROSS_COMPILE_OBJCOPY "${CROSS_COMPILE}-objcopy") set(CROSS_COMPILE_SIZE "${CROSS_COMPILE}-size") # At the very least, cross compile gcc and g++ have to be set! -find_program (CMAKE_C_COMPILER ${CROSS_COMPILE_CC} REQUIRED) -find_program (CMAKE_CXX_COMPILER ${CROSS_COMPILE_CXX} REQUIRED) +find_program (CMAKE_C_COMPILER ${CROSS_COMPILE_CC} HINTS $ENV{CROSS_COMPILE_BIN_PATH} REQUIRED) +find_program (CMAKE_CXX_COMPILER ${CROSS_COMPILE_CXX} HINTS $ENV{CROSS_COMPILE_BIN_PATH} REQUIRED) # Useful utilities, not strictly necessary find_program(CMAKE_SIZE ${CROSS_COMPILE_SIZE}) find_program(CMAKE_OBJCOPY ${CROSS_COMPILE_OBJCOPY}) diff --git a/common/config/devices/gpioIds.h b/common/config/devices/gpioIds.h index 640f4ead..bed82142 100644 --- a/common/config/devices/gpioIds.h +++ b/common/config/devices/gpioIds.h @@ -77,6 +77,8 @@ enum gpioId_t { CS_RAD_SENSOR, ENABLE_RADFET, + PL_I2C_ARESETN, + PAPB_BUSY_N, PAPB_EMPTY, @@ -93,15 +95,10 @@ enum gpioId_t { EN_RW_CS, SPI_MUX, - VC0_PAPB_EMPTY, - VC0_PAPB_BUSY, VC1_PAPB_EMPTY, - VC1_PAPB_BUSY, VC2_PAPB_EMPTY, - VC2_PAPB_BUSY, VC3_PAPB_EMPTY, - VC3_PAPB_BUSY, PTME_RESETN, PDEC_RESET, diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h index 8c460f53..b369f512 100644 --- a/common/config/eive/definitions.h +++ b/common/config/eive/definitions.h @@ -11,6 +11,8 @@ static constexpr char SD_1_MOUNT_POINT[] = "/mnt/sd1"; static constexpr char OBSW_UPDATE_ARCHIVE_FILE_NAME[] = "eive-sw-update.tar.xz"; static constexpr char STRIPPED_OBSW_BINARY_FILE_NAME[] = "eive-obsw-stripped"; static constexpr char OBSW_VERSION_FILE_NAME[] = "obsw_version.txt"; +static constexpr char PUS_SEQUENCE_COUNT_FILE[] = "pus-sequence-count.txt"; +static constexpr char CFDP_SEQUENCE_COUNT_FILE[] = "cfdp-sequence-count.txt"; static constexpr char OBSW_PATH[] = "/usr/bin/eive-obsw"; static constexpr char OBSW_VERSION_FILE_PATH[] = "/usr/share/eive-obsw/obsw_version.txt"; @@ -34,8 +36,8 @@ static constexpr uint32_t STR_IMG_HELPER_QUEUE_SIZE = 50; static constexpr uint8_t LIVE_TM = 0; /* Limits for filename and path checks */ -static constexpr uint32_t MAX_PATH_SIZE = 100; -static constexpr uint32_t MAX_FILENAME_SIZE = 50; +static constexpr uint32_t MAX_PATH_SIZE = 200; +static constexpr uint32_t MAX_FILENAME_SIZE = 100; static constexpr uint32_t SA_DEPL_INIT_BUFFER_SECS = 120; // Burn time for autonomous deployment @@ -58,7 +60,9 @@ 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 VERIFICATION_SERVICE_QUEUE_DEPTH = 120; static constexpr uint32_t HK_SERVICE_QUEUE_DEPTH = 60; +static constexpr uint32_t ACTION_SERVICE_QUEUE_DEPTH = 60; static constexpr uint32_t MAX_STORED_CMDS_UDP = 150; static constexpr uint32_t MAX_STORED_CMDS_TCP = 180; diff --git a/dummies/AcuDummy.cpp b/dummies/AcuDummy.cpp index 7c18f6bf..c9844eb1 100644 --- a/dummies/AcuDummy.cpp +++ b/dummies/AcuDummy.cpp @@ -2,8 +2,11 @@ #include -AcuDummy::AcuDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) - : DeviceHandlerBase(objectId, comif, comCookie) {} +AcuDummy::AcuDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, bool enableHkSets) + : DeviceHandlerBase(objectId, comif, comCookie), + coreHk(this), + auxHk(this), + enableHkSets(enableHkSets) {} AcuDummy::~AcuDummy() {} @@ -37,7 +40,49 @@ uint32_t AcuDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return ReturnValue_t AcuDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + using namespace ACU; + localDataPoolMap.emplace(pool::ACU_CURRENT_IN_CHANNELS, new PoolEntry(6)); + localDataPoolMap.emplace(pool::ACU_VOLTAGE_IN_CHANNELS, new PoolEntry(6)); + + localDataPoolMap.emplace(pool::ACU_VCC, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_VBAT, new PoolEntry({0})); + localDataPoolMap.emplace(ACU::pool::ACU_TEMPERATURES, new PoolEntry({10.0, 10.0, 10.0}, true)); + + localDataPoolMap.emplace(pool::ACU_MPPT_MODE, new PoolEntry({0})); + + localDataPoolMap.emplace(pool::ACU_VBOOST_IN_CHANNELS, new PoolEntry(6)); + localDataPoolMap.emplace(pool::ACU_POWER_IN_CHANNELS, new PoolEntry(6)); + + localDataPoolMap.emplace(pool::ACU_DAC_ENABLES, new PoolEntry(3)); + localDataPoolMap.emplace(pool::ACU_DAC_RAW_CHANNELS, new PoolEntry(6)); + + localDataPoolMap.emplace(pool::ACU_BOOTCAUSE, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_BOOTCNT, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_UPTIME, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_RESET_CAUSE, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_MPPT_TIME, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_MPPT_PERIOD, new PoolEntry({0})); + + localDataPoolMap.emplace(pool::ACU_DEVICES, new PoolEntry(8)); + localDataPoolMap.emplace(pool::ACU_DEVICES_STATUS, new PoolEntry(8)); + + localDataPoolMap.emplace(pool::ACU_WDT_CNT_GND, new PoolEntry({0})); + localDataPoolMap.emplace(pool::ACU_WDT_GND_LEFT, new PoolEntry({0})); + + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), enableHkSets, 30.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(auxHk.getSid(), enableHkSets, 6000.0)); return returnvalue::OK; } + +LocalPoolDataSetBase *AcuDummy::getDataSetHandle(sid_t sid) { + if (sid == coreHk.getSid()) { + return &coreHk; + } else if (sid == auxHk.getSid()) { + return &auxHk; + } + return nullptr; +} diff --git a/dummies/AcuDummy.h b/dummies/AcuDummy.h index d5527222..8d855281 100644 --- a/dummies/AcuDummy.h +++ b/dummies/AcuDummy.h @@ -2,6 +2,7 @@ #define DUMMIES_ACUDUMMY_H_ #include +#include class AcuDummy : public DeviceHandlerBase { public: @@ -11,10 +12,14 @@ class AcuDummy : public DeviceHandlerBase { static const uint8_t SIMPLE_COMMAND_DATA = 1; static const uint8_t PERIODIC_REPLY_DATA = 2; - AcuDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie); + AcuDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, bool enableHkSets); virtual ~AcuDummy(); protected: + ACU::CoreHk coreHk; + ACU::AuxHk auxHk; + bool enableHkSets; + void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; @@ -28,6 +33,7 @@ class AcuDummy : 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; }; #endif /* DUMMIES_ACUDUMMY_H_ */ diff --git a/dummies/CoreControllerDummy.cpp b/dummies/CoreControllerDummy.cpp index 8a027dbf..df2bef03 100644 --- a/dummies/CoreControllerDummy.cpp +++ b/dummies/CoreControllerDummy.cpp @@ -1,6 +1,6 @@ #include "CoreControllerDummy.h" -#include +#include #include #include diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index b2f61bb3..8570a9be 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -2,15 +2,34 @@ #include -ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) - : DeviceHandlerBase(objectId, comif, comCookie) {} +ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, + power::Switch_t pwrSwitcher, bool enableHkSets) + : DeviceHandlerBase(objectId, comif, comCookie), + enableHkSets(enableHkSets), + statusSet(this), + dipoleSet(*this), + rawMtmNoTorque(this), + hkDatasetNoTorque(this), + rawMtmWithTorque(this), + hkDatasetWithTorque(this), + calMtmMeasurementSet(this), + switcher(pwrSwitcher) {} ImtqDummy::~ImtqDummy() = default; -void ImtqDummy::doStartUp() { setMode(MODE_NORMAL); } +void ImtqDummy::doStartUp() { setMode(MODE_ON); } void ImtqDummy::doShutDown() { setMode(_MODE_POWER_DOWN); } +ReturnValue_t ImtqDummy::getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches) { + if (switcher != power::NO_SWITCH) { + *numberOfSwitches = 1; + *switches = &switcher; + return returnvalue::OK; + } + return DeviceHandlerBase::NO_SWITCH; +} + ReturnValue_t ImtqDummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; } ReturnValue_t ImtqDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { @@ -45,5 +64,57 @@ ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataP localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, new PoolEntry({0})); localDataPoolMap.emplace(imtq::DIPOLES_ID, new PoolEntry({0, 0, 0})); localDataPoolMap.emplace(imtq::CURRENT_TORQUE_DURATION, new PoolEntry({0})); + + // 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_CURRENTS, &coilCurrentsMilliampsNoTorque); + localDataPoolMap.emplace(imtq::COIL_TEMPERATURES, &coilTempsNoTorque); + localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry({0})); + + // 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})); + + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(hkDatasetNoTorque.getSid(), enableHkSets, 30.0)); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(hkDatasetWithTorque.getSid(), enableHkSets, 30.0)); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(rawMtmNoTorque.getSid(), false, 10.0)); + poolManager.subscribeForDiagPeriodicPacket( + 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); } + +LocalPoolDataSetBase *ImtqDummy::getDataSetHandle(sid_t sid) { + if (sid == hkDatasetNoTorque.getSid()) { + 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()) { + return &rawMtmWithTorque; + } else if (sid == calMtmMeasurementSet.getSid()) { + return &calMtmMeasurementSet; + } else if (sid == rawMtmNoTorque.getSid()) { + return &rawMtmNoTorque; + } + return nullptr; +} diff --git a/dummies/ImtqDummy.h b/dummies/ImtqDummy.h index 0cfdf518..8c76affc 100644 --- a/dummies/ImtqDummy.h +++ b/dummies/ImtqDummy.h @@ -3,6 +3,8 @@ #include +#include "mission/acs/imtqHelpers.h" + class ImtqDummy : public DeviceHandlerBase { public: static const DeviceCommandId_t SIMPLE_COMMAND = 1; @@ -11,10 +13,44 @@ class ImtqDummy : public DeviceHandlerBase { static const uint8_t SIMPLE_COMMAND_DATA = 1; static const uint8_t PERIODIC_REPLY_DATA = 2; - ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie); + ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie, + power::Switch_t pwrSwitcher, bool enableHkSets); ~ImtqDummy() override; protected: + bool enableHkSets; + + imtq::StatusDataset statusSet; + imtq::DipoleActuationSet dipoleSet; + imtq::RawMtmMeasurementNoTorque rawMtmNoTorque; + imtq::HkDatasetNoTorque hkDatasetNoTorque; + + imtq::RawMtmMeasurementWithTorque rawMtmWithTorque; + imtq::HkDatasetWithTorque hkDatasetWithTorque; + + imtq::CalibratedMtmMeasurementSet calMtmMeasurementSet; + + PoolEntry statusMode = PoolEntry({0}); + PoolEntry statusError = PoolEntry({0}); + PoolEntry statusConfig = PoolEntry({0}); + PoolEntry statusUptime = PoolEntry({0}); + + PoolEntry mgmCalEntry = PoolEntry(3); + 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; + + ReturnValue_t getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches) override; + void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; @@ -28,6 +64,7 @@ class ImtqDummy : 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; }; #endif /* DUMMIES_IMTQDUMMY_H_ */ diff --git a/dummies/Max31865Dummy.cpp b/dummies/Max31865Dummy.cpp index 6ec1a716..99fc336a 100644 --- a/dummies/Max31865Dummy.cpp +++ b/dummies/Max31865Dummy.cpp @@ -7,15 +7,21 @@ using namespace returnvalue; Max31865Dummy::Max31865Dummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie), set(this, EiveMax31855::EXCHANGE_SET_ID) {} void Max31865Dummy::doStartUp() { setMode(MODE_ON); } -void Max31865Dummy::doShutDown() { setMode(_MODE_POWER_DOWN); } +void Max31865Dummy::doShutDown() { + PoolReadGuard pg(&set); + set.setValidity(false, true); + setMode(MODE_OFF); +} ReturnValue_t Max31865Dummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; } -ReturnValue_t Max31865Dummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { return OK; } +ReturnValue_t Max31865Dummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + return NOTHING_TO_SEND; +} ReturnValue_t Max31865Dummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return 0; + return NOTHING_TO_SEND; } ReturnValue_t Max31865Dummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { diff --git a/dummies/RwDummy.cpp b/dummies/RwDummy.cpp index 0f97f24a..0bf4db42 100644 --- a/dummies/RwDummy.cpp +++ b/dummies/RwDummy.cpp @@ -3,13 +3,24 @@ #include RwDummy::RwDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) - : DeviceHandlerBase(objectId, comif, comCookie) {} + : DeviceHandlerBase(objectId, comif, comCookie), + + statusSet(this), + lastResetStatusSet(this), + tmDataset(this), + rwSpeedActuationSet(*this) {} RwDummy::~RwDummy() {} -void RwDummy::doStartUp() { setMode(MODE_ON); } +void RwDummy::doStartUp() { + statusSet.setReportingEnabled(true); + setMode(MODE_ON); +} -void RwDummy::doShutDown() { setMode(MODE_OFF); } +void RwDummy::doShutDown() { + statusSet.setReportingEnabled(false); + setMode(MODE_OFF); +} ReturnValue_t RwDummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; } @@ -37,6 +48,9 @@ uint32_t RwDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return ReturnValue_t RwDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(rws::RW_SPEED, &rwSpeed); + localDataPoolMap.emplace(rws::RAMP_TIME, &rampTime); + localDataPoolMap.emplace(rws::TEMPERATURE_C, new PoolEntry({0})); localDataPoolMap.emplace(rws::CURR_SPEED, new PoolEntry({0})); @@ -71,5 +85,11 @@ ReturnValue_t RwDummy::initializeLocalDataPool(localpool::DataPool &localDataPoo localDataPoolMap.emplace(rws::SPI_BYTES_READ, new PoolEntry({0})); localDataPoolMap.emplace(rws::SPI_REG_OVERRUN_ERRORS, new PoolEntry({0})); localDataPoolMap.emplace(rws::SPI_TOTAL_ERRORS, new PoolEntry({0})); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(statusSet.getSid(), false, 12.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(tmDataset.getSid(), false, 30.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(lastResetStatusSet.getSid(), false, 30.0)); return returnvalue::OK; } diff --git a/dummies/RwDummy.h b/dummies/RwDummy.h index e5738420..03629937 100644 --- a/dummies/RwDummy.h +++ b/dummies/RwDummy.h @@ -2,6 +2,7 @@ #define DUMMIES_RWDUMMY_H_ #include +#include class RwDummy : public DeviceHandlerBase { public: @@ -15,6 +16,14 @@ class RwDummy : public DeviceHandlerBase { virtual ~RwDummy(); protected: + rws::StatusSet statusSet; + rws::LastResetSatus lastResetStatusSet; + rws::TmDataset tmDataset; + rws::RwSpeedActuationSet rwSpeedActuationSet; + + PoolEntry rwSpeed = PoolEntry({0}); + PoolEntry rampTime = PoolEntry({10}); + void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; diff --git a/dummies/TemperatureSensorInserter.cpp b/dummies/TemperatureSensorInserter.cpp index cfba0ef9..942231f5 100644 --- a/dummies/TemperatureSensorInserter.cpp +++ b/dummies/TemperatureSensorInserter.cpp @@ -7,9 +7,9 @@ #include #include -TemperatureSensorInserter::TemperatureSensorInserter(object_id_t objectId, - Max31865DummyMap tempSensorDummies_, - Tmp1075DummyMap tempTmpSensorDummies_) +TemperatureSensorInserter::TemperatureSensorInserter( + object_id_t objectId, Max31865DummyMap tempSensorDummies_, + std::optional tempTmpSensorDummies_) : SystemObject(objectId), max31865DummyMap(std::move(tempSensorDummies_)), tmp1075DummyMap(std::move(tempTmpSensorDummies_)) {} @@ -25,8 +25,10 @@ ReturnValue_t TemperatureSensorInserter::performOperation(uint8_t opCode) { for (auto& rtdDummy : max31865DummyMap) { rtdDummy.second->setTemperature(10, true); } - for (auto& tmpDummy : tmp1075DummyMap) { - tmpDummy.second->setTemperature(10, true); + if (tmp1075DummyMap.has_value()) { + for (auto& tmpDummy : tmp1075DummyMap.value()) { + tmpDummy.second->setTemperature(10, true); + } } tempsWereInitialized = true; } @@ -96,6 +98,50 @@ ReturnValue_t TemperatureSensorInserter::performOperation(uint8_t opCode) { } break; } + case (TestCase::COLD_PLOC_CONSECUTIVE): { + if (cycles == 15) { + sif::debug << "Setting cold PLOC temperature" << std::endl; + max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(-15, true); + } + if (cycles == 30) { + sif::debug << "Setting warmer PLOC temperature" << std::endl; + max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(0, true); + } + if (cycles == 45) { + sif::debug << "Setting cold PLOC temperature again" << std::endl; + max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(-15, true); + } + if (cycles == 60) { + sif::debug << "Setting warmer PLOC temperature again" << std::endl; + max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(0, true); + } + break; + } + case (TestCase::COLD_CAMERA): { + if (cycles == 15) { + sif::debug << "Setting cold CAM temperature" << std::endl; + max31865DummyMap[objects::RTD_2_IC5_4K_CAMERA]->setTemperature(-40, true); + } + if (cycles == 30) { + sif::debug << "Setting CAM temperature back to normal" << std::endl; + max31865DummyMap[objects::RTD_2_IC5_4K_CAMERA]->setTemperature(0, true); + } + break; + } + case (TestCase::COLD_PLOC_STAYS_COLD): { + if (cycles == 15) { + sif::debug << "Setting cold PLOC temperature" << std::endl; + max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(-40, true); + } + break; + } + case (TestCase::COLD_CAMERA_STAYS_COLD): { + if (cycles == 15) { + sif::debug << "Setting cold PLOC temperature" << std::endl; + max31865DummyMap[objects::RTD_2_IC5_4K_CAMERA]->setTemperature(-40, true); + } + break; + } } cycles++; return returnvalue::OK; diff --git a/dummies/TemperatureSensorInserter.h b/dummies/TemperatureSensorInserter.h index 675bcd91..9ca3c936 100644 --- a/dummies/TemperatureSensorInserter.h +++ b/dummies/TemperatureSensorInserter.h @@ -12,7 +12,7 @@ class TemperatureSensorInserter : public ExecutableObjectIF, public SystemObject using Max31865DummyMap = std::map; using Tmp1075DummyMap = std::map; explicit TemperatureSensorInserter(object_id_t objectId, Max31865DummyMap tempSensorDummies_, - Tmp1075DummyMap tempTmpSensorDummies_); + std::optional tempTmpSensorDummies_); ReturnValue_t initialize() override; ReturnValue_t initializeAfterTaskCreation() override; @@ -22,7 +22,7 @@ class TemperatureSensorInserter : public ExecutableObjectIF, public SystemObject private: Max31865DummyMap max31865DummyMap; - Tmp1075DummyMap tmp1075DummyMap; + std::optional tmp1075DummyMap; enum TestCase { NONE = 0, @@ -31,6 +31,10 @@ class TemperatureSensorInserter : public ExecutableObjectIF, public SystemObject COLD_MGT = 3, COLD_STR = 4, COLD_STR_CONSECUTIVE = 5, + COLD_CAMERA = 6, + COLD_PLOC_CONSECUTIVE = 7, + COLD_PLOC_STAYS_COLD = 8, + COLD_CAMERA_STAYS_COLD = 9 }; int iteration = 0; uint32_t cycles = 0; diff --git a/dummies/Tmp1075Dummy.cpp b/dummies/Tmp1075Dummy.cpp index 91a50774..7e61acef 100644 --- a/dummies/Tmp1075Dummy.cpp +++ b/dummies/Tmp1075Dummy.cpp @@ -8,35 +8,57 @@ using namespace returnvalue; Tmp1075Dummy::Tmp1075Dummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie), set(this) {} -void Tmp1075Dummy::doStartUp() { setMode(MODE_NORMAL); } -void Tmp1075Dummy::doShutDown() { setMode(MODE_OFF); } +void Tmp1075Dummy::doStartUp() { setMode(MODE_ON); } +void Tmp1075Dummy::doShutDown() { + PoolReadGuard pg(&set); + set.setValidity(false, true); + setMode(MODE_OFF); +} ReturnValue_t Tmp1075Dummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; } -ReturnValue_t Tmp1075Dummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { return OK; } + +ReturnValue_t Tmp1075Dummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + return NOTHING_TO_SEND; +} + ReturnValue_t Tmp1075Dummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return 0; + return NOTHING_TO_SEND; } + ReturnValue_t Tmp1075Dummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { return 0; } + ReturnValue_t Tmp1075Dummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { return 0; } + void Tmp1075Dummy::setTemperature(float temperature, bool valid) { PoolReadGuard pg(&set); set.temperatureCelcius.value = temperature; set.setValidity(valid, true); } + void Tmp1075Dummy::fillCommandAndReplyMap() {} + uint32_t Tmp1075Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 1000; } + ReturnValue_t Tmp1075Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(TMP1075::TEMPERATURE_C_TMP1075, new PoolEntry({10.0}, true)); return OK; } + +ReturnValue_t Tmp1075Dummy::setHealth(HealthState health) { + if (health == FAULTY or health == PERMANENT_FAULTY) { + setMode(_MODE_SHUT_DOWN); + } + return DeviceHandlerBase::setHealth(health); +} + LocalPoolDataSetBase *Tmp1075Dummy::getDataSetHandle(sid_t sid) { return &set; } diff --git a/dummies/Tmp1075Dummy.h b/dummies/Tmp1075Dummy.h index 570fcd42..feab4f98 100644 --- a/dummies/Tmp1075Dummy.h +++ b/dummies/Tmp1075Dummy.h @@ -26,6 +26,7 @@ class Tmp1075Dummy : public DeviceHandlerBase { uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; + ReturnValue_t setHealth(HealthState health) override; protected: LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; diff --git a/dummies/helperFactory.cpp b/dummies/helperFactory.cpp index 98d2ecfa..84d6acc9 100644 --- a/dummies/helperFactory.cpp +++ b/dummies/helperFactory.cpp @@ -42,10 +42,13 @@ #include "mission/system/tree/payloadModeTree.h" #include "mission/tcs/defs.h" -void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpioIF) { +void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpioIF, + bool enableHkSets) { new ComIFDummy(objects::DUMMY_COM_IF); auto* comCookieDummy = new ComCookieDummy(); - new BpxDummy(objects::BPX_BATT_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + if (cfg.addBpxBattDummy) { + new BpxDummy(objects::BPX_BATT_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + } if (cfg.addCoreCtrlCfg) { new CoreControllerDummy(objects::CORE_CONTROLLER); } @@ -72,11 +75,15 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio } auto* imtqAssy = new ImtqAssembly(objects::IMTQ_ASSY); imtqAssy->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM); - auto* imtqDummy = new ImtqDummy(objects::IMTQ_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + auto* imtqDummy = new ImtqDummy(objects::IMTQ_HANDLER, objects::DUMMY_COM_IF, comCookieDummy, + power::Switches::PDU1_CH3_MGT_5V, enableHkSets); imtqDummy->enableThermalModule(ThermalStateCfg()); + imtqDummy->setPowerSwitcher(&pwrSwitcher); imtqDummy->connectModeTreeParent(*imtqAssy); - if (cfg.addPowerDummies) { - new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + if (cfg.addOnlyAcuDummy) { + new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy, enableHkSets); + } else if (cfg.addPowerDummies) { + new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy, enableHkSets); new PduDummy(objects::PDU1_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new PduDummy(objects::PDU2_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new P60DockDummy(objects::P60DOCK_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); @@ -184,25 +191,36 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio objects::RTD_15_IC18_IMTQ, new Max31865Dummy(objects::RTD_15_IC18_IMTQ, objects::DUMMY_COM_IF, comCookieDummy)); - std::map tmpSensorDummies; - tmpSensorDummies.emplace( - objects::TMP1075_HANDLER_TCS_0, - new Tmp1075Dummy(objects::TMP1075_HANDLER_TCS_0, objects::DUMMY_COM_IF, comCookieDummy)); - tmpSensorDummies.emplace( - objects::TMP1075_HANDLER_TCS_1, - new Tmp1075Dummy(objects::TMP1075_HANDLER_TCS_1, objects::DUMMY_COM_IF, comCookieDummy)); - tmpSensorDummies.emplace( - objects::TMP1075_HANDLER_PLPCDU_0, - new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_0, objects::DUMMY_COM_IF, comCookieDummy)); - // damaged. - // tmpSensorDummies.emplace( - // objects::TMP1075_HANDLER_PLPCDU_1, - // new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_1, objects::DUMMY_COM_IF, - // comCookieDummy)); - tmpSensorDummies.emplace( - objects::TMP1075_HANDLER_IF_BOARD, - new Tmp1075Dummy(objects::TMP1075_HANDLER_IF_BOARD, objects::DUMMY_COM_IF, comCookieDummy)); - + std::optional tmpSensorDummies; + if (cfg.addTmpDummies) { + TemperatureSensorInserter::Tmp1075DummyMap tmpDummyMap; + if (cfg.tmp1075Cfg.addTcsBrd0) { + tmpDummyMap.emplace(objects::TMP1075_HANDLER_TCS_0, + new Tmp1075Dummy(objects::TMP1075_HANDLER_TCS_0, objects::DUMMY_COM_IF, + comCookieDummy)); + } + if (cfg.tmp1075Cfg.addTcsBrd1) { + tmpDummyMap.emplace(objects::TMP1075_HANDLER_TCS_1, + new Tmp1075Dummy(objects::TMP1075_HANDLER_TCS_1, objects::DUMMY_COM_IF, + comCookieDummy)); + } + if (cfg.tmp1075Cfg.addPlPcdu0) { + tmpDummyMap.emplace(objects::TMP1075_HANDLER_PLPCDU_0, + new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_0, + objects::DUMMY_COM_IF, comCookieDummy)); + } + if (cfg.tmp1075Cfg.addPlPcdu1) { + tmpDummyMap.emplace(objects::TMP1075_HANDLER_PLPCDU_1, + new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_1, + objects::DUMMY_COM_IF, comCookieDummy)); + } + if (cfg.tmp1075Cfg.addIfBrd) { + tmpDummyMap.emplace(objects::TMP1075_HANDLER_IF_BOARD, + new Tmp1075Dummy(objects::TMP1075_HANDLER_IF_BOARD, + objects::DUMMY_COM_IF, comCookieDummy)); + } + tmpSensorDummies = std::move(tmpDummyMap); + } new TemperatureSensorInserter(objects::THERMAL_TEMP_INSERTER, rtdSensorDummies, tmpSensorDummies); TcsBoardAssembly* tcsBoardAssy = @@ -210,13 +228,17 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio for (auto& rtd : rtdSensorDummies) { rtd.second->connectModeTreeParent(*tcsBoardAssy); } - for (auto& tmp : tmpSensorDummies) { - tmp.second->connectModeTreeParent(satsystem::tcs::SUBSYSTEM); + if (tmpSensorDummies.has_value()) { + for (auto& tmp : tmpSensorDummies.value()) { + tmp.second->connectModeTreeParent(satsystem::tcs::SUBSYSTEM); + } } } - auto* camSwitcher = - new CamSwitcher(objects::CAM_SWITCHER, pwrSwitcher, power::Switches::PDU2_CH8_PAYLOAD_CAMERA); - camSwitcher->connectModeTreeParent(satsystem::payload::SUBSYSTEM); + if (cfg.addCamSwitcherDummy) { + auto* camSwitcher = new CamSwitcher(objects::CAM_SWITCHER, pwrSwitcher, + power::Switches::PDU2_CH8_PAYLOAD_CAMERA); + camSwitcher->connectModeTreeParent(satsystem::payload::SUBSYSTEM); + } auto* scexDummy = new ScexDummy(objects::SCEX, objects::DUMMY_COM_IF, comCookieDummy); scexDummy->connectModeTreeParent(satsystem::payload::SUBSYSTEM); auto* plPcduDummy = diff --git a/dummies/helperFactory.h b/dummies/helperFactory.h index 2181c79c..9c1a93d3 100644 --- a/dummies/helperFactory.h +++ b/dummies/helperFactory.h @@ -6,17 +6,32 @@ class GpioIF; namespace dummy { +struct Tmp1075Cfg { + bool addTcsBrd0 = true; + bool addTcsBrd1 = true; + bool addPlPcdu0 = true; + bool addPlPcdu1 = true; + bool addIfBrd = true; +}; + +// Default values targeted towards EM. struct DummyCfg { bool addCoreCtrlCfg = true; + // Special variant because the ACU broke. Overrides addPowerDummies, only ACU dummy will be added. + bool addOnlyAcuDummy = false; bool addPowerDummies = true; + bool addBpxBattDummy = true; bool addSyrlinksDummies = true; bool addAcsBoardDummies = true; bool addSusDummies = true; bool addTempSensorDummies = true; bool addRtdComIFDummy = true; bool addPlocDummies = true; + bool addTmpDummies = true; + Tmp1075Cfg tmp1075Cfg; + bool addCamSwitcherDummy = false; }; -void createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitch, GpioIF* gpioIF); +void createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitch, GpioIF* gpioIF, bool enableHkSets); } // namespace dummy diff --git a/fsfw b/fsfw index 5eb9ee8b..88e86652 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 5eb9ee8bc1e6f8640cbea46febd11e4b92241881 +Subproject commit 88e8665280a0381c41b724ab035a8c3eff1a23c1 diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index 54233239..3f1bbb77 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -177,20 +177,24 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 12515;0x30e3;STR_HELPER_FILE_NOT_EXISTS;LOW;Specified file does not exist P1: Internal state of str helper;linux/acs/StrComHandler.h 12516;0x30e4;STR_HELPER_SENDING_PACKET_FAILED;LOW;No description;linux/acs/StrComHandler.h 12517;0x30e5;STR_HELPER_REQUESTING_MSG_FAILED;LOW;No description;linux/acs/StrComHandler.h -12600;0x3138;MPSOC_FLASH_WRITE_FAILED;LOW;Flash write fails;linux/payload/PlocMpsocHelper.h -12601;0x3139;MPSOC_FLASH_WRITE_SUCCESSFUL;LOW;Flash write successful;linux/payload/PlocMpsocHelper.h -12602;0x313a;MPSOC_SENDING_COMMAND_FAILED;LOW;No description;linux/payload/PlocMpsocHelper.h -12603;0x313b;MPSOC_HELPER_REQUESTING_REPLY_FAILED;LOW;Request receive message of communication interface failed P1: Return value returned by the communication interface requestReceiveMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12604;0x313c;MPSOC_HELPER_READING_REPLY_FAILED;LOW;Reading receive message of communication interface failed P1: Return value returned by the communication interface readingReceivedMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12605;0x313d;MPSOC_MISSING_ACK;LOW;Did not receive acknowledgment report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12606;0x313e;MPSOC_MISSING_EXE;LOW;Did not receive execution report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12607;0x313f;MPSOC_ACK_FAILURE_REPORT;LOW;Received acknowledgment failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12608;0x3140;MPSOC_EXE_FAILURE_REPORT;LOW;Received execution failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12609;0x3141;MPSOC_ACK_INVALID_APID;LOW;Expected acknowledgment report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12610;0x3142;MPSOC_EXE_INVALID_APID;LOW;Expected execution report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12611;0x3143;MPSOC_HELPER_SEQ_CNT_MISMATCH;LOW;Received sequence count does not match expected sequence count P1: Expected sequence count P2: Received sequence count;linux/payload/PlocMpsocHelper.h -12612;0x3144;MPSOC_TM_SIZE_ERROR;LOW;No description;linux/payload/PlocMpsocHelper.h -12613;0x3145;MPSOC_TM_CRC_MISSMATCH;LOW;No description;linux/payload/PlocMpsocHelper.h +12600;0x3138;MPSOC_FLASH_WRITE_FAILED;LOW;Flash write fails;linux/payload/PlocMpsocSpecialComHelper.h +12601;0x3139;MPSOC_FLASH_WRITE_SUCCESSFUL;INFO;Flash write successful;linux/payload/PlocMpsocSpecialComHelper.h +12602;0x313a;MPSOC_SENDING_COMMAND_FAILED;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12603;0x313b;MPSOC_HELPER_REQUESTING_REPLY_FAILED;LOW;Request receive message of communication interface failed P1: Return value returned by the communication interface requestReceiveMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12604;0x313c;MPSOC_HELPER_READING_REPLY_FAILED;LOW;Reading receive message of communication interface failed P1: Return value returned by the communication interface readingReceivedMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12605;0x313d;MPSOC_MISSING_ACK;LOW;Did not receive acknowledgment report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12606;0x313e;MPSOC_MISSING_EXE;LOW;Did not receive execution report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12607;0x313f;MPSOC_ACK_FAILURE_REPORT;LOW;Received acknowledgment failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12608;0x3140;MPSOC_EXE_FAILURE_REPORT;LOW;Received execution failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12609;0x3141;MPSOC_ACK_INVALID_APID;LOW;Expected acknowledgment report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12610;0x3142;MPSOC_EXE_INVALID_APID;LOW;Expected execution report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12611;0x3143;MPSOC_HELPER_SEQ_CNT_MISMATCH;LOW;Received sequence count does not match expected sequence count P1: Expected sequence count P2: Received sequence count;linux/payload/PlocMpsocSpecialComHelper.h +12612;0x3144;MPSOC_TM_SIZE_ERROR;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12613;0x3145;MPSOC_TM_CRC_MISSMATCH;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12614;0x3146;MPSOC_FLASH_READ_PACKET_ERROR;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12615;0x3147;MPSOC_FLASH_READ_FAILED;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12616;0x3148;MPSOC_FLASH_READ_SUCCESSFUL;INFO;No description;linux/payload/PlocMpsocSpecialComHelper.h +12617;0x3149;MPSOC_READ_TIMEOUT;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h 12700;0x319c;TRANSITION_BACK_TO_OFF;MEDIUM;Could not transition properly and went back to ALL OFF;mission/payload/PayloadPcduHandler.h 12701;0x319d;NEG_V_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/payload/PayloadPcduHandler.h 12702;0x319e;U_DRO_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/payload/PayloadPcduHandler.h @@ -267,6 +271,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14010;0x36ba;TRYING_I2C_RECOVERY;HIGH;I2C is unavailable. Trying recovery of I2C bus by power cycling all I2C devices.;mission/sysDefs.h 14011;0x36bb;I2C_REBOOT;HIGH;I2C is unavailable. Recovery did not work, performing full reboot.;mission/sysDefs.h 14012;0x36bc;PDEC_REBOOT;HIGH;PDEC recovery through reset was not possible, performing full reboot.;mission/sysDefs.h +14013;0x36bd;FIRMWARE_INFO;INFO;Version information of the firmware (not OBSW). 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.;mission/sysDefs.h 14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/tcsDefs.h 14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/tcsDefs.h 14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h @@ -274,6 +279,10 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14105;0x3719;CAMERA_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14106;0x371a;PCDU_SYSTEM_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14107;0x371b;HEATER_NOT_OFF_FOR_OFF_MODE;MEDIUM;No description;mission/controller/tcsDefs.h +14108;0x371c;MGT_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h +14109;0x371d;TCS_SWITCHING_HEATER_ON;INFO;P1: Module index. P2: Heater index;mission/controller/tcsDefs.h +14110;0x371e;TCS_SWITCHING_HEATER_OFF;INFO;P1: Module index. P2: Heater index;mission/controller/tcsDefs.h +14111;0x371f;TCS_HEATER_MAX_BURN_TIME_REACHED;MEDIUM;P1: Heater index. P2: Maximum burn time for heater.;mission/controller/tcsDefs.h 14201;0x3779;TX_TIMER_EXPIRED;INFO;The transmit timer to protect the Syrlinks expired P1: The current timer value;mission/system/com/ComSubsystem.h 14202;0x377a;BIT_LOCK_TX_ON;INFO;Transmitter will be turned on due to detection of bitlock;mission/system/com/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 diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 54233239..3f1bbb77 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -177,20 +177,24 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 12515;0x30e3;STR_HELPER_FILE_NOT_EXISTS;LOW;Specified file does not exist P1: Internal state of str helper;linux/acs/StrComHandler.h 12516;0x30e4;STR_HELPER_SENDING_PACKET_FAILED;LOW;No description;linux/acs/StrComHandler.h 12517;0x30e5;STR_HELPER_REQUESTING_MSG_FAILED;LOW;No description;linux/acs/StrComHandler.h -12600;0x3138;MPSOC_FLASH_WRITE_FAILED;LOW;Flash write fails;linux/payload/PlocMpsocHelper.h -12601;0x3139;MPSOC_FLASH_WRITE_SUCCESSFUL;LOW;Flash write successful;linux/payload/PlocMpsocHelper.h -12602;0x313a;MPSOC_SENDING_COMMAND_FAILED;LOW;No description;linux/payload/PlocMpsocHelper.h -12603;0x313b;MPSOC_HELPER_REQUESTING_REPLY_FAILED;LOW;Request receive message of communication interface failed P1: Return value returned by the communication interface requestReceiveMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12604;0x313c;MPSOC_HELPER_READING_REPLY_FAILED;LOW;Reading receive message of communication interface failed P1: Return value returned by the communication interface readingReceivedMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12605;0x313d;MPSOC_MISSING_ACK;LOW;Did not receive acknowledgment report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12606;0x313e;MPSOC_MISSING_EXE;LOW;Did not receive execution report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocHelper.h -12607;0x313f;MPSOC_ACK_FAILURE_REPORT;LOW;Received acknowledgment failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12608;0x3140;MPSOC_EXE_FAILURE_REPORT;LOW;Received execution failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12609;0x3141;MPSOC_ACK_INVALID_APID;LOW;Expected acknowledgment report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12610;0x3142;MPSOC_EXE_INVALID_APID;LOW;Expected execution report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocHelper.h -12611;0x3143;MPSOC_HELPER_SEQ_CNT_MISMATCH;LOW;Received sequence count does not match expected sequence count P1: Expected sequence count P2: Received sequence count;linux/payload/PlocMpsocHelper.h -12612;0x3144;MPSOC_TM_SIZE_ERROR;LOW;No description;linux/payload/PlocMpsocHelper.h -12613;0x3145;MPSOC_TM_CRC_MISSMATCH;LOW;No description;linux/payload/PlocMpsocHelper.h +12600;0x3138;MPSOC_FLASH_WRITE_FAILED;LOW;Flash write fails;linux/payload/PlocMpsocSpecialComHelper.h +12601;0x3139;MPSOC_FLASH_WRITE_SUCCESSFUL;INFO;Flash write successful;linux/payload/PlocMpsocSpecialComHelper.h +12602;0x313a;MPSOC_SENDING_COMMAND_FAILED;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12603;0x313b;MPSOC_HELPER_REQUESTING_REPLY_FAILED;LOW;Request receive message of communication interface failed P1: Return value returned by the communication interface requestReceiveMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12604;0x313c;MPSOC_HELPER_READING_REPLY_FAILED;LOW;Reading receive message of communication interface failed P1: Return value returned by the communication interface readingReceivedMessage function P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12605;0x313d;MPSOC_MISSING_ACK;LOW;Did not receive acknowledgment report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12606;0x313e;MPSOC_MISSING_EXE;LOW;Did not receive execution report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/payload/PlocMpsocSpecialComHelper.h +12607;0x313f;MPSOC_ACK_FAILURE_REPORT;LOW;Received acknowledgment failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12608;0x3140;MPSOC_EXE_FAILURE_REPORT;LOW;Received execution failure report P1: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12609;0x3141;MPSOC_ACK_INVALID_APID;LOW;Expected acknowledgment report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12610;0x3142;MPSOC_EXE_INVALID_APID;LOW;Expected execution report but received space packet with other apid P1: Apid of received space packet P2: Internal state of MPSoC;linux/payload/PlocMpsocSpecialComHelper.h +12611;0x3143;MPSOC_HELPER_SEQ_CNT_MISMATCH;LOW;Received sequence count does not match expected sequence count P1: Expected sequence count P2: Received sequence count;linux/payload/PlocMpsocSpecialComHelper.h +12612;0x3144;MPSOC_TM_SIZE_ERROR;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12613;0x3145;MPSOC_TM_CRC_MISSMATCH;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12614;0x3146;MPSOC_FLASH_READ_PACKET_ERROR;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12615;0x3147;MPSOC_FLASH_READ_FAILED;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h +12616;0x3148;MPSOC_FLASH_READ_SUCCESSFUL;INFO;No description;linux/payload/PlocMpsocSpecialComHelper.h +12617;0x3149;MPSOC_READ_TIMEOUT;LOW;No description;linux/payload/PlocMpsocSpecialComHelper.h 12700;0x319c;TRANSITION_BACK_TO_OFF;MEDIUM;Could not transition properly and went back to ALL OFF;mission/payload/PayloadPcduHandler.h 12701;0x319d;NEG_V_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/payload/PayloadPcduHandler.h 12702;0x319e;U_DRO_OUT_OF_BOUNDS;MEDIUM;P1: 0 -> too low, 1 -> too high P2: Float value;mission/payload/PayloadPcduHandler.h @@ -267,6 +271,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14010;0x36ba;TRYING_I2C_RECOVERY;HIGH;I2C is unavailable. Trying recovery of I2C bus by power cycling all I2C devices.;mission/sysDefs.h 14011;0x36bb;I2C_REBOOT;HIGH;I2C is unavailable. Recovery did not work, performing full reboot.;mission/sysDefs.h 14012;0x36bc;PDEC_REBOOT;HIGH;PDEC recovery through reset was not possible, performing full reboot.;mission/sysDefs.h +14013;0x36bd;FIRMWARE_INFO;INFO;Version information of the firmware (not OBSW). 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.;mission/sysDefs.h 14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/tcsDefs.h 14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/tcsDefs.h 14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h @@ -274,6 +279,10 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14105;0x3719;CAMERA_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14106;0x371a;PCDU_SYSTEM_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h 14107;0x371b;HEATER_NOT_OFF_FOR_OFF_MODE;MEDIUM;No description;mission/controller/tcsDefs.h +14108;0x371c;MGT_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h +14109;0x371d;TCS_SWITCHING_HEATER_ON;INFO;P1: Module index. P2: Heater index;mission/controller/tcsDefs.h +14110;0x371e;TCS_SWITCHING_HEATER_OFF;INFO;P1: Module index. P2: Heater index;mission/controller/tcsDefs.h +14111;0x371f;TCS_HEATER_MAX_BURN_TIME_REACHED;MEDIUM;P1: Heater index. P2: Maximum burn time for heater.;mission/controller/tcsDefs.h 14201;0x3779;TX_TIMER_EXPIRED;INFO;The transmit timer to protect the Syrlinks expired P1: The current timer value;mission/system/com/ComSubsystem.h 14202;0x377a;BIT_LOCK_TX_ON;INFO;Transmitter will be turned on due to detection of bitlock;mission/system/com/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 diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index 20b76df6..1bc91860 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -478,8 +478,8 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x53b6;STRH_StartrackerAlreadyBooted;Star tracker is already in firmware mode;182;STR_HANDLER;mission/acs/str/StarTrackerHandler.h 0x53b7;STRH_StartrackerNotRunningFirmware;Star tracker must be in firmware mode to run this command;183;STR_HANDLER;mission/acs/str/StarTrackerHandler.h 0x53b8;STRH_StartrackerNotRunningBootloader;Star tracker must be in bootloader mode to run this command;184;STR_HANDLER;mission/acs/str/StarTrackerHandler.h -0x54e0;DWLPWRON_InvalidMode;Received command has invalid JESD mode (valid modes are 0 - 5);224;DWLPWRON_CMD;linux/payload/plocMpscoDefs.h -0x54e1;DWLPWRON_InvalidLaneRate;Received command has invalid lane rate (valid lane rate are 0 - 9);225;DWLPWRON_CMD;linux/payload/plocMpscoDefs.h +0x54e0;DWLPWRON_InvalidMode;Received command has invalid JESD mode (valid modes are 0 - 5);224;DWLPWRON_CMD;linux/payload/plocMpsocHelpers.h +0x54e1;DWLPWRON_InvalidLaneRate;Received command has invalid lane rate (valid lane rate are 0 - 9);225;DWLPWRON_CMD;linux/payload/plocMpsocHelpers.h 0x5700;PLSPVhLP_RequestDone;No description;0;PLOC_SUPV_HELPER;linux/payload/PlocSupvUartMan.h 0x5701;PLSPVhLP_NoPacketFound;No description;1;PLOC_SUPV_HELPER;linux/payload/PlocSupvUartMan.h 0x5702;PLSPVhLP_DecodeBufTooSmall;No description;2;PLOC_SUPV_HELPER;linux/payload/PlocSupvUartMan.h @@ -543,7 +543,8 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NvmParameterBase.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 -0x65a0;PLMPHLP_FileClosedAccidentally;File accidentally close;160;PLOC_MPSOC_HELPER;linux/payload/PlocMpsocHelper.h +0x65a0;PLMPHLP_FileWriteError;File error occured for file transfers from OBC to the MPSoC.;160;PLOC_MPSOC_HELPER;linux/payload/PlocMpsocSpecialComHelper.h +0x65a1;PLMPHLP_FileReadError;File error occured for file transfers from MPSoC to OBC.;161;PLOC_MPSOC_HELPER;linux/payload/PlocMpsocSpecialComHelper.h 0x66a0;SADPL_CommandNotSupported;No description;160;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h 0x66a1;SADPL_DeploymentAlreadyExecuting;No description;161;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h 0x66a2;SADPL_MainSwitchTimeoutFailure;No description;162;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 4c1fdb55..b98926e5 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 290 translations. + * @brief Auto-generated event translation file. Contains 299 translations. * @details - * Generated on: 2023-04-14 20:23:17 + * Generated on: 2023-07-07 12:06:06 */ #include "translateEvents.h" @@ -197,6 +197,10 @@ const char *MPSOC_EXE_INVALID_APID_STRING = "MPSOC_EXE_INVALID_APID"; const char *MPSOC_HELPER_SEQ_CNT_MISMATCH_STRING = "MPSOC_HELPER_SEQ_CNT_MISMATCH"; const char *MPSOC_TM_SIZE_ERROR_STRING = "MPSOC_TM_SIZE_ERROR"; const char *MPSOC_TM_CRC_MISSMATCH_STRING = "MPSOC_TM_CRC_MISSMATCH"; +const char *MPSOC_FLASH_READ_PACKET_ERROR_STRING = "MPSOC_FLASH_READ_PACKET_ERROR"; +const char *MPSOC_FLASH_READ_FAILED_STRING = "MPSOC_FLASH_READ_FAILED"; +const char *MPSOC_FLASH_READ_SUCCESSFUL_STRING = "MPSOC_FLASH_READ_SUCCESSFUL"; +const char *MPSOC_READ_TIMEOUT_STRING = "MPSOC_READ_TIMEOUT"; const char *TRANSITION_BACK_TO_OFF_STRING = "TRANSITION_BACK_TO_OFF"; const char *NEG_V_OUT_OF_BOUNDS_STRING = "NEG_V_OUT_OF_BOUNDS"; const char *U_DRO_OUT_OF_BOUNDS_STRING = "U_DRO_OUT_OF_BOUNDS"; @@ -273,6 +277,7 @@ const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS"; const char *TRYING_I2C_RECOVERY_STRING = "TRYING_I2C_RECOVERY"; const char *I2C_REBOOT_STRING = "I2C_REBOOT"; const char *PDEC_REBOOT_STRING = "PDEC_REBOOT"; +const char *FIRMWARE_INFO_STRING = "FIRMWARE_INFO"; 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"; @@ -280,6 +285,10 @@ const char *OBC_OVERHEATING_STRING = "OBC_OVERHEATING"; const char *CAMERA_OVERHEATING_STRING = "CAMERA_OVERHEATING"; const char *PCDU_SYSTEM_OVERHEATING_STRING = "PCDU_SYSTEM_OVERHEATING"; const char *HEATER_NOT_OFF_FOR_OFF_MODE_STRING = "HEATER_NOT_OFF_FOR_OFF_MODE"; +const char *MGT_OVERHEATING_STRING = "MGT_OVERHEATING"; +const char *TCS_SWITCHING_HEATER_ON_STRING = "TCS_SWITCHING_HEATER_ON"; +const char *TCS_SWITCHING_HEATER_OFF_STRING = "TCS_SWITCHING_HEATER_OFF"; +const char *TCS_HEATER_MAX_BURN_TIME_REACHED_STRING = "TCS_HEATER_MAX_BURN_TIME_REACHED"; 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"; @@ -682,6 +691,14 @@ const char *translateEvents(Event event) { return MPSOC_TM_SIZE_ERROR_STRING; case (12613): return MPSOC_TM_CRC_MISSMATCH_STRING; + case (12614): + return MPSOC_FLASH_READ_PACKET_ERROR_STRING; + case (12615): + return MPSOC_FLASH_READ_FAILED_STRING; + case (12616): + return MPSOC_FLASH_READ_SUCCESSFUL_STRING; + case (12617): + return MPSOC_READ_TIMEOUT_STRING; case (12700): return TRANSITION_BACK_TO_OFF_STRING; case (12701): @@ -834,6 +851,8 @@ const char *translateEvents(Event event) { return I2C_REBOOT_STRING; case (14012): return PDEC_REBOOT_STRING; + case (14013): + return FIRMWARE_INFO_STRING; case (14100): return NO_VALID_SENSOR_TEMPERATURE_STRING; case (14101): @@ -848,6 +867,14 @@ const char *translateEvents(Event event) { return PCDU_SYSTEM_OVERHEATING_STRING; case (14107): return HEATER_NOT_OFF_FOR_OFF_MODE_STRING; + case (14108): + return MGT_OVERHEATING_STRING; + case (14109): + return TCS_SWITCHING_HEATER_ON_STRING; + case (14110): + return TCS_SWITCHING_HEATER_OFF_STRING; + case (14111): + return TCS_HEATER_MAX_BURN_TIME_REACHED_STRING; case (14201): return TX_TIMER_EXPIRED_STRING; case (14202): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index bb0ef28a..5a9af4d7 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-04-14 20:23:17 + * Generated on: 2023-07-07 12:06:06 */ #include "translateObjects.h" diff --git a/linux/acs/AcsBoardPolling.cpp b/linux/acs/AcsBoardPolling.cpp index e525bf53..1ba55357 100644 --- a/linux/acs/AcsBoardPolling.cpp +++ b/linux/acs/AcsBoardPolling.cpp @@ -113,6 +113,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send if (req->mode != adis.mode) { if (req->mode == acs::SimpleSensorMode::NORMAL) { adis.type = req->type; + adis.decRate = req->cfg.decRateReg; // The initial countdown is handled by the device handler now. // adis.countdown.setTimeout(adis1650x::START_UP_TIME); if (adis.type == adis1650x::Type::ADIS16507) { @@ -376,6 +377,80 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) { } } +ReturnValue_t AcsBoardPolling::writeAdisReg(SpiCookie& cookie) { + 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 spi::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; + for (idx = 0; idx < 4; idx += 2) { + 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 = spi::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(); + + transferStruct->tx_buf += 2; + transferStruct->rx_buf += 2; + if (idx < 4) { + usleep(adis1650x::STALL_TIME_MICROSECONDS); + } + } + transferStruct->tx_buf = origTx; + transferStruct->rx_buf = origRx; + cookie.setTransferSize(0); + return returnvalue::OK; +} + ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) { ReturnValue_t result = returnvalue::OK; int retval = 0; @@ -455,15 +530,20 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { ReturnValue_t result; acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; bool mustPerformStartup = false; + uint16_t decRate = 0; { MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); mode = gyro.mode; + decRate = gyro.decRate; mustPerformStartup = gyro.performStartup; } if (mode == acs::SimpleSensorMode::OFF) { return; } if (mustPerformStartup) { + adis1650x::prepareWriteRegCommand(adis1650x::DEC_RATE_REG, decRate, cmdBuf.data(), + cmdBuf.size()); + writeAdisReg(*gyro.cookie); uint8_t regList[6]; // Read configuration regList[0] = adis1650x::DIAG_STAT_REG; @@ -491,13 +571,19 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { gyro.replyResult = returnvalue::FAILED; return; } + uint16_t decRateReadBack = (rawReply[10] << 8) | rawReply[11]; + if (decRateReadBack != decRate) { + sif::warning << "AcsPollingTask: DEC rate configuration failed. Read " << decRateReadBack + << ", expected " << decRate << std::endl; + gyro.replyResult = returnvalue::FAILED; + } 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.decRateReg = decRateReadBack; gyro.ownReply.cfg.prodId = prodId; gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl); gyro.performStartup = false; diff --git a/linux/acs/AcsBoardPolling.h b/linux/acs/AcsBoardPolling.h index 794c9c47..5b4d0390 100644 --- a/linux/acs/AcsBoardPolling.h +++ b/linux/acs/AcsBoardPolling.h @@ -37,6 +37,7 @@ class AcsBoardPolling : public SystemObject, struct GyroAdis : public DevBase { adis1650x::Type type; + uint16_t decRate; Countdown countdown; acs::Adis1650XReply ownReply; acs::Adis1650XReply readerReply; @@ -84,6 +85,8 @@ class AcsBoardPolling : public SystemObject, void gyroAdisHandler(GyroAdis& gyro); void mgmLis3Handler(MgmLis3& mgm); void mgmRm3100Handler(MgmRm3100& mgm); + // This fumction configures the register as specified on p.21 of the datasheet. + ReturnValue_t writeAdisReg(SpiCookie& cookie); // Special readout: 16us stall time between small 2 byte transfers. ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen); }; diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 4c1fdb55..b98926e5 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 290 translations. + * @brief Auto-generated event translation file. Contains 299 translations. * @details - * Generated on: 2023-04-14 20:23:17 + * Generated on: 2023-07-07 12:06:06 */ #include "translateEvents.h" @@ -197,6 +197,10 @@ const char *MPSOC_EXE_INVALID_APID_STRING = "MPSOC_EXE_INVALID_APID"; const char *MPSOC_HELPER_SEQ_CNT_MISMATCH_STRING = "MPSOC_HELPER_SEQ_CNT_MISMATCH"; const char *MPSOC_TM_SIZE_ERROR_STRING = "MPSOC_TM_SIZE_ERROR"; const char *MPSOC_TM_CRC_MISSMATCH_STRING = "MPSOC_TM_CRC_MISSMATCH"; +const char *MPSOC_FLASH_READ_PACKET_ERROR_STRING = "MPSOC_FLASH_READ_PACKET_ERROR"; +const char *MPSOC_FLASH_READ_FAILED_STRING = "MPSOC_FLASH_READ_FAILED"; +const char *MPSOC_FLASH_READ_SUCCESSFUL_STRING = "MPSOC_FLASH_READ_SUCCESSFUL"; +const char *MPSOC_READ_TIMEOUT_STRING = "MPSOC_READ_TIMEOUT"; const char *TRANSITION_BACK_TO_OFF_STRING = "TRANSITION_BACK_TO_OFF"; const char *NEG_V_OUT_OF_BOUNDS_STRING = "NEG_V_OUT_OF_BOUNDS"; const char *U_DRO_OUT_OF_BOUNDS_STRING = "U_DRO_OUT_OF_BOUNDS"; @@ -273,6 +277,7 @@ const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS"; const char *TRYING_I2C_RECOVERY_STRING = "TRYING_I2C_RECOVERY"; const char *I2C_REBOOT_STRING = "I2C_REBOOT"; const char *PDEC_REBOOT_STRING = "PDEC_REBOOT"; +const char *FIRMWARE_INFO_STRING = "FIRMWARE_INFO"; 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"; @@ -280,6 +285,10 @@ const char *OBC_OVERHEATING_STRING = "OBC_OVERHEATING"; const char *CAMERA_OVERHEATING_STRING = "CAMERA_OVERHEATING"; const char *PCDU_SYSTEM_OVERHEATING_STRING = "PCDU_SYSTEM_OVERHEATING"; const char *HEATER_NOT_OFF_FOR_OFF_MODE_STRING = "HEATER_NOT_OFF_FOR_OFF_MODE"; +const char *MGT_OVERHEATING_STRING = "MGT_OVERHEATING"; +const char *TCS_SWITCHING_HEATER_ON_STRING = "TCS_SWITCHING_HEATER_ON"; +const char *TCS_SWITCHING_HEATER_OFF_STRING = "TCS_SWITCHING_HEATER_OFF"; +const char *TCS_HEATER_MAX_BURN_TIME_REACHED_STRING = "TCS_HEATER_MAX_BURN_TIME_REACHED"; 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"; @@ -682,6 +691,14 @@ const char *translateEvents(Event event) { return MPSOC_TM_SIZE_ERROR_STRING; case (12613): return MPSOC_TM_CRC_MISSMATCH_STRING; + case (12614): + return MPSOC_FLASH_READ_PACKET_ERROR_STRING; + case (12615): + return MPSOC_FLASH_READ_FAILED_STRING; + case (12616): + return MPSOC_FLASH_READ_SUCCESSFUL_STRING; + case (12617): + return MPSOC_READ_TIMEOUT_STRING; case (12700): return TRANSITION_BACK_TO_OFF_STRING; case (12701): @@ -834,6 +851,8 @@ const char *translateEvents(Event event) { return I2C_REBOOT_STRING; case (14012): return PDEC_REBOOT_STRING; + case (14013): + return FIRMWARE_INFO_STRING; case (14100): return NO_VALID_SENSOR_TEMPERATURE_STRING; case (14101): @@ -848,6 +867,14 @@ const char *translateEvents(Event event) { return PCDU_SYSTEM_OVERHEATING_STRING; case (14107): return HEATER_NOT_OFF_FOR_OFF_MODE_STRING; + case (14108): + return MGT_OVERHEATING_STRING; + case (14109): + return TCS_SWITCHING_HEATER_ON_STRING; + case (14110): + return TCS_SWITCHING_HEATER_OFF_STRING; + case (14111): + return TCS_HEATER_MAX_BURN_TIME_REACHED_STRING; case (14201): return TX_TIMER_EXPIRED_STRING; case (14202): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index bb0ef28a..5a9af4d7 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-04-14 20:23:17 + * Generated on: 2023-07-07 12:06:06 */ #include "translateObjects.h" diff --git a/linux/ipcore/AxiPtmeConfig.cpp b/linux/ipcore/AxiPtmeConfig.cpp index 6dee3e2f..b21edf5b 100644 --- a/linux/ipcore/AxiPtmeConfig.cpp +++ b/linux/ipcore/AxiPtmeConfig.cpp @@ -16,9 +16,9 @@ AxiPtmeConfig::AxiPtmeConfig(object_id_t objectId, std::string axiUio, int mapNu AxiPtmeConfig::~AxiPtmeConfig() {} ReturnValue_t AxiPtmeConfig::initialize() { - ReturnValue_t result = returnvalue::OK; UioMapper uioMapper(axiUio, mapNum); - result = uioMapper.getMappedAdress(&baseAddress, UioMapper::Permissions::READ_WRITE); + ReturnValue_t result = + uioMapper.getMappedAdress(&baseAddress, UioMapper::Permissions::READ_WRITE); if (result != returnvalue::OK) { return result; } @@ -26,8 +26,7 @@ ReturnValue_t AxiPtmeConfig::initialize() { } ReturnValue_t AxiPtmeConfig::writeCaduRateReg(uint8_t rateVal) { - ReturnValue_t result = returnvalue::OK; - result = mutex->lockMutex(timeoutType, mutexTimeout); + ReturnValue_t result = mutex->lockMutex(timeoutType, mutexTimeout); if (result != returnvalue::OK) { sif::warning << "AxiPtmeConfig::writeCaduRateReg: Failed to lock mutex" << std::endl; return returnvalue::FAILED; @@ -41,6 +40,11 @@ ReturnValue_t AxiPtmeConfig::writeCaduRateReg(uint8_t rateVal) { return returnvalue::OK; } +uint8_t AxiPtmeConfig::readCaduRateReg() { + MutexGuard mg(mutex); + return static_cast(*(baseAddress + CADU_BITRATE_REG)); +} + void AxiPtmeConfig::enableTxclockManipulator() { writeBit(COMMON_CONFIG_REG, true, BitPos::EN_TX_CLK_MANIPULATOR); } diff --git a/linux/ipcore/AxiPtmeConfig.h b/linux/ipcore/AxiPtmeConfig.h index 98188775..ebdf4d38 100644 --- a/linux/ipcore/AxiPtmeConfig.h +++ b/linux/ipcore/AxiPtmeConfig.h @@ -38,6 +38,7 @@ class AxiPtmeConfig : public SystemObject { * frequency of the clock connected to the bit clock input of PTME. */ ReturnValue_t writeCaduRateReg(uint8_t rateVal); + uint8_t readCaduRateReg(); /** * @brief Next to functions control the tx clock manipulator component diff --git a/linux/ipcore/PapbVcInterface.cpp b/linux/ipcore/PapbVcInterface.cpp index 60968cc6..5dcb4519 100644 --- a/linux/ipcore/PapbVcInterface.cpp +++ b/linux/ipcore/PapbVcInterface.cpp @@ -7,20 +7,16 @@ #include "fsfw/serviceinterface/ServiceInterface.h" -PapbVcInterface::PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbBusyId, - gpioId_t papbEmptyId, std::string uioFile, int mapNum) - : gpioComIF(gpioComIF), - papbBusyId(papbBusyId), - papbEmptyId(papbEmptyId), - uioFile(std::move(uioFile)), - mapNum(mapNum) {} +PapbVcInterface::PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbEmptyId, + std::string uioFile, int mapNum) + : gpioComIF(gpioComIF), papbEmptyId(papbEmptyId), uioFile(std::move(uioFile)), mapNum(mapNum) {} PapbVcInterface::~PapbVcInterface() {} ReturnValue_t PapbVcInterface::initialize() { UioMapper uioMapper(uioFile, mapNum); ReturnValue_t result = uioMapper.getMappedAdress(const_cast(&vcBaseReg), - UioMapper::Permissions::WRITE_ONLY); + UioMapper::Permissions::READ_WRITE); if (result != returnvalue::OK) { return result; } @@ -32,63 +28,27 @@ ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) { if (size < 4) { return returnvalue::FAILED; } - if (pollInterfaceReadiness(0, true) == returnvalue::OK) { + if (pollReadyForPacket()) { startPacketTransfer(ByteWidthCfg::ONE); } else { return DirectTmSinkIF::IS_BUSY; } - // TODO: This should work but does not.. :( - // size_t idx = 0; - // while (idx < size) { - // - // nanosleep(&BETWEEN_POLL_DELAY, &remDelay); - // if ((size - idx) < 4) { - // *vcBaseReg = CONFIG_DATA_INPUT | (size - idx - 1); - // usleep(1); - // } - // if (pollPapbBusySignal(2) == returnvalue::OK) { - // // vcBaseReg + DATA_REG_OFFSET + 3 = static_cast(data + idx); - // // vcBaseReg + DATA_REG_OFFSET + 2 = static_cast(data + idx + 1); - // // vcBaseReg + DATA_REG_OFFSET + 1 = static_cast(data + idx + 2); - // // vcBaseReg + DATA_REG_OFFSET = static_cast(data + idx + 3); - // - // // std::memcpy((vcBaseReg + DATA_REG_OFFSET), data + idx , nextWriteSize); - // *(vcBaseReg + DATA_REG_OFFSET) = *reinterpret_cast(data + idx); - // //uint8_t* byteReg = reinterpret_cast(vcBaseReg + DATA_REG_OFFSET); - // - // //byteReg[0] = data[idx]; - // //byteReg[1] = data[idx]; - // } else { - // abortPacketTransfer(); - // return returnvalue::FAILED; - // } - // // TODO: Change this after the bugfix. Right now, the PAPB ignores the content of the byte - // // width configuration.5 - // // It's okay to increment by a larger amount for the last segment here, loop will be over - // // in any case. - // idx += 4; - // } - for (size_t idx = 0; idx < size; idx++) { - // This delay is super-important, DO NOT REMOVE! - // Polling the GPIO or the config register too often messes up the scheduler. - // TODO: Maybe this should not be done like this. It would be better if there was a custom - // FPGA module which can accept packets and then takes care of dumping that packet into - // the PTME. DMA would be an ideal solution for this. - nanosleep(&BETWEEN_POLL_DELAY, &remDelay); - if (pollInterfaceReadiness(2, false) == returnvalue::OK) { - *(vcBaseReg + DATA_REG_OFFSET) = static_cast(data[idx]); - } else { - abortPacketTransfer(); - return returnvalue::FAILED; - } - } - nanosleep(&BETWEEN_POLL_DELAY, &remDelay); - if (pollInterfaceReadiness(2, false) == returnvalue::OK) { - completePacketTransfer(); - } else { + if (not pollReadyForOctet(MAX_BUSY_POLLS)) { abortPacketTransfer(); return returnvalue::FAILED; } + for (size_t idx = 0; idx < size; idx++) { + if (not pollReadyForOctet(MAX_BUSY_POLLS)) { + abortPacketTransfer(); + return returnvalue::FAILED; + } + *(vcBaseReg + DATA_REG_OFFSET) = static_cast(data[idx]); + } + if (not pollReadyForOctet(MAX_BUSY_POLLS)) { + abortPacketTransfer(); + return returnvalue::FAILED; + } + completePacketTransfer(); return returnvalue::OK; } @@ -98,63 +58,49 @@ void PapbVcInterface::startPacketTransfer(ByteWidthCfg initWidth) { void PapbVcInterface::completePacketTransfer() { *vcBaseReg = CONFIG_END; } -ReturnValue_t PapbVcInterface::pollInterfaceReadiness(uint32_t maxPollRetries, - bool checkReadyState) const { - uint32_t busyIdx = 0; - nextDelay.tv_nsec = FIRST_DELAY_PAPB_POLLING_NS; - - while (true) { - // Check if PAPB interface is ready to receive data. Use the configuration register for this. - // Bit 5, see PTME ptme_001_01-0-7-r2 Table 31. - uint32_t reg = *vcBaseReg; - bool busy = (reg >> 5) & 0b1; - bool ready = (reg >> 6) & 0b1; - if (not busy) { - return returnvalue::OK; - } - if (checkReadyState and not ready) { - return PAPB_BUSY; - } - - busyIdx++; - if (busyIdx >= maxPollRetries) { - return PAPB_BUSY; - } - - // Ignore signal handling here for now. - nanosleep(&nextDelay, &remDelay); - // Adaptive delay. - if (nextDelay.tv_nsec * 2 <= MAX_DELAY_PAPB_POLLING_NS) { - nextDelay.tv_nsec *= 2; - } - } - return returnvalue::OK; +bool PapbVcInterface::pollReadyForPacket() const { + // Check if PAPB interface is ready to receive data. Use the configuration register for this. + // Bit 5, see PTME ptme_001_01-0-7-r2 Table 31. + uint32_t reg = *vcBaseReg; + return (reg >> 6) & 0b1; } -void PapbVcInterface::isVcInterfaceBufferEmpty() { +bool PapbVcInterface::isVcInterfaceBufferEmpty() { ReturnValue_t result = returnvalue::OK; gpio::Levels papbEmptyState = gpio::Levels::HIGH; result = gpioComIF->readGpio(papbEmptyId, papbEmptyState); if (result != returnvalue::OK) { - sif::warning << "PapbVcInterface::isVcInterfaceBufferEmpty: Failed to read papb empty signal" - << std::endl; - return; + sif::error << "PapbVcInterface::isVcInterfaceBufferEmpty: Failed to read papb empty signal" + << std::endl; + return true; } if (papbEmptyState == gpio::Levels::HIGH) { - sif::debug << "PapbVcInterface::isVcInterfaceBufferEmpty: Buffer is empty" << std::endl; - } else { - sif::debug << "PapbVcInterface::isVcInterfaceBufferEmpty: Buffer is not empty" << std::endl; + return true; } - return; + return false; } -bool PapbVcInterface::isBusy() const { return pollInterfaceReadiness(0, true) == PAPB_BUSY; } +bool PapbVcInterface::isBusy() const { return not pollReadyForPacket(); } void PapbVcInterface::cancelTransfer() { abortPacketTransfer(); } +inline bool PapbVcInterface::pollReadyForOctet(uint32_t maxCycles) const { + uint32_t reg; + uint32_t idx = 0; + while (idx < maxCycles) { + reg = *vcBaseReg; + // Busy bit. + if (not((reg >> 5) & 0b1)) { + return true; + } + idx++; + } + return false; +} + 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 e54def5d..b5160748 100644 --- a/linux/ipcore/PapbVcInterface.h +++ b/linux/ipcore/PapbVcInterface.h @@ -30,8 +30,7 @@ class PapbVcInterface : public VirtualChannelIF { * @param uioFile UIO file providing access to the PAPB bus * @param mapNum Map number of UIO map associated with this virtual channel */ - PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbBusyId, gpioId_t papbEmptyId, - std::string uioFile, int mapNum); + PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbEmptyId, std::string uioFile, int mapNum); virtual ~PapbVcInterface(); bool isBusy() const override; @@ -81,11 +80,9 @@ class PapbVcInterface : public VirtualChannelIF { static constexpr long int FIRST_DELAY_PAPB_POLLING_NS = 10; static constexpr long int MAX_DELAY_PAPB_POLLING_NS = 40; + static constexpr uint32_t MAX_BUSY_POLLS = 1000; LinuxLibgpioIF* gpioComIF = nullptr; - - /** Pulled to low when virtual channel not ready to receive data */ - gpioId_t papbBusyId = gpio::NO_GPIO; /** High when external buffer memory of virtual channel is empty */ gpioId_t papbEmptyId = gpio::NO_GPIO; @@ -120,13 +117,15 @@ class PapbVcInterface : public VirtualChannelIF { * * @return returnvalue::OK when ready to receive data else PAPB_BUSY. */ - inline ReturnValue_t pollInterfaceReadiness(uint32_t maxPollRetries, bool checkReadyState) const; + inline bool pollReadyForPacket() const; + + inline bool pollReadyForOctet(uint32_t maxCycles) const; /** * @brief This function can be used for debugging to check whether there are packets in * the packet buffer of the virtual channel or not. */ - void isVcInterfaceBufferEmpty(); + bool isVcInterfaceBufferEmpty(); /** * @brief This function sends a complete telemetry transfer frame data field (1105 bytes) diff --git a/linux/ipcore/PdecHandler.cpp b/linux/ipcore/PdecHandler.cpp index 7506dd11..cc074ddd 100644 --- a/linux/ipcore/PdecHandler.cpp +++ b/linux/ipcore/PdecHandler.cpp @@ -435,8 +435,7 @@ ReturnValue_t PdecHandler::releasePdec() { } ReturnValue_t PdecHandler::pdecToReset() { - ReturnValue_t result = returnvalue::OK; - result = gpioComIF->pullLow(pdecReset); + ReturnValue_t result = gpioComIF->pullLow(pdecReset); if (result != returnvalue::OK) { sif::error << "PdecHandler::pdecToReset: Failed to pull PDEC reset line" " to low" diff --git a/linux/ipcore/PtmeConfig.cpp b/linux/ipcore/PtmeConfig.cpp index 5f247b54..5b6b9343 100644 --- a/linux/ipcore/PtmeConfig.cpp +++ b/linux/ipcore/PtmeConfig.cpp @@ -26,6 +26,11 @@ ReturnValue_t PtmeConfig::setRate(uint32_t bitRate) { return axiPtmeConfig->writeCaduRateReg(static_cast(rateVal)); } +uint32_t PtmeConfig::getRate() { + uint8_t rateReg = axiPtmeConfig->readCaduRateReg(); + return (BIT_CLK_FREQ / (rateReg + 1)); +} + void PtmeConfig::invertTxClock(bool invert) { if (invert) { axiPtmeConfig->enableTxclockInversion(); diff --git a/linux/ipcore/PtmeConfig.h b/linux/ipcore/PtmeConfig.h index 87614187..11eeff7d 100644 --- a/linux/ipcore/PtmeConfig.h +++ b/linux/ipcore/PtmeConfig.h @@ -32,6 +32,7 @@ class PtmeConfig : public SystemObject { * of the CADU clock due to the convolutional code added by the s-Band transceiver. */ ReturnValue_t setRate(uint32_t bitRate); + uint32_t getRate(); /** * @brief Will change the time the tx data signal is updated with respect to the tx clock diff --git a/linux/payload/CMakeLists.txt b/linux/payload/CMakeLists.txt index 7b2c4486..e697fac6 100644 --- a/linux/payload/CMakeLists.txt +++ b/linux/payload/CMakeLists.txt @@ -2,7 +2,8 @@ target_sources( ${OBSW_NAME} PUBLIC PlocMemoryDumper.cpp PlocMpsocHandler.cpp - PlocMpsocHelper.cpp + PlocMpsocSpecialComHelper.cpp + plocMpsocHelpers.cpp PlocSupervisorHandler.cpp PlocSupvUartMan.cpp ScexDleParser.cpp diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 72f2355f..cdf59c52 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -7,11 +8,12 @@ #include "fsfw/datapool/PoolReadGuard.h" #include "fsfw/globalfunctions/CRC.h" -PlocMPSoCHandler::PlocMPSoCHandler(object_id_t objectId, object_id_t uartComIFid, - CookieIF* comCookie, PlocMPSoCHelper* plocMPSoCHelper, +PlocMpsocHandler::PlocMpsocHandler(object_id_t objectId, object_id_t uartComIFid, + CookieIF* comCookie, PlocMpsocSpecialComHelper* plocMPSoCHelper, Gpio uartIsolatorSwitch, object_id_t supervisorHandler) : DeviceHandlerBase(objectId, uartComIFid, comCookie), - plocMPSoCHelper(plocMPSoCHelper), + hkReport(this), + specialComHelper(plocMPSoCHelper), uartIsolatorSwitch(uartIsolatorSwitch), supervisorHandler(supervisorHandler), commandActionHelper(this) { @@ -25,9 +27,9 @@ PlocMPSoCHandler::PlocMPSoCHandler(object_id_t objectId, object_id_t uartComIFid spParams.buf = commandBuffer; } -PlocMPSoCHandler::~PlocMPSoCHandler() {} +PlocMpsocHandler::~PlocMpsocHandler() {} -ReturnValue_t PlocMPSoCHandler::initialize() { +ReturnValue_t PlocMpsocHandler::initialize() { ReturnValue_t result = returnvalue::OK; result = DeviceHandlerBase::initialize(); if (result != returnvalue::OK) { @@ -51,24 +53,35 @@ ReturnValue_t PlocMPSoCHandler::initialize() { if (result != returnvalue::OK) { return result; } - result = manager->subscribeToEventRange( - eventQueue->getId(), event::getEventId(PlocMPSoCHelper::MPSOC_FLASH_WRITE_FAILED), - event::getEventId(PlocMPSoCHelper::MPSOC_FLASH_WRITE_SUCCESSFUL)); + result = manager->subscribeToEvent( + eventQueue->getId(), event::getEventId(PlocMpsocSpecialComHelper::MPSOC_FLASH_WRITE_FAILED)); + if (result != returnvalue::OK) { + return ObjectManagerIF::CHILD_INIT_FAILED; + } + result = manager->subscribeToEvent( + eventQueue->getId(), + event::getEventId(PlocMpsocSpecialComHelper::MPSOC_FLASH_WRITE_SUCCESSFUL)); + if (result != returnvalue::OK) { + return ObjectManagerIF::CHILD_INIT_FAILED; + } + result = manager->subscribeToEvent( + eventQueue->getId(), + event::getEventId(PlocMpsocSpecialComHelper::MPSOC_FLASH_READ_SUCCESSFUL)); + if (result != returnvalue::OK) { + return ObjectManagerIF::CHILD_INIT_FAILED; + } + result = manager->subscribeToEvent( + eventQueue->getId(), event::getEventId(PlocMpsocSpecialComHelper::MPSOC_FLASH_READ_FAILED)); if (result != returnvalue::OK) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "PlocMPSoCHandler::initialize: Failed to subscribe to events from " - " ploc mpsoc helper" - << std::endl; -#endif return ObjectManagerIF::CHILD_INIT_FAILED; } - result = plocMPSoCHelper->setComIF(communicationInterface); + result = specialComHelper->setComIF(communicationInterface); if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } - plocMPSoCHelper->setComCookie(comCookie); - plocMPSoCHelper->setSequenceCount(&sequenceCount); + specialComHelper->setComCookie(comCookie); + specialComHelper->setSequenceCount(&sequenceCount); result = commandActionHelper.initialize(); if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; @@ -76,7 +89,12 @@ ReturnValue_t PlocMPSoCHandler::initialize() { return result; } -void PlocMPSoCHandler::performOperationHook() { +void PlocMpsocHandler::performOperationHook() { + if (commandIsPending and cmdCountdown.hasTimedOut()) { + commandIsPending = false; + // TODO: Better returnvalue? + cmdDoneHandler(false, returnvalue::FAILED); + } EventMessage event; for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK; result = eventQueue->receiveMessage(&event)) { @@ -100,7 +118,7 @@ void PlocMPSoCHandler::performOperationHook() { } } -ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, +ReturnValue_t PlocMpsocHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size) { ReturnValue_t result = returnvalue::OK; switch (actionId) { @@ -118,26 +136,38 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI } } - if (plocMPSoCHelperExecuting) { + if (specialComHelperExecuting) { return MPSoCReturnValuesIF::MPSOC_HELPER_EXECUTING; } switch (actionId) { - case mpsoc::TC_FLASHWRITE: { - if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) { - return MPSoCReturnValuesIF::FILENAME_TOO_LONG; - } - mpsoc::FlashWritePusCmd flashWritePusCmd; + case mpsoc::TC_FLASH_WRITE_FULL_FILE: { + mpsoc::FlashBasePusCmd flashWritePusCmd; result = flashWritePusCmd.extractFields(data, size); if (result != returnvalue::OK) { return result; } - result = plocMPSoCHelper->startFlashWrite(flashWritePusCmd.getObcFile(), - flashWritePusCmd.getMPSoCFile()); + result = specialComHelper->startFlashWrite(flashWritePusCmd.getObcFile(), + flashWritePusCmd.getMPSoCFile()); if (result != returnvalue::OK) { return result; } - plocMPSoCHelperExecuting = true; + specialComHelperExecuting = true; + return EXECUTION_FINISHED; + } + case mpsoc::TC_FLASH_READ_FULL_FILE: { + mpsoc::FlashReadPusCmd flashReadPusCmd; + result = flashReadPusCmd.extractFields(data, size); + if (result != returnvalue::OK) { + return result; + } + result = specialComHelper->startFlashRead(flashReadPusCmd.getObcFile(), + flashReadPusCmd.getMPSoCFile(), + flashReadPusCmd.getReadSize()); + if (result != returnvalue::OK) { + return result; + } + specialComHelperExecuting = true; return EXECUTION_FINISHED; } case (mpsoc::OBSW_RESET_SEQ_COUNT): { @@ -147,69 +177,100 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI default: break; } + // For longer commands, do not set these. + commandIsPending = true; + cmdCountdown.resetTimer(); return DeviceHandlerBase::executeAction(actionId, commandedBy, data, size); } -void PlocMPSoCHandler::doStartUp() { +void PlocMpsocHandler::doStartUp() { + if (startupState == StartupState::IDLE) { + startupState = StartupState::HW_INIT; + } + if (startupState == StartupState::HW_INIT) { #ifdef XIPHOS_Q7S #if not OBSW_MPSOC_JTAG_BOOT == 1 - switch (powerState) { - case PowerState::OFF: - commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC); - powerState = PowerState::BOOTING; - break; - case PowerState::ON: - setMode(_MODE_TO_ON); - uartIsolatorSwitch.pullHigh(); - break; - default: - break; - } + switch (powerState) { + case PowerState::OFF: + commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC); + powerState = PowerState::BOOTING; + return; + case PowerState::ON: + uartIsolatorSwitch.pullHigh(); + startupState = StartupState::WAIT_CYCLES; + break; + default: + return; + } #else - powerState = PowerState::ON; - setMode(_MODE_TO_ON); - uartIsolatorSwitch.pullHigh(); + uartIsolatorSwitch.pullHigh(); + startupState = StartupState::WAIT_CYCLES; #endif /* not MSPOC_JTAG_BOOT == 1 */ #else - powerState = PowerState::ON; - setMode(_MODE_TO_ON); + startupState = StartupState::WAIT_CYCLES; + powerState = PowerState::ON; #endif /* XIPHOS_Q7S */ + } + // Need to wait, MPSoC still not booted properly, requesting HK without these wait cycles does + // not work, no replies.. + if (startupState == StartupState::WAIT_CYCLES) { + waitCycles++; + if (waitCycles >= 8) { + startupState = StartupState::DONE; + waitCycles = 0; + } + } + if (startupState == StartupState::DONE) { + setMode(_MODE_TO_ON); + hkReport.setReportingEnabled(true); + startupState = StartupState::IDLE; + } } -void PlocMPSoCHandler::doShutDown() { +void PlocMpsocHandler::doShutDown() { #ifdef XIPHOS_Q7S #if not OBSW_MPSOC_JTAG_BOOT == 1 - switch (powerState) { - case PowerState::ON: - uartIsolatorSwitch.pullLow(); - commandActionHelper.commandAction(supervisorHandler, supv::SHUTDOWN_MPSOC); - powerState = PowerState::SHUTDOWN; - break; - case PowerState::OFF: - sequenceCount = 0; - setMode(_MODE_POWER_DOWN); - break; - default: - break; + if (powerState == PowerState::ON) { + uartIsolatorSwitch.pullLow(); + commandActionHelper.commandAction(supervisorHandler, supv::SHUTDOWN_MPSOC); + powerState = PowerState::SHUTDOWN; + return; + } else if (powerState == PowerState::SHUTDOWN) { + // Wait till power state is OFF. + return; } #else - sequenceCount = 0; uartIsolatorSwitch.pullLow(); - setMode(_MODE_POWER_DOWN); powerState = PowerState::OFF; #endif #endif + + if (specialComHelper != nullptr) { + specialComHelper->stopProcess(); + } + hkReport.setReportingEnabled(false); + setMode(_MODE_POWER_DOWN); + commandIsPending = false; + sequenceCount = 0; + specialComHelperExecuting = false; + startupState = StartupState::IDLE; } -ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { +ReturnValue_t PlocMpsocHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { + if (not commandIsPending and not specialComHelperExecuting) { + *id = mpsoc::TC_GET_HK_REPORT; + commandIsPending = true; + cmdCountdown.resetTimer(); + return buildCommandFromCommand(*id, nullptr, 0); + } return NOTHING_TO_SEND; } -ReturnValue_t PlocMPSoCHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { +ReturnValue_t PlocMpsocHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { return NOTHING_TO_SEND; } -ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, +ReturnValue_t PlocMpsocHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, size_t commandDataLen) { spParams.buf = commandBuffer; @@ -247,6 +308,14 @@ ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t device result = prepareTcReplayWriteSequence(commandData, commandDataLen); break; } + case (mpsoc::TC_GET_HK_REPORT): { + result = prepareTcGetHkReport(); + break; + } + case (mpsoc::TC_FLASH_GET_DIRECTORY_CONTENT): { + result = prepareTcGetDirContent(commandData, commandDataLen); + break; + } case (mpsoc::TC_MODE_REPLAY): { result = prepareTcModeReplay(); break; @@ -292,10 +361,12 @@ ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t device return result; } -void PlocMPSoCHandler::fillCommandAndReplyMap() { +void PlocMpsocHandler::fillCommandAndReplyMap() { this->insertInCommandMap(mpsoc::TC_MEM_WRITE); this->insertInCommandMap(mpsoc::TC_MEM_READ); this->insertInCommandMap(mpsoc::TC_FLASHDELETE); + insertInCommandMap(mpsoc::TC_FLASH_WRITE_FULL_FILE); + insertInCommandMap(mpsoc::TC_FLASH_READ_FULL_FILE); this->insertInCommandMap(mpsoc::TC_REPLAY_START); this->insertInCommandMap(mpsoc::TC_REPLAY_STOP); this->insertInCommandMap(mpsoc::TC_DOWNLINK_PWR_ON); @@ -304,19 +375,23 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() { this->insertInCommandMap(mpsoc::TC_MODE_REPLAY); this->insertInCommandMap(mpsoc::TC_MODE_IDLE); this->insertInCommandMap(mpsoc::TC_CAM_CMD_SEND); + this->insertInCommandMap(mpsoc::TC_GET_HK_REPORT); this->insertInCommandMap(mpsoc::RELEASE_UART_TX); this->insertInCommandMap(mpsoc::SET_UART_TX_TRISTATE); this->insertInCommandMap(mpsoc::TC_CAM_TAKE_PIC); + this->insertInCommandMap(mpsoc::TC_FLASH_GET_DIRECTORY_CONTENT); this->insertInCommandMap(mpsoc::TC_SIMPLEX_SEND_FILE); this->insertInCommandMap(mpsoc::TC_DOWNLINK_DATA_MODULATE); this->insertInCommandMap(mpsoc::TC_MODE_SNAPSHOT); this->insertInReplyMap(mpsoc::ACK_REPORT, 3, nullptr, mpsoc::SIZE_ACK_REPORT); this->insertInReplyMap(mpsoc::EXE_REPORT, 3, nullptr, mpsoc::SIZE_EXE_REPORT); this->insertInReplyMap(mpsoc::TM_MEMORY_READ_REPORT, 2, nullptr, mpsoc::SIZE_TM_MEM_READ_REPORT); + this->insertInReplyMap(mpsoc::TM_GET_HK_REPORT, 5, nullptr, mpsoc::SIZE_TM_HK_REPORT); this->insertInReplyMap(mpsoc::TM_CAM_CMD_RPT, 2, nullptr, mpsoc::SP_MAX_SIZE); + this->insertInReplyMap(mpsoc::TM_FLASH_DIRECTORY_CONTENT, 2, nullptr, mpsoc::SP_MAX_SIZE); } -ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remainingSize, +ReturnValue_t PlocMpsocHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { ReturnValue_t result = returnvalue::OK; @@ -331,6 +406,11 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain } uint16_t apid = spacePacket.getApid(); + auto handleDedicatedReply = [&](DeviceCommandId_t replyId) { + *foundLen = spacePacket.getFullPacketLen(); + foundPacketLen = *foundLen; + *foundId = replyId; + }; switch (apid) { case (mpsoc::apid::ACK_SUCCESS): *foundLen = mpsoc::SIZE_ACK_REPORT; @@ -345,10 +425,16 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain *foundId = mpsoc::TM_MEMORY_READ_REPORT; break; case (mpsoc::apid::TM_CAM_CMD_RPT): - *foundLen = spacePacket.getFullPacketLen(); - tmCamCmdRpt.rememberSpacePacketSize = *foundLen; - *foundId = mpsoc::TM_CAM_CMD_RPT; + handleDedicatedReply(mpsoc::TM_CAM_CMD_RPT); break; + case (mpsoc::apid::TM_HK_GET_REPORT): { + handleDedicatedReply(mpsoc::TM_GET_HK_REPORT); + break; + } + case (mpsoc::apid::TM_FLASH_DIRECTORY_CONTENT): { + handleDedicatedReply(mpsoc::TM_FLASH_DIRECTORY_CONTENT); + break; + } case (mpsoc::apid::EXE_SUCCESS): *foundLen = mpsoc::SIZE_EXE_REPORT; *foundId = mpsoc::EXE_REPORT; @@ -358,23 +444,25 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain *foundId = mpsoc::EXE_REPORT; break; default: { - sif::debug << "PlocMPSoCHandler::scanForReply: Reply has invalid apid" << std::endl; + sif::debug << "PlocMPSoCHandler::scanForReply: Reply has invalid APID 0x" << std::hex + << std::setfill('0') << std::setw(2) << apid << std::dec << std::endl; *foundLen = remainingSize; return MPSoCReturnValuesIF::INVALID_APID; } } - uint16_t recvSeqCnt = (*(start + 2) << 8 | *(start + 3)) & PACKET_SEQUENCE_COUNT_MASK; + uint16_t recvSeqCnt = ((*(start + 2) << 8) | *(start + 3)) & PACKET_SEQUENCE_COUNT_MASK; if (recvSeqCnt != sequenceCount) { triggerEvent(MPSOC_HANDLER_SEQUENCE_COUNT_MISMATCH, sequenceCount, recvSeqCnt); sequenceCount = recvSeqCnt; } // This sequence count ping pong does not make any sense but it is how the MPSoC expects it. sequenceCount++; + return result; } -ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { +ReturnValue_t PlocMpsocHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { ReturnValue_t result = returnvalue::OK; switch (id) { @@ -386,10 +474,26 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const result = handleMemoryReadReport(packet); break; } + case (mpsoc::TM_GET_HK_REPORT): { + result = handleGetHkReport(packet); + break; + } case (mpsoc::TM_CAM_CMD_RPT): { result = handleCamCmdRpt(packet); break; } + case (mpsoc::TM_FLASH_DIRECTORY_CONTENT): { + result = verifyPacket(packet, foundPacketLen); + if (result == MPSoCReturnValuesIF::CRC_FAILURE) { + sif::warning << "PLOC MPSoC: Flash directory content reply invalid CRC" << std::endl; + } + /** Send data to commanding queue */ + handleDeviceTm(packet + mpsoc::DATA_FIELD_OFFSET, + foundPacketLen - mpsoc::DATA_FIELD_OFFSET - mpsoc::CRC_SIZE, + mpsoc::TM_FLASH_DIRECTORY_CONTENT); + nextReplyId = mpsoc::EXE_REPORT; + return result; + } case (mpsoc::EXE_REPORT): { result = handleExecutionReport(packet); break; @@ -403,20 +507,59 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const return result; } -void PlocMPSoCHandler::setNormalDatapoolEntriesInvalid() {} +void PlocMpsocHandler::setNormalDatapoolEntriesInvalid() { + PoolReadGuard pg(&hkReport); + hkReport.setValidity(false, true); +} -uint32_t PlocMPSoCHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } +uint32_t PlocMpsocHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 10000; } -ReturnValue_t PlocMPSoCHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, +ReturnValue_t PlocMpsocHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { + localDataPoolMap.emplace(mpsoc::poolid::STATUS, &peStatus); + localDataPoolMap.emplace(mpsoc::poolid::MODE, &peMode); + localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_PWR_ON, &peDownlinkPwrOn); + localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_REPLY_ACTIIVE, &peDownlinkReplyActive); + localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_JESD_SYNC_STATUS, &peDownlinkJesdSyncStatus); + localDataPoolMap.emplace(mpsoc::poolid::DOWNLINK_DAC_STATUS, &peDownlinkDacStatus); + localDataPoolMap.emplace(mpsoc::poolid::CAM_STATUS, &peCameraStatus); + localDataPoolMap.emplace(mpsoc::poolid::CAM_SDI_STATUS, &peCameraSdiStatus); + localDataPoolMap.emplace(mpsoc::poolid::CAM_FPGA_TEMP, &peCameraFpgaTemp); + localDataPoolMap.emplace(mpsoc::poolid::CAM_SOC_TEMP, &peCameraSocTemp); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_TEMP, &peSysmonTemp); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCINT, &peSysmonVccInt); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCAUX, &peSysmonVccAux); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCBRAM, &peSysmonVccBram); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCPAUX, &peSysmonVccPaux); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCPINT, &peSysmonVccPint); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCCPDRO, &peSysmonVccPdro); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_MB12V, &peSysmonMb12V); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_MB3V3, &peSysmonMb3V3); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_MB1V8, &peSysmonMb1V8); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC12V, &peSysmonVcc12V); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC5V, &peSysmonVcc5V); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC3V3, &peSysmonVcc3V3); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC3V3VA, &peSysmonVcc3V3VA); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC2V5DDR, &peSysmonVcc2V5DDR); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC1V2DDR, &peSysmonVcc1V2DDR); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC0V9, &peSysmonVcc0V9); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_VCC0V6VTT, &peSysmonVcc0V6VTT); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_SAFE_COTS_CUR, &peSysmonSafeCotsCur); + localDataPoolMap.emplace(mpsoc::poolid::SYSMON_NVM4_XO_CUR, &peSysmonNvm4XoCur); + localDataPoolMap.emplace(mpsoc::poolid::SEM_UNCORRECTABLE_ERRS, &peSemUncorrectableErrs); + localDataPoolMap.emplace(mpsoc::poolid::SEM_CORRECTABLE_ERRS, &peSemCorrectableErrs); + localDataPoolMap.emplace(mpsoc::poolid::SEM_STATUS, &peSemStatus); + localDataPoolMap.emplace(mpsoc::poolid::REBOOT_MPSOC_REQUIRED, &peRebootMpsocRequired); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(hkReport.getSid(), false, 10.0)); return returnvalue::OK; } -void PlocMPSoCHandler::handleEvent(EventMessage* eventMessage) { +void PlocMpsocHandler::handleEvent(EventMessage* eventMessage) { object_id_t objectId = eventMessage->getReporter(); switch (objectId) { case objects::PLOC_MPSOC_HELPER: { - plocMPSoCHelperExecuting = false; + specialComHelperExecuting = false; break; } default: @@ -425,202 +568,194 @@ void PlocMPSoCHandler::handleEvent(EventMessage* eventMessage) { } } -ReturnValue_t PlocMPSoCHandler::prepareTcMemWrite(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcMemWrite(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; mpsoc::TcMemWrite tcMemWrite(spParams, sequenceCount); - result = tcMemWrite.buildPacket(commandData, commandDataLen); + result = tcMemWrite.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcMemWrite.getFullPacketLen()); + finishTcPrep(tcMemWrite); return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcMemRead(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcMemRead(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; mpsoc::TcMemRead tcMemRead(spParams, sequenceCount); - result = tcMemRead.buildPacket(commandData, commandDataLen); + result = tcMemRead.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcMemRead.getFullPacketLen()); + finishTcPrep(tcMemRead); tmMemReadReport.rememberRequestedSize = tcMemRead.getMemLen() * 4 + TmMemReadReport::FIX_SIZE; return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcFlashDelete(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcFlashDelete(const uint8_t* commandData, size_t commandDataLen) { if (commandDataLen > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) { return MPSoCReturnValuesIF::NAME_TOO_LONG; } ReturnValue_t result = returnvalue::OK; mpsoc::TcFlashDelete tcFlashDelete(spParams, sequenceCount); - result = tcFlashDelete.buildPacket( - std::string(reinterpret_cast(commandData), commandDataLen)); + std::string filename = std::string(reinterpret_cast(commandData), commandDataLen); + result = tcFlashDelete.setPayload(filename); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcFlashDelete.getFullPacketLen()); + finishTcPrep(tcFlashDelete); return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcReplayStart(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcReplayStart(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; mpsoc::TcReplayStart tcReplayStart(spParams, sequenceCount); - result = tcReplayStart.buildPacket(commandData, commandDataLen); + result = tcReplayStart.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcReplayStart.getFullPacketLen()); + finishTcPrep(tcReplayStart); return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcReplayStop() { - ReturnValue_t result = returnvalue::OK; +ReturnValue_t PlocMpsocHandler::prepareTcReplayStop() { mpsoc::TcReplayStop tcReplayStop(spParams, sequenceCount); - result = tcReplayStop.buildPacket(); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(tcReplayStop.getFullPacketLen()); + finishTcPrep(tcReplayStop); return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOn(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcDownlinkPwrOn(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; mpsoc::TcDownlinkPwrOn tcDownlinkPwrOn(spParams, sequenceCount); - result = tcDownlinkPwrOn.buildPacket(commandData, commandDataLen); + result = tcDownlinkPwrOn.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcDownlinkPwrOn.getFullPacketLen()); + finishTcPrep(tcDownlinkPwrOn); return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOff() { - ReturnValue_t result = returnvalue::OK; +ReturnValue_t PlocMpsocHandler::prepareTcDownlinkPwrOff() { mpsoc::TcDownlinkPwrOff tcDownlinkPwrOff(spParams, sequenceCount); - result = tcDownlinkPwrOff.buildPacket(); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(tcDownlinkPwrOff.getFullPacketLen()); + finishTcPrep(tcDownlinkPwrOff); return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcReplayWriteSequence(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcGetHkReport() { + mpsoc::TcGetHkReport tcGetHkReport(spParams, sequenceCount); + finishTcPrep(tcGetHkReport); + return returnvalue::OK; +} + +ReturnValue_t PlocMpsocHandler::prepareTcReplayWriteSequence(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = returnvalue::OK; mpsoc::TcReplayWriteSeq tcReplayWriteSeq(spParams, sequenceCount); - result = tcReplayWriteSeq.buildPacket(commandData, commandDataLen); + ReturnValue_t result = tcReplayWriteSeq.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcReplayWriteSeq.getFullPacketLen()); + finishTcPrep(tcReplayWriteSeq); return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcModeReplay() { - ReturnValue_t result = returnvalue::OK; +ReturnValue_t PlocMpsocHandler::prepareTcModeReplay() { mpsoc::TcModeReplay tcModeReplay(spParams, sequenceCount); - result = tcModeReplay.buildPacket(); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(tcModeReplay.getFullPacketLen()); + finishTcPrep(tcModeReplay); return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcModeIdle() { - ReturnValue_t result = returnvalue::OK; +ReturnValue_t PlocMpsocHandler::prepareTcModeIdle() { mpsoc::TcModeIdle tcModeIdle(spParams, sequenceCount); - result = tcModeIdle.buildPacket(); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(tcModeIdle.getFullPacketLen()); + finishTcPrep(tcModeIdle); return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcCamCmdSend(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcCamCmdSend(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = returnvalue::OK; mpsoc::TcCamcmdSend tcCamCmdSend(spParams, sequenceCount); - result = tcCamCmdSend.buildPacket(commandData, commandDataLen); + ReturnValue_t result = tcCamCmdSend.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcCamCmdSend.getFullPacketLen()); + finishTcPrep(tcCamCmdSend); nextReplyId = mpsoc::TM_CAM_CMD_RPT; return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcCamTakePic(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcCamTakePic(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = returnvalue::OK; mpsoc::TcCamTakePic tcCamTakePic(spParams, sequenceCount); - result = tcCamTakePic.buildPacket(commandData, commandDataLen); + ReturnValue_t result = tcCamTakePic.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcCamTakePic.getFullPacketLen()); + finishTcPrep(tcCamTakePic); return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcSimplexSendFile(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcSimplexSendFile(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = returnvalue::OK; mpsoc::TcSimplexSendFile tcSimplexSendFile(spParams, sequenceCount); - result = tcSimplexSendFile.buildPacket(commandData, commandDataLen); + ReturnValue_t result = tcSimplexSendFile.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcSimplexSendFile.getFullPacketLen()); + finishTcPrep(tcSimplexSendFile); return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkDataModulate(const uint8_t* commandData, +ReturnValue_t PlocMpsocHandler::prepareTcGetDirContent(const uint8_t* commandData, + size_t commandDataLen) { + mpsoc::TcGetDirContent tcGetDirContent(spParams, sequenceCount); + ReturnValue_t result = tcGetDirContent.setPayload(commandData, commandDataLen); + if (result != returnvalue::OK) { + return result; + } + finishTcPrep(tcGetDirContent); + return returnvalue::OK; +} + +ReturnValue_t PlocMpsocHandler::prepareTcDownlinkDataModulate(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = returnvalue::OK; mpsoc::TcDownlinkDataModulate tcDownlinkDataModulate(spParams, sequenceCount); - result = tcDownlinkDataModulate.buildPacket(commandData, commandDataLen); + ReturnValue_t result = tcDownlinkDataModulate.setPayload(commandData, commandDataLen); if (result != returnvalue::OK) { return result; } - finishTcPrep(tcDownlinkDataModulate.getFullPacketLen()); + finishTcPrep(tcDownlinkDataModulate); return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::prepareTcModeSnapshot() { - ReturnValue_t result = returnvalue::OK; +ReturnValue_t PlocMpsocHandler::prepareTcModeSnapshot() { mpsoc::TcModeSnapshot tcModeSnapshot(spParams, sequenceCount); - result = tcModeSnapshot.buildPacket(); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(tcModeSnapshot.getFullPacketLen()); + finishTcPrep(tcModeSnapshot); return returnvalue::OK; } -void PlocMPSoCHandler::finishTcPrep(size_t packetLen) { +ReturnValue_t PlocMpsocHandler::finishTcPrep(mpsoc::TcBase& tcBase) { nextReplyId = mpsoc::ACK_REPORT; + ReturnValue_t result = tcBase.finishPacket(); + if (result != returnvalue::OK) { + return result; + } rawPacket = commandBuffer; - rawPacketLen = packetLen; + rawPacketLen = tcBase.getFullPacketLen(); sequenceCount++; + return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::verifyPacket(const uint8_t* start, size_t foundLen) { +ReturnValue_t PlocMpsocHandler::verifyPacket(const uint8_t* start, size_t foundLen) { if (CRC::crc16ccitt(start, foundLen) != 0) { return MPSoCReturnValuesIF::CRC_FAILURE; } return returnvalue::OK; } -ReturnValue_t PlocMPSoCHandler::handleAckReport(const uint8_t* data) { +ReturnValue_t PlocMpsocHandler::handleAckReport(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, mpsoc::SIZE_ACK_REPORT); @@ -638,10 +773,9 @@ ReturnValue_t PlocMPSoCHandler::handleAckReport(const uint8_t* data) { switch (apid) { case mpsoc::apid::ACK_FAILURE: { - sif::debug << "PlocMPSoCHandler::handleAckReport: Received Ack failure report" << std::endl; DeviceCommandId_t commandId = getPendingCommand(); - uint16_t status = getStatus(data); - printStatus(data); + uint16_t status = mpsoc::getStatusFromRawData(data); + sif::warning << "MPSoC ACK Failure: " << mpsoc::getStatusString(status) << std::endl; if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { triggerEvent(ACK_FAILURE, commandId, status); } @@ -665,7 +799,7 @@ ReturnValue_t PlocMPSoCHandler::handleAckReport(const uint8_t* data) { return result; } -ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { +ReturnValue_t PlocMpsocHandler::handleExecutionReport(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, mpsoc::SIZE_EXE_REPORT); @@ -679,23 +813,20 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { switch (apid) { case (mpsoc::apid::EXE_SUCCESS): { + cmdDoneHandler(true, result); break; } case (mpsoc::apid::EXE_FAILURE): { - // TODO: Interpretation of status field in execution report - sif::warning << "PlocMPSoCHandler::handleExecutionReport: Received execution failure report" - << std::endl; DeviceCommandId_t commandId = getPendingCommand(); - if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { - uint16_t status = getStatus(data); - triggerEvent(EXE_FAILURE, commandId, status); - } else { + if (commandId == DeviceHandlerIF::NO_COMMAND_ID) { sif::debug << "PlocMPSoCHandler::handleExecutionReport: Unknown command id" << std::endl; } - printStatus(data); + uint16_t status = mpsoc::getStatusFromRawData(data); + sif::warning << "MPSoC EXE Failure: " << mpsoc::getStatusString(status) << std::endl; + triggerEvent(EXE_FAILURE, commandId, status); sendFailureReport(mpsoc::EXE_REPORT, MPSoCReturnValuesIF::RECEIVED_EXE_FAILURE); - disableExeReportReply(); result = IGNORE_REPLY_DATA; + cmdDoneHandler(false, MPSoCReturnValuesIF::RECEIVED_EXE_FAILURE); break; } default: { @@ -708,7 +839,7 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { return result; } -ReturnValue_t PlocMPSoCHandler::handleMemoryReadReport(const uint8_t* data) { +ReturnValue_t PlocMpsocHandler::handleMemoryReadReport(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, tmMemReadReport.rememberRequestedSize); if (result == MPSoCReturnValuesIF::CRC_FAILURE) { @@ -718,41 +849,227 @@ ReturnValue_t PlocMPSoCHandler::handleMemoryReadReport(const uint8_t* data) { uint16_t memLen = *(data + mpsoc::MEM_READ_RPT_LEN_OFFSET) << 8 | *(data + mpsoc::MEM_READ_RPT_LEN_OFFSET + 1); /** Send data to commanding queue */ - handleDeviceTM(data + mpsoc::DATA_FIELD_OFFSET, mpsoc::SIZE_MEM_READ_RPT_FIX + memLen * 4, + handleDeviceTm(data + mpsoc::DATA_FIELD_OFFSET, mpsoc::SIZE_MEM_READ_RPT_FIX + memLen * 4, mpsoc::TM_MEMORY_READ_REPORT); nextReplyId = mpsoc::EXE_REPORT; return result; } -ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) { - ReturnValue_t result = returnvalue::OK; - result = verifyPacket(data, tmCamCmdRpt.rememberSpacePacketSize); +ReturnValue_t PlocMpsocHandler::handleGetHkReport(const uint8_t* data) { + ReturnValue_t result = verifyPacket(data, foundPacketLen); + if (result != returnvalue::OK) { + return result; + } + SpacePacketReader packetReader(data, foundPacketLen); + const uint8_t* dataStart = data + 6; + PoolReadGuard pg(&hkReport); + size_t deserLen = mpsoc::SIZE_TM_HK_REPORT; + SerializeIF::Endianness endianness = SerializeIF::Endianness::NETWORK; + result = SerializeAdapter::deSerialize(&hkReport.status.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.mode.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.downlinkPwrOn.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.downlinkReplyActive.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.downlinkJesdSyncStatus.value, &dataStart, + &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.downlinkDacStatus.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.camStatus.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.camSdiStatus.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.camFpgaTemp.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonTemp.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccInt.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccAux.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccBram.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccPaux.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccPint.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVccPdro.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonMb12V.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonMb3V3.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonMb1V8.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc12V.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.sysmonVcc5V.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc3V3.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc3V3VA.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc2V5DDR.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc1V2DDR.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc0V9.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonVcc0V6VTT.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonSafeCotsCur.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.sysmonNvm4XoCur.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.semUncorrectableErrs.value, &dataStart, + &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&hkReport.semCorrectableErrs.value, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + result = + SerializeAdapter::deSerialize(&hkReport.semStatus.value, &dataStart, &deserLen, endianness); + if (result != returnvalue::OK) { + return result; + } + // Skip the weird filename + dataStart += 256; + result = SerializeAdapter::deSerialize(&hkReport.rebootMpsocRequired, &dataStart, &deserLen, + endianness); + if (result != returnvalue::OK) { + return result; + } + return returnvalue::OK; +} + +ReturnValue_t PlocMpsocHandler::handleCamCmdRpt(const uint8_t* data) { + ReturnValue_t result = verifyPacket(data, foundPacketLen); if (result == MPSoCReturnValuesIF::CRC_FAILURE) { sif::warning << "PlocMPSoCHandler::handleCamCmdRpt: CRC failure" << std::endl; } - SpacePacketReader packetReader(data, tmCamCmdRpt.rememberSpacePacketSize); + SpacePacketReader packetReader(data, foundPacketLen); const uint8_t* dataFieldPtr = data + mpsoc::SPACE_PACKET_HEADER_SIZE + sizeof(uint16_t); - std::string camCmdRptMsg( - reinterpret_cast(dataFieldPtr), - tmCamCmdRpt.rememberSpacePacketSize - mpsoc::SPACE_PACKET_HEADER_SIZE - sizeof(uint16_t) - 3); + std::string camCmdRptMsg(reinterpret_cast(dataFieldPtr), + foundPacketLen - mpsoc::SPACE_PACKET_HEADER_SIZE - sizeof(uint16_t) - 3); #if OBSW_DEBUG_PLOC_MPSOC == 1 uint8_t ackValue = *(packetReader.getFullData() + packetReader.getFullPacketLen() - 2); sif::info << "PlocMPSoCHandler: CamCmdRpt message: " << camCmdRptMsg << std::endl; sif::info << "PlocMPSoCHandler: CamCmdRpt Ack value: 0x" << std::hex << static_cast(ackValue) << std::endl; #endif /* OBSW_DEBUG_PLOC_MPSOC == 1 */ - handleDeviceTM(packetReader.getPacketData() + sizeof(uint16_t), + handleDeviceTm(packetReader.getPacketData() + sizeof(uint16_t), packetReader.getPacketDataLen() - 1, mpsoc::TM_CAM_CMD_RPT); return result; } -ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator command, +ReturnValue_t PlocMpsocHandler::enableReplyInReplyMap(DeviceCommandMap::iterator command, uint8_t expectedReplies, bool useAlternateId, DeviceCommandId_t alternateReplyID) { ReturnValue_t result = returnvalue::OK; uint8_t enabledReplies = 0; + auto enableThreeReplies = [&](DeviceCommandId_t replyId) { + enabledReplies = 3; + result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, replyId); + if (result != returnvalue::OK) { + sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " + << mpsoc::TM_MEMORY_READ_REPORT << " not in replyMap" << std::endl; + return result; + } + return returnvalue::OK; + }; switch (command->first) { case mpsoc::TC_MEM_WRITE: case mpsoc::TC_FLASHDELETE: @@ -769,24 +1086,30 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator case mpsoc::TC_MODE_SNAPSHOT: enabledReplies = 2; break; - case mpsoc::TC_MEM_READ: { - enabledReplies = 3; - result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, - mpsoc::TM_MEMORY_READ_REPORT); + case mpsoc::TC_GET_HK_REPORT: { + result = enableThreeReplies(mpsoc::TM_GET_HK_REPORT); + if (result != returnvalue::OK) { + return result; + } + break; + } + case mpsoc::TC_MEM_READ: { + result = enableThreeReplies(mpsoc::TM_MEMORY_READ_REPORT); if (result != returnvalue::OK) { - sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " - << mpsoc::TM_MEMORY_READ_REPORT << " not in replyMap" << std::endl; return result; } break; } case mpsoc::TC_CAM_CMD_SEND: { - enabledReplies = 3; - result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, - mpsoc::TM_CAM_CMD_RPT); + result = enableThreeReplies(mpsoc::TM_CAM_CMD_RPT); + if (result != returnvalue::OK) { + return result; + } + break; + } + case mpsoc::TC_FLASH_GET_DIRECTORY_CONTENT: { + result = enableThreeReplies(mpsoc::TM_FLASH_DIRECTORY_CONTENT); if (result != returnvalue::OK) { - sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " - << mpsoc::TM_CAM_CMD_RPT << " not in replyMap" << std::endl; return result; } break; @@ -846,18 +1169,27 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator return returnvalue::OK; } -void PlocMPSoCHandler::setNextReplyId() { +void PlocMpsocHandler::setNextReplyId() { switch (getPendingCommand()) { case mpsoc::TC_MEM_READ: nextReplyId = mpsoc::TM_MEMORY_READ_REPORT; break; + case mpsoc::TC_FLASH_GET_DIRECTORY_CONTENT: { + nextReplyId = mpsoc::TM_FLASH_DIRECTORY_CONTENT; + break; + } + case mpsoc::TC_GET_HK_REPORT: { + nextReplyId = mpsoc::TM_GET_HK_REPORT; + break; + } default: /* If no telemetry is expected the next reply is always the execution report */ nextReplyId = mpsoc::EXE_REPORT; break; } } -size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) { + +size_t PlocMpsocHandler::getNextReplyLength(DeviceCommandId_t commandId) { size_t replyLen = 0; if (nextReplyId == mpsoc::NONE) { @@ -881,6 +1213,10 @@ size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) { // report is not fixed replyLen = mpsoc::SP_MAX_SIZE; break; + case mpsoc::TM_FLASH_DIRECTORY_CONTENT: + // I think the reply size is not fixed either. + replyLen = mpsoc::SP_MAX_SIZE; + break; default: { replyLen = iter->second.replyLen; break; @@ -894,19 +1230,19 @@ size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) { return replyLen; } -ReturnValue_t PlocMPSoCHandler::doSendReadHook() { +ReturnValue_t PlocMpsocHandler::doSendReadHook() { // Prevent DHB from polling UART during commands executed by the mpsoc helper task - if (plocMPSoCHelperExecuting) { + if (specialComHelperExecuting) { return returnvalue::FAILED; } return returnvalue::OK; } -MessageQueueIF* PlocMPSoCHandler::getCommandQueuePtr() { return commandActionHelperQueue; } +MessageQueueIF* PlocMpsocHandler::getCommandQueuePtr() { return commandActionHelperQueue; } -void PlocMPSoCHandler::stepSuccessfulReceived(ActionId_t actionId, uint8_t step) { return; } +void PlocMpsocHandler::stepSuccessfulReceived(ActionId_t actionId, uint8_t step) { return; } -void PlocMPSoCHandler::stepFailedReceived(ActionId_t actionId, uint8_t step, +void PlocMpsocHandler::stepFailedReceived(ActionId_t actionId, uint8_t step, ReturnValue_t returnCode) { switch (actionId) { case supv::START_MPSOC: { @@ -929,11 +1265,11 @@ void PlocMPSoCHandler::stepFailedReceived(ActionId_t actionId, uint8_t step, } } -void PlocMPSoCHandler::dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) { +void PlocMpsocHandler::dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) { return; } -void PlocMPSoCHandler::completionSuccessfulReceived(ActionId_t actionId) { +void PlocMpsocHandler::completionSuccessfulReceived(ActionId_t actionId) { if (actionId != supv::EXE_REPORT) { sif::debug << "PlocMPSoCHandler::completionSuccessfulReceived: Did not expect this action " << "ID" << std::endl; @@ -954,11 +1290,11 @@ void PlocMPSoCHandler::completionSuccessfulReceived(ActionId_t actionId) { } } -void PlocMPSoCHandler::completionFailedReceived(ActionId_t actionId, ReturnValue_t returnCode) { +void PlocMpsocHandler::completionFailedReceived(ActionId_t actionId, ReturnValue_t returnCode) { handleActionCommandFailure(actionId); } -void PlocMPSoCHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, +void PlocMpsocHandler::handleDeviceTm(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) { ReturnValue_t result = returnvalue::OK; @@ -984,7 +1320,7 @@ void PlocMPSoCHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, } } -void PlocMPSoCHandler::disableAllReplies() { +void PlocMpsocHandler::disableAllReplies() { using namespace mpsoc; DeviceReplyMap::iterator iter; @@ -996,6 +1332,13 @@ void PlocMPSoCHandler::disableAllReplies() { DeviceCommandId_t commandId = getPendingCommand(); + auto disableCommandWithReply = [&](DeviceCommandId_t replyId) { + iter = deviceReplyMap.find(replyId); + info = &(iter->second); + info->delayCycles = 0; + info->active = false; + info->command = deviceCommandMap.end(); + }; /* If the command expects a telemetry packet the appropriate tm reply will be disabled here */ switch (commandId) { case TC_MEM_WRITE: @@ -1013,19 +1356,19 @@ void PlocMPSoCHandler::disableAllReplies() { case TC_MODE_SNAPSHOT: break; case TC_MEM_READ: { - iter = deviceReplyMap.find(TM_MEMORY_READ_REPORT); - info = &(iter->second); - info->delayCycles = 0; - info->active = false; - info->command = deviceCommandMap.end(); + disableCommandWithReply(TM_MEMORY_READ_REPORT); + break; + } + case TC_GET_HK_REPORT: { + disableCommandWithReply(TM_GET_HK_REPORT); + break; + } + case TC_FLASH_GET_DIRECTORY_CONTENT: { + disableCommandWithReply(TM_FLASH_DIRECTORY_CONTENT); break; } case TC_CAM_CMD_SEND: { - iter = deviceReplyMap.find(TM_CAM_CMD_RPT); - info = &(iter->second); - info->delayCycles = 0; - info->active = false; - info->command = deviceCommandMap.end(); + disableCommandWithReply(TM_CAM_CMD_RPT); break; } default: { @@ -1040,7 +1383,7 @@ void PlocMPSoCHandler::disableAllReplies() { nextReplyId = mpsoc::NONE; } -void PlocMPSoCHandler::sendFailureReport(DeviceCommandId_t replyId, ReturnValue_t status) { +void PlocMpsocHandler::sendFailureReport(DeviceCommandId_t replyId, ReturnValue_t status) { DeviceReplyIter iter = deviceReplyMap.find(replyId); if (iter == deviceReplyMap.end()) { sif::debug << "PlocMPSoCHandler::sendFailureReport: Reply not in reply map" << std::endl; @@ -1057,7 +1400,7 @@ void PlocMPSoCHandler::sendFailureReport(DeviceCommandId_t replyId, ReturnValue_ info->isExecuting = false; } -void PlocMPSoCHandler::disableExeReportReply() { +void PlocMpsocHandler::disableExeReportReply() { DeviceReplyIter iter = deviceReplyMap.find(mpsoc::EXE_REPORT); DeviceReplyInfo* info = &(iter->second); info->delayCycles = 0; @@ -1066,16 +1409,7 @@ void PlocMPSoCHandler::disableExeReportReply() { info->command->second.expectedReplies = 0; } -void PlocMPSoCHandler::printStatus(const uint8_t* data) { - uint16_t status = *(data + STATUS_OFFSET) << 8 | *(data + STATUS_OFFSET + 1); - sif::info << "Verification report status: " << getStatusString(status) << std::endl; -} - -uint16_t PlocMPSoCHandler::getStatus(const uint8_t* data) { - return *(data + STATUS_OFFSET) << 8 | *(data + STATUS_OFFSET + 1); -} - -void PlocMPSoCHandler::handleActionCommandFailure(ActionId_t actionId) { +void PlocMpsocHandler::handleActionCommandFailure(ActionId_t actionId) { switch (actionId) { case supv::ACK_REPORT: case supv::EXE_REPORT: @@ -1108,89 +1442,27 @@ void PlocMPSoCHandler::handleActionCommandFailure(ActionId_t actionId) { return; } -std::string PlocMPSoCHandler::getStatusString(uint16_t status) { - switch (status) { - case (mpsoc::status_code::UNKNOWN_APID): { - return "Unknown APID"; - break; - } - case (mpsoc::status_code::INCORRECT_LENGTH): { - return "Incorrect length"; - break; - } - case (mpsoc::status_code::INCORRECT_CRC): { - return "Incorrect crc"; - break; - } - case (mpsoc::status_code::INCORRECT_PKT_SEQ_CNT): { - return "Incorrect packet sequence count"; - break; - } - case (mpsoc::status_code::TC_NOT_ALLOWED_IN_MODE): { - return "TC not allowed in this mode"; - break; - } - case (mpsoc::status_code::TC_EXEUTION_DISABLED): { - return "TC execution disabled"; - break; - } - case (mpsoc::status_code::FLASH_MOUNT_FAILED): { - return "Flash mount failed"; - break; - } - case (mpsoc::status_code::FLASH_FILE_ALREADY_CLOSED): { - return "Flash file already closed"; - break; - } - case (mpsoc::status_code::FLASH_FILE_OPEN_FAILED): { - return "Flash file open failed"; - break; - } - case (mpsoc::status_code::FLASH_FILE_NOT_OPEN): { - return "Flash file not open"; - break; - } - case (mpsoc::status_code::FLASH_UNMOUNT_FAILED): { - return "Flash unmount failed"; - break; - } - case (mpsoc::status_code::HEAP_ALLOCATION_FAILED): { - return "Heap allocation failed"; - break; - } - case (mpsoc::status_code::INVALID_PARAMETER): { - return "Invalid parameter"; - break; - } - case (mpsoc::status_code::NOT_INITIALIZED): { - return "Not initialized"; - break; - } - case (mpsoc::status_code::REBOOT_IMMINENT): { - return "Reboot imminent"; - break; - } - case (mpsoc::status_code::CORRUPT_DATA): { - return "Corrupt data"; - break; - } - case (mpsoc::status_code::FLASH_CORRECTABLE_MISMATCH): { - return "Flash correctable mismatch"; - break; - } - case (mpsoc::status_code::FLASH_UNCORRECTABLE_MISMATCH): { - return "Flash uncorrectable mismatch"; - break; - } - case (mpsoc::status_code::DEFAULT_ERROR_CODE): { - return "Default error code"; - break; - } - default: - std::stringstream ss; - ss << "0x" << std::hex << status; - return ss.str(); - break; +LocalPoolDataSetBase* PlocMpsocHandler::getDataSetHandle(sid_t sid) { + if (sid == hkReport.getSid()) { + return &hkReport; } - return ""; + return nullptr; +} + +bool PlocMpsocHandler::dontCheckQueue() { + // The TC and TMs need to be handled strictly sequentially, so while a command is pending, + // more specifically while replies are still expected, do not check the queue.s + return commandIsPending; +} + +void PlocMpsocHandler::cmdDoneHandler(bool success, ReturnValue_t result) { + commandIsPending = false; + auto commandIter = deviceCommandMap.find(getPendingCommand()); + if (commandIter != deviceCommandMap.end()) { + commandIter->second.isExecuting = false; + if (commandIter->second.sendReplyTo != MessageQueueIF::NO_QUEUE) { + actionHelper.finish(success, commandIter->second.sendReplyTo, getPendingCommand(), result); + } + } + disableAllReplies(); } diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index 10e6bd5d..a82623b0 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -1,9 +1,9 @@ #ifndef BSP_Q7S_DEVICES_PLOC_PLOCMPSOCHANDLER_H_ #define BSP_Q7S_DEVICES_PLOC_PLOCMPSOCHANDLER_H_ -#include +#include #include -#include +#include #include #include @@ -28,9 +28,10 @@ * @note The sequence count in the space packets must be incremented with each received and sent * packet otherwise the MPSoC will reply with an acknowledgment failure report. * - * @author J. Meier + * NOTE: This is not an example for a good device handler, DO NOT USE THIS AS A REFERENCE HANDLER. + * @author J. Meier, R. Mueller */ -class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { +class PlocMpsocHandler : public DeviceHandlerBase, public CommandsActionsIF { public: /** * @brief Constructor @@ -43,10 +44,10 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { * module in the programmable logic * @param supervisorHandler Object ID of the supervisor handler */ - PlocMPSoCHandler(object_id_t objectId, object_id_t uartComIFid, CookieIF* comCookie, - PlocMPSoCHelper* plocMPSoCHelper, Gpio uartIsolatorSwitch, + PlocMpsocHandler(object_id_t objectId, object_id_t uartComIFid, CookieIF* comCookie, + PlocMpsocSpecialComHelper* plocMPSoCHelper, Gpio uartIsolatorSwitch, object_id_t supervisorHandler); - virtual ~PlocMPSoCHandler(); + virtual ~PlocMpsocHandler(); virtual ReturnValue_t initialize() override; ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size) override; @@ -77,7 +78,9 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { uint8_t expectedReplies = 1, bool useAlternateId = false, DeviceCommandId_t alternateReplyID = 0) override; size_t getNextReplyLength(DeviceCommandId_t deviceCommand) override; - virtual ReturnValue_t doSendReadHook() override; + ReturnValue_t doSendReadHook() override; + LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; + bool dontCheckQueue() override; private: static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_MPSOC_HANDLER; @@ -103,7 +106,8 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { static const uint16_t APID_MASK = 0x7FF; static const uint16_t PACKET_SEQUENCE_COUNT_MASK = 0x3FFF; - static const uint8_t STATUS_OFFSET = 10; + + mpsoc::HkReport hkReport; MessageQueueIF* eventQueue = nullptr; MessageQueueIF* commandActionHelperQueue = nullptr; @@ -114,6 +118,41 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { SpacePacketCreator creator; ploc::SpTcParams spParams = ploc::SpTcParams(creator); + PoolEntry peStatus = PoolEntry(); + PoolEntry peMode = PoolEntry(); + PoolEntry peDownlinkPwrOn = PoolEntry(); + PoolEntry peDownlinkReplyActive = PoolEntry(); + PoolEntry peDownlinkJesdSyncStatus = PoolEntry(); + PoolEntry peDownlinkDacStatus = PoolEntry(); + PoolEntry peCameraStatus = PoolEntry(); + PoolEntry peCameraSdiStatus = PoolEntry(); + PoolEntry peCameraFpgaTemp = PoolEntry(); + PoolEntry peCameraSocTemp = PoolEntry(); + PoolEntry peSysmonTemp = PoolEntry(); + PoolEntry peSysmonVccInt = PoolEntry(); + PoolEntry peSysmonVccAux = PoolEntry(); + PoolEntry peSysmonVccBram = PoolEntry(); + PoolEntry peSysmonVccPaux = PoolEntry(); + PoolEntry peSysmonVccPint = PoolEntry(); + PoolEntry peSysmonVccPdro = PoolEntry(); + PoolEntry peSysmonMb12V = PoolEntry(); + PoolEntry peSysmonMb3V3 = PoolEntry(); + PoolEntry peSysmonMb1V8 = PoolEntry(); + PoolEntry peSysmonVcc12V = PoolEntry(); + PoolEntry peSysmonVcc5V = PoolEntry(); + PoolEntry peSysmonVcc3V3 = PoolEntry(); + PoolEntry peSysmonVcc3V3VA = PoolEntry(); + PoolEntry peSysmonVcc2V5DDR = PoolEntry(); + PoolEntry peSysmonVcc1V2DDR = PoolEntry(); + PoolEntry peSysmonVcc0V9 = PoolEntry(); + PoolEntry peSysmonVcc0V6VTT = PoolEntry(); + PoolEntry peSysmonSafeCotsCur = PoolEntry(); + PoolEntry peSysmonNvm4XoCur = PoolEntry(); + PoolEntry peSemUncorrectableErrs = PoolEntry(); + PoolEntry peSemCorrectableErrs = PoolEntry(); + PoolEntry peSemStatus = PoolEntry(); + PoolEntry peRebootMpsocRequired = PoolEntry(); + /** * This variable is used to store the id of the next reply to receive. This is necessary * because the PLOC sends as reply to each command at least one acknowledgment and execution @@ -123,13 +162,14 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { SerialComIF* uartComIf = nullptr; - PlocMPSoCHelper* plocMPSoCHelper = nullptr; + PlocMpsocSpecialComHelper* specialComHelper = nullptr; Gpio uartIsolatorSwitch; object_id_t supervisorHandler = 0; CommandActionHelper commandActionHelper; // Used to block incoming commands when MPSoC helper class is currently executing a command - bool plocMPSoCHelperExecuting = false; + bool specialComHelperExecuting = false; + bool commandIsPending = false; struct TmMemReadReport { static const uint8_t FIX_SIZE = 14; @@ -137,20 +177,18 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { }; TmMemReadReport tmMemReadReport; - - struct TmCamCmdRpt { - size_t rememberSpacePacketSize = 0; - }; - - TmCamCmdRpt tmCamCmdRpt; + Countdown cmdCountdown = Countdown(10000); struct TelemetryBuffer { uint16_t length = 0; uint8_t buffer[mpsoc::SP_MAX_SIZE]; }; + size_t foundPacketLen = 0; TelemetryBuffer tmBuffer; + uint32_t waitCycles = 0; + enum class StartupState { IDLE, HW_INIT, WAIT_CYCLES, DONE } startupState = StartupState::IDLE; enum class PowerState { OFF, BOOTING, SHUTDOWN, ON }; PowerState powerState = PowerState::OFF; @@ -167,6 +205,8 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { ReturnValue_t prepareTcReplayStop(); ReturnValue_t prepareTcDownlinkPwrOn(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcDownlinkPwrOff(); + ReturnValue_t prepareTcGetHkReport(); + ReturnValue_t prepareTcGetDirContent(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcReplayWriteSequence(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcCamCmdSend(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcModeIdle(); @@ -174,7 +214,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { ReturnValue_t prepareTcSimplexSendFile(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcDownlinkDataModulate(const uint8_t* commandData, size_t commandDataLen); ReturnValue_t prepareTcModeSnapshot(); - void finishTcPrep(size_t packetLen); + ReturnValue_t finishTcPrep(mpsoc::TcBase& tcBase); /** * @brief This function checks the crc of the received PLOC reply. @@ -213,6 +253,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { */ ReturnValue_t handleMemoryReadReport(const uint8_t* data); + ReturnValue_t handleGetHkReport(const uint8_t* data); ReturnValue_t handleCamCmdRpt(const uint8_t* data); /** @@ -231,7 +272,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { * @param dataSize Size of telemetry in bytes. * @param replyId Id of the reply. This will be added to the ActionMessage. */ - void handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId); + void handleDeviceTm(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId); /** * @brief In case an acknowledgment failure reply has been received this function disables @@ -255,15 +296,11 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { */ void disableExeReportReply(); - void printStatus(const uint8_t* data); - ReturnValue_t prepareTcModeReplay(); - uint16_t getStatus(const uint8_t* data); + void cmdDoneHandler(bool success, ReturnValue_t result); void handleActionCommandFailure(ActionId_t actionId); - - std::string getStatusString(uint16_t status); }; #endif /* BSP_Q7S_DEVICES_PLOC_PLOCMPSOCHANDLER_H_ */ diff --git a/linux/payload/PlocMpsocHelper.cpp b/linux/payload/PlocMpsocHelper.cpp deleted file mode 100644 index a22462e2..00000000 --- a/linux/payload/PlocMpsocHelper.cpp +++ /dev/null @@ -1,355 +0,0 @@ -#include - -#include -#include - -#ifdef XIPHOS_Q7S -#include "bsp_q7s/fs/FilesystemHelper.h" -#endif - -#include "mission/utility/Timestamp.h" - -using namespace ploc; - -PlocMPSoCHelper::PlocMPSoCHelper(object_id_t objectId) : SystemObject(objectId) { - spParams.buf = commandBuffer; - spParams.maxSize = sizeof(commandBuffer); -} - -PlocMPSoCHelper::~PlocMPSoCHelper() {} - -ReturnValue_t PlocMPSoCHelper::initialize() { -#ifdef XIPHOS_Q7S - sdcMan = SdCardManager::instance(); - if (sdcMan == nullptr) { - sif::warning << "PlocMPSoCHelper::initialize: Invalid SD Card Manager" << std::endl; - return returnvalue::FAILED; - } -#endif - return returnvalue::OK; -} - -ReturnValue_t PlocMPSoCHelper::performOperation(uint8_t operationCode) { - ReturnValue_t result = returnvalue::OK; - semaphore.acquire(); - while (true) { -#if OBSW_THREAD_TRACING == 1 - trace::threadTrace(opCounter, "PLOC MPSOC Helper"); -#endif - switch (internalState) { - case InternalState::IDLE: { - semaphore.acquire(); - break; - } - case InternalState::FLASH_WRITE: { - result = performFlashWrite(); - if (result == returnvalue::OK) { - triggerEvent(MPSOC_FLASH_WRITE_SUCCESSFUL); - } else { - triggerEvent(MPSOC_FLASH_WRITE_FAILED); - } - internalState = InternalState::IDLE; - break; - } - default: - sif::debug << "PlocMPSoCHelper::performOperation: Invalid state" << std::endl; - break; - } - } -} - -ReturnValue_t PlocMPSoCHelper::setComIF(DeviceCommunicationIF* communicationInterface_) { - uartComIF = dynamic_cast(communicationInterface_); - if (uartComIF == nullptr) { - sif::warning << "PlocMPSoCHelper::initialize: Invalid uart com if" << std::endl; - return returnvalue::FAILED; - } - return returnvalue::OK; -} - -void PlocMPSoCHelper::setComCookie(CookieIF* comCookie_) { comCookie = comCookie_; } - -void PlocMPSoCHelper::setSequenceCount(SourceSequenceCounter* sequenceCount_) { - sequenceCount = sequenceCount_; -} - -ReturnValue_t PlocMPSoCHelper::startFlashWrite(std::string obcFile, std::string mpsocFile) { - ReturnValue_t result = returnvalue::OK; -#ifdef XIPHOS_Q7S - result = FilesystemHelper::checkPath(obcFile); - if (result != returnvalue::OK) { - return result; - } - result = FilesystemHelper::fileExists(mpsocFile); - if (result != returnvalue::OK) { - return result; - } -#endif -#ifdef TE0720_1CFA - if (not std::filesystem::exists(obcFile)) { - sif::warning << "PlocMPSoCHelper::startFlashWrite: File " << obcFile << "does not exist" - << std::endl; - return returnvalue::FAILED; - } -#endif - - flashWrite.obcFile = obcFile; - flashWrite.mpsocFile = mpsocFile; - internalState = InternalState::FLASH_WRITE; - result = resetHelper(); - if (result != returnvalue::OK) { - return result; - } - return result; -} - -ReturnValue_t PlocMPSoCHelper::resetHelper() { - ReturnValue_t result = returnvalue::OK; - semaphore.release(); - spParams.buf = commandBuffer; - terminate = false; - result = uartComIF->flushUartRxBuffer(comCookie); - return result; -} - -void PlocMPSoCHelper::stopProcess() { terminate = true; } - -ReturnValue_t PlocMPSoCHelper::performFlashWrite() { - ReturnValue_t result = returnvalue::OK; - result = flashfopen(); - if (result != returnvalue::OK) { - return result; - } - uint8_t tempData[mpsoc::MAX_DATA_SIZE]; - std::ifstream file(flashWrite.obcFile, std::ifstream::binary); - // Set position of next character to end of file input stream - file.seekg(0, file.end); - // tellg returns position of character in input stream - size_t remainingSize = file.tellg(); - size_t dataLength = 0; - size_t bytesRead = 0; - while (remainingSize > 0) { - if (terminate) { - return returnvalue::OK; - } - if (remainingSize > mpsoc::MAX_DATA_SIZE) { - dataLength = mpsoc::MAX_DATA_SIZE; - } else { - dataLength = remainingSize; - } - if (file.is_open()) { - file.seekg(bytesRead, file.beg); - file.read(reinterpret_cast(tempData), dataLength); - bytesRead += dataLength; - remainingSize -= dataLength; - } else { - return FILE_CLOSED_ACCIDENTALLY; - } - (*sequenceCount)++; - mpsoc::TcFlashWrite tc(spParams, *sequenceCount); - result = tc.buildPacket(tempData, dataLength); - if (result != returnvalue::OK) { - return result; - } - result = handlePacketTransmission(tc); - if (result != returnvalue::OK) { - return result; - } - } - result = flashfclose(); - if (result != returnvalue::OK) { - return result; - } - return result; -} - -ReturnValue_t PlocMPSoCHelper::flashfopen() { - ReturnValue_t result = returnvalue::OK; - spParams.buf = commandBuffer; - (*sequenceCount)++; - mpsoc::FlashFopen flashFopen(spParams, *sequenceCount); - result = flashFopen.createPacket(flashWrite.mpsocFile, mpsoc::FlashFopen::APPEND); - if (result != returnvalue::OK) { - return result; - } - result = handlePacketTransmission(flashFopen); - if (result != returnvalue::OK) { - return result; - } - return returnvalue::OK; -} - -ReturnValue_t PlocMPSoCHelper::flashfclose() { - ReturnValue_t result = returnvalue::OK; - spParams.buf = commandBuffer; - (*sequenceCount)++; - mpsoc::FlashFclose flashFclose(spParams, *sequenceCount); - result = flashFclose.createPacket(flashWrite.mpsocFile); - if (result != returnvalue::OK) { - return result; - } - result = handlePacketTransmission(flashFclose); - if (result != returnvalue::OK) { - return result; - } - return returnvalue::OK; -} - -ReturnValue_t PlocMPSoCHelper::handlePacketTransmission(ploc::SpTcBase& tc) { - ReturnValue_t result = returnvalue::OK; - result = sendCommand(tc); - if (result != returnvalue::OK) { - return result; - } - result = handleAck(); - if (result != returnvalue::OK) { - return result; - } - result = handleExe(); - if (result != returnvalue::OK) { - return result; - } - return returnvalue::OK; -} - -ReturnValue_t PlocMPSoCHelper::sendCommand(ploc::SpTcBase& tc) { - ReturnValue_t result = returnvalue::OK; - result = uartComIF->sendMessage(comCookie, tc.getFullPacket(), tc.getFullPacketLen()); - if (result != returnvalue::OK) { - sif::warning << "PlocMPSoCHelper::sendCommand: Failed to send command" << std::endl; - triggerEvent(MPSOC_SENDING_COMMAND_FAILED, result, static_cast(internalState)); - return result; - } - return result; -} - -ReturnValue_t PlocMPSoCHelper::handleAck() { - ReturnValue_t result = returnvalue::OK; - result = handleTmReception(mpsoc::SIZE_ACK_REPORT); - if (result != returnvalue::OK) { - return result; - } - SpTmReader tmPacket(tmBuf.data(), tmBuf.size()); - result = checkReceivedTm(tmPacket); - if (result != returnvalue::OK) { - return result; - } - uint16_t apid = tmPacket.getApid(); - if (apid != mpsoc::apid::ACK_SUCCESS) { - handleAckApidFailure(apid); - return returnvalue::FAILED; - } - return returnvalue::OK; -} - -void PlocMPSoCHelper::handleAckApidFailure(uint16_t apid) { - if (apid == mpsoc::apid::ACK_FAILURE) { - triggerEvent(MPSOC_ACK_FAILURE_REPORT, static_cast(internalState)); - sif::warning << "PlocMPSoCHelper::handleAckApidFailure: Received acknowledgement failure " - << "report" << std::endl; - } else { - triggerEvent(MPSOC_ACK_INVALID_APID, apid, static_cast(internalState)); - sif::warning << "PlocMPSoCHelper::handleAckApidFailure: Expected acknowledgement report " - << "but received space packet with apid " << std::hex << apid << std::endl; - } -} - -ReturnValue_t PlocMPSoCHelper::handleExe() { - ReturnValue_t result = returnvalue::OK; - - result = handleTmReception(mpsoc::SIZE_EXE_REPORT); - if (result != returnvalue::OK) { - return result; - } - ploc::SpTmReader tmPacket(tmBuf.data(), tmBuf.size()); - result = checkReceivedTm(tmPacket); - if (result != returnvalue::OK) { - return result; - } - uint16_t apid = tmPacket.getApid(); - if (apid != mpsoc::apid::EXE_SUCCESS) { - handleExeApidFailure(apid); - return returnvalue::FAILED; - } - return returnvalue::OK; -} - -void PlocMPSoCHelper::handleExeApidFailure(uint16_t apid) { - if (apid == mpsoc::apid::EXE_FAILURE) { - triggerEvent(MPSOC_EXE_FAILURE_REPORT, static_cast(internalState)); - sif::warning << "PlocMPSoCHelper::handleExeApidFailure: Received execution failure " - << "report" << std::endl; - } else { - triggerEvent(MPSOC_EXE_INVALID_APID, apid, static_cast(internalState)); - sif::warning << "PlocMPSoCHelper::handleExeApidFailure: Expected execution report " - << "but received space packet with apid " << std::hex << apid << std::endl; - } -} - -ReturnValue_t PlocMPSoCHelper::handleTmReception(size_t remainingBytes) { - ReturnValue_t result = returnvalue::OK; - size_t readBytes = 0; - size_t currentBytes = 0; - for (int retries = 0; retries < RETRIES; retries++) { - result = receive(tmBuf.data() + readBytes, ¤tBytes, remainingBytes); - if (result != returnvalue::OK) { - return result; - } - readBytes += currentBytes; - remainingBytes = remainingBytes - currentBytes; - if (remainingBytes == 0) { - break; - } - } - if (remainingBytes != 0) { - sif::warning << "PlocMPSoCHelper::handleTmReception: Failed to receive reply" << std::endl; - triggerEvent(MPSOC_MISSING_EXE, remainingBytes, static_cast(internalState)); - return returnvalue::FAILED; - } - return result; -} - -ReturnValue_t PlocMPSoCHelper::checkReceivedTm(SpTmReader& reader) { - ReturnValue_t result = reader.checkSize(); - if (result != returnvalue::OK) { - sif::error << "PlocMPSoCHelper::handleTmReception: Size check on received TM failed" - << std::endl; - triggerEvent(MPSOC_TM_SIZE_ERROR); - return result; - } - reader.checkCrc(); - if (result != returnvalue::OK) { - sif::warning << "PlocMPSoCHelper::handleTmReception: CRC check failed" << std::endl; - triggerEvent(MPSOC_TM_CRC_MISSMATCH, *sequenceCount); - return result; - } - (*sequenceCount)++; - uint16_t recvSeqCnt = reader.getSequenceCount(); - if (recvSeqCnt != *sequenceCount) { - triggerEvent(MPSOC_HELPER_SEQ_CNT_MISMATCH, *sequenceCount, recvSeqCnt); - *sequenceCount = recvSeqCnt; - } - return returnvalue::OK; -} - -ReturnValue_t PlocMPSoCHelper::receive(uint8_t* data, size_t* readBytes, size_t requestBytes) { - ReturnValue_t result = returnvalue::OK; - uint8_t* buffer = nullptr; - result = uartComIF->requestReceiveMessage(comCookie, requestBytes); - if (result != returnvalue::OK) { - sif::warning << "PlocMPSoCHelper::receive: Failed to request reply" << std::endl; - triggerEvent(MPSOC_HELPER_REQUESTING_REPLY_FAILED, result, - static_cast(static_cast(internalState))); - return returnvalue::FAILED; - } - result = uartComIF->readReceivedMessage(comCookie, &buffer, readBytes); - if (result != returnvalue::OK) { - sif::warning << "PlocMPSoCHelper::receive: Failed to read received message" << std::endl; - triggerEvent(MPSOC_HELPER_READING_REPLY_FAILED, result, static_cast(internalState)); - return returnvalue::FAILED; - } - if (*readBytes > 0) { - std::memcpy(data, buffer, *readBytes); - } - return result; -} diff --git a/linux/payload/PlocMpsocSpecialComHelper.cpp b/linux/payload/PlocMpsocSpecialComHelper.cpp new file mode 100644 index 00000000..acf88abd --- /dev/null +++ b/linux/payload/PlocMpsocSpecialComHelper.cpp @@ -0,0 +1,544 @@ +#include +#include +#include +#include + +#include +#include + +#ifdef XIPHOS_Q7S +#include "bsp_q7s/fs/FilesystemHelper.h" +#endif + +#include "mission/utility/Timestamp.h" + +using namespace ploc; + +PlocMpsocSpecialComHelper::PlocMpsocSpecialComHelper(object_id_t objectId) + : SystemObject(objectId) { + spParams.buf = commandBuffer; + spParams.maxSize = sizeof(commandBuffer); +} + +PlocMpsocSpecialComHelper::~PlocMpsocSpecialComHelper() {} + +ReturnValue_t PlocMpsocSpecialComHelper::initialize() { +#ifdef XIPHOS_Q7S + sdcMan = SdCardManager::instance(); + if (sdcMan == nullptr) { + sif::warning << "PlocMPSoCHelper::initialize: Invalid SD Card Manager" << std::endl; + return returnvalue::FAILED; + } +#endif + return returnvalue::OK; +} + +ReturnValue_t PlocMpsocSpecialComHelper::performOperation(uint8_t operationCode) { + ReturnValue_t result = returnvalue::OK; + semaphore.acquire(); + while (true) { +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "PLOC MPSOC Helper"); +#endif + switch (internalState) { + case InternalState::IDLE: { + semaphore.acquire(); + break; + } + case InternalState::FLASH_WRITE: { + result = performFlashWrite(); + if (result == returnvalue::OK) { + triggerEvent(MPSOC_FLASH_WRITE_SUCCESSFUL, sequenceCount->get()); + } else { + triggerEvent(MPSOC_FLASH_WRITE_FAILED, sequenceCount->get()); + } + internalState = InternalState::IDLE; + break; + } + case InternalState::FLASH_READ: { + result = performFlashRead(); + if (result == returnvalue::OK) { + triggerEvent(MPSOC_FLASH_READ_SUCCESSFUL, sequenceCount->get()); + } else { + triggerEvent(MPSOC_FLASH_READ_FAILED, sequenceCount->get()); + } + internalState = InternalState::IDLE; + break; + } + default: + sif::debug << "PlocMPSoCHelper::performOperation: Invalid state" << std::endl; + break; + } + } +} + +ReturnValue_t PlocMpsocSpecialComHelper::setComIF(DeviceCommunicationIF* communicationInterface_) { + uartComIF = dynamic_cast(communicationInterface_); + if (uartComIF == nullptr) { + sif::warning << "PlocMPSoCHelper::initialize: Invalid uart com if" << std::endl; + return returnvalue::FAILED; + } + return returnvalue::OK; +} + +void PlocMpsocSpecialComHelper::setComCookie(CookieIF* comCookie_) { comCookie = comCookie_; } + +void PlocMpsocSpecialComHelper::setSequenceCount(SourceSequenceCounter* sequenceCount_) { + sequenceCount = sequenceCount_; +} + +ReturnValue_t PlocMpsocSpecialComHelper::startFlashWrite(std::string obcFile, + std::string mpsocFile) { + if (internalState != InternalState::IDLE) { + return returnvalue::FAILED; + } + ReturnValue_t result = startFlashReadOrWriteBase(std::move(obcFile), std::move(mpsocFile)); + if (result != returnvalue::OK) { + return result; + } + internalState = InternalState::FLASH_WRITE; + return semaphore.release(); +} + +ReturnValue_t PlocMpsocSpecialComHelper::startFlashRead(std::string obcFile, std::string mpsocFile, + size_t readFileSize) { + if (internalState != InternalState::IDLE) { + return returnvalue::FAILED; + } + ReturnValue_t result = startFlashReadOrWriteBase(std::move(obcFile), std::move(mpsocFile)); + if (result != returnvalue::OK) { + return result; + } + flashReadAndWrite.totalReadSize = readFileSize; + internalState = InternalState::FLASH_READ; + return semaphore.release(); +} + +void PlocMpsocSpecialComHelper::resetHelper() { + spParams.buf = commandBuffer; + terminate = false; + uartComIF->flushUartRxBuffer(comCookie); +} + +void PlocMpsocSpecialComHelper::stopProcess() { terminate = true; } + +ReturnValue_t PlocMpsocSpecialComHelper::performFlashWrite() { + ReturnValue_t result = returnvalue::OK; + std::ifstream file(flashReadAndWrite.obcFile, std::ifstream::binary); + if (file.bad()) { + return returnvalue::FAILED; + } + result = flashfopen(mpsoc::FileAccessModes::WRITE | mpsoc::FileAccessModes::OPEN_ALWAYS); + if (result != returnvalue::OK) { + return result; + } + // Set position of next character to end of file input stream + file.seekg(0, file.end); + // tellg returns position of character in input stream + size_t remainingSize = file.tellg(); + size_t dataLength = 0; + size_t bytesRead = 0; + while (remainingSize > 0) { + if (terminate) { + return returnvalue::OK; + } + // The minus 4 is necessary for unknown reasons. Maybe some bug in the ILH software? + if (remainingSize > mpsoc::MAX_FLASH_WRITE_DATA_SIZE - 4) { + dataLength = mpsoc::MAX_FLASH_WRITE_DATA_SIZE - 4; + } else { + dataLength = remainingSize; + } + if (file.bad() or not file.is_open()) { + return FILE_WRITE_ERROR; + } + file.seekg(bytesRead, file.beg); + file.read(reinterpret_cast(fileBuf.data()), dataLength); + bytesRead += dataLength; + remainingSize -= dataLength; + mpsoc::TcFlashWrite tc(spParams, *sequenceCount); + result = tc.setPayload(fileBuf.data(), dataLength); + if (result != returnvalue::OK) { + return result; + } + result = tc.finishPacket(); + if (result != returnvalue::OK) { + return result; + } + (*sequenceCount)++; + result = handlePacketTransmissionNoReply(tc); + if (result != returnvalue::OK) { + return result; + } + } + result = flashfclose(); + if (result != returnvalue::OK) { + return result; + } + return result; +} + +ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() { + std::error_code e; + std::ofstream ofile(flashReadAndWrite.obcFile, std::ios::trunc | std::ios::binary); + if (ofile.bad()) { + return returnvalue::FAILED; + } + ReturnValue_t result = flashfopen(mpsoc::FileAccessModes::READ); + if (result != returnvalue::OK) { + std::filesystem::remove(flashReadAndWrite.obcFile, e); + return result; + } + size_t readSoFar = 0; + size_t nextReadSize = mpsoc::MAX_FLASH_READ_DATA_SIZE; + while (readSoFar < flashReadAndWrite.totalReadSize) { + if (terminate) { + std::filesystem::remove(flashReadAndWrite.obcFile, e); + return returnvalue::OK; + } + nextReadSize = mpsoc::MAX_FLASH_READ_DATA_SIZE; + if (flashReadAndWrite.totalReadSize - readSoFar < mpsoc::MAX_FLASH_READ_DATA_SIZE) { + nextReadSize = flashReadAndWrite.totalReadSize - readSoFar; + } + if (ofile.bad() or not ofile.is_open()) { + std::filesystem::remove(flashReadAndWrite.obcFile, e); + return FILE_READ_ERROR; + } + mpsoc::TcFlashRead flashReadRequest(spParams, *sequenceCount); + result = flashReadRequest.setPayload(nextReadSize); + if (result != returnvalue::OK) { + std::filesystem::remove(flashReadAndWrite.obcFile, e); + return result; + } + result = flashReadRequest.finishPacket(); + if (result != returnvalue::OK) { + std::filesystem::remove(flashReadAndWrite.obcFile, e); + return result; + } + (*sequenceCount)++; + result = handlePacketTransmissionFlashRead(flashReadRequest, ofile, nextReadSize); + if (result != returnvalue::OK) { + std::filesystem::remove(flashReadAndWrite.obcFile, e); + return result; + } + readSoFar += nextReadSize; + } + result = flashfclose(); + if (result != returnvalue::OK) { + return result; + } + return result; +} + +ReturnValue_t PlocMpsocSpecialComHelper::flashfopen(uint8_t mode) { + spParams.buf = commandBuffer; + mpsoc::FlashFopen flashFopen(spParams, *sequenceCount); + ReturnValue_t result = flashFopen.setPayload(flashReadAndWrite.mpsocFile, mode); + if (result != returnvalue::OK) { + return result; + } + result = flashFopen.finishPacket(); + if (result != returnvalue::OK) { + return result; + } + (*sequenceCount)++; + result = handlePacketTransmissionNoReply(flashFopen); + if (result != returnvalue::OK) { + return result; + } + return returnvalue::OK; +} + +ReturnValue_t PlocMpsocSpecialComHelper::flashfclose() { + spParams.buf = commandBuffer; + mpsoc::FlashFclose flashFclose(spParams, *sequenceCount); + ReturnValue_t result = flashFclose.finishPacket(); + if (result != returnvalue::OK) { + return result; + } + (*sequenceCount)++; + result = handlePacketTransmissionNoReply(flashFclose); + if (result != returnvalue::OK) { + return result; + } + return result; +} + +ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionFlashRead(mpsoc::TcFlashRead& tc, + std::ofstream& ofile, + size_t expectedReadLen) { + ReturnValue_t result = sendCommand(tc); + if (result != returnvalue::OK) { + return result; + } + result = handleAck(); + if (result != returnvalue::OK) { + return result; + } + result = handleTmReception(); + if (result != returnvalue::OK) { + return result; + } + + // We have the nominal case where the flash read report appears first, or the case where we + // get an EXE failure immediately. + if (spReader.getApid() == mpsoc::apid::TM_FLASH_READ_REPORT) { + result = handleFlashReadReply(ofile, expectedReadLen); + if (result != returnvalue::OK) { + return result; + } + return handleExe(); + } else if (spReader.getApid() == mpsoc::apid::EXE_FAILURE) { + handleExeFailure(); + } else { + triggerEvent(MPSOC_EXE_INVALID_APID, spReader.getApid(), static_cast(internalState)); + sif::warning << "PLOC MPSoC: Expected execution report " + << "but received space packet with apid " << std::hex << spReader.getApid() + << std::endl; + } + return returnvalue::FAILED; +} + +ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionNoReply(ploc::SpTcBase& tc) { + ReturnValue_t result = sendCommand(tc); + if (result != returnvalue::OK) { + return result; + } + result = handleAck(); + if (result != returnvalue::OK) { + return result; + } + return handleExe(); +} + +ReturnValue_t PlocMpsocSpecialComHelper::sendCommand(ploc::SpTcBase& tc) { + ReturnValue_t result = returnvalue::OK; + result = uartComIF->sendMessage(comCookie, tc.getFullPacket(), tc.getFullPacketLen()); + if (result != returnvalue::OK) { + sif::warning << "PlocMPSoCHelper::sendCommand: Failed to send command" << std::endl; + triggerEvent(MPSOC_SENDING_COMMAND_FAILED, result, static_cast(internalState)); + return result; + } + return result; +} + +ReturnValue_t PlocMpsocSpecialComHelper::handleAck() { + ReturnValue_t result = returnvalue::OK; + result = handleTmReception(); + if (result != returnvalue::OK) { + return result; + } + result = checkReceivedTm(); + if (result != returnvalue::OK) { + return result; + } + uint16_t apid = spReader.getApid(); + if (apid != mpsoc::apid::ACK_SUCCESS) { + handleAckApidFailure(spReader); + return returnvalue::FAILED; + } + return returnvalue::OK; +} + +void PlocMpsocSpecialComHelper::handleAckApidFailure(const ploc::SpTmReader& reader) { + uint16_t apid = reader.getApid(); + if (apid == mpsoc::apid::ACK_FAILURE) { + uint16_t status = mpsoc::getStatusFromRawData(reader.getFullData()); + sif::warning << "PLOC MPSoC ACK Failure: " << mpsoc::getStatusString(status) << std::endl; + triggerEvent(MPSOC_ACK_FAILURE_REPORT, static_cast(internalState), status); + } else { + triggerEvent(MPSOC_ACK_INVALID_APID, apid, static_cast(internalState)); + sif::warning << "PlocMPSoCHelper::handleAckApidFailure: Expected acknowledgement report " + << "but received space packet with apid " << std::hex << apid << std::endl; + } +} + +ReturnValue_t PlocMpsocSpecialComHelper::handleExe() { + ReturnValue_t result = returnvalue::OK; + + result = handleTmReception(); + if (result != returnvalue::OK) { + return result; + } + result = checkReceivedTm(); + if (result != returnvalue::OK) { + return result; + } + uint16_t apid = spReader.getApid(); + if (apid == mpsoc::apid::EXE_FAILURE) { + handleExeFailure(); + return returnvalue::FAILED; + } else if (apid != mpsoc::apid::EXE_SUCCESS) { + triggerEvent(MPSOC_EXE_INVALID_APID, apid, static_cast(internalState)); + sif::warning << "PLOC MPSoC: Expected execution report " + << "but received space packet with apid " << std::hex << apid << std::endl; + } + return returnvalue::OK; +} + +void PlocMpsocSpecialComHelper::handleExeFailure() { + uint16_t status = mpsoc::getStatusFromRawData(spReader.getFullData()); + sif::warning << "PLOC MPSoC EXE Failure: " << mpsoc::getStatusString(status) << std::endl; + triggerEvent(MPSOC_EXE_FAILURE_REPORT, static_cast(internalState)); +} + +ReturnValue_t PlocMpsocSpecialComHelper::handleTmReception() { + ReturnValue_t result = returnvalue::OK; + tmCountdown.resetTimer(); + size_t readBytes = 0; + size_t currentBytes = 0; + uint32_t usleepDelay = 5; + size_t fullPacketLen = 0; + while (true) { + if (tmCountdown.hasTimedOut()) { + triggerEvent(MPSOC_READ_TIMEOUT, tmCountdown.getTimeoutMs()); + return returnvalue::FAILED; + } + result = receive(tmBuf.data() + readBytes, 6, ¤tBytes); + if (result != returnvalue::OK) { + return result; + } + spReader.setReadOnlyData(tmBuf.data(), tmBuf.size()); + fullPacketLen = spReader.getFullPacketLen(); + readBytes += currentBytes; + if (readBytes == 6) { + break; + } + usleep(usleepDelay); + if (usleepDelay < 200000) { + usleepDelay *= 4; + } + } + while (true) { + if (tmCountdown.hasTimedOut()) { + triggerEvent(MPSOC_READ_TIMEOUT, tmCountdown.getTimeoutMs()); + return returnvalue::FAILED; + } + result = receive(tmBuf.data() + readBytes, fullPacketLen - readBytes, ¤tBytes); + readBytes += currentBytes; + if (fullPacketLen == readBytes) { + break; + } + usleep(usleepDelay); + if (usleepDelay < 200000) { + usleepDelay *= 4; + } + } + // arrayprinter::print(tmBuf.data(), readBytes); + return result; +} + +ReturnValue_t PlocMpsocSpecialComHelper::handleFlashReadReply(std::ofstream& ofile, + size_t expectedReadLen) { + ReturnValue_t result = checkReceivedTm(); + if (result != returnvalue::OK) { + return result; + } + uint16_t apid = spReader.getApid(); + if (apid != mpsoc::apid::TM_FLASH_READ_REPORT) { + triggerEvent(MPSOC_FLASH_READ_PACKET_ERROR, FlashReadErrorType::FLASH_READ_APID_ERROR); + sif::warning << "PLOC MPSoC Flash Read: Unexpected APID" << std::endl; + return result; + } + const uint8_t* packetData = spReader.getPacketData(); + size_t deserDummy = spReader.getPacketDataLen() - mpsoc::CRC_SIZE; + uint32_t receivedReadLen = 0; + // I think this is buggy, weird stuff in the short name field. + // std::string receivedShortName = std::string(reinterpret_cast(packetData), 12); + // if (receivedShortName != flashReadAndWrite.mpsocFile.substr(0, 11)) { + // sif::warning << "PLOC MPSoC Flash Read: Missmatch between request file name and " + // "received file name" + // << std::endl; + // triggerEvent(MPSOC_FLASH_READ_PACKET_ERROR, FlashReadErrorType::FLASH_READ_FILENAME_ERROR); + // return returnvalue::FAILED; + // } + packetData += 12; + result = SerializeAdapter::deSerialize(&receivedReadLen, &packetData, &deserDummy, + SerializeIF::Endianness::NETWORK); + if (result != returnvalue::OK) { + return result; + } + if (receivedReadLen != expectedReadLen) { + sif::warning << "PLOC MPSoC Flash Read: Missmatch between request read length and " + "received read length" + << std::endl; + triggerEvent(MPSOC_FLASH_READ_PACKET_ERROR, FlashReadErrorType::FLASH_READ_READLEN_ERROR); + return returnvalue::FAILED; + } + ofile.write(reinterpret_cast(packetData), receivedReadLen); + return returnvalue::OK; +} + +ReturnValue_t PlocMpsocSpecialComHelper::fileCheck(std::string obcFile) { +#ifdef XIPHOS_Q7S + ReturnValue_t result = FilesystemHelper::checkPath(obcFile); + if (result != returnvalue::OK) { + return result; + } +#elif defined(TE0720_1CFA) + if (not std::filesystem::exists(obcFile)) { + sif::warning << "PlocMPSoCHelper::startFlashWrite: File " << obcFile << "does not exist" + << std::endl; + return returnvalue::FAILED; + } +#endif + return returnvalue::OK; +} + +ReturnValue_t PlocMpsocSpecialComHelper::startFlashReadOrWriteBase(std::string obcFile, + std::string mpsocFile) { + ReturnValue_t result = fileCheck(obcFile); + if (result != returnvalue::OK) { + return result; + } + + flashReadAndWrite.obcFile = std::move(obcFile); + flashReadAndWrite.mpsocFile = std::move(mpsocFile); + resetHelper(); + return returnvalue::OK; +} + +ReturnValue_t PlocMpsocSpecialComHelper::checkReceivedTm() { + ReturnValue_t result = spReader.checkSize(); + if (result != returnvalue::OK) { + sif::error << "PLOC MPSoC: Size check on received TM failed" << std::endl; + triggerEvent(MPSOC_TM_SIZE_ERROR); + return result; + } + spReader.checkCrc(); + if (result != returnvalue::OK) { + sif::warning << "PLOC MPSoC: CRC check failed" << std::endl; + triggerEvent(MPSOC_TM_CRC_MISSMATCH, *sequenceCount); + return result; + } + uint16_t recvSeqCnt = spReader.getSequenceCount(); + if (recvSeqCnt != *sequenceCount) { + triggerEvent(MPSOC_HELPER_SEQ_CNT_MISMATCH, *sequenceCount, recvSeqCnt); + *sequenceCount = recvSeqCnt; + } + // This sequence count ping pong does not make any sense but it is how the MPSoC expects it. + (*sequenceCount)++; + return returnvalue::OK; +} + +ReturnValue_t PlocMpsocSpecialComHelper::receive(uint8_t* data, size_t requestBytes, + size_t* readBytes) { + ReturnValue_t result = returnvalue::OK; + uint8_t* buffer = nullptr; + result = uartComIF->requestReceiveMessage(comCookie, requestBytes); + if (result != returnvalue::OK) { + sif::warning << "PlocMPSoCHelper::receive: Failed to request reply" << std::endl; + triggerEvent(MPSOC_HELPER_REQUESTING_REPLY_FAILED, result, + static_cast(static_cast(internalState))); + return returnvalue::FAILED; + } + result = uartComIF->readReceivedMessage(comCookie, &buffer, readBytes); + if (result != returnvalue::OK) { + sif::warning << "PlocMPSoCHelper::receive: Failed to read received message" << std::endl; + triggerEvent(MPSOC_HELPER_READING_REPLY_FAILED, result, static_cast(internalState)); + return returnvalue::FAILED; + } + if (*readBytes > 0) { + std::memcpy(data, buffer, *readBytes); + } + return result; +} diff --git a/linux/payload/PlocMpsocHelper.h b/linux/payload/PlocMpsocSpecialComHelper.h similarity index 69% rename from linux/payload/PlocMpsocHelper.h rename to linux/payload/PlocMpsocSpecialComHelper.h index b74c0844..bc6bec8c 100644 --- a/linux/payload/PlocMpsocHelper.h +++ b/linux/payload/PlocMpsocSpecialComHelper.h @@ -1,11 +1,12 @@ #ifndef BSP_Q7S_DEVICES_PLOCMPSOCHELPER_H_ #define BSP_Q7S_DEVICES_PLOCMPSOCHELPER_H_ -#include +#include #include #include +#include "OBSWConfig.h" #include "fsfw/devicehandlers/CookieIF.h" #include "fsfw/objectmanager/SystemObject.h" #include "fsfw/osal/linux/BinarySemaphore.h" @@ -22,14 +23,14 @@ * MPSoC and OBC. * @author J. Meier */ -class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { +class PlocMpsocSpecialComHelper : public SystemObject, public ExecutableObjectIF { public: static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_MPSOC_HELPER; //! [EXPORT] : [COMMENT] Flash write fails static const Event MPSOC_FLASH_WRITE_FAILED = MAKE_EVENT(0, severity::LOW); //! [EXPORT] : [COMMENT] Flash write successful - static const Event MPSOC_FLASH_WRITE_SUCCESSFUL = MAKE_EVENT(1, severity::LOW); + static const Event MPSOC_FLASH_WRITE_SUCCESSFUL = MAKE_EVENT(1, severity::INFO); //! [EXPORT] : [COMMENT] Communication interface returned failure when trying to send the command //! to the MPSoC //! P1: Return value returned by the communication interface sendMessage function @@ -71,9 +72,19 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { static const Event MPSOC_HELPER_SEQ_CNT_MISMATCH = MAKE_EVENT(11, severity::LOW); static const Event MPSOC_TM_SIZE_ERROR = MAKE_EVENT(12, severity::LOW); static const Event MPSOC_TM_CRC_MISSMATCH = MAKE_EVENT(13, severity::LOW); + static const Event MPSOC_FLASH_READ_PACKET_ERROR = MAKE_EVENT(14, severity::LOW); + static const Event MPSOC_FLASH_READ_FAILED = MAKE_EVENT(15, severity::LOW); + static const Event MPSOC_FLASH_READ_SUCCESSFUL = MAKE_EVENT(16, severity::INFO); + static const Event MPSOC_READ_TIMEOUT = MAKE_EVENT(17, severity::LOW); - PlocMPSoCHelper(object_id_t objectId); - virtual ~PlocMPSoCHelper(); + enum FlashReadErrorType : uint32_t { + FLASH_READ_APID_ERROR = 0, + FLASH_READ_FILENAME_ERROR = 1, + FLASH_READ_READLEN_ERROR = 2 + }; + + PlocMpsocSpecialComHelper(object_id_t objectId); + virtual ~PlocMpsocSpecialComHelper(); ReturnValue_t initialize() override; ReturnValue_t performOperation(uint8_t operationCode = 0) override; @@ -90,6 +101,14 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { * @return returnvalue::OK if successful, otherwise error return value */ ReturnValue_t startFlashWrite(std::string obcFile, std::string mpsocFile); + /** + * + * @param obcFile Full target file name on OBC + * @param mpsocFile The file on the MPSoC which should be copied ot the OBC + * @param readFileSize The size of the file on the MPSoC. + * @return + */ + ReturnValue_t startFlashRead(std::string obcFile, std::string mpsocFile, size_t readFileSize); /** * @brief Can be used to interrupt a running data transfer. @@ -104,20 +123,25 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { private: static const uint8_t INTERFACE_ID = CLASS_ID::PLOC_MPSOC_HELPER; - //! [EXPORT] : [COMMENT] File accidentally close - static const ReturnValue_t FILE_CLOSED_ACCIDENTALLY = MAKE_RETURN_CODE(0xA0); + //! [EXPORT] : [COMMENT] File error occured for file transfers from OBC to the MPSoC. + static const ReturnValue_t FILE_WRITE_ERROR = MAKE_RETURN_CODE(0xA0); + //! [EXPORT] : [COMMENT] File error occured for file transfers from MPSoC to OBC. + static const ReturnValue_t FILE_READ_ERROR = MAKE_RETURN_CODE(0xA1); // Maximum number of times the communication interface retries polling data from the reply // buffer static const int RETRIES = 10000; - struct FlashWrite { + struct FlashInfo { std::string obcFile; std::string mpsocFile; }; - struct FlashWrite flashWrite; + struct FlashRead : public FlashInfo { + size_t totalReadSize = 0; + }; + struct FlashRead flashReadAndWrite; #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; #endif @@ -134,7 +158,10 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { SpacePacketCreator creator; ploc::SpTcParams spParams = ploc::SpTcParams(creator); - std::array tmBuf; + Countdown tmCountdown = Countdown(5000); + + std::array fileBuf{}; + std::array tmBuf{}; bool terminate = false; @@ -147,20 +174,27 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { CookieIF* comCookie = nullptr; // Sequence count, must be set by Ploc MPSoC Handler SourceSequenceCounter* sequenceCount = nullptr; + ploc::SpTmReader spReader; - ReturnValue_t resetHelper(); + void resetHelper(); ReturnValue_t performFlashWrite(); - ReturnValue_t flashfopen(); + ReturnValue_t performFlashRead(); + ReturnValue_t flashfopen(uint8_t accessMode); ReturnValue_t flashfclose(); - ReturnValue_t handlePacketTransmission(ploc::SpTcBase& tc); + ReturnValue_t handlePacketTransmissionNoReply(ploc::SpTcBase& tc); + ReturnValue_t handlePacketTransmissionFlashRead(mpsoc::TcFlashRead& tc, std::ofstream& ofile, + size_t expectedReadLen); + ReturnValue_t handleFlashReadReply(std::ofstream& ofile, size_t expectedReadLen); ReturnValue_t sendCommand(ploc::SpTcBase& tc); - ReturnValue_t receive(uint8_t* data, size_t* readBytes, size_t requestBytes); + ReturnValue_t receive(uint8_t* data, size_t requestBytes, size_t* readBytes); ReturnValue_t handleAck(); ReturnValue_t handleExe(); - void handleAckApidFailure(uint16_t apid); - void handleExeApidFailure(uint16_t apid); - ReturnValue_t handleTmReception(size_t remainingBytes); - ReturnValue_t checkReceivedTm(ploc::SpTmReader& reader); + ReturnValue_t startFlashReadOrWriteBase(std::string obcFile, std::string mpsocFile); + ReturnValue_t fileCheck(std::string obcFile); + void handleAckApidFailure(const ploc::SpTmReader& reader); + void handleExeFailure(); + ReturnValue_t handleTmReception(); + ReturnValue_t checkReceivedTm(); }; #endif /* BSP_Q7S_DEVICES_PLOCMPSOCHELPER_H_ */ diff --git a/linux/payload/PlocSupervisorHandler.cpp b/linux/payload/PlocSupervisorHandler.cpp index 06a4cf07..682b8020 100644 --- a/linux/payload/PlocSupervisorHandler.cpp +++ b/linux/payload/PlocSupervisorHandler.cpp @@ -151,7 +151,7 @@ void PlocSupervisorHandler::doStartUp() { } } } - if (startupState == StartupState::SET_TIME_EXECUTING) { + if (startupState == StartupState::TIME_WAS_SET) { startupState = StartupState::ON; } if (startupState == StartupState::ON) { @@ -176,7 +176,7 @@ ReturnValue_t PlocSupervisorHandler::buildNormalDeviceCommand(DeviceCommandId_t* ReturnValue_t PlocSupervisorHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { if (startupState == StartupState::SET_TIME) { *id = supv::SET_TIME_REF; - startupState = StartupState::SET_TIME_EXECUTING; + startupState = StartupState::WAIT_FOR_TIME_REPLY; return buildCommandFromCommand(*id, nullptr, 0); } return NOTHING_TO_SEND; @@ -1909,6 +1909,10 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionSuccessReport(ExecutionRepor case supv::SET_TIME_REF: { // We could only allow proper bootup when the time was set successfully, but // this makes debugging difficult. + + if (startupState == StartupState::WAIT_FOR_TIME_REPLY) { + startupState = StartupState::TIME_WAS_SET; + } break; } default: diff --git a/linux/payload/PlocSupervisorHandler.h b/linux/payload/PlocSupervisorHandler.h index 78c20205..3e5ac2a0 100644 --- a/linux/payload/PlocSupervisorHandler.h +++ b/linux/payload/PlocSupervisorHandler.h @@ -99,7 +99,14 @@ class PlocSupervisorHandler : public DeviceHandlerBase { static const uint32_t MRAM_DUMP_TIMEOUT = 60000; // 4 s static const uint32_t BOOT_TIMEOUT = 4000; - enum class StartupState : uint8_t { OFF, BOOTING, SET_TIME, SET_TIME_EXECUTING, ON }; + enum class StartupState : uint8_t { + OFF, + BOOTING, + SET_TIME, + WAIT_FOR_TIME_REPLY, + TIME_WAS_SET, + ON + }; static constexpr bool SET_TIME_DURING_BOOT = true; diff --git a/linux/payload/plocMpsocHelpers.cpp b/linux/payload/plocMpsocHelpers.cpp new file mode 100644 index 00000000..9e5b11c9 --- /dev/null +++ b/linux/payload/plocMpsocHelpers.cpp @@ -0,0 +1,95 @@ +#include "plocMpsocHelpers.h" + +uint16_t mpsoc::getStatusFromRawData(const uint8_t* data) { + return (*(data + STATUS_OFFSET) << 8) | *(data + STATUS_OFFSET + 1); +} +std::string mpsoc::getStatusString(uint16_t status) { + switch (status) { + case (mpsoc::status_code::UNKNOWN_APID): { + return "Unknown APID"; + break; + } + case (mpsoc::status_code::INCORRECT_LENGTH): { + return "Incorrect length"; + break; + } + case (mpsoc::status_code::INCORRECT_CRC): { + return "Incorrect crc"; + break; + } + case (mpsoc::status_code::INCORRECT_PKT_SEQ_CNT): { + return "Incorrect packet sequence count"; + break; + } + case (mpsoc::status_code::TC_NOT_ALLOWED_IN_MODE): { + return "TC not allowed in this mode"; + break; + } + case (mpsoc::status_code::TC_EXEUTION_DISABLED): { + return "TC execution disabled"; + break; + } + case (mpsoc::status_code::FLASH_MOUNT_FAILED): { + return "Flash mount failed"; + break; + } + case (mpsoc::status_code::FLASH_FILE_ALREADY_OPEN): { + return "Flash file already open"; + break; + } + case (mpsoc::status_code::FLASH_FILE_ALREADY_CLOSED): { + return "Flash file already closed"; + break; + } + case (mpsoc::status_code::FLASH_FILE_OPEN_FAILED): { + return "Flash file open failed"; + break; + } + case (mpsoc::status_code::FLASH_FILE_NOT_OPEN): { + return "Flash file not open"; + break; + } + case (mpsoc::status_code::FLASH_UNMOUNT_FAILED): { + return "Flash unmount failed"; + break; + } + case (mpsoc::status_code::HEAP_ALLOCATION_FAILED): { + return "Heap allocation failed"; + break; + } + case (mpsoc::status_code::INVALID_PARAMETER): { + return "Invalid parameter"; + break; + } + case (mpsoc::status_code::NOT_INITIALIZED): { + return "Not initialized"; + break; + } + case (mpsoc::status_code::REBOOT_IMMINENT): { + return "Reboot imminent"; + break; + } + case (mpsoc::status_code::CORRUPT_DATA): { + return "Corrupt data"; + break; + } + case (mpsoc::status_code::FLASH_CORRECTABLE_MISMATCH): { + return "Flash correctable mismatch"; + break; + } + case (mpsoc::status_code::FLASH_UNCORRECTABLE_MISMATCH): { + return "Flash uncorrectable mismatch"; + break; + } + case (mpsoc::status_code::DEFAULT_ERROR_CODE): { + return "Default error code"; + break; + } + default: + std::stringstream ss; + ss << "0x" << std::hex << status; + return ss.str().c_str(); + break; + } + return ""; +} diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpsocHelpers.h similarity index 63% rename from linux/payload/plocMpscoDefs.h rename to linux/payload/plocMpsocHelpers.h index 975dad32..c9b08a28 100644 --- a/linux/payload/plocMpscoDefs.h +++ b/linux/payload/plocMpsocHelpers.h @@ -1,6 +1,7 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_PLOCMPSOCDEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_PLOCMPSOCDEFINITIONS_H_ +#include #include #include #include @@ -12,6 +13,61 @@ namespace mpsoc { +enum FileAccessModes : uint8_t { + // Opens a file, fails if the file does not exist. + OPEN_EXISTING = 0x00, + READ = 0x01, + WRITE = 0x02, + // Creates a new file, fails if it already exists. + CREATE_NEW = 0x04, + // Creates a new file, existing file is truncated and overwritten. + CREATE_ALWAYS = 0x08, + // Opens the file if it is existing. If not, a new file is created. + OPEN_ALWAYS = 0x10, + OPEN_APPEND = 0x30 +}; + +static constexpr uint32_t HK_SET_ID = 0; + +namespace poolid { +enum { + STATUS = 0, + MODE = 1, + DOWNLINK_PWR_ON = 2, + DOWNLINK_REPLY_ACTIIVE = 3, + DOWNLINK_JESD_SYNC_STATUS = 4, + DOWNLINK_DAC_STATUS = 5, + CAM_STATUS = 6, + CAM_SDI_STATUS = 7, + CAM_FPGA_TEMP = 8, + CAM_SOC_TEMP = 9, + SYSMON_TEMP = 10, + SYSMON_VCCINT = 11, + SYSMON_VCCAUX = 12, + SYSMON_VCCBRAM = 13, + SYSMON_VCCPAUX = 14, + SYSMON_VCCPINT = 15, + SYSMON_VCCPDRO = 16, + SYSMON_MB12V = 17, + SYSMON_MB3V3 = 18, + SYSMON_MB1V8 = 19, + SYSMON_VCC12V = 20, + SYSMON_VCC5V = 21, + SYSMON_VCC3V3 = 22, + SYSMON_VCC3V3VA = 23, + SYSMON_VCC2V5DDR = 24, + SYSMON_VCC1V2DDR = 25, + SYSMON_VCC0V9 = 26, + SYSMON_VCC0V6VTT = 27, + SYSMON_SAFE_COTS_CUR = 28, + SYSMON_NVM4_XO_CUR = 29, + SEM_UNCORRECTABLE_ERRS = 30, + SEM_CORRECTABLE_ERRS = 31, + SEM_STATUS = 32, + REBOOT_MPSOC_REQUIRED = 33, +}; +} + static const DeviceCommandId_t NONE = 0; static const DeviceCommandId_t TC_MEM_WRITE = 1; static const DeviceCommandId_t TC_MEM_READ = 2; @@ -20,7 +76,7 @@ static const DeviceCommandId_t EXE_REPORT = 5; static const DeviceCommandId_t TM_MEMORY_READ_REPORT = 6; static const DeviceCommandId_t TC_FLASHFOPEN = 7; static const DeviceCommandId_t TC_FLASHFCLOSE = 8; -static const DeviceCommandId_t TC_FLASHWRITE = 9; +static const DeviceCommandId_t TC_FLASH_WRITE_FULL_FILE = 9; static const DeviceCommandId_t TC_FLASHDELETE = 10; static const DeviceCommandId_t TC_REPLAY_START = 11; static const DeviceCommandId_t TC_REPLAY_STOP = 12; @@ -37,6 +93,11 @@ static const DeviceCommandId_t TC_CAM_TAKE_PIC = 22; static const DeviceCommandId_t TC_SIMPLEX_SEND_FILE = 23; static const DeviceCommandId_t TC_DOWNLINK_DATA_MODULATE = 24; static const DeviceCommandId_t TC_MODE_SNAPSHOT = 25; +static const DeviceCommandId_t TC_GET_HK_REPORT = 26; +static const DeviceCommandId_t TM_GET_HK_REPORT = 27; +static const DeviceCommandId_t TC_FLASH_GET_DIRECTORY_CONTENT = 28; +static const DeviceCommandId_t TM_FLASH_DIRECTORY_CONTENT = 29; +static constexpr DeviceCommandId_t TC_FLASH_READ_FULL_FILE = 30; // Will reset the sequence count of the OBSW static const DeviceCommandId_t OBSW_RESET_SEQ_COUNT = 50; @@ -45,6 +106,7 @@ static const uint16_t SIZE_ACK_REPORT = 14; static const uint16_t SIZE_EXE_REPORT = 14; static const uint16_t SIZE_TM_MEM_READ_REPORT = 18; static const uint16_t SIZE_TM_CAM_CMD_RPT = 18; +static constexpr size_t SIZE_TM_HK_REPORT = 369; /** * SpacePacket apids of PLOC telecommands and telemetry. @@ -57,23 +119,32 @@ static const uint16_t TC_DOWNLINK_PWR_ON = 0x113; static const uint16_t TC_MEM_WRITE = 0x114; static const uint16_t TC_MEM_READ = 0x115; static const uint16_t TC_CAM_TAKE_PIC = 0x116; -static const uint16_t TC_FLASHWRITE = 0x117; +static constexpr uint16_t TC_FLASHWRITE = 0x117; +static constexpr uint16_t TC_FLASHREAD = 0x118; static const uint16_t TC_FLASHFOPEN = 0x119; static const uint16_t TC_FLASHFCLOSE = 0x11A; +static constexpr uint16_t TC_FLASH_GET_DIRECTORY_CONTENT = 0x11B; static const uint16_t TC_FLASHDELETE = 0x11C; +static constexpr uint16_t TC_FLASH_CREATE_DIR = 0x11D; static const uint16_t TC_MODE_IDLE = 0x11E; static const uint16_t TC_MODE_REPLAY = 0x11F; static const uint16_t TC_MODE_SNAPSHOT = 0x120; static const uint16_t TC_DOWNLINK_DATA_MODULATE = 0x121; +static constexpr uint16_t TC_HK_GET_REPORT = 0x123; static const uint16_t TC_DOWNLINK_PWR_OFF = 0x124; static const uint16_t TC_CAM_CMD_SEND = 0x12C; +static constexpr uint16_t TC_FLASH_COPY_FILE = 0x12E; static const uint16_t TC_SIMPLEX_SEND_FILE = 0x130; -static const uint16_t TM_MEMORY_READ_REPORT = 0x404; + static const uint16_t ACK_SUCCESS = 0x400; static const uint16_t ACK_FAILURE = 0x401; static const uint16_t EXE_SUCCESS = 0x402; static const uint16_t EXE_FAILURE = 0x403; +static const uint16_t TM_MEMORY_READ_REPORT = 0x404; +static const uint16_t TM_FLASH_READ_REPORT = 0x405; +static constexpr uint16_t TM_FLASH_DIRECTORY_CONTENT = 0x406; static const uint16_t TM_CAM_CMD_RPT = 0x407; +static constexpr uint16_t TM_HK_GET_REPORT = 0x408; } // namespace apid /** Offset from first byte in space packet to first byte of data field */ @@ -83,6 +154,8 @@ static const char NULL_TERMINATOR = '\0'; static const uint8_t MIN_SPACE_PACKET_LENGTH = 7; static const uint8_t SPACE_PACKET_HEADER_SIZE = 6; +static const uint8_t STATUS_OFFSET = 10; + static constexpr size_t CRC_SIZE = 2; /** @@ -106,9 +179,15 @@ static const uint16_t LENGTH_TC_MEM_READ = 8; * at sheet README */ static constexpr size_t SP_MAX_SIZE = 1024; -static const size_t MAX_REPLY_SIZE = SP_MAX_SIZE * 3; -static const size_t MAX_COMMAND_SIZE = SP_MAX_SIZE; -static const size_t MAX_DATA_SIZE = 1016; +static constexpr size_t MAX_REPLY_SIZE = SP_MAX_SIZE * 3; +static constexpr size_t MAX_COMMAND_SIZE = SP_MAX_SIZE; +// 1016 bytes. +static constexpr size_t SP_MAX_DATA_SIZE = SP_MAX_SIZE - ccsds::HEADER_LEN - CRC_SIZE; +static constexpr size_t FLASH_READ_MIN_OVERHEAD = 16; +// 1000 bytes. +static const size_t MAX_FLASH_READ_DATA_SIZE = SP_MAX_DATA_SIZE - FLASH_READ_MIN_OVERHEAD; +// 1012 bytes, 4 bytes for the write length. +static constexpr size_t MAX_FLASH_WRITE_DATA_SIZE = SP_MAX_DATA_SIZE - sizeof(uint32_t); /** * The replay write sequence command has a maximum delay for the execution report which amounts to @@ -131,7 +210,7 @@ static const uint16_t TC_EXEUTION_DISABLED = 0x5E2; static const uint16_t FLASH_MOUNT_FAILED = 0x5E3; static const uint16_t FLASH_FILE_ALREADY_CLOSED = 0x5E4; static const uint16_t FLASH_FILE_OPEN_FAILED = 0x5E5; -static const uint16_t FLASH_FILE_ALREDY_OPEN = 0x5E6; +static const uint16_t FLASH_FILE_ALREADY_OPEN = 0x5E6; static const uint16_t FLASH_FILE_NOT_OPEN = 0x5E7; static const uint16_t FLASH_UNMOUNT_FAILED = 0x5E8; static const uint16_t HEAP_ALLOCATION_FAILED = 0x5E9; @@ -156,7 +235,7 @@ class TcBase : public ploc::SpTcBase, public MPSoCReturnValuesIF { virtual ~TcBase() = default; // Initial length field of space packet. Will always be updated when packet is created. - static const uint16_t INIT_LENGTH = 2; + static const uint16_t INIT_LENGTH = CRC_SIZE; /** * @brief Constructor @@ -166,47 +245,23 @@ class TcBase : public ploc::SpTcBase, public MPSoCReturnValuesIF { */ TcBase(ploc::SpTcParams params, uint16_t apid, uint16_t sequenceCount) : ploc::SpTcBase(params, apid, 0, sequenceCount) { + payloadStart = spParams.buf + ccsds::HEADER_LEN; spParams.setFullPayloadLen(INIT_LENGTH); } - ReturnValue_t buildPacket() { return buildPacket(nullptr, 0); } - /** - * @brief Function to initialize the space packet - * - * @param commandData Pointer to command specific data - * @param commandDataLen Length of command data - * + * @brief Function to finsh and write the space packet. It is expected that the user has + * set the payload fields in the child class* * @return returnvalue::OK if packet creation was successful, otherwise error return value */ - ReturnValue_t buildPacket(const uint8_t* commandData, size_t commandDataLen) { - payloadStart = spParams.buf + ccsds::HEADER_LEN; - ReturnValue_t res; - if (commandData != nullptr and commandDataLen > 0) { - res = initPacket(commandData, commandDataLen); - if (res != returnvalue::OK) { - return res; - } - } - + ReturnValue_t finishPacket() { updateSpFields(); - res = checkSizeAndSerializeHeader(); + ReturnValue_t res = checkSizeAndSerializeHeader(); if (res != returnvalue::OK) { return res; } return calcAndSetCrc(); } - - protected: - /** - * @brief Must be overwritten by the child class to define the command specific parameters - * - * @param commandData Pointer to received command data - * @param commandDataLen Length of received command data - */ - virtual ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) { - return returnvalue::OK; - } }; /** @@ -224,8 +279,7 @@ class TcMemRead : public TcBase { uint16_t getMemLen() const { return memLen; } - protected: - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; result = lengthCheck(commandDataLen); if (result != returnvalue::OK) { @@ -271,8 +325,7 @@ class TcMemWrite : public TcBase { TcMemWrite(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_MEM_WRITE, sequenceCount) {} - protected: - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; result = lengthCheck(commandDataLen); if (result != returnvalue::OK) { @@ -314,72 +367,58 @@ class TcMemWrite : public TcBase { /** * @brief Class to help creation of flash fopen command. */ -class FlashFopen : public ploc::SpTcBase { +class FlashFopen : public TcBase { public: FlashFopen(ploc::SpTcParams params, uint16_t sequenceCount) - : ploc::SpTcBase(params, apid::TC_FLASHFOPEN, sequenceCount) {} + : TcBase(params, apid::TC_FLASHFOPEN, sequenceCount) {} - static const char APPEND = 'a'; - static const char WRITE = 'w'; - static const char READ = 'r'; - - ReturnValue_t createPacket(std::string filename, char accessMode_) { - accessMode = accessMode_; + ReturnValue_t setPayload(std::string filename, uint8_t mode) { + accessMode = mode; size_t nameSize = filename.size(); - spParams.setFullPayloadLen(nameSize + sizeof(NULL_TERMINATOR) + sizeof(accessMode) + CRC_SIZE); + spParams.setFullPayloadLen(256 + sizeof(uint8_t) + CRC_SIZE); ReturnValue_t result = checkPayloadLen(); if (result != returnvalue::OK) { return result; } std::memcpy(payloadStart, filename.c_str(), nameSize); - *(spParams.buf + nameSize) = NULL_TERMINATOR; - std::memcpy(payloadStart + nameSize + sizeof(NULL_TERMINATOR), &accessMode, sizeof(accessMode)); - updateSpFields(); - return calcAndSetCrc(); + // payloadStart[nameSize] = NULL_TERMINATOR; + std::memset(payloadStart + nameSize, 0, 256 - nameSize); + // payloadStart[255] = NULL_TERMINATOR; + payloadStart[256] = accessMode; + return returnvalue::OK; } private: - char accessMode = APPEND; + uint8_t accessMode = FileAccessModes::OPEN_EXISTING; }; /** * @brief Class to help creation of flash fclose command. */ -class FlashFclose : public ploc::SpTcBase { +class FlashFclose : public TcBase { public: FlashFclose(ploc::SpTcParams params, uint16_t sequenceCount) - : ploc::SpTcBase(params, apid::TC_FLASHFCLOSE, sequenceCount) {} - - ReturnValue_t createPacket(std::string filename) { - size_t nameSize = filename.size(); - spParams.setFullPayloadLen(nameSize + sizeof(NULL_TERMINATOR) + CRC_SIZE); - ReturnValue_t result = checkPayloadLen(); - if (result != returnvalue::OK) { - return result; - } - std::memcpy(payloadStart, filename.c_str(), nameSize); - *(payloadStart + nameSize) = NULL_TERMINATOR; - return calcAndSetCrc(); + : TcBase(params, apid::TC_FLASHFCLOSE, sequenceCount) { + spParams.setFullPayloadLen(CRC_SIZE); } }; /** * @brief Class to build flash write space packet. */ -class TcFlashWrite : public ploc::SpTcBase { +class TcFlashWrite : public TcBase { public: TcFlashWrite(ploc::SpTcParams params, uint16_t sequenceCount) - : ploc::SpTcBase(params, apid::TC_FLASHWRITE, sequenceCount) {} + : TcBase(params, apid::TC_FLASHWRITE, sequenceCount) {} - ReturnValue_t buildPacket(const uint8_t* writeData, uint32_t writeLen_) { - ReturnValue_t result = returnvalue::OK; + ReturnValue_t setPayload(const uint8_t* writeData, uint32_t writeLen_) { writeLen = writeLen_; - if (writeLen > MAX_DATA_SIZE) { - sif::debug << "FlashWrite::createPacket: Command data too big" << std::endl; + if (writeLen > MAX_FLASH_WRITE_DATA_SIZE) { + sif::error << "TcFlashWrite: Command data too big" << std::endl; return returnvalue::FAILED; } - spParams.setFullPayloadLen(static_cast(writeLen) + 4 + CRC_SIZE); - result = checkPayloadLen(); + spParams.setFullPayloadLen(sizeof(uint32_t) + static_cast(writeLen) + CRC_SIZE); + ReturnValue_t result = checkPayloadLen(); if (result != returnvalue::OK) { return result; } @@ -389,11 +428,11 @@ class TcFlashWrite : public ploc::SpTcBase { if (result != returnvalue::OK) { return result; } - std::memcpy(payloadStart + sizeof(writeLen), writeData, writeLen); + std::memcpy(payloadStart + sizeof(uint32_t), writeData, writeLen); updateSpFields(); - auto res = checkSizeAndSerializeHeader(); - if (res != returnvalue::OK) { - return res; + result = checkSizeAndSerializeHeader(); + if (result != returnvalue::OK) { + return result; } return calcAndSetCrc(); } @@ -402,15 +441,52 @@ class TcFlashWrite : public ploc::SpTcBase { uint32_t writeLen = 0; }; +class TcFlashRead : public TcBase { + public: + TcFlashRead(ploc::SpTcParams params, uint16_t sequenceCount) + : TcBase(params, apid::TC_FLASHREAD, sequenceCount) {} + + ReturnValue_t setPayload(uint32_t readLen) { + if (readLen > MAX_FLASH_READ_DATA_SIZE) { + sif::error << "TcFlashRead: Read length " << readLen << " too large" << std::endl; + return returnvalue::FAILED; + } + spParams.setFullPayloadLen(sizeof(uint32_t) + CRC_SIZE); + ReturnValue_t result = checkPayloadLen(); + if (result != returnvalue::OK) { + return result; + } + size_t serializedSize = ccsds::HEADER_LEN; + result = SerializeAdapter::serialize(&readLen, payloadStart, &serializedSize, spParams.maxSize, + SerializeIF::Endianness::NETWORK); + if (result != returnvalue::OK) { + return result; + } + updateSpFields(); + result = checkSizeAndSerializeHeader(); + if (result != returnvalue::OK) { + return result; + } + result = calcAndSetCrc(); + if (result != returnvalue::OK) { + return result; + } + readSize = readLen; + return result; + } + + uint32_t readSize = 0; +}; + /** * @brief Class to help creation of flash delete command. */ -class TcFlashDelete : public ploc::SpTcBase { +class TcFlashDelete : public TcBase { public: TcFlashDelete(ploc::SpTcParams params, uint16_t sequenceCount) - : ploc::SpTcBase(params, apid::TC_FLASHDELETE, sequenceCount) {} + : TcBase(params, apid::TC_FLASHDELETE, sequenceCount) {} - ReturnValue_t buildPacket(std::string filename) { + ReturnValue_t setPayload(std::string filename) { size_t nameSize = filename.size(); spParams.setFullPayloadLen(nameSize + sizeof(NULL_TERMINATOR) + CRC_SIZE); auto res = checkPayloadLen(); @@ -449,8 +525,7 @@ class TcReplayStart : public TcBase { TcReplayStart(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_REPLAY_START, sequenceCount) {} - protected: - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; spParams.setFullPayloadLen(commandDataLen + CRC_SIZE); result = lengthCheck(commandDataLen); @@ -498,8 +573,7 @@ class TcDownlinkPwrOn : public TcBase { TcDownlinkPwrOn(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_DOWNLINK_PWR_ON, sequenceCount) {} - protected: - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; result = lengthCheck(commandDataLen); if (result != returnvalue::OK) { @@ -561,6 +635,30 @@ class TcDownlinkPwrOn : public TcBase { } }; +class TcGetHkReport : public TcBase { + public: + TcGetHkReport(ploc::SpTcParams params, uint16_t sequenceCount) + : TcBase(params, apid::TC_HK_GET_REPORT, sequenceCount) {} +}; + +class TcGetDirContent : public TcBase { + public: + TcGetDirContent(ploc::SpTcParams params, uint16_t sequenceCount) + : TcBase(params, apid::TC_FLASH_GET_DIRECTORY_CONTENT, sequenceCount) {} + + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { + ReturnValue_t result = returnvalue::OK; + // Yeah it needs to be 256.. even if the path is shorter. + spParams.setFullPayloadLen(256 + CRC_SIZE); + if (result != returnvalue::OK) { + return result; + } + std::memcpy(payloadStart, commandData, commandDataLen); + payloadStart[255] = '\0'; + return result; + } +}; + /** * @brief Class to build replay stop space packet. */ @@ -581,8 +679,7 @@ class TcReplayWriteSeq : public TcBase { TcReplayWriteSeq(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_REPLAY_WRITE_SEQUENCE, sequenceCount) {} - protected: - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; spParams.setFullPayloadLen(commandDataLen + sizeof(NULL_TERMINATOR) + CRC_SIZE); result = lengthCheck(commandDataLen); @@ -611,36 +708,69 @@ class TcReplayWriteSeq : public TcBase { /** * @brief Helps to extract the fields of the flash write command from the PUS packet. */ -class FlashWritePusCmd : public MPSoCReturnValuesIF { +class FlashBasePusCmd : public MPSoCReturnValuesIF { public: - FlashWritePusCmd(){}; + FlashBasePusCmd() = default; + virtual ~FlashBasePusCmd() = default; - ReturnValue_t extractFields(const uint8_t* commandData, size_t commandDataLen) { + virtual ReturnValue_t extractFields(const uint8_t* commandData, size_t commandDataLen) { if (commandDataLen > (config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE + MAX_FILENAME_SIZE)) { return INVALID_LENGTH; } - obcFile = std::string(reinterpret_cast(commandData)); - if (obcFile.size() > (config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE)) { + size_t fileLen = strnlen(reinterpret_cast(commandData), commandDataLen); + if (fileLen > (config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE)) { return FILENAME_TOO_LONG; } - mpsocFile = std::string( - reinterpret_cast(commandData + obcFile.size() + SIZE_NULL_TERMINATOR)); - if (mpsocFile.size() > MAX_FILENAME_SIZE) { + obcFile = std::string(reinterpret_cast(commandData), fileLen); + fileLen = + strnlen(reinterpret_cast(commandData + obcFile.size() + SIZE_NULL_TERMINATOR), + commandDataLen - obcFile.size() - 1); + if (fileLen > MAX_FILENAME_SIZE) { return MPSOC_FILENAME_TOO_LONG; } + mpsocFile = std::string( + reinterpret_cast(commandData + obcFile.size() + SIZE_NULL_TERMINATOR), + fileLen); return returnvalue::OK; } - std::string getObcFile() { return obcFile; } + const std::string& getObcFile() const { return obcFile; } - std::string getMPSoCFile() { return mpsocFile; } + const std::string& getMPSoCFile() const { return mpsocFile; } + + protected: + size_t getParsedSize() const { + return getObcFile().size() + getMPSoCFile().size() + 2 * SIZE_NULL_TERMINATOR; + } + static const size_t SIZE_NULL_TERMINATOR = 1; private: - static const size_t SIZE_NULL_TERMINATOR = 1; - std::string obcFile = ""; - std::string mpsocFile = ""; + std::string obcFile; + std::string mpsocFile; }; +class FlashReadPusCmd : public FlashBasePusCmd { + public: + FlashReadPusCmd(){}; + + ReturnValue_t extractFields(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t result = FlashBasePusCmd::extractFields(commandData, commandDataLen); + if (result != returnvalue::OK) { + return result; + } + if (commandDataLen < (getParsedSize() + 4)) { + return returnvalue::FAILED; + } + size_t deserDummy = 4; + return SerializeAdapter::deSerialize(&readSize, commandData + getParsedSize(), &deserDummy, + SerializeIF::Endianness::NETWORK); + } + + size_t getReadSize() const { return readSize; } + + private: + size_t readSize = 0; +}; /** * @brief Class to build replay stop space packet. */ @@ -676,7 +806,7 @@ class TcCamTakePic : public TcBase { TcCamTakePic(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_CAM_TAKE_PIC, sequenceCount) {} - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { if (commandDataLen > MAX_DATA_LENGTH) { return INVALID_LENGTH; } @@ -705,7 +835,7 @@ class TcSimplexSendFile : public TcBase { TcSimplexSendFile(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_SIMPLEX_SEND_FILE, sequenceCount) {} - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { if (commandDataLen > MAX_DATA_LENGTH) { return INVALID_LENGTH; } @@ -730,7 +860,7 @@ class TcDownlinkDataModulate : public TcBase { TcDownlinkDataModulate(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_DOWNLINK_DATA_MODULATE, sequenceCount) {} - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { if (commandDataLen > MAX_DATA_LENGTH) { return INVALID_LENGTH; } @@ -748,8 +878,7 @@ class TcCamcmdSend : public TcBase { TcCamcmdSend(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_CAM_CMD_SEND, sequenceCount) {} - protected: - ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { + ReturnValue_t setPayload(const uint8_t* commandData, size_t commandDataLen) { if (commandDataLen > MAX_DATA_LENGTH) { return INVALID_LENGTH; } @@ -774,6 +903,69 @@ class TcCamcmdSend : public TcBase { static const uint8_t CARRIAGE_RETURN = 0xD; }; +class HkReport : public StaticLocalDataSet<36> { + public: + HkReport(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, HK_SET_ID) {} + + HkReport(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, HK_SET_ID)) {} + + lp_var_t status = lp_var_t(sid.objectId, mpsoc::poolid::STATUS, this); + lp_var_t mode = lp_var_t(sid.objectId, mpsoc::poolid::MODE, this); + lp_var_t downlinkPwrOn = + lp_var_t(sid.objectId, mpsoc::poolid::DOWNLINK_PWR_ON, this); + lp_var_t downlinkReplyActive = + lp_var_t(sid.objectId, mpsoc::poolid::DOWNLINK_REPLY_ACTIIVE, this); + lp_var_t downlinkJesdSyncStatus = + lp_var_t(sid.objectId, mpsoc::poolid::DOWNLINK_JESD_SYNC_STATUS, this); + lp_var_t downlinkDacStatus = + lp_var_t(sid.objectId, mpsoc::poolid::DOWNLINK_DAC_STATUS, this); + lp_var_t camStatus = lp_var_t(sid.objectId, mpsoc::poolid::CAM_STATUS, this); + lp_var_t camSdiStatus = + lp_var_t(sid.objectId, mpsoc::poolid::CAM_SDI_STATUS, this); + lp_var_t camFpgaTemp = lp_var_t(sid.objectId, mpsoc::poolid::CAM_FPGA_TEMP, this); + lp_var_t camSocTemp = lp_var_t(sid.objectId, mpsoc::poolid::CAM_SOC_TEMP, this); + lp_var_t sysmonTemp = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_TEMP, this); + lp_var_t sysmonVccInt = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCINT, this); + lp_var_t sysmonVccAux = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCAUX, this); + lp_var_t sysmonVccBram = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCBRAM, this); + lp_var_t sysmonVccPaux = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCPAUX, this); + lp_var_t sysmonVccPint = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCPINT, this); + lp_var_t sysmonVccPdro = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCCPDRO, this); + lp_var_t sysmonMb12V = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_MB12V, this); + lp_var_t sysmonMb3V3 = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_MB3V3, this); + lp_var_t sysmonMb1V8 = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_MB1V8, this); + lp_var_t sysmonVcc12V = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC12V, this); + lp_var_t sysmonVcc5V = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC5V, this); + lp_var_t sysmonVcc3V3 = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC3V3, this); + lp_var_t sysmonVcc3V3VA = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC3V3VA, this); + lp_var_t sysmonVcc2V5DDR = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC2V5DDR, this); + lp_var_t sysmonVcc1V2DDR = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC1V2DDR, this); + lp_var_t sysmonVcc0V9 = lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC0V9, this); + lp_var_t sysmonVcc0V6VTT = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_VCC0V6VTT, this); + lp_var_t sysmonSafeCotsCur = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_SAFE_COTS_CUR, this); + lp_var_t sysmonNvm4XoCur = + lp_var_t(sid.objectId, mpsoc::poolid::SYSMON_NVM4_XO_CUR, this); + lp_var_t semUncorrectableErrs = + lp_var_t(sid.objectId, mpsoc::poolid::SEM_UNCORRECTABLE_ERRS, this); + lp_var_t semCorrectableErrs = + lp_var_t(sid.objectId, mpsoc::poolid::SEM_CORRECTABLE_ERRS, this); + lp_var_t semStatus = lp_var_t(sid.objectId, mpsoc::poolid::SEM_STATUS, this); + lp_var_t rebootMpsocRequired = + lp_var_t(sid.objectId, mpsoc::poolid::REBOOT_MPSOC_REQUIRED, this); +}; + +uint16_t getStatusFromRawData(const uint8_t* data); +std::string getStatusString(uint16_t status); + } // namespace mpsoc #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_PLOCMPSOCDEFINITIONS_H_ */ diff --git a/misc/eclipse/.cproject b/misc/eclipse/.cproject index 4cfe3fa1..154cb27e 100644 --- a/misc/eclipse/.cproject +++ b/misc/eclipse/.cproject @@ -57,7 +57,8 @@ - + + @@ -119,7 +120,8 @@ - + + @@ -187,7 +189,8 @@ - + + @@ -255,7 +258,8 @@ - + + @@ -418,7 +422,8 @@ - + + @@ -580,7 +585,8 @@ - + + @@ -750,7 +756,8 @@ - + + @@ -917,7 +924,8 @@ - + + @@ -1084,7 +1092,8 @@ - + + @@ -1149,7 +1158,8 @@ - + + @@ -1172,7 +1182,7 @@ - + - + + + @@ -1386,7 +1398,8 @@ - + + diff --git a/mission/acs/GyrAdis1650XHandler.cpp b/mission/acs/GyrAdis1650XHandler.cpp index b9e0b049..fe10c214 100644 --- a/mission/acs/GyrAdis1650XHandler.cpp +++ b/mission/acs/GyrAdis1650XHandler.cpp @@ -15,7 +15,7 @@ GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t devic } void GyrAdis1650XHandler::doStartUp() { - if (internalState != InternalState::STARTUP) { + if (internalState == InternalState::NONE) { internalState = InternalState::STARTUP; commandExecuted = false; } @@ -24,19 +24,21 @@ void GyrAdis1650XHandler::doStartUp() { if (not commandExecuted) { warningSwitch = true; breakCountdown.setTimeout(adis1650x::START_UP_TIME); + updatePeriodicReply(true, adis1650x::REPLY); commandExecuted = true; } - if (breakCountdown.hasTimedOut()) { - updatePeriodicReply(true, adis1650x::REPLY); - setMode(MODE_ON); - internalState = InternalState::NONE; - } + } + if (internalState == InternalState::STARTUP_DONE) { + setMode(MODE_ON); + commandExecuted = false; + internalState = InternalState::NONE; } } void GyrAdis1650XHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { commandExecuted = false; + PoolReadGuard pg(&primaryDataset); primaryDataset.setValidity(false, true); internalState = InternalState::SHUTDOWN; } @@ -60,6 +62,7 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_ return NOTHING_TO_SEND; } *id = adis1650x::REQUEST; + adisRequest.cfg.decRateReg = adis1650x::DEC_RATE; return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL); } case (InternalState::SHUTDOWN): { @@ -90,6 +93,9 @@ ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t rem getMode() == _MODE_POWER_DOWN) { return IGNORE_FULL_PACKET; } + if (internalState == InternalState::STARTUP) { + internalState = InternalState::STARTUP_DONE; + } *foundLen = remainingSize; if (remainingSize != sizeof(acs::Adis1650XReply)) { return returnvalue::FAILED; diff --git a/mission/acs/GyrAdis1650XHandler.h b/mission/acs/GyrAdis1650XHandler.h index 5d906f61..308d472b 100644 --- a/mission/acs/GyrAdis1650XHandler.h +++ b/mission/acs/GyrAdis1650XHandler.h @@ -48,7 +48,7 @@ class GyrAdis1650XHandler : public DeviceHandlerBase { bool warningSwitch = true; - enum class InternalState { NONE, STARTUP, SHUTDOWN }; + enum class InternalState { NONE, STARTUP, STARTUP_DONE, SHUTDOWN }; InternalState internalState = InternalState::STARTUP; bool commandExecuted = false; diff --git a/mission/acs/GyrL3gCustomHandler.cpp b/mission/acs/GyrL3gCustomHandler.cpp index 934fba99..ebc6ba42 100644 --- a/mission/acs/GyrL3gCustomHandler.cpp +++ b/mission/acs/GyrL3gCustomHandler.cpp @@ -33,6 +33,7 @@ void GyrL3gCustomHandler::doStartUp() { void GyrL3gCustomHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { internalState = InternalState::SHUTDOWN; + PoolReadGuard pg(&dataset); dataset.setValidity(false, true); commandExecuted = false; } diff --git a/mission/acs/MgmLis3CustomHandler.cpp b/mission/acs/MgmLis3CustomHandler.cpp index d1dd5eff..4b2e992d 100644 --- a/mission/acs/MgmLis3CustomHandler.cpp +++ b/mission/acs/MgmLis3CustomHandler.cpp @@ -29,6 +29,7 @@ void MgmLis3CustomHandler::doStartUp() { void MgmLis3CustomHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { + PoolReadGuard pg(&dataset); dataset.setValidity(false, true); internalState = InternalState::SHUTDOWN; commandExecuted = false; diff --git a/mission/acs/MgmRm3100CustomHandler.cpp b/mission/acs/MgmRm3100CustomHandler.cpp index e57effda..24c94938 100644 --- a/mission/acs/MgmRm3100CustomHandler.cpp +++ b/mission/acs/MgmRm3100CustomHandler.cpp @@ -33,6 +33,7 @@ void MgmRm3100CustomHandler::doStartUp() { void MgmRm3100CustomHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { commandExecuted = false; + PoolReadGuard pg(&primaryDataset); primaryDataset.setValidity(false, true); internalState = InternalState::SHUTDOWN; } diff --git a/mission/acs/SusHandler.cpp b/mission/acs/SusHandler.cpp index 0032a11d..6fa4d231 100644 --- a/mission/acs/SusHandler.cpp +++ b/mission/acs/SusHandler.cpp @@ -29,6 +29,8 @@ void SusHandler::doStartUp() { void SusHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { + PoolReadGuard pg(&dataset); + dataset.tempC = thermal::INVALID_TEMPERATURE; dataset.setValidity(false, true); internalState = InternalState::SHUTDOWN; commandExecuted = false; diff --git a/mission/acs/acsBoardPolling.h b/mission/acs/acsBoardPolling.h index 90f0f19c..9f794fb0 100644 --- a/mission/acs/acsBoardPolling.h +++ b/mission/acs/acsBoardPolling.h @@ -8,11 +8,6 @@ namespace acs { -struct Adis1650XRequest { - SimpleSensorMode mode; - adis1650x::Type type; -}; - struct Adis1650XConfig { uint16_t diagStat; uint16_t filterSetting; @@ -22,6 +17,12 @@ struct Adis1650XConfig { uint16_t prodId; }; +struct Adis1650XRequest { + SimpleSensorMode mode; + adis1650x::Type type; + Adis1650XConfig cfg; +}; + struct Adis1650XData { double sensitivity = 0.0; // Angular velocities in all axes (X, Y and Z) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 7748562d..41d09976 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -3,6 +3,7 @@ #include #include +#include namespace acs { @@ -11,12 +12,12 @@ enum class SimpleSensorMode { NORMAL = 0, OFF = 1 }; // These modes are the modes of the ACS controller and of the ACS subsystem. enum AcsMode : Mode_t { OFF = HasModesIF::MODE_OFF, - SAFE = 10, - PTG_IDLE = 11, - PTG_NADIR = 12, - PTG_TARGET = 13, - PTG_TARGET_GS = 14, - PTG_INERTIAL = 15, + SAFE = satsystem::Mode::SAFE, + PTG_IDLE = satsystem::Mode::PTG_IDLE, + PTG_NADIR = satsystem::Mode::PTG_NADIR, + PTG_TARGET = satsystem::Mode::PTG_TARGET, + PTG_TARGET_GS = satsystem::Mode::PTG_TARGET_GS, + PTG_INERTIAL = satsystem::Mode::PTG_INERTIAL, }; enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 }; diff --git a/mission/acs/gyroAdisHelpers.cpp b/mission/acs/gyroAdisHelpers.cpp index 0f41b058..5c1446cc 100644 --- a/mission/acs/gyroAdisHelpers.cpp +++ b/mission/acs/gyroAdisHelpers.cpp @@ -52,3 +52,14 @@ double adis1650x::rangMdlToSensitivity(uint16_t rangMdl) { } } } + +void adis1650x::prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf, + size_t maxLen) { + if (maxLen < 4) { + return; + } + outBuf[0] = regStart | adis1650x::WRITE_MASK; + outBuf[1] = val & 0xff; + outBuf[2] = (regStart + 1) | adis1650x::WRITE_MASK; + outBuf[3] = (val >> 8) & 0xff; +} diff --git a/mission/acs/gyroAdisHelpers.h b/mission/acs/gyroAdisHelpers.h index 09f0aa1e..f360db76 100644 --- a/mission/acs/gyroAdisHelpers.h +++ b/mission/acs/gyroAdisHelpers.h @@ -16,6 +16,8 @@ enum class BurstModes { }; size_t prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, size_t maxLen); +void prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf, size_t maxLen); + BurstModes burstModeFromMscCtrl(uint16_t mscCtrl); double rangMdlToSensitivity(uint16_t rangMdl); @@ -92,6 +94,9 @@ static constexpr size_t SENSOR_READOUT_SIZE = 20 + 2; static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA; static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG; +static constexpr uint16_t FILT_CTRL = 0x0000; +static constexpr uint16_t DEC_RATE = 0x00C7; + enum GlobCmds : uint8_t { FACTORY_CALIBRATION = 0b0000'0010, SENSOR_SELF_TEST = 0b0000'0100, diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 53a75e5b..7776bf17 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -17,6 +17,8 @@ extern "C" { #include #include "OBSWConfig.h" +#include "eive/definitions.h" +#include "fsfw/thermal/tcsDefinitions.h" std::atomic_bool JCFG_DONE(false); @@ -99,6 +101,22 @@ void StarTrackerHandler::doShutDown() { startupState = StartupState::IDLE; bootState = FwBootState::NONE; solutionSet.setReportingEnabled(false); + { + PoolReadGuard pg(&solutionSet); + solutionSet.caliQw.value = 0.0; + solutionSet.caliQx.value = 0.0; + solutionSet.caliQy.value = 0.0; + solutionSet.caliQz.value = 0.0; + solutionSet.isTrustWorthy.value = 0; + solutionSet.setValidity(false, true); + } + { + PoolReadGuard pg(&temperatureSet); + temperatureSet.fpgaTemperature = thermal::INVALID_TEMPERATURE; + temperatureSet.cmosTemperature = thermal::INVALID_TEMPERATURE; + temperatureSet.mcuTemperature = thermal::INVALID_TEMPERATURE; + temperatureSet.setValidity(false, true); + } reinitNextSetParam = false; setMode(_MODE_POWER_DOWN); } @@ -152,7 +170,7 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu return EXECUTION_FINISHED; } case (startracker::SET_JSON_FILE_NAME): { - if (size > MAX_PATH_SIZE) { + if (size > config::MAX_PATH_SIZE) { return FILE_PATH_TOO_LONG; } paramJsonFile = std::string(reinterpret_cast(data), size); @@ -189,7 +207,7 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu if (result != returnvalue::OK) { return result; } - if (size > MAX_PATH_SIZE + MAX_FILE_NAME) { + if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) { return FILE_PATH_TOO_LONG; } result = strHelper->startImageUpload(std::string(reinterpret_cast(data), size)); @@ -204,7 +222,7 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu if (result != returnvalue::OK) { return result; } - if (size > MAX_PATH_SIZE) { + if (size > config::MAX_PATH_SIZE) { return FILE_PATH_TOO_LONG; } result = @@ -228,14 +246,14 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu return EXECUTION_FINISHED; } case (startracker::CHANGE_IMAGE_DOWNLOAD_FILE): { - if (size > MAX_FILE_NAME) { + if (size > config::MAX_FILENAME_SIZE) { return FILENAME_TOO_LONG; } strHelper->setDownloadImageName(std::string(reinterpret_cast(data), size)); return EXECUTION_FINISHED; } case (startracker::SET_FLASH_READ_FILENAME): { - if (size > MAX_FILE_NAME) { + if (size > config::MAX_FILENAME_SIZE) { return FILENAME_TOO_LONG; } strHelper->setFlashReadFilename(std::string(reinterpret_cast(data), size)); @@ -246,7 +264,7 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu if (result != returnvalue::OK) { return result; } - if (size > MAX_PATH_SIZE + MAX_FILE_NAME) { + if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) { return FILE_PATH_TOO_LONG; } result = @@ -1589,7 +1607,7 @@ ReturnValue_t StarTrackerHandler::executeFlashReadCommand(const uint8_t* command << std::endl; return result; } - if (commandDataLen - sizeof(startRegion) - sizeof(length) > MAX_PATH_SIZE) { + if (commandDataLen - sizeof(startRegion) - sizeof(length) > config::MAX_PATH_SIZE) { sif::warning << "StarTrackerHandler::executeFlashReadCommand: Received command with invalid" << " path and filename" << std::endl; return FILE_PATH_TOO_LONG; @@ -1729,7 +1747,7 @@ ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData bool reinitSet) { // Stopwatch watch; ReturnValue_t result = returnvalue::OK; - if (commandDataLen > MAX_PATH_SIZE) { + if (commandDataLen > config::MAX_PATH_SIZE) { return FILE_PATH_TOO_LONG; } if (reinitSet) { diff --git a/mission/acs/str/StarTrackerHandler.h b/mission/acs/str/StarTrackerHandler.h index 13855161..10397148 100644 --- a/mission/acs/str/StarTrackerHandler.h +++ b/mission/acs/str/StarTrackerHandler.h @@ -145,9 +145,6 @@ class StarTrackerHandler : public DeviceHandlerBase { //! [EXPORT] : [COMMENT] Failed to boot star tracker into bootloader mode static const Event BOOTING_BOOTLOADER_FAILED_EVENT = MAKE_EVENT(2, severity::LOW); - static const size_t MAX_PATH_SIZE = 50; - static const size_t MAX_FILE_NAME = 30; - static const uint8_t STATUS_OFFSET = 2; static const uint8_t PARAMS_OFFSET = 2; static const uint8_t TICKS_OFFSET = 3; diff --git a/mission/cfdp/CfdpHandler.cpp b/mission/cfdp/CfdpHandler.cpp index 902097b6..fa35535c 100644 --- a/mission/cfdp/CfdpHandler.cpp +++ b/mission/cfdp/CfdpHandler.cpp @@ -93,6 +93,8 @@ ReturnValue_t CfdpHandler::handleCfdpPacket(TmTcMessage& msg) { return INVALID_PDU_FORMAT; } if (not FileDirectiveReader::checkFileDirective(pduDataField[0])) { + sif::error << "CfdpHandler: Invalid PDU directive field " << static_cast(pduDataField[0]) + << std::endl; return INVALID_DIRECTIVE_FIELD; } auto directive = static_cast(pduDataField[0]); diff --git a/mission/com/CcsdsIpCoreHandler.cpp b/mission/com/CcsdsIpCoreHandler.cpp index 625c90cd..19dd4f5a 100644 --- a/mission/com/CcsdsIpCoreHandler.cpp +++ b/mission/com/CcsdsIpCoreHandler.cpp @@ -246,7 +246,13 @@ ReturnValue_t CcsdsIpCoreHandler::checkModeCommand(Mode_t mode, Submode_t submod void CcsdsIpCoreHandler::startTransition(Mode_t mode, Submode_t submode) { triggerEvent(CHANGING_MODE, mode, submode); if (mode == HasModesIF::MODE_ON) { - if (this->submode != submode) { + uint32_t currentRate = ptmeConfig.getRate(); + // Check whether the rate actually changes. + if ((this->submode != submode) and + (((submode == static_cast(com::CcsdsSubmode::DATARATE_LOW) and + (currentRate != RATE_100KBPS))) or + ((submode == static_cast(com::CcsdsSubmode::DATARATE_HIGH) and + (currentRate != RATE_500KBPS))))) { initPtmeUpdateAfterXCycles(); updateContext.enableTransmitAfterPtmeUpdate = true; updateContext.updateClockRate = true; diff --git a/mission/com/LiveTmTask.cpp b/mission/com/LiveTmTask.cpp index d09c6ced..39648c15 100644 --- a/mission/com/LiveTmTask.cpp +++ b/mission/com/LiveTmTask.cpp @@ -19,13 +19,8 @@ LiveTmTask::LiveTmTask(object_id_t objectId, PusTmFunnel& pusFunnel, CfdpTmFunne ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) { readCommandQueue(); while (true) { - bool performWriteOp = true; - if (mode == MODE_OFF or ptmeLocked) { - performWriteOp = false; - } - // The funnel tasks are scheduled here directly as well. - ReturnValue_t result = channel.handleNextTm(performWriteOp); + ReturnValue_t result = channel.handleNextTm(!ptmeLocked); if (result == DirectTmSinkIF::IS_BUSY) { sif::error << "Lost live TM, PAPB busy" << std::endl; } diff --git a/mission/com/PersistentLogTmStoreTask.cpp b/mission/com/PersistentLogTmStoreTask.cpp index 77f2bb7d..28545457 100644 --- a/mission/com/PersistentLogTmStoreTask.cpp +++ b/mission/com/PersistentLogTmStoreTask.cpp @@ -42,13 +42,7 @@ ReturnValue_t PersistentLogTmStoreTask::performOperation(uint8_t opCode) { if (not someonesBusy) { TaskFactory::delayTask(100); } else if (vcBusyDuringDump) { - // TODO: Might not be necessary - sif::debug << "VC busy, delaying" << std::endl; TaskFactory::delayTask(10); - } else { - // TODO: Would be best to remove this, but not delaying here can lead to evil issues. - // Polling the PAPB of the PTME core too often leads to scheuduling issues. - TaskFactory::delayTask(2); } } } diff --git a/mission/com/PersistentSingleTmStoreTask.cpp b/mission/com/PersistentSingleTmStoreTask.cpp index 1b77365b..d6f43289 100644 --- a/mission/com/PersistentSingleTmStoreTask.cpp +++ b/mission/com/PersistentSingleTmStoreTask.cpp @@ -24,13 +24,7 @@ ReturnValue_t PersistentSingleTmStoreTask::performOperation(uint8_t opCode) { if (not busy) { TaskFactory::delayTask(100); } else if (dumpContext.vcBusyDuringDump) { - sif::debug << "VC busy, delaying" << std::endl; - // TODO: Might not be necessary TaskFactory::delayTask(10); - } else { - // TODO: Would be best to remove this, but not delaying here can lead to evil issues. - // Polling the PAPB of the PTME core too often leads to scheuduling issues. - TaskFactory::delayTask(2); } } } diff --git a/mission/com/SyrlinksHandler.cpp b/mission/com/SyrlinksHandler.cpp index 6fbc8dc2..50e6a56e 100644 --- a/mission/com/SyrlinksHandler.cpp +++ b/mission/com/SyrlinksHandler.cpp @@ -773,11 +773,13 @@ void SyrlinksHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { auto txStandbyHandler = [&]() { txDataset.setReportingEnabled(false); poolManager.changeCollectionInterval(temperatureSet.getSid(), 60.0); + poolManager.changeCollectionInterval(rxDataset.getSid(), 60.0); transState = TransitionState::SET_TX_STANDBY; internalState = InternalState::TX_TRANSITION; }; auto txOnHandler = [&](TransitionState tgtTransitionState) { txDataset.setReportingEnabled(true); + poolManager.changeCollectionInterval(rxDataset.getSid(), 5.0); poolManager.changeCollectionInterval(txDataset.getSid(), 10.0); poolManager.changeCollectionInterval(temperatureSet.getSid(), 5.0); transState = tgtTransitionState; diff --git a/mission/com/TmStoreTaskBase.cpp b/mission/com/TmStoreTaskBase.cpp index 6598225d..80900975 100644 --- a/mission/com/TmStoreTaskBase.cpp +++ b/mission/com/TmStoreTaskBase.cpp @@ -45,13 +45,19 @@ bool TmStoreTaskBase::handleOneStore(PersistentTmStoreWithTmQueue& store, } else { Command_t execCmd; // Handle TC requests, for example deletion or retrieval requests. + // TODO: Not really clean here.. would be better if the executed command is returns as an + // enumeration. result = store.handleCommandQueue(ipcStore, execCmd); - if (result == returnvalue::OK) { - if (execCmd == TmStoreMessage::DOWNLINK_STORE_CONTENT_TIME) { + if (execCmd == TmStoreMessage::DOWNLINK_STORE_CONTENT_TIME) { + if (result == PersistentTmStore::DUMP_DONE) { + dumpDoneHandler(store, dumpContext); + } else if (result == returnvalue::OK) { cancelDumpCd.resetTimer(); tmSinkBusyCd.resetTimer(); dumpContext.reset(); } + } + if (execCmd != CommandMessageIF::CMD_NONE) { tcRequestReceived = true; } } @@ -94,13 +100,6 @@ void TmStoreTaskBase::cancelDump(DumpContext& ctx, PersistentTmStore& store, boo ReturnValue_t TmStoreTaskBase::handleOneDump(PersistentTmStoreWithTmQueue& store, DumpContext& dumpContext, bool& dumpPerformed) { ReturnValue_t result = returnvalue::OK; - // The PTME might have been reset an transmitter state change, so there is no point in continuing - // the dump. - // TODO: Will be solved in a cleaner way, this is kind of a hack. - if (not channel.isTxOn()) { - cancelDump(dumpContext, store, false); - return returnvalue::FAILED; - } // It is assumed that the PTME will only be locked for a short period (e.g. to change datarate). if (not channel.isBusy() and not ptmeLocked) { performDump(store, dumpContext, dumpPerformed); @@ -126,37 +125,26 @@ ReturnValue_t TmStoreTaskBase::performDump(PersistentTmStoreWithTmQueue& store, DumpContext& dumpContext, bool& dumpPerformed) { size_t dumpedLen = 0; - auto dumpDoneHandler = [&]() { - uint32_t startTime; - uint32_t endTime; - store.getStartAndEndTimeCurrentOrLastDump(startTime, endTime); - triggerEvent(dumpContext.eventIfDone, dumpContext.numberOfDumpedPackets, - dumpContext.dumpedBytes); - dumpContext.reset(); - }; // Dump the next packet into the PTME. dumpContext.ptmeBusyCounter = 0; tmSinkBusyCd.resetTimer(); ReturnValue_t result = store.getNextDumpPacket(tmReader, fileHasSwapped); - if (result != returnvalue::OK) { - sif::error << "PersistentTmStore: Getting next dump packet failed" << std::endl; - } else if (fileHasSwapped or result == PersistentTmStore::DUMP_DONE) { + if (fileHasSwapped and result == PersistentTmStore::DUMP_DONE) { // This can happen if a file is corrupted and the next file swap completes the dump. - dumpDoneHandler(); + dumpDoneHandler(store, dumpContext); return returnvalue::OK; + } else if (result != returnvalue::OK) { + sif::error << "PersistentTmStore: Getting next dump packet failed" << std::endl; + return result; } dumpedLen = tmReader.getFullPacketLen(); - // Only write to VC if mode is on, but always confirm the dump. - // If the mode is OFF, it is assumed the PTME is not usable and is not allowed to be written - // (e.g. to confirm a reset or the transmitter is off anyway). - if (mode == MODE_ON) { - result = channel.write(tmReader.getFullData(), dumpedLen); - if (result == DirectTmSinkIF::IS_BUSY) { - sif::warning << "PersistentTmStore: Unexpected VC channel busy" << std::endl; - } else if (result != returnvalue::OK) { - sif::warning << "PersistentTmStore: Unexpected VC channel write failure" << std::endl; - } + result = channel.write(tmReader.getFullData(), dumpedLen); + if (result == DirectTmSinkIF::IS_BUSY) { + sif::warning << "PersistentTmStore: Unexpected VC channel busy" << std::endl; + } else if (result != returnvalue::OK) { + sif::warning << "PersistentTmStore: Unexpected VC channel write failure" << std::endl; } + result = store.confirmDump(tmReader, fileHasSwapped); if ((result == PersistentTmStore::DUMP_DONE or result == returnvalue::OK)) { dumpPerformed = true; @@ -167,7 +155,7 @@ ReturnValue_t TmStoreTaskBase::performDump(PersistentTmStoreWithTmQueue& store, } } if (result == PersistentTmStore::DUMP_DONE) { - dumpDoneHandler(); + dumpDoneHandler(store, dumpContext); } return returnvalue::OK; } @@ -208,6 +196,14 @@ ReturnValue_t TmStoreTaskBase::connectModeTreeParent(HasModeTreeChildrenIF& pare return modetree::connectModeTreeParent(parent, *this, nullptr, modeHelper); } +void TmStoreTaskBase::dumpDoneHandler(PersistentTmStore& store, DumpContext& dumpContext) { + uint32_t startTime; + uint32_t endTime; + store.getStartAndEndTimeCurrentOrLastDump(startTime, endTime); + triggerEvent(dumpContext.eventIfDone, dumpContext.numberOfDumpedPackets, dumpContext.dumpedBytes); + dumpContext.reset(); +} + ModeTreeChildIF& TmStoreTaskBase::getModeTreeChildIF() { return *this; } void TmStoreTaskBase::readCommandQueue(void) { diff --git a/mission/com/TmStoreTaskBase.h b/mission/com/TmStoreTaskBase.h index ef61bd19..2bcd3b1e 100644 --- a/mission/com/TmStoreTaskBase.h +++ b/mission/com/TmStoreTaskBase.h @@ -96,6 +96,8 @@ class TmStoreTaskBase : public SystemObject, ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t* msToReachTheMode) override; + void dumpDoneHandler(PersistentTmStore& store, DumpContext& dumpContext); + void announceMode(bool recursive) override; object_id_t getObjectId() const override; const HasHealthIF* getOptHealthIF() const override; diff --git a/mission/com/VirtualChannel.cpp b/mission/com/VirtualChannel.cpp index 8e225674..ff3749a9 100644 --- a/mission/com/VirtualChannel.cpp +++ b/mission/com/VirtualChannel.cpp @@ -11,23 +11,14 @@ ReturnValue_t VirtualChannel::sendNextTm(const uint8_t* data, size_t size) { } ReturnValue_t VirtualChannel::write(const uint8_t* data, size_t size) { - if (txOn) { - return ptme.writeToVc(vcId, data, size); - } - return returnvalue::OK; + return ptme.writeToVc(vcId, data, size); } uint8_t VirtualChannel::getVcid() const { return vcId; } const char* VirtualChannel::getName() const { return vcName.c_str(); } -bool VirtualChannel::isBusy() const { - // Data is discarded, so channel is not busy. - if (not txOn) { - return false; - } - return ptme.isBusy(vcId); -} +bool VirtualChannel::isBusy() const { return ptme.isBusy(vcId); } void VirtualChannel::cancelTransfer() { ptme.cancelTransfer(vcId); } diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 0b49de04..04009cb2 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -169,7 +169,7 @@ void AcsController::performSafe() { guidance.getTargetParamsSafe(sunTargetDir); double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; - uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy( + acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), acsParameters.safeModeControllerParameters.useMekf, @@ -205,11 +205,13 @@ void AcsController::performSafe() { case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; + default: + sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl; + break; } - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, - *acsParameters.magnetorquerParameter.inverseAlignment, - acsParameters.magnetorquerParameter.dipolMax); + actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); // detumble check and switch if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf && @@ -231,8 +233,8 @@ void AcsController::performSafe() { } updateCtrlValData(errAng, safeCtrlStrat); - updateActuatorCmdData(cmdDipolMtqs); - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + updateActuatorCmdData(cmdDipoleMtqs); + commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration); } @@ -259,7 +261,7 @@ void AcsController::performDetumble() { triggerEvent(acs::MEKF_RECOVERY); mekfInvalidFlag = false; } - uint8_t safeCtrlStrat = detumble.detumbleStrategy( + acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy( mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(), acsParameters.detumbleParameter.useFullDetumbleLaw); @@ -279,11 +281,13 @@ void AcsController::performDetumble() { case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; + default: + sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl; + break; } - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, - *acsParameters.magnetorquerParameter.inverseAlignment, - acsParameters.magnetorquerParameter.dipolMax); + actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < @@ -304,8 +308,8 @@ void AcsController::performDetumble() { } disableCtrlValData(); - updateActuatorCmdData(cmdDipolMtqs); - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + updateActuatorCmdData(cmdDipoleMtqs); + commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration); } @@ -349,8 +353,10 @@ 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 == 5) { + if (multipleRwUnavailableCounter >= + acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { triggerEvent(acs::MULTIPLE_RW_INVALID); + multipleRwUnavailableCounter = 0; } multipleRwUnavailableCounter++; return; @@ -364,24 +370,26 @@ void AcsController::performPointingCtrl() { // 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.targetQuatPtgSun(now, 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); + 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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, 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); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; break; @@ -395,17 +403,17 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, + 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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, 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); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; @@ -416,17 +424,17 @@ void AcsController::performPointingCtrl() { targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &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); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; break; @@ -440,63 +448,61 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters, + 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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, 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); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; break; case acs::PTG_INERTIAL: std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, - 4 * sizeof(double)); + sizeof(targetQuat)); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef, acsParameters.inertialModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters, + 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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, 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); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; default: - sif::error << "AcsController: Invalid mode for performPointingCtrl"; + sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl; 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); + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel, + acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws); if (enableAntiStiction) { ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); } - actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, - *acsParameters.magnetorquerParameter.inverseAlignment, - acsParameters.magnetorquerParameter.dipolMax); + actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); - updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs); - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs); + commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], acsParameters.rwHandlingParameters.rampTime); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 6ec26c57..0c8b94bb 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -69,7 +69,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes bool mekfLost = false; int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; - int16_t cmdDipolMtqs[3] = {0, 0, 0}; + int16_t cmdDipoleMtqs[3] = {0, 0, 0}; #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 82efb85c..5f2277a9 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1,6 +1,6 @@ #include "ThermalController.h" -#include +#include #include #include #include @@ -20,15 +20,20 @@ #define LOWER_SYRLINKS_UPPER_LIMITS 0 #define LOWER_EBAND_UPPER_LIMITS 0 #define LOWER_PLOC_UPPER_LIMITS 0 +#define LOWER_MGT_UPPER_LIMITS 0 +#define LOWER_RW_UPPER_LIMITS 0 ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater, - const std::atomic_bool& tcsBoardShortUnavailable) + const std::atomic_bool& tcsBoardShortUnavailable, + bool pollPcdu1Tmp) : ExtendedControllerBase(objectId), heaterHandler(heater), + pollPcdu1Tmp(pollPcdu1Tmp), sensorTemperatures(this), susTemperatures(this), deviceTemperatures(this), heaterInfo(this), + tcsCtrlInfo(this), imtqThermalSet(objects::IMTQ_HANDLER, ThermalStateCfg()), maxSet0PlocHspd(objects::RTD_0_IC3_PLOC_HEATSPREADER, EiveMax31855::RtdCommands::EXCHANGE_SET_ID), @@ -53,8 +58,6 @@ ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater tmp1075SetTcs0(objects::TMP1075_HANDLER_TCS_0), tmp1075SetTcs1(objects::TMP1075_HANDLER_TCS_1), tmp1075SetPlPcdu0(objects::TMP1075_HANDLER_PLPCDU_0), - // damaged - // tmp1075SetPlPcdu1(objects::TMP1075_HANDLER_PLPCDU_1), tmp1075SetIfBoard(objects::TMP1075_HANDLER_IF_BOARD), susSet0(objects::SUS_0_N_LOC_XFYFZM_PT_XF), susSet1(objects::SUS_1_N_LOC_XBYFZM_PT_XB), @@ -69,6 +72,9 @@ ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater susSet10(objects::SUS_10_N_LOC_XMYBZF_PT_ZF), susSet11(objects::SUS_11_R_LOC_XBYMZB_PT_ZB), tcsBrdShortlyUnavailable(tcsBoardShortUnavailable) { + if (pollPcdu1Tmp) { + tmp1075SetPlPcdu1 = new TMP1075::Tmp1075Dataset(objects::TMP1075_HANDLER_PLPCDU_1); + } resetSensorsArray(); } @@ -111,7 +117,7 @@ void ThermalController::performControlOperation() { break; } - if (cycles == 50) { + if (cycles == 40) { bool changedLimits = false; #if LOWER_SYRLINKS_UPPER_LIMITS == 1 changedLimits = true; @@ -130,9 +136,21 @@ void ThermalController::performControlOperation() { hpaLimits.cutOffLimit = 0; hpaLimits.opUpperLimit = 0; hpaLimits.nopUpperLimit = 0; +#endif +#if LOWER_MGT_UPPER_LIMITS == 1 + changedLimits = true; + mgtLimits.cutOffLimit = 0; + mgtLimits.opUpperLimit = 0; + mgtLimits.nopUpperLimit = 0; +#endif +#if LOWER_RW_UPPER_LIMITS == 1 + changedLimits = true; + rwLimits.cutOffLimit = 0; + rwLimits.opUpperLimit = 0; + rwLimits.nopUpperLimit = 0; #endif if (changedLimits) { - sif::debug << "ThermalController: changing limits" << std::endl; // TODO: rausschmeissen + sif::debug << "ThermalController: changing limits" << std::endl; } } @@ -157,11 +175,11 @@ void ThermalController::performControlOperation() { } } - HeaterSwitchStates heaterSwitchStateArray{}; + tcsCtrl::HeaterSwitchStates heaterSwitchStateArray{}; heaterHandler.getAllSwitchStates(heaterSwitchStateArray); { PoolReadGuard pg(&heaterInfo); - std::memcpy(heaterInfo.heaterSwitchState.value, heaterStates.data(), 8); + std::memcpy(heaterInfo.heaterSwitchState.value, heaterSwitchStateArray.data(), 8); { PoolReadGuard pg2(¤tVecPdu2); if (pg.getReadResult() == returnvalue::OK and pg2.getReadResult() == returnvalue::OK) { @@ -174,12 +192,11 @@ void ThermalController::performControlOperation() { if (transitionWhenHeatersOff) { bool allSwitchersOff = true; for (size_t idx = 0; idx < heaterSwitchStateArray.size(); idx++) { - if (heaterSwitchStateArray[idx] != HeaterHandler::SwitchState::OFF) { + if (heaterSwitchStateArray[idx] != heater::SwitchState::OFF) { allSwitchersOff = false; // if heater still ON after 3 cycles, switch OFF again if (transitionWhenHeatersOffCycles == 3) { - heaterHandler.switchHeater(static_cast(idx), - HeaterHandler::SwitchState::OFF); + heaterHandler.switchHeater(static_cast(idx), heater::SwitchState::OFF); triggerEvent(tcsCtrl::HEATER_NOT_OFF_FOR_OFF_MODE); } } @@ -192,8 +209,21 @@ void ThermalController::performControlOperation() { } else { transitionWhenHeatersOffCycles++; } - } else if (mode != MODE_OFF and not tcsBrdShortlyUnavailable) { - performThermalModuleCtrl(heaterSwitchStateArray); + } else if (mode != MODE_OFF) { + if (not tcsBrdShortlyUnavailable) { + performThermalModuleCtrl(heaterSwitchStateArray); + } + heaterTransitionControl(heaterSwitchStateArray); + heaterMaxDurationControl(heaterSwitchStateArray); + // This dataset makes the TCS CTRL observable. + PoolReadGuard pg(&tcsCtrlInfo); + for (uint8_t i = 0; i < thermalStates.size(); i++) { + tcsCtrlInfo.heatingOnVec[i] = thermalStates[i].heating; + tcsCtrlInfo.sensorIdxUsedForTcsCtrl[i] = thermalStates[i].sensorIndex; + tcsCtrlInfo.heaterSwitchIdx[i] = thermalStates[i].heaterSwitch; + tcsCtrlInfo.heaterStartTimes[i] = thermalStates[i].heaterStartTime; + tcsCtrlInfo.heaterEndTimes[i] = thermalStates[i].heaterEndTime; + } } } @@ -263,6 +293,11 @@ ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& lo localDataPoolMap.emplace(tcsCtrl::TEMP_ADC_PAYLOAD_PCDU, new PoolEntry({0.0})); localDataPoolMap.emplace(tcsCtrl::HEATER_SWITCH_LIST, &heaterSwitchStates); localDataPoolMap.emplace(tcsCtrl::HEATER_CURRENT, &heaterCurrent); + localDataPoolMap.emplace(tcsCtrl::HEATER_ON_FOR_COMPONENT_VEC, &tcsCtrlHeaterOn); + localDataPoolMap.emplace(tcsCtrl::SENSOR_USED_FOR_TCS_CTRL, &tcsCtrlSensorIdx); + localDataPoolMap.emplace(tcsCtrl::HEATER_IDX_USED_FOR_TCS_CTRL, &tcsCtrlHeaterIdx); + localDataPoolMap.emplace(tcsCtrl::HEATER_START_TIME, &tcsCtrlStartTimes); + localDataPoolMap.emplace(tcsCtrl::HEATER_END_TIME, &tcsCtrlEndTimes); bool enableHkSets = false; #if OBSW_ENABLE_PERIODIC_HK == 1 @@ -276,6 +311,8 @@ ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& lo subdp::RegularHkPeriodicParams(deviceTemperatures.getSid(), enableHkSets, 120.0)); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(heaterInfo.getSid(), enableHkSets, 120.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(tcsCtrlInfo.getSid(), enableHkSets, 120.0)); return returnvalue::OK; } @@ -290,6 +327,8 @@ LocalPoolDataSetBase* ThermalController::getDataSetHandle(sid_t sid) { return &deviceTemperatures; case tcsCtrl::HEATER_SET: return &heaterInfo; + case tcsCtrl::TCS_CTRL_INFO: + return &tcsCtrlInfo; default: return nullptr; } @@ -522,19 +561,19 @@ void ThermalController::copySensors() { } } } - // damaged - /* - { - PoolReadGuard pg(&tmp1075SetPlPcdu1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); - if (pg.getReadResult() == returnvalue::OK) { - sensorTemperatures.tmp1075PlPcdu1.value = tmp1075SetPlPcdu1.temperatureCelcius.value; - sensorTemperatures.tmp1075PlPcdu1.setValid(tmp1075SetPlPcdu1.temperatureCelcius.isValid()); - if (not tmp1075SetPlPcdu1.temperatureCelcius.isValid()) { - sensorTemperatures.tmp1075PlPcdu1.value = INVALID_TEMPERATURE; + // damaged on FM, and no dummies for now + if (pollPcdu1Tmp) { + { + PoolReadGuard pg(tmp1075SetPlPcdu1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); + if (pg.getReadResult() == returnvalue::OK) { + sensorTemperatures.tmp1075PlPcdu1.value = tmp1075SetPlPcdu1->temperatureCelcius.value; + sensorTemperatures.tmp1075PlPcdu1.setValid(tmp1075SetPlPcdu1->temperatureCelcius.isValid()); + if (not tmp1075SetPlPcdu1->temperatureCelcius.isValid()) { + sensorTemperatures.tmp1075PlPcdu1.value = INVALID_TEMPERATURE; + } } } } - */ { PoolReadGuard pg(&tmp1075SetIfBoard, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() == returnvalue::OK) { @@ -988,10 +1027,10 @@ void ThermalController::copyDevices() { void ThermalController::ctrlAcsBoard() { heater::Switch switchNr = heater::HEATER_2_ACS_BRD; - heater::Switch redSwitchNr = heater::HEATER_0_OBC_BRD; + heater::Switch redSwitchNr = heater::HEATER_3_OBC_BRD; // A side - thermalComponent = ACS_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::ACS_BOARD; sensors[0].first = deviceTemperatures.gyro0SideA.isValid(); sensors[0].second = deviceTemperatures.gyro0SideA.value; sensors[1].first = deviceTemperatures.gyro2SideB.isValid(); @@ -1035,7 +1074,7 @@ void ThermalController::ctrlAcsBoard() { if (chooseHeater(switchNr, redSwitchNr)) { if (heaterHandler.getSwitchState(switchNr)) { if (submode != SUBMODE_NO_HEATER_CTRL) { - heaterSwitchHelper(switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + heaterSwitchHelper(switchNr, heater::SwitchState::OFF, ctrlCtx.thermalComponent); } } } @@ -1045,7 +1084,7 @@ void ThermalController::ctrlAcsBoard() { } void ThermalController::ctrlMgt() { - thermalComponent = MGT; + ctrlCtx.thermalComponent = tcsCtrl::MGT; sensors[0].first = sensorTemperatures.mgt.isValid(); sensors[0].second = sensorTemperatures.mgt.value; sensors[1].first = deviceTemperatures.mgt.isValid(); @@ -1053,9 +1092,14 @@ void ThermalController::ctrlMgt() { sensors[2].first = sensorTemperatures.plpcduHeatspreader.isValid(); sensors[2].second = sensorTemperatures.plpcduHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, mgtLimits); + HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_1_PCDU_PDU, mgtLimits); ctrlComponentTemperature(htrCtx); - // TODO: trigger special event + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.mgtTooHotFlag) { + triggerEvent(tcsCtrl::MGT_OVERHEATING, tempFloatToU32()); + tooHotFlags.mgtTooHotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.mgtTooHotFlag = false; + } } void ThermalController::ctrlRw() { @@ -1065,7 +1109,7 @@ void ThermalController::ctrlRw() { std::array sensorTemps{}; // RW1 - thermalComponent = RW; + ctrlCtx.thermalComponent = tcsCtrl::RW; sensors[0].first = sensorTemperatures.rw1.isValid(); sensors[0].second = sensorTemperatures.rw1.value; sensors[1].first = deviceTemperatures.rw1.isValid(); @@ -1079,14 +1123,14 @@ void ThermalController::ctrlRw() { HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits); ctrlComponentTemperature(htrCtx); sensorTemps[0] = tempFloatToU32(); - if (componentAboveUpperLimit) { + if (ctrlCtx.componentAboveUpperLimit) { oneIsAboveLimit = true; - eventToTrigger = overHeatEventToTrigger; + eventToTrigger = ctrlCtx.overHeatEventToTrigger; } } // RW2 - thermalComponent = RW; + ctrlCtx.thermalComponent = tcsCtrl::RW; sensors[0].first = deviceTemperatures.rw2.isValid(); sensors[0].second = deviceTemperatures.rw2.value; sensors[1].first = deviceTemperatures.rw3.isValid(); @@ -1100,15 +1144,15 @@ void ThermalController::ctrlRw() { HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits); ctrlComponentTemperature(htrCtx); sensorTemps[1] = tempFloatToU32(); - if (componentAboveUpperLimit) { + if (ctrlCtx.componentAboveUpperLimit) { oneIsAboveLimit = true; if (eventToTrigger != ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH) { - eventToTrigger = overHeatEventToTrigger; + eventToTrigger = ctrlCtx.overHeatEventToTrigger; } } } // RW3 - thermalComponent = RW; + ctrlCtx.thermalComponent = tcsCtrl::RW; sensors[0].first = deviceTemperatures.rw3.isValid(); sensors[0].second = deviceTemperatures.rw3.value; sensors[1].first = deviceTemperatures.rw4.isValid(); @@ -1122,16 +1166,16 @@ void ThermalController::ctrlRw() { HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits); ctrlComponentTemperature(htrCtx); sensorTemps[2] = tempFloatToU32(); - if (componentAboveUpperLimit) { + if (ctrlCtx.componentAboveUpperLimit) { oneIsAboveLimit = true; if (eventToTrigger != ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH) { - eventToTrigger = overHeatEventToTrigger; + eventToTrigger = ctrlCtx.overHeatEventToTrigger; } } } // RW4 - thermalComponent = RW; + ctrlCtx.thermalComponent = tcsCtrl::RW; sensors[0].first = deviceTemperatures.rw4.isValid(); sensors[0].second = deviceTemperatures.rw4.value; sensors[1].first = deviceTemperatures.rw1.isValid(); @@ -1145,27 +1189,27 @@ void ThermalController::ctrlRw() { HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits); ctrlComponentTemperature(htrCtx); sensorTemps[3] = tempFloatToU32(); - if (componentAboveUpperLimit) { + if (ctrlCtx.componentAboveUpperLimit) { oneIsAboveLimit = true; if (eventToTrigger != ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH) { - eventToTrigger = overHeatEventToTrigger; + eventToTrigger = ctrlCtx.overHeatEventToTrigger; } } } - if (oneIsAboveLimit and not rwTooHotFlag) { + if (oneIsAboveLimit and not tooHotFlags.rwTooHotFlag) { EventManagerIF::triggerEvent(objects::RW1, eventToTrigger, sensorTemps[0]); EventManagerIF::triggerEvent(objects::RW2, eventToTrigger, sensorTemps[1]); EventManagerIF::triggerEvent(objects::RW3, eventToTrigger, sensorTemps[2]); EventManagerIF::triggerEvent(objects::RW4, eventToTrigger, sensorTemps[3]); - rwTooHotFlag = true; + tooHotFlags.rwTooHotFlag = true; } else if (not oneIsAboveLimit) { - rwTooHotFlag = false; + tooHotFlags.rwTooHotFlag = false; } } void ThermalController::ctrlStr() { - thermalComponent = STR; + ctrlCtx.thermalComponent = tcsCtrl::STR; sensors[0].first = sensorTemperatures.startracker.isValid(); sensors[0].second = sensorTemperatures.startracker.value; sensors[1].first = deviceTemperatures.startracker.isValid(); @@ -1175,11 +1219,11 @@ void ThermalController::ctrlStr() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_5_STR, heater::HEATER_6_DRO, strLimits); ctrlComponentTemperature(htrCtx); - tooHotHandlerWhichClearsOneShotFlag(objects::STAR_TRACKER, strTooHotFlag); + tooHotHandlerWhichClearsOneShotFlag(objects::STAR_TRACKER, tooHotFlags.strTooHotFlag); } void ThermalController::ctrlIfBoard() { - thermalComponent = IF_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::IF_BOARD; sensors[0].first = sensorTemperatures.tmp1075IfBrd.isValid(); sensors[0].second = sensorTemperatures.tmp1075IfBrd.value; sensors[1].first = sensorTemperatures.mgt.isValid(); @@ -1187,13 +1231,13 @@ void ThermalController::ctrlIfBoard() { sensors[2].first = deviceTemperatures.mgm2SideB.isValid(); sensors[2].second = deviceTemperatures.mgm2SideB.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, ifBoardLimits); + HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_1_PCDU_PDU, ifBoardLimits); ctrlComponentTemperature(htrCtx); // TODO: special event overheating + could go back to safe mode } void ThermalController::ctrlTcsBoard() { - thermalComponent = TCS_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::TCS_BOARD; sensors[0].first = sensorTemperatures.tcsBoard.isValid(); sensors[0].second = sensorTemperatures.tcsBoard.value; sensors[1].first = sensorTemperatures.tmp1075Tcs0.isValid(); @@ -1201,13 +1245,13 @@ void ThermalController::ctrlTcsBoard() { sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs1.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, tcsBoardLimits); + HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, tcsBoardLimits); ctrlComponentTemperature(htrCtx); // TODO: special event overheating + could go back to safe mode } void ThermalController::ctrlObc() { - thermalComponent = OBC; + ctrlCtx.thermalComponent = tcsCtrl::OBC; sensors[0].first = deviceTemperatures.q7s.isValid(); sensors[0].second = deviceTemperatures.q7s.value; sensors[1].first = sensorTemperatures.tmp1075Tcs1.isValid(); @@ -1215,37 +1259,18 @@ void ThermalController::ctrlObc() { sensors[2].first = sensorTemperatures.tmp1075Tcs0.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs0.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, obcLimits); + HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, obcLimits); ctrlComponentTemperature(htrCtx); - if (componentAboveUpperLimit and not obcTooHotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.obcTooHotFlag) { triggerEvent(tcsCtrl::OBC_OVERHEATING, tempFloatToU32()); - obcTooHotFlag = true; - } else if (not componentAboveUpperLimit) { - obcTooHotFlag = false; - } -} - -void ThermalController::ctrlObcIfBoard() { - thermalComponent = OBCIF_BOARD; - sensors[0].first = deviceTemperatures.q7s.isValid(); - sensors[0].second = deviceTemperatures.q7s.value; - sensors[1].first = sensorTemperatures.tmp1075Tcs0.isValid(); - sensors[1].second = sensorTemperatures.tmp1075Tcs0.value; - sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid(); - sensors[2].second = sensorTemperatures.tmp1075Tcs1.value; - numSensors = 3; - HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits); - ctrlComponentTemperature(htrCtx); - if (componentAboveUpperLimit and not obcTooHotFlag) { - triggerEvent(tcsCtrl::OBC_OVERHEATING, tempFloatToU32()); - obcTooHotFlag = true; - } else if (not componentAboveUpperLimit) { - obcTooHotFlag = false; // TODO: !! + tooHotFlags.obcTooHotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.obcTooHotFlag = false; } } void ThermalController::ctrlSBandTransceiver() { - thermalComponent = SBAND_TRANSCEIVER; + ctrlCtx.thermalComponent = tcsCtrl::SBAND_TRANSCEIVER; sensors[0].first = deviceTemperatures.syrlinksPowerAmplifier.isValid(); sensors[0].second = deviceTemperatures.syrlinksPowerAmplifier.value; sensors[1].first = deviceTemperatures.syrlinksBasebandBoard.isValid(); @@ -1255,48 +1280,48 @@ void ThermalController::ctrlSBandTransceiver() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_7_S_BAND, heater::HEATER_4_CAMERA, sBandTransceiverLimits); ctrlComponentTemperature(htrCtx); - if (componentAboveUpperLimit and not syrlinksTooHotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.syrlinksTooHotFlag) { triggerEvent(tcsCtrl::SYRLINKS_OVERHEATING, tempFloatToU32()); - syrlinksTooHotFlag = true; - } else if (not componentAboveUpperLimit) { - syrlinksTooHotFlag = false; + tooHotFlags.syrlinksTooHotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.syrlinksTooHotFlag = false; } } void ThermalController::ctrlPcduP60Board() { - thermalComponent = PCDUP60_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::PCDUP60_BOARD; sensors[0].first = deviceTemperatures.temp1P60dock.isValid(); sensors[0].second = deviceTemperatures.temp1P60dock.value; sensors[1].first = deviceTemperatures.temp2P60dock.isValid(); sensors[1].second = deviceTemperatures.temp2P60dock.value; numSensors = 2; - HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduP60BoardLimits); + HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduP60BoardLimits); ctrlComponentTemperature(htrCtx); - if (componentAboveUpperLimit and not pcduSystemTooHotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.pcduSystemTooHotFlag) { triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32()); - pcduSystemTooHotFlag = true; - } else if (not componentAboveUpperLimit) { - pcduSystemTooHotFlag = false; + tooHotFlags.pcduSystemTooHotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.pcduSystemTooHotFlag = false; } // TODO: ! } void ThermalController::ctrlPcduAcu() { - thermalComponent = PCDUACU; - heater::Switch switchNr = heater::HEATER_3_PCDU_PDU; + ctrlCtx.thermalComponent = tcsCtrl::PCDUACU; + heater::Switch switchNr = heater::HEATER_1_PCDU_PDU; heater::Switch redSwitchNr = heater::HEATER_2_ACS_BRD; if (chooseHeater(switchNr, redSwitchNr)) { bool sensorTempAvailable = true; // TODO: check if (deviceTemperatures.acu.value[0] != INVALID_TEMPERATURE) { - sensorTemp = deviceTemperatures.acu.value[0]; + ctrlCtx.sensorTemp = deviceTemperatures.acu.value[0]; } else if (deviceTemperatures.acu.value[1] != INVALID_TEMPERATURE) { - sensorTemp = deviceTemperatures.acu.value[1]; + ctrlCtx.sensorTemp = deviceTemperatures.acu.value[1]; } else if (deviceTemperatures.acu.value[2] != INVALID_TEMPERATURE) { - sensorTemp = deviceTemperatures.acu.value[2]; + ctrlCtx.sensorTemp = deviceTemperatures.acu.value[2]; } else if (sensorTemperatures.acu.isValid()) { - sensorTemp = sensorTemperatures.acu.value; + ctrlCtx.sensorTemp = sensorTemperatures.acu.value; } else { - triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, thermalComponent); + triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, ctrlCtx.thermalComponent); sensorTempAvailable = false; } if (sensorTempAvailable) { @@ -1304,16 +1329,16 @@ void ThermalController::ctrlPcduAcu() { checkLimitsAndCtrlHeater(htrCtx); } } - if (componentAboveUpperLimit and not pcduSystemTooHotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.pcduSystemTooHotFlag) { triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32()); - pcduSystemTooHotFlag = true; - } else if (not componentAboveUpperLimit) { - pcduSystemTooHotFlag = false; + tooHotFlags.pcduSystemTooHotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.pcduSystemTooHotFlag = false; } } void ThermalController::ctrlPcduPdu() { - thermalComponent = PCDUPDU; + ctrlCtx.thermalComponent = tcsCtrl::PCDUPDU; sensors[0].first = deviceTemperatures.pdu1.isValid(); sensors[0].second = deviceTemperatures.pdu1.value; sensors[1].first = deviceTemperatures.pdu2.isValid(); @@ -1321,18 +1346,18 @@ void ThermalController::ctrlPcduPdu() { sensors[2].first = sensorTemperatures.tmp1075Tcs0.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs0.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduPduLimits); + HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduPduLimits); ctrlComponentTemperature(htrCtx); - if (componentAboveUpperLimit and not pcduSystemTooHotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.pcduSystemTooHotFlag) { triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32()); - pcduSystemTooHotFlag = true; - } else if (not componentAboveUpperLimit) { - pcduSystemTooHotFlag = false; + tooHotFlags.pcduSystemTooHotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.pcduSystemTooHotFlag = false; } } void ThermalController::ctrlPlPcduBoard() { - thermalComponent = PLPCDU_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::PLPCDU_BOARD; sensors[0].first = sensorTemperatures.tmp1075PlPcdu0.isValid(); sensors[0].second = sensorTemperatures.tmp1075PlPcdu0.value; sensors[1].first = sensorTemperatures.tmp1075PlPcdu1.isValid(); @@ -1342,13 +1367,13 @@ void ThermalController::ctrlPlPcduBoard() { sensors[3].first = sensorTemperatures.plpcduHeatspreader.isValid(); sensors[3].second = sensorTemperatures.plpcduHeatspreader.value; numSensors = 4; - HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits); + HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); + tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag); } void ThermalController::ctrlPlocMissionBoard() { - thermalComponent = PLOCMISSION_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::PLOCMISSION_BOARD; sensors[0].first = sensorTemperatures.plocHeatspreader.isValid(); sensors[0].second = sensorTemperatures.plocHeatspreader.value; sensors[1].first = sensorTemperatures.plocMissionboard.isValid(); @@ -1356,14 +1381,14 @@ void ThermalController::ctrlPlocMissionBoard() { sensors[2].first = sensorTemperatures.dacHeatspreader.isValid(); sensors[2].second = sensorTemperatures.dacHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, + HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, plocMissionBoardLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, plocTooHotFlag); + tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, tooHotFlags.plocTooHotFlag); } void ThermalController::ctrlPlocProcessingBoard() { - thermalComponent = PLOCPROCESSING_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::PLOCPROCESSING_BOARD; sensors[0].first = sensorTemperatures.plocMissionboard.isValid(); sensors[0].second = sensorTemperatures.plocMissionboard.value; sensors[1].first = sensorTemperatures.plocHeatspreader.isValid(); @@ -1371,14 +1396,14 @@ void ThermalController::ctrlPlocProcessingBoard() { sensors[2].first = sensorTemperatures.dacHeatspreader.isValid(); sensors[2].second = sensorTemperatures.dacHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, + HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, plocProcessingBoardLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, plocTooHotFlag); + tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, tooHotFlags.plocTooHotFlag); } void ThermalController::ctrlDac() { - thermalComponent = DAC; + ctrlCtx.thermalComponent = tcsCtrl::DAC; sensors[0].first = sensorTemperatures.dacHeatspreader.isValid(); sensors[0].second = sensorTemperatures.dacHeatspreader.value; sensors[1].first = sensorTemperatures.plocMissionboard.isValid(); @@ -1386,13 +1411,13 @@ void ThermalController::ctrlDac() { sensors[2].first = sensorTemperatures.plocHeatspreader.isValid(); sensors[2].second = sensorTemperatures.plocHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, dacLimits); + HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, dacLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); + tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag); } void ThermalController::ctrlCameraBody() { - thermalComponent = CAMERA; + ctrlCtx.thermalComponent = tcsCtrl::CAMERA; sensors[0].first = sensorTemperatures.payload4kCamera.isValid(); sensors[0].second = sensorTemperatures.payload4kCamera.value; sensors[1].first = sensorTemperatures.dro.isValid(); @@ -1402,7 +1427,7 @@ void ThermalController::ctrlCameraBody() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_4_CAMERA, heater::HEATER_6_DRO, cameraLimits); ctrlComponentTemperature(htrCtx); - if (componentAboveUpperLimit and not camTooHotOneShotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not tooHotFlags.camTooHotOneShotFlag) { triggerEvent(tcsCtrl::CAMERA_OVERHEATING, tempFloatToU32()); CommandMessage msg; HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HealthState::FAULTY); @@ -1411,14 +1436,14 @@ void ThermalController::ctrlCameraBody() { sif::error << "ThermalController::ctrlCameraBody(): Sending health message failed" << std::endl; } - camTooHotOneShotFlag = true; - } else if (not componentAboveUpperLimit) { - camTooHotOneShotFlag = false; + tooHotFlags.camTooHotOneShotFlag = true; + } else if (not ctrlCtx.componentAboveUpperLimit) { + tooHotFlags.camTooHotOneShotFlag = false; } } void ThermalController::ctrlDro() { - thermalComponent = DRO; + ctrlCtx.thermalComponent = tcsCtrl::DRO; sensors[0].first = sensorTemperatures.dro.isValid(); sensors[0].second = sensorTemperatures.dro.value; sensors[1].first = sensorTemperatures.payload4kCamera.isValid(); @@ -1428,11 +1453,11 @@ void ThermalController::ctrlDro() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, droLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); + tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag); } void ThermalController::ctrlX8() { - thermalComponent = X8; + ctrlCtx.thermalComponent = tcsCtrl::X8; sensors[0].first = sensorTemperatures.x8.isValid(); sensors[0].second = sensorTemperatures.x8.value; sensors[1].first = sensorTemperatures.hpa.isValid(); @@ -1442,11 +1467,11 @@ void ThermalController::ctrlX8() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, x8Limits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); + tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag); } void ThermalController::ctrlTx() { - thermalComponent = TX; + ctrlCtx.thermalComponent = tcsCtrl::TX; sensors[0].first = sensorTemperatures.eBandTx.isValid(); sensors[0].second = sensorTemperatures.eBandTx.value; sensors[1].first = sensorTemperatures.x8.isValid(); @@ -1456,11 +1481,11 @@ void ThermalController::ctrlTx() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, txLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); + tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag); } void ThermalController::ctrlMpa() { - thermalComponent = MPA; + ctrlCtx.thermalComponent = tcsCtrl::MPA; sensors[0].first = sensorTemperatures.mpa.isValid(); sensors[0].second = sensorTemperatures.mpa.value; sensors[1].first = sensorTemperatures.hpa.isValid(); @@ -1470,11 +1495,11 @@ void ThermalController::ctrlMpa() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, mpaLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); + tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag); } void ThermalController::ctrlHpa() { - thermalComponent = HPA; + ctrlCtx.thermalComponent = tcsCtrl::HPA; sensors[0].first = sensorTemperatures.hpa.isValid(); sensors[0].second = sensorTemperatures.hpa.value; sensors[1].first = sensorTemperatures.x8.isValid(); @@ -1484,11 +1509,11 @@ void ThermalController::ctrlHpa() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, hpaLimits); ctrlComponentTemperature(htrCtx); - tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); + tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag); } void ThermalController::ctrlScexBoard() { - thermalComponent = SCEX_BOARD; + ctrlCtx.thermalComponent = tcsCtrl::SCEX_BOARD; sensors[0].first = sensorTemperatures.scex.isValid(); sensors[0].second = sensorTemperatures.scex.value; sensors[1].first = sensorTemperatures.x8.isValid(); @@ -1498,10 +1523,11 @@ void ThermalController::ctrlScexBoard() { numSensors = 3; HeaterContext htrCtx(heater::HEATER_6_DRO, heater::HEATER_5_STR, scexBoardLimits); ctrlComponentTemperature(htrCtx); - tooHotHandlerWhichClearsOneShotFlag(objects::SCEX, scexTooHotFlag); + tooHotHandlerWhichClearsOneShotFlag(objects::SCEX, tooHotFlags.scexTooHotFlag); } -void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heaterSwitchStates) { +void ThermalController::performThermalModuleCtrl( + const tcsCtrl::HeaterSwitchStates& heaterSwitchStates) { ctrlAcsBoard(); ctrlMgt(); ctrlRw(); @@ -1509,7 +1535,6 @@ void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heate ctrlIfBoard(); ctrlTcsBoard(); ctrlObc(); - ctrlObcIfBoard(); ctrlSBandTransceiver(); ctrlPcduP60Board(); ctrlPcduAcu(); @@ -1518,11 +1543,11 @@ void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heate // Payload components std::array plocInAllowedRange{}; ctrlPlocMissionBoard(); - plocInAllowedRange.at(0) = not componentAboveUpperLimit; + plocInAllowedRange.at(0) = not ctrlCtx.componentAboveUpperLimit; ctrlPlocProcessingBoard(); - plocInAllowedRange.at(1) = not componentAboveUpperLimit; + plocInAllowedRange.at(1) = not ctrlCtx.componentAboveUpperLimit; - if (plocTooHotFlag) { + if (tooHotFlags.plocTooHotFlag) { bool clearFlag = true; for (const auto& inRange : plocInAllowedRange) { if (not inRange) { @@ -1530,7 +1555,7 @@ void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heate } } if (clearFlag) { - plocTooHotFlag = false; + tooHotFlags.plocTooHotFlag = false; } } ctrlCameraBody(); @@ -1539,21 +1564,21 @@ void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heate // E-Band std::array eBandInAllowedRange{}; ctrlPlPcduBoard(); - eBandInAllowedRange.at(0) = not componentAboveUpperLimit; + eBandInAllowedRange.at(0) = not ctrlCtx.componentAboveUpperLimit; ctrlDac(); - eBandInAllowedRange.at(1) = not componentAboveUpperLimit; + eBandInAllowedRange.at(1) = not ctrlCtx.componentAboveUpperLimit; ctrlDro(); - eBandInAllowedRange.at(2) = not componentAboveUpperLimit; + eBandInAllowedRange.at(2) = not ctrlCtx.componentAboveUpperLimit; ctrlX8(); - eBandInAllowedRange.at(3) = not componentAboveUpperLimit; + eBandInAllowedRange.at(3) = not ctrlCtx.componentAboveUpperLimit; ctrlHpa(); - eBandInAllowedRange.at(4) = not componentAboveUpperLimit; + eBandInAllowedRange.at(4) = not ctrlCtx.componentAboveUpperLimit; ctrlTx(); - eBandInAllowedRange.at(5) = not componentAboveUpperLimit; + eBandInAllowedRange.at(5) = not ctrlCtx.componentAboveUpperLimit; ctrlMpa(); - eBandInAllowedRange.at(6) = not componentAboveUpperLimit; + eBandInAllowedRange.at(6) = not ctrlCtx.componentAboveUpperLimit; - if (eBandTooHotFlag) { + if (tooHotFlags.eBandTooHotFlag) { bool clearFlag = true; for (const auto& inRange : eBandInAllowedRange) { if (not inRange) { @@ -1561,12 +1586,11 @@ void ThermalController::performThermalModuleCtrl(const HeaterSwitchStates& heate } } if (clearFlag) { - eBandTooHotFlag = false; + tooHotFlags.eBandTooHotFlag = false; } } - - heaterTransitionControl(heaterSwitchStates); } + void ThermalController::ctrlComponentTemperature(HeaterContext& htrCtx) { if (selectAndReadSensorTemp(htrCtx)) { if (chooseHeater(htrCtx.switchNr, htrCtx.redSwitchNr)) { @@ -1577,9 +1601,11 @@ void ThermalController::ctrlComponentTemperature(HeaterContext& htrCtx) { // No sensors available, so switch the heater off. We can not perform control tasks if we // are blind.. if (chooseHeater(htrCtx.switchNr, htrCtx.redSwitchNr)) { + // Also track the counter to prevent heater handler message spam. The heater handle can only + // process 2 messages per cycle. if (heaterCtrlAllowed() and - (heaterHandler.getSwitchState(htrCtx.switchNr) == HeaterHandler::SwitchState::ON)) { - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + (thermalStates[ctrlCtx.thermalComponent].noSensorAvailableCounter < 3)) { + heaterSwitchHelper(htrCtx.switchNr, heater::SwitchState::OFF, ctrlCtx.thermalComponent); } } } @@ -1590,20 +1616,21 @@ bool ThermalController::selectAndReadSensorTemp(HeaterContext& htrCtx) { if (sensors[i].first and sensors[i].second != INVALID_TEMPERATURE and sensors[i].second > SANITY_LIMIT_LOWER_TEMP and sensors[i].second < SANITY_LIMIT_UPPER_TEMP) { - sensorTemp = sensors[i].second; - thermalStates[thermalComponent].errorCounter = 0; + ctrlCtx.sensorTemp = sensors[i].second; + ctrlCtx.currentSensorIndex = i; + thermalStates[ctrlCtx.thermalComponent].noSensorAvailableCounter = 0; return true; } } - thermalStates[thermalComponent].errorCounter++; - if (thermalComponent != RW and thermalComponent != ACS_BOARD) { - if (thermalStates[thermalComponent].errorCounter <= 3) { - triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, thermalComponent); + thermalStates[ctrlCtx.thermalComponent].noSensorAvailableCounter++; + if (ctrlCtx.thermalComponent != tcsCtrl::RW and ctrlCtx.thermalComponent != tcsCtrl::ACS_BOARD) { + if (thermalStates[ctrlCtx.thermalComponent].noSensorAvailableCounter <= 3) { + triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, ctrlCtx.thermalComponent); } } else { - if (thermalStates[thermalComponent].errorCounter <= 8) { - triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, thermalComponent); + if (thermalStates[ctrlCtx.thermalComponent].noSensorAvailableCounter <= 8) { + triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, ctrlCtx.thermalComponent); } } @@ -1612,16 +1639,21 @@ bool ThermalController::selectAndReadSensorTemp(HeaterContext& htrCtx) { bool ThermalController::chooseHeater(heater::Switch& switchNr, heater::Switch redSwitchNr) { bool heaterAvailable = true; - if (heaterHandler.getHealth(switchNr) != HasHealthIF::HEALTHY) { - if (heaterHandler.getHealth(redSwitchNr) == HasHealthIF::HEALTHY) { + HasHealthIF::HealthState mainHealth = heaterHandler.getHealth(switchNr); + HasHealthIF::HealthState redHealth = heaterHandler.getHealth(redSwitchNr); + if (mainHealth != HasHealthIF::HEALTHY) { + if (redHealth == HasHealthIF::HEALTHY) { switchNr = redSwitchNr; - redSwitchNrInUse = true; + ctrlCtx.redSwitchNrInUse = true; } else { heaterAvailable = false; - triggerEvent(tcsCtrl::NO_HEALTHY_HEATER_AVAILABLE, switchNr, redSwitchNr); + // Special case: Ground might command/do something with the heaters, so prevent spam. + if (not(mainHealth == EXTERNAL_CONTROL and redHealth == EXTERNAL_CONTROL)) { + triggerEvent(tcsCtrl::NO_HEALTHY_HEATER_AVAILABLE, switchNr, redSwitchNr); + } } } else { - redSwitchNrInUse = false; + ctrlCtx.redSwitchNrInUse = false; } return heaterAvailable; } @@ -1630,23 +1662,23 @@ void ThermalController::heaterCtrlTempTooHighHandler(HeaterContext& htrCtx, cons if (not heaterCtrlAllowed()) { return; } - if (htrCtx.switchState == HeaterHandler::SwitchState::ON) { - sif::info << "TCS: Component " << static_cast(thermalComponent) << " too warm, above " - << whatLimit << ", switching off heater" << std::endl; - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + if (htrCtx.switchState == heater::SwitchState::ON) { + sif::info << "TCS: Component " << static_cast(ctrlCtx.thermalComponent) + << " too warm, above " << whatLimit << ", switching off heater" << std::endl; + heaterSwitchHelper(htrCtx.switchNr, heater::SwitchState::OFF, ctrlCtx.thermalComponent); heaterStates[htrCtx.switchNr].switchTransition = true; - heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::OFF; + heaterStates[htrCtx.switchNr].target = heater::SwitchState::OFF; } - if (heaterHandler.getSwitchState(htrCtx.redSwitchNr) == HeaterHandler::SwitchState::ON) { - heaterSwitchHelper(htrCtx.redSwitchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + if (heaterHandler.getSwitchState(htrCtx.redSwitchNr) == heater::SwitchState::ON) { + heaterSwitchHelper(htrCtx.redSwitchNr, heater::SwitchState::OFF, ctrlCtx.thermalComponent); heaterStates[htrCtx.redSwitchNr].switchTransition = true; - heaterStates[htrCtx.redSwitchNr].target = HeaterHandler::SwitchState::OFF; + heaterStates[htrCtx.redSwitchNr].target = heater::SwitchState::OFF; } } void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { - componentAboveCutOffLimit = false; - componentAboveUpperLimit = false; + ctrlCtx.componentAboveCutOffLimit = false; + ctrlCtx.componentAboveUpperLimit = false; // Stay passive during switch transitions, wait for heater switching to complete. Otherwise, // still check whether components are out of range, which might be important information for the // top level control loop. @@ -1656,39 +1688,41 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { return; } - htrCtx.switchState = heaterHandler.getSwitchState(htrCtx.switchNr); + htrCtx.switchState = + static_cast(heaterInfo.heaterSwitchState[htrCtx.switchNr]); // Heater off - if (htrCtx.switchState == HeaterHandler::SwitchState::OFF) { - if (sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) { - sif::info << "TCS: Heater " << static_cast(thermalComponent) << " ON" << std::endl; - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::ON, thermalComponent); - heaterStates[htrCtx.switchNr].switchTransition = true; - heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::ON; + if (htrCtx.switchState == heater::SwitchState::OFF) { + if (ctrlCtx.sensorTemp < htrCtx.tempLimit.opLowerLimit and heaterCtrlAllowed()) { + sif::info << "TCS: Heater " << static_cast(ctrlCtx.thermalComponent) << " ON" + << std::endl; + heaterSwitchHelper(htrCtx.switchNr, heater::SwitchState::ON, ctrlCtx.thermalComponent); } else { // Even if heater control is now allowed, we can update the state. - thermalStates[thermalComponent].heating = false; + thermalStates[ctrlCtx.thermalComponent].heating = false; } heaterCtrlCheckUpperLimits(htrCtx); return; } // Heater on - if (htrCtx.switchState == HeaterHandler::SwitchState::ON) { - if (thermalStates[thermalComponent].heating) { + if (htrCtx.switchState == heater::SwitchState::ON) { + if (thermalStates[ctrlCtx.thermalComponent].heating) { // We are already in a heating cycle, so need to check whether heating task is complete. - if (sensorTemp >= htrCtx.tempLimit.opLowerLimit + TEMP_OFFSET and heaterCtrlAllowed()) { - sif::info << "TCS: Heater " << static_cast(thermalComponent) << " OFF" << std::endl; - heaterSwitchHelper(htrCtx.switchNr, HeaterHandler::SwitchState::OFF, thermalComponent); + if (ctrlCtx.sensorTemp >= htrCtx.tempLimit.opLowerLimit + TEMP_OFFSET and + heaterCtrlAllowed()) { + sif::info << "TCS: Heater " << static_cast(ctrlCtx.thermalComponent) << " OFF" + << std::endl; + heaterSwitchHelper(htrCtx.switchNr, heater::SwitchState::OFF, ctrlCtx.thermalComponent); heaterStates[htrCtx.switchNr].switchTransition = true; - heaterStates[htrCtx.switchNr].target = HeaterHandler::SwitchState::OFF; + heaterStates[htrCtx.switchNr].target = heater::SwitchState::OFF; } return; } // This can happen if heater is used as alternative heater (no regular heating cycle), so we // should still check the upper limits. bool tooHighHandlerAlreadyCalled = heaterCtrlCheckUpperLimits(htrCtx); - if (sensorTemp >= htrCtx.tempLimit.cutOffLimit) { - componentAboveCutOffLimit = true; + if (ctrlCtx.sensorTemp >= htrCtx.tempLimit.cutOffLimit) { + ctrlCtx.componentAboveCutOffLimit = true; if (not tooHighHandlerAlreadyCalled) { heaterCtrlTempTooHighHandler(htrCtx, "CutOff-Limit"); } @@ -1697,19 +1731,19 @@ void ThermalController::checkLimitsAndCtrlHeater(HeaterContext& htrCtx) { } bool ThermalController::heaterCtrlCheckUpperLimits(HeaterContext& htrCtx) { - if (sensorTemp >= htrCtx.tempLimit.nopUpperLimit) { - componentAboveUpperLimit = true; + if (ctrlCtx.sensorTemp >= htrCtx.tempLimit.nopUpperLimit) { + ctrlCtx.componentAboveUpperLimit = true; if (htrCtx.doHeaterHandling) { heaterCtrlTempTooHighHandler(htrCtx, "NOP-Limit"); } - overHeatEventToTrigger = ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH; + ctrlCtx.overHeatEventToTrigger = ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH; return true; - } else if (sensorTemp >= htrCtx.tempLimit.opUpperLimit) { - componentAboveUpperLimit = true; + } else if (ctrlCtx.sensorTemp >= htrCtx.tempLimit.opUpperLimit) { + ctrlCtx.componentAboveUpperLimit = true; if (htrCtx.doHeaterHandling) { heaterCtrlTempTooHighHandler(htrCtx, "OP-Limit"); } - overHeatEventToTrigger = ThermalComponentIF::COMPONENT_TEMP_HIGH; + ctrlCtx.overHeatEventToTrigger = ThermalComponentIF::COMPONENT_TEMP_HIGH; return true; } return false; @@ -1720,17 +1754,30 @@ void ThermalController::resetSensorsArray() { validValuePair.first = false; validValuePair.second = INVALID_TEMPERATURE; } - thermalComponent = NONE; + ctrlCtx.thermalComponent = tcsCtrl::NONE; } -void ThermalController::heaterTransitionControl(const HeaterSwitchStates& currentHeaterStates) { - for (unsigned i = 0; i < 7; i++) { +void ThermalController::heaterTransitionControl( + const tcsCtrl::HeaterSwitchStates& currentHeaterStates) { + for (unsigned i = 0; i < heater::Switch::NUMBER_OF_SWITCHES; i++) { if (heaterStates[i].switchTransition) { if (currentHeaterStates[i] == heaterStates[i].target) { + // Required for max heater period control + if (currentHeaterStates[i] == heater::SwitchState::ON) { + heaterStates[i].heaterOnMaxBurnTime.setTimeout(MAX_HEATER_ON_DURATIONS_MS[i]); + heaterStates[i].heaterOnMaxBurnTime.resetTimer(); + heaterStates[i].trackHeaterMaxBurnTime = true; + } else { + heaterStates[i].trackHeaterMaxBurnTime = false; + // The heater might still be one for some thermal components, so cross-check + // those components + crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(static_cast(i)); + } heaterStates[i].switchTransition = false; + heaterStates[i].heaterSwitchControlCycles = 0; continue; } - if (heaterStates[i].heaterSwitchControlCycles > 3) { + if (heaterStates[i].heaterSwitchControlCycles > 5) { heaterStates[i].switchTransition = false; heaterStates[i].heaterSwitchControlCycles = 0; } @@ -1738,8 +1785,30 @@ void ThermalController::heaterTransitionControl(const HeaterSwitchStates& curren } } } + +void ThermalController::heaterMaxDurationControl( + const tcsCtrl::HeaterSwitchStates& currentHeaterStates) { + for (unsigned i = 0; i < heater::Switch::NUMBER_OF_SWITCHES; i++) { + // Right now, we only track the maximum duration for heater which were commanded by the TCS + // controller. + if (currentHeaterStates[i] == heater::SwitchState::ON and + heaterStates[i].trackHeaterMaxBurnTime and + heaterStates[i].heaterOnMaxBurnTime.hasTimedOut()) { + heaterStates[i].switchTransition = false; + heaterStates[i].heaterSwitchControlCycles = 0; + heaterStates[i].trackHeaterMaxBurnTime = false; + triggerEvent(tcsCtrl::TCS_HEATER_MAX_BURN_TIME_REACHED, static_cast(i), + MAX_HEATER_ON_DURATIONS_MS[i]); + heaterSwitchHelper(static_cast(i), heater::SwitchState::OFF, std::nullopt); + // The heater might still be one for some thermal components, so cross-check + // those components + crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(static_cast(i)); + } + } +} + uint32_t ThermalController::tempFloatToU32() const { - auto sensorTempAsFloat = static_cast(sensorTemp); + auto sensorTempAsFloat = static_cast(ctrlCtx.sensorTemp); uint32_t tempRaw = 0; size_t dummyLen = 0; SerializeAdapter::serialize(&sensorTempAsFloat, reinterpret_cast(&tempRaw), &dummyLen, @@ -1758,9 +1827,9 @@ void ThermalController::setMode(Mode_t mode, Submode_t submode) { } bool ThermalController::tooHotHandler(object_id_t object, bool& oneShotFlag) { - if (componentAboveUpperLimit and not oneShotFlag) { + if (ctrlCtx.componentAboveUpperLimit and not oneShotFlag) { // Too hot -> returns true - EventManagerIF::triggerEvent(object, overHeatEventToTrigger, tempFloatToU32()); + EventManagerIF::triggerEvent(object, ctrlCtx.overHeatEventToTrigger, tempFloatToU32()); oneShotFlag = true; return true; } @@ -1772,31 +1841,48 @@ bool ThermalController::heaterCtrlAllowed() const { return submode != SUBMODE_NO void ThermalController::resetThermalStates() { for (auto& thermalState : thermalStates) { thermalState.heating = false; + thermalState.noSensorAvailableCounter = 0; + thermalState.heaterStartTime = 0; + thermalState.heaterEndTime = 0; + thermalState.sensorIndex = 0; + thermalState.heaterSwitch = heater::Switch::NUMBER_OF_SWITCHES; } } -void ThermalController::heaterSwitchHelper(heater::Switch switchNr, - HeaterHandler::SwitchState state, - unsigned componentIdx) { +void ThermalController::heaterSwitchHelper(heater::Switch switchNr, heater::SwitchState targetState, + std::optional componentIdx) { timeval currentTime; - Clock::getClockMonotonic(¤tTime); - if (state == HeaterHandler::SwitchState::ON) { - heaterHandler.switchHeater(switchNr, state); - thermalStates[componentIdx].heating = true; - thermalStates[componentIdx].heaterStartTime = currentTime.tv_sec; + Clock::getClock(¤tTime); + if (targetState == heater::SwitchState::ON) { + heaterHandler.switchHeater(switchNr, targetState); + heaterStates[switchNr].target = heater::SwitchState::ON; + heaterStates[switchNr].switchTransition = true; + if (componentIdx.has_value()) { + unsigned componentIdxVal = componentIdx.value(); + thermalStates[componentIdxVal].sensorIndex = ctrlCtx.currentSensorIndex; + thermalStates[componentIdxVal].heaterSwitch = switchNr; + thermalStates[componentIdxVal].heating = true; + thermalStates[componentIdxVal].heaterStartTime = currentTime.tv_sec; + } + triggerEvent(tcsCtrl::TCS_SWITCHING_HEATER_ON, static_cast(ctrlCtx.thermalComponent), + static_cast(switchNr)); } else { - heaterHandler.switchHeater(switchNr, state); - thermalStates[componentIdx].heating = false; - thermalStates[componentIdx].heaterEndTime = currentTime.tv_sec; + heaterHandler.switchHeater(switchNr, targetState); + if (componentIdx.has_value()) { + thermalStates[componentIdx.value()].heating = false; + thermalStates[componentIdx.value()].heaterEndTime = currentTime.tv_sec; + } + triggerEvent(tcsCtrl::TCS_SWITCHING_HEATER_OFF, static_cast(ctrlCtx.thermalComponent), + static_cast(switchNr)); } } void ThermalController::heaterSwitchHelperAllOff() { timeval currentTime; - Clock::getClockMonotonic(¤tTime); + Clock::getClock(¤tTime); size_t idx = 0; for (; idx < heater::Switch::NUMBER_OF_SWITCHES; idx++) { - heaterHandler.switchHeater(static_cast(idx), HeaterHandler::SwitchState::OFF); + heaterHandler.switchHeater(static_cast(idx), heater::SwitchState::OFF); } for (idx = 0; idx < thermalStates.size(); idx++) { thermalStates[idx].heating = false; @@ -1804,9 +1890,27 @@ void ThermalController::heaterSwitchHelperAllOff() { } } +ThermalController::~ThermalController() { + if (tmp1075SetPlPcdu1 != nullptr) { + delete tmp1075SetPlPcdu1; + } +} + +void ThermalController::crossCheckHeaterStateOfComponentsWhenHeaterGoesOff( + heater::Switch switchIdx) { + for (unsigned j = 0; j < thermalStates.size(); j++) { + if (thermalStates[j].heating and thermalStates[j].heaterSwitch == switchIdx) { + timeval currentTime; + Clock::getClock(¤tTime); + thermalStates[j].heating = false; + thermalStates[j].heaterEndTime = currentTime.tv_sec; + } + } +} + void ThermalController::tooHotHandlerWhichClearsOneShotFlag(object_id_t object, bool& oneShotFlag) { // Clear the one shot flag is the component is in acceptable temperature range. - if (not tooHotHandler(object, oneShotFlag) and not componentAboveUpperLimit) { + if (not tooHotHandler(object, oneShotFlag) and not ctrlCtx.componentAboveUpperLimit) { oneShotFlag = false; } } diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 7aa05b1e..1062fe97 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -1,7 +1,7 @@ #ifndef MISSION_CONTROLLER_THERMALCONTROLLER_H_ #define MISSION_CONTROLLER_THERMALCONTROLLER_H_ -#include +#include #include #include #include @@ -24,74 +24,7 @@ #include #include - -/** - * NOP Limit: Hard limit for device, usually from datasheet. Device damage is possible lif NOP limit - * is exceeded. - * OP Limit: Soft limit. Device should be switched off or TCS controller should take action if the - * limit is exceeded to avoid reaching NOP limit - */ -struct TempLimits { - TempLimits(float nopLowerLimit, float opLowerLimit, float cutOffLimit, float opUpperLimit, - float nopUpperLimit) - : opLowerLimit(opLowerLimit), - opUpperLimit(opUpperLimit), - cutOffLimit(cutOffLimit), - nopLowerLimit(nopLowerLimit), - nopUpperLimit(nopUpperLimit) {} - float opLowerLimit; - float opUpperLimit; - float cutOffLimit; - float nopLowerLimit; - float nopUpperLimit; -}; - -struct ThermalState { - uint8_t errorCounter; - // Is heating on for that thermal module? - bool heating = false; - heater::Switch heaterSwitch = heater::Switch::NUMBER_OF_SWITCHES; - // Heater start time and end times as UNIX seconds. Please note that these times will be updated - // when a switch command is sent, with no guarantess that the heater actually went on. - uint32_t heaterStartTime = 0; - uint32_t heaterEndTime = 0; -}; - -struct HeaterState { - bool switchTransition; - HeaterHandler::SwitchState target; - uint8_t heaterSwitchControlCycles; -}; - -using HeaterSwitchStates = std::array; - -enum ThermalComponents : uint8_t { - NONE = 0, - ACS_BOARD = 1, - MGT = 2, - RW = 3, - STR = 4, - IF_BOARD = 5, - TCS_BOARD = 6, - OBC = 7, - OBCIF_BOARD = 8, - SBAND_TRANSCEIVER = 9, - PCDUP60_BOARD = 10, - PCDUACU = 11, - PCDUPDU = 12, - PLPCDU_BOARD = 13, - PLOCMISSION_BOARD = 14, - PLOCPROCESSING_BOARD = 15, - DAC = 16, - CAMERA = 17, - DRO = 18, - X8 = 19, - HPA = 20, - TX = 21, - MPA = 22, - SCEX_BOARD = 23, - NUM_ENTRIES -}; +#include class ThermalController : public ExtendedControllerBase { public: @@ -102,8 +35,28 @@ class ThermalController : public ExtendedControllerBase { static constexpr int16_t SANITY_LIMIT_LOWER_TEMP = -80; static constexpr int16_t SANITY_LIMIT_UPPER_TEMP = 160; + // 1 hour + static constexpr uint32_t DEFAULT_MAX_HEATER_ON_DURATION_MS = 60 * 60 * 1000; + static constexpr uint32_t MAX_HEATER_ON_DURATIONS_MS[8] = {// PLOC PROC board + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // PCDU PDU + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // ACS Board + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // OBC Board + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // Camera + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // STR + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // DRO + DEFAULT_MAX_HEATER_ON_DURATION_MS, + // S-Band + DEFAULT_MAX_HEATER_ON_DURATION_MS}; + ThermalController(object_id_t objectId, HeaterHandler& heater, - const std::atomic_bool& tcsBoardShortUnavailable); + const std::atomic_bool& tcsBoardShortUnavailable, bool pollPcdu1Tmp); + virtual ~ThermalController(); ReturnValue_t initialize() override; @@ -111,16 +64,16 @@ class ThermalController : public ExtendedControllerBase { struct HeaterContext { public: HeaterContext(heater::Switch switchNr, heater::Switch redundantSwitchNr, - const TempLimits& tempLimit) + const tcsCtrl::TempLimits& tempLimit) : switchNr(switchNr), redSwitchNr(redundantSwitchNr), tempLimit(tempLimit) {} bool doHeaterHandling = true; heater::Switch switchNr; - HeaterHandler::SwitchState switchState = HeaterHandler::SwitchState::OFF; + heater::SwitchState switchState = heater::SwitchState::OFF; heater::Switch redSwitchNr; - const TempLimits& tempLimit; + const tcsCtrl::TempLimits& tempLimit; }; - void performThermalModuleCtrl(const HeaterSwitchStates& heaterSwitchStates); + void performThermalModuleCtrl(const tcsCtrl::HeaterSwitchStates& heaterSwitchStates); ReturnValue_t handleCommandMessage(CommandMessage* message) override; void performControlOperation() override; ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, @@ -142,12 +95,12 @@ class ThermalController : public ExtendedControllerBase { HeaterHandler& heaterHandler; + bool pollPcdu1Tmp; tcsCtrl::SensorTemperatures sensorTemperatures; tcsCtrl::SusTemperatures susTemperatures; tcsCtrl::DeviceTemperatures deviceTemperatures; tcsCtrl::HeaterInfo heaterInfo; - lp_vec_t currentVecPdu2 = - lp_vec_t(gp_id_t(objects::PDU2_HANDLER, PDU::pool::PDU_CURRENTS)); + tcsCtrl::TcsCtrlInfo tcsCtrlInfo; DeviceHandlerThermalSet imtqThermalSet; @@ -173,7 +126,7 @@ class ThermalController : public ExtendedControllerBase { TMP1075::Tmp1075Dataset tmp1075SetTcs1; TMP1075::Tmp1075Dataset tmp1075SetPlPcdu0; // damaged - // TMP1075::Tmp1075Dataset tmp1075SetPlPcdu1; + TMP1075::Tmp1075Dataset* tmp1075SetPlPcdu1; TMP1075::Tmp1075Dataset tmp1075SetIfBoard; // SUS @@ -227,56 +180,67 @@ class ThermalController : public ExtendedControllerBase { lp_var_t tempMgm2 = lp_var_t(objects::MGM_2_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS); lp_var_t tempAdcPayloadPcdu = lp_var_t(objects::PLPCDU_HANDLER, plpcdu::TEMP); + lp_vec_t currentVecPdu2 = + lp_vec_t(gp_id_t(objects::PDU2_HANDLER, PDU::pool::PDU_CURRENTS)); // TempLimits - TempLimits acsBoardLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0); - TempLimits mgtLimits = TempLimits(-40.0, -40.0, 65.0, 70.0, 70.0); - TempLimits rwLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0); - TempLimits strLimits = TempLimits(-30.0, -20.0, 65.0, 70.0, 80.0); - TempLimits ifBoardLimits = TempLimits(-65.0, -40.0, 80.0, 85.0, 150.0); - TempLimits tcsBoardLimits = TempLimits(-60.0, -40.0, 80.0, 85.0, 130.0); - TempLimits obcLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0); - TempLimits obcIfBoardLimits = TempLimits(-65.0, -40.0, 80.0, 85.0, 125.0); - TempLimits sBandTransceiverLimits = TempLimits(-40.0, -25.0, 35.0, 40.0, 65.0); - TempLimits pcduP60BoardLimits = TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0); - TempLimits pcduAcuLimits = TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0); - TempLimits pcduPduLimits = TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0); - TempLimits plPcduBoardLimits = TempLimits(-55.0, -40.0, 80.0, 85.0, 125.0); - TempLimits plocMissionBoardLimits = TempLimits(-30.0, -10.0, 40.0, 45.0, 60); - TempLimits plocProcessingBoardLimits = TempLimits(-30.0, -10.0, 40.0, 45.0, 60.0); - TempLimits dacLimits = TempLimits(-65.0, -40.0, 113.0, 118.0, 150.0); - TempLimits cameraLimits = TempLimits(-40.0, -30.0, 60.0, 65.0, 85.0); - TempLimits droLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); - TempLimits x8Limits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); - TempLimits hpaLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); - TempLimits txLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); - TempLimits mpaLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); - TempLimits scexBoardLimits = TempLimits(-60.0, -40.0, 80.0, 85.0, 150.0); + tcsCtrl::TempLimits acsBoardLimits = tcsCtrl::TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0); + tcsCtrl::TempLimits mgtLimits = tcsCtrl::TempLimits(-40.0, -40.0, 65.0, 70.0, 70.0); + tcsCtrl::TempLimits rwLimits = tcsCtrl::TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0); + tcsCtrl::TempLimits strLimits = tcsCtrl::TempLimits(-30.0, -20.0, 65.0, 70.0, 80.0); + tcsCtrl::TempLimits ifBoardLimits = tcsCtrl::TempLimits(-65.0, -40.0, 80.0, 85.0, 150.0); + tcsCtrl::TempLimits tcsBoardLimits = tcsCtrl::TempLimits(-60.0, -40.0, 80.0, 85.0, 130.0); + tcsCtrl::TempLimits obcLimits = tcsCtrl::TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0); + tcsCtrl::TempLimits obcIfBoardLimits = tcsCtrl::TempLimits(-65.0, -40.0, 80.0, 85.0, 125.0); + tcsCtrl::TempLimits sBandTransceiverLimits = tcsCtrl::TempLimits(-40.0, -25.0, 35.0, 40.0, 65.0); + tcsCtrl::TempLimits pcduP60BoardLimits = tcsCtrl::TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0); + tcsCtrl::TempLimits pcduAcuLimits = tcsCtrl::TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0); + tcsCtrl::TempLimits pcduPduLimits = tcsCtrl::TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0); + tcsCtrl::TempLimits plPcduBoardLimits = tcsCtrl::TempLimits(-55.0, -40.0, 80.0, 85.0, 125.0); + tcsCtrl::TempLimits plocMissionBoardLimits = tcsCtrl::TempLimits(-30.0, -10.0, 40.0, 45.0, 60); + tcsCtrl::TempLimits plocProcessingBoardLimits = + tcsCtrl::TempLimits(-30.0, -10.0, 40.0, 45.0, 60.0); + tcsCtrl::TempLimits dacLimits = tcsCtrl::TempLimits(-65.0, -40.0, 113.0, 118.0, 150.0); + tcsCtrl::TempLimits cameraLimits = tcsCtrl::TempLimits(-40.0, -30.0, 60.0, 65.0, 85.0); + tcsCtrl::TempLimits droLimits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); + tcsCtrl::TempLimits x8Limits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); + tcsCtrl::TempLimits hpaLimits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); + tcsCtrl::TempLimits txLimits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); + tcsCtrl::TempLimits mpaLimits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0); + tcsCtrl::TempLimits scexBoardLimits = tcsCtrl::TempLimits(-60.0, -40.0, 80.0, 85.0, 150.0); + + struct CtrlContext { + double sensorTemp = INVALID_TEMPERATURE; + uint8_t currentSensorIndex = 0; + tcsCtrl::ThermalComponents thermalComponent = tcsCtrl::NONE; + bool redSwitchNrInUse = false; + bool componentAboveCutOffLimit = false; + bool componentAboveUpperLimit = false; + Event overHeatEventToTrigger; + } ctrlCtx; - double sensorTemp = INVALID_TEMPERATURE; - ThermalComponents thermalComponent = NONE; - bool redSwitchNrInUse = false; MessageQueueId_t camId = MessageQueueIF::NO_QUEUE; - bool componentAboveCutOffLimit = false; - bool componentAboveUpperLimit = false; - Event overHeatEventToTrigger; - bool eBandTooHotFlag = false; - bool camTooHotOneShotFlag = false; - bool scexTooHotFlag = false; - bool plocTooHotFlag = false; - bool pcduSystemTooHotFlag = false; - bool syrlinksTooHotFlag = false; - bool obcTooHotFlag = false; - bool strTooHotFlag = false; - bool rwTooHotFlag = false; + + struct TooHotFlags { + bool eBandTooHotFlag = false; + bool camTooHotOneShotFlag = false; + bool scexTooHotFlag = false; + bool plocTooHotFlag = false; + bool pcduSystemTooHotFlag = false; + bool syrlinksTooHotFlag = false; + bool obcTooHotFlag = false; + bool mgtTooHotFlag = false; + bool strTooHotFlag = false; + bool rwTooHotFlag = false; + } tooHotFlags; bool transitionWhenHeatersOff = false; uint32_t transitionWhenHeatersOffCycles = 0; Mode_t targetMode = MODE_OFF; Submode_t targetSubmode = SUBMODE_NONE; uint32_t cycles = 0; - std::array thermalStates{}; - std::array heaterStates{}; + std::array thermalStates{}; + std::array heaterStates{}; // Initial delay to make sure all pool variables have been initialized their owners. // Also, wait for system initialization to complete. @@ -297,6 +261,12 @@ class ThermalController : public ExtendedControllerBase { PoolEntry heaterSwitchStates = PoolEntry(heater::NUMBER_OF_SWITCHES); PoolEntry heaterCurrent = PoolEntry(); + PoolEntry tcsCtrlHeaterOn = PoolEntry(tcsCtrl::NUM_THERMAL_COMPONENTS); + PoolEntry tcsCtrlSensorIdx = PoolEntry(tcsCtrl::NUM_THERMAL_COMPONENTS); + PoolEntry tcsCtrlHeaterIdx = PoolEntry(tcsCtrl::NUM_THERMAL_COMPONENTS); + PoolEntry tcsCtrlStartTimes = PoolEntry(tcsCtrl::NUM_THERMAL_COMPONENTS); + PoolEntry tcsCtrlEndTimes = PoolEntry(tcsCtrl::NUM_THERMAL_COMPONENTS); + static constexpr dur_millis_t MUTEX_TIMEOUT = 50; void startTransition(Mode_t mode, Submode_t submode) override; @@ -318,8 +288,8 @@ class ThermalController : public ExtendedControllerBase { bool selectAndReadSensorTemp(HeaterContext& htrCtx); void heaterSwitchHelperAllOff(); - void heaterSwitchHelper(heater::Switch switchNr, HeaterHandler::SwitchState state, - unsigned componentIdx); + void heaterSwitchHelper(heater::Switch switchNr, heater::SwitchState state, + std::optional componentIdx); void ctrlAcsBoard(); void ctrlMgt(); @@ -328,7 +298,6 @@ class ThermalController : public ExtendedControllerBase { void ctrlIfBoard(); void ctrlTcsBoard(); void ctrlObc(); - void ctrlObcIfBoard(); void ctrlSBandTransceiver(); void ctrlPcduP60Board(); void ctrlPcduAcu(); @@ -344,7 +313,22 @@ class ThermalController : public ExtendedControllerBase { void ctrlTx(); void ctrlMpa(); void ctrlScexBoard(); - void heaterTransitionControl(const HeaterSwitchStates& currentHeaterStates); + + /** + * The transition of heaters might take some time. As long as a transition is + * going on, the TCS controller works in a reduced form. This function takes care + * of tracking transition and capturing their completion. + * @param currentHeaterStates + */ + void heaterTransitionControl(const tcsCtrl::HeaterSwitchStates& currentHeaterStates); + /** + * Control tasks to prevent heaters being on for prolonged periods. Ideally, this + * should never happen, but this task prevents bugs from causing heaters to stay on + * for a long time, which draws a lot of power. + * @param currentHeaterStates + */ + void heaterMaxDurationControl(const tcsCtrl::HeaterSwitchStates& currentHeaterStates); + void crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(heater::Switch switchIdx); void setMode(Mode_t mode, Submode_t submode); uint32_t tempFloatToU32() const; bool tooHotHandler(object_id_t object, bool& oneShotFlag); diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 03500cc3..eea30389 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -221,6 +221,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x23: parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta); break; + case 0x24: + parameterWrapper->set(susHandlingParameters.susBrightnessThreshold); + break; default: return INVALID_IDENTIFIER_ID; } @@ -290,6 +293,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x6: parameterWrapper->set(rwHandlingParameters.rampTime); break; + case 0x7: + parameterWrapper->set(rwHandlingParameters.multipleRwInvalidTimeout); + break; default: return INVALID_IDENTIFIER_ID; } @@ -315,7 +321,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setMatrix(rwMatrices.without4); break; case 0x6: - parameterWrapper->setVector(rwMatrices.nullspace); + parameterWrapper->setVector(rwMatrices.nullspaceVector); break; default: return INVALID_IDENTIFIER_ID; @@ -375,15 +381,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(idleModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef); + parameterWrapper->set(idleModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(idleModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(idleModeControllerParameters.desatOn); + parameterWrapper->set(idleModeControllerParameters.deSatGainFactor); break; case 0x8: + parameterWrapper->set(idleModeControllerParameters.desatOn); + break; + case 0x9: parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); break; default: @@ -408,48 +417,51 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->set(targetModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(targetModeControllerParameters.desatOn); + parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + parameterWrapper->set(targetModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(targetModeControllerParameters.refDirection); + parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(targetModeControllerParameters.refRotRate); + parameterWrapper->setVector(targetModeControllerParameters.refDirection); break; case 0xB: - parameterWrapper->setVector(targetModeControllerParameters.quatRef); + parameterWrapper->setVector(targetModeControllerParameters.refRotRate); break; case 0xC: - parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(targetModeControllerParameters.quatRef); break; case 0xD: - parameterWrapper->set(targetModeControllerParameters.latitudeTgt); + parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); break; case 0xE: - parameterWrapper->set(targetModeControllerParameters.longitudeTgt); + parameterWrapper->set(targetModeControllerParameters.latitudeTgt); break; case 0xF: - parameterWrapper->set(targetModeControllerParameters.altitudeTgt); + parameterWrapper->set(targetModeControllerParameters.longitudeTgt); break; case 0x10: - parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); + parameterWrapper->set(targetModeControllerParameters.altitudeTgt); break; case 0x11: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); + parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); break; case 0x12: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); break; case 0x13: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + break; + case 0x14: parameterWrapper->set(targetModeControllerParameters.blindRotRate); break; default: @@ -474,30 +486,33 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef); + parameterWrapper->set(gsTargetModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(gsTargetModeControllerParameters.desatOn); + parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); + parameterWrapper->set(gsTargetModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); + parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); break; case 0xB: - parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); break; case 0xC: - parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); break; case 0xD: + parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + break; + case 0xE: parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); break; default: @@ -522,24 +537,30 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef); + parameterWrapper->set(nadirModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(nadirModeControllerParameters.desatOn); + parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); + parameterWrapper->set(nadirModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(nadirModeControllerParameters.refDirection); + parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); break; case 0xA: + parameterWrapper->setVector(nadirModeControllerParameters.refDirection); + break; + case 0xB: parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; case 0xC: + parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); + break; + case 0xD: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); break; default: @@ -564,21 +585,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef); + parameterWrapper->set(inertialModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(inertialModeControllerParameters.desatOn); + parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); + parameterWrapper->set(inertialModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); + parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); break; case 0xA: + parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); + break; + case 0xB: parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); break; case 0xC: @@ -690,7 +714,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment); break; case 0x5: - parameterWrapper->set(magnetorquerParameter.dipolMax); + parameterWrapper->set(magnetorquerParameter.dipoleMax); break; case 0x6: parameterWrapper->set(magnetorquerParameter.torqueDuration); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 9600ca7e..9e13070f 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -766,6 +766,7 @@ class AcsParameters : public HasParametersIF { {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136, 0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513, -0.000889232196185857, -0.00168429567131815}}; + float susBrightnessThreshold = 0.7; } susHandlingParameters; struct GyrHandlingParameters { @@ -781,9 +782,9 @@ class AcsParameters : public HasParametersIF { /* 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 - pow(4.3e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms + float gyr02variance[3] = {pow(4.6e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms + pow(4.6e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms + pow(6.1e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; uint8_t preferAdis = false; float gyrFilterWeight = 0.6; @@ -798,6 +799,8 @@ class AcsParameters : public HasParametersIF { double stictionTorque = 0.0006; uint16_t rampTime = 10; + + uint32_t multipleRwInvalidTimeout = 25; } rwHandlingParameters; struct RwMatrices { @@ -814,7 +817,7 @@ class AcsParameters : public HasParametersIF { {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}}; double without4[4][3] = { {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}}; - double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000}; + double nullspaceVector[4] = {-1, 1, -1, 1}; } rwMatrices; struct SafeModeControllerParameters { @@ -838,7 +841,9 @@ class AcsParameters : public HasParametersIF { double om = 0.3; double omMax = 1 * M_PI / 180; double qiMin = 0.1; + double gainNullspace = 0.01; + double nullspaceSpeed = 32500; // 0.1 RPM double desatMomentumRef[3] = {0, 0, 0}; double deSatGainFactor = 1000; @@ -931,7 +936,7 @@ class AcsParameters : public HasParametersIF { 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 dipoleMax = 0.2; // [Am^2] uint16_t torqueDuration = 300; // [ms] } magnetorquerParameter; diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index d2fe2d65..0fb8bdf5 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -5,11 +5,6 @@ #include #include -#include - -#include "util/CholeskyDecomposition.h" -#include "util/MathOperations.h" - ActuatorCmd::ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {} @@ -25,24 +20,30 @@ void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) { } } -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 +void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, + const int32_t speedRw2, const int32_t speedRw3, + const double sampleTime, const double inertiaWheel, + const int32_t maxRwSpeed, const double *rwTorque, + int32_t *rwCmdSpeed) { + // group RW speed values (in 0.1 [RPM]) in vector int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3}; + + // calculate required RW speed as sum of current RW speed and RW speed delta + // delta w_rw = delta t / I_RW * torque_RW [rad/s] double deltaSpeed[4] = {0, 0, 0, 0}; - double radToRpm = 60 / (2 * PI); // factor for conversion to RPM - // W_RW = Torque_RW / I_RW * delta t [rad/s] - double factor = sampleTime / inertiaWheel * radToRpm; - int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; + const double factor = sampleTime / inertiaWheel * RAD_PER_SEC_TO_RPM * 10; VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); + + // convert double to int32 + int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; for (int i = 0; i < 4; i++) { deltaSpeedInt[i] = std::round(deltaSpeed[i]); } + + // sum of current RW speed and RW speed delta VectorOperations::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4); - VectorOperations::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4); + + // crop values which would exceed the maximum possible RPM for (uint8_t i = 0; i < 4; i++) { if (rwCmdSpeed[i] > maxRwSpeed) { rwCmdSpeed[i] = maxRwSpeed; @@ -52,24 +53,25 @@ void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t spee } } -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(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3, - 1); - // Scaling along largest element if dipol exceeds maximum +void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxDipole, + const double *dipoleMoment, int16_t *dipoleMomentActuator) { + // convert to actuator frame + double dipoleMomentActuatorDouble[3] = {0, 0, 0}; + MatrixOperations::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3, + 3, 1); + // scaling along largest element if dipole exceeds maximum 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); + VectorOperations::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx); + double maxAbsValue = std::abs(dipoleMomentActuatorDouble[maxIdx]); + if (maxAbsValue > maxDipole) { + double scalingFactor = maxDipole / maxAbsValue; + VectorOperations::mulScalar(dipoleMomentActuatorDouble, scalingFactor, + dipoleMomentActuatorDouble, 3); } // scale dipole from 1 Am^2 to 1e^-4 Am^2 - VectorOperations::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3); + VectorOperations::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble, + 3); for (int i = 0; i < 3; i++) { - dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]); + dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]); } } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 5d5d47f3..b14a4a25 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -1,9 +1,7 @@ #ifndef ACTUATORCMD_H_ #define ACTUATORCMD_H_ -#include "MultiplicativeKalmanFilter.h" -#include "SensorProcessing.h" -#include "SensorValues.h" +#include class ActuatorCmd { public: @@ -19,29 +17,30 @@ class ActuatorCmd { void scalingTorqueRws(double *rwTrq, double maxTorque); /* - * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction - * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given - * as Input to the RWs - * @param: rwTrqIn given torque from pointing controller - * rwTrqNS Nullspace torque + * @brief: cmdSpeedToRws() Calculates the RPM for the reaction wheel configuration, + * given the required torque calculated by the controller. Will also scale down the RPM of the + * wheels if they exceed the maximum possible RPM + * @param: rwTrq given torque from pointing controller * rwCmdSpeed output revolutions per minute for every * reaction wheel */ - 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); + void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, const double sampleTime, const double inertiaWheel, + const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed); /* - * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques + * @brief: cmdDipoleMtq() gives the commanded dipole moment for the + * magnetorquer * - * @param: dipolMoment given dipol moment in spacecraft frame - * dipolMomentActuator resulting dipol moment in actuator reference frame + * @param: dipoleMoment given dipole moment in spacecraft frame + * dipoleMomentActuator resulting dipole moment in actuator reference frame */ - void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, - const double *inverseAlignment, double maxDipol); + void cmdDipoleMtq(const double *inverseAlignment, const double maxDipole, + const double *dipoleMoment, int16_t *dipoleMomentActuator); protected: private: + static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI); }; #endif /* ACTUATORCMD_H_ */ diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 79c6b416..1d82d317 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -266,7 +266,8 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4], + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- @@ -296,9 +297,8 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double //---------------------------------------------------------------------------- // Calculation of reference rotation rate //---------------------------------------------------------------------------- - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; + int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; + targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], @@ -412,7 +412,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], - double errorQuat[4], double errorSatRotRate[3], double errorAngle) { + double errorQuat[4], double errorSatRotRate[3], double &errorAngle) { // First calculate error quaternion between current and target orientation QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); // Last calculate add rotation from reference quaternion @@ -424,26 +424,17 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do // Calculate error angle errorAngle = QuaternionOperations::getAngle(errorQuat, true); - // Only give back error satellite rotational rate if orientation has already been aquired - if (errorAngle < 2. / 180. * M_PI) { - // First combine the target and reference satellite rotational rates - double combinedRefSatRotRate[3] = {0, 0, 0}; - VectorOperations::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); - // Then substract the combined required satellite rotational rates from the actual rate - VectorOperations::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, - 3); - } else { - // If orientation has not been aquired yet set satellite rotational rate to zero - errorSatRotRate = 0; - } - - // target flag in matlab, importance, does look like it only gives feedback if pointing control is - // under 150 arcsec ?? + // Calculate error satellite rotational rate + // First combine the target and reference satellite rotational rates + double combinedRefSatRotRate[3] = {0, 0, 0}; + VectorOperations::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); + // Then subtract the combined required satellite rotational rates from the actual rate + VectorOperations::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, 3); } void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double errorQuat[4], - double errorSatRotRate[3], double errorAngle) { + double errorSatRotRate[3], double &errorAngle) { // First calculate error quaternion between current and target orientation QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); // Keep scalar part of quaternion positive @@ -453,17 +444,8 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do // Calculate error angle errorAngle = QuaternionOperations::getAngle(errorQuat, true); - // Only give back error satellite rotational rate if orientation has already been aquired - if (errorAngle < 2. / 180. * M_PI) { - // Then substract the combined required satellite rotational rates from the actual rate - VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); - } else { - // If orientation has not been aquired yet set satellite rotational rate to zero - errorSatRotRate = 0; - } - - // target flag in matlab, importance, does look like it only gives feedback if pointing control is - // under 150 arcsec ?? + // Calculate error satellite rotational rate + VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); } void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], @@ -471,20 +453,25 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua //------------------------------------------------------------------------------------- // Calculation of target rotation rate //------------------------------------------------------------------------------------- - double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - - (timeSavedQuaternion.tv_sec + - timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); + double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 - + (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6); + if (VectorOperations::norm(savedQuaternion, 4) == 0) { + std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion)); + } if (timeElapsed < timeElapsedMax) { + double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0}; + QuaternionOperations::inverse(quatInertialTarget, q); + QuaternionOperations::inverse(savedQuaternion, qS); double qDiff[4] = {0, 0, 0, 0}; - VectorOperations::subtract(quatInertialTarget, savedQuaternion, qDiff, 4); + VectorOperations::subtract(q, qS, qDiff, 4); VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); - double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, - qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; + double tgtQuatVec[3] = {q[0], q[1], q[2]}; + double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; - VectorOperations::cross(quatInertialTarget, qDiff, sum1); + VectorOperations::cross(tgtQuatVec, qDiffVec, sum1); VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); - VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); + VectorOperations::mulScalar(qDiffVec, q[3], sum3, 3); VectorOperations::add(sum1, sum2, sum, 3); VectorOperations::subtract(sum, sum3, sum, 3); double omegaRefNew[3] = {0, 0, 0}; @@ -531,10 +518,6 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, 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. - // Does not make sense, but is implemented that way in MATLAB ?! - // Thought: It does not really play a role, because in case there are more then one - // reaction wheel invalid the pointing control is destined to fail. return returnvalue::FAILED; } } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index a52c476a..7b81e411 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -15,7 +15,7 @@ class Guidance { void getTargetParamsSafe(double sunTargetSafe[3]); ReturnValue_t solarArrayDeploymentComplete(); - // Function to get the target quaternion and refence rotation rate from gps position and + // Function to get the target quaternion and reference rotation rate from gps position and // position of the ground station void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3], double refDirB[3], double quatBI[4], double targetQuat[4], @@ -25,9 +25,10 @@ class Guidance { void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], double targetSatRotRate[3]); - // Function to get the target quaternion and refence rotation rate for sun pointing after ground + // Function to get the target quaternion and reference rotation rate for sun pointing after ground // station - void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]); + void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4], + double targetSatRotRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing @@ -37,15 +38,15 @@ class Guidance { double targetQuat[4], double refSatRate[3]); // @note: Calculates the error quaternion between the current orientation and the target - // quaternion, considering a reference quaternion. Additionally the difference between the actual + // quaternion, considering a reference quaternion. Additionally the difference between the actual // and a desired satellite rotational rate is calculated, again considering a reference rotational // rate. Lastly gives back the error angle of the error quaternion. void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], - double errorQuat[4], double errorSatRotRate[3], double errorAngle); + double errorQuat[4], double errorSatRotRate[3], double &errorAngle); void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], - double errorAngle); + double &errorAngle); void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *targetSatRotRate); diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 9e8b3719..7c822409 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -19,9 +19,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, 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() && - sensorValues->strSet.caliQy.isValid() && - sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid(); + bool quatIBValid = sensorValues->strSet.isTrustWorthy.value; if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) { mekfStatus = multiplicativeKalmanFilter.init( diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index c1030c7b..0279215f 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -30,10 +30,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const // ------------------------------------------------ double magIgrfModel[3] = {0.0, 0.0, 0.0}; if (gpsValid) { - // Should be existing class object which will be called and modified here. Igrf13Model igrf13; - // So the line above should not be done here. Update: Can be done here as long updated coffs - // stored in acsParameters ? igrf13.schmidtNormalization(); igrf13.updateCoeffGH(timeOfMgmMeasurement); // maybe put a condition here, to only update after a full day, this @@ -45,14 +42,13 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const { PoolReadGuard pg(mgmDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - float zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm0vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm1vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm2vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm3vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm4vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTot.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(double)); mgmDataProcessed->setValidity(false, true); std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double)); mgmDataProcessed->magIgrfModel.setValid(gpsValid); @@ -150,7 +146,8 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const } timeOfSavedMagFieldEst = timeOfMgmMeasurement; - if (mgmDataProcessed->mgmVecTotDerivative.isValid()) { + if (VectorOperations::norm(mgmVecTotDerivative, 3) != 0 and + mgmDataProcessed->mgmVecTotDerivative.isValid()) { lowPassFilter(mgmVecTotDerivative, mgmDataProcessed->mgmVecTotDerivative.value, mgmParameters->mgmDerivativeFilterWeight); } @@ -209,63 +206,68 @@ void SensorProcessing::processSus( sunIjkModel[0] = cos(eclipticLongitude); sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon); sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon); + + uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; if (sus0valid) { - sus0valid = susConverter.checkSunSensorData(sus0Value); + susBrightness[0] = susConverter.checkSunSensorData(sus0Value); } if (sus1valid) { - sus1valid = susConverter.checkSunSensorData(sus1Value); + susBrightness[1] = susConverter.checkSunSensorData(sus1Value); } if (sus2valid) { - sus2valid = susConverter.checkSunSensorData(sus2Value); + susBrightness[2] = susConverter.checkSunSensorData(sus2Value); } if (sus3valid) { - sus3valid = susConverter.checkSunSensorData(sus3Value); + susBrightness[3] = susConverter.checkSunSensorData(sus3Value); } if (sus4valid) { - sus4valid = susConverter.checkSunSensorData(sus4Value); + susBrightness[4] = susConverter.checkSunSensorData(sus4Value); } if (sus5valid) { - sus5valid = susConverter.checkSunSensorData(sus5Value); + susBrightness[5] = susConverter.checkSunSensorData(sus5Value); } if (sus6valid) { - sus6valid = susConverter.checkSunSensorData(sus6Value); + susBrightness[6] = susConverter.checkSunSensorData(sus6Value); } if (sus7valid) { - sus7valid = susConverter.checkSunSensorData(sus7Value); + susBrightness[7] = susConverter.checkSunSensorData(sus7Value); } if (sus8valid) { - sus8valid = susConverter.checkSunSensorData(sus8Value); + susBrightness[8] = susConverter.checkSunSensorData(sus8Value); } if (sus9valid) { - sus9valid = susConverter.checkSunSensorData(sus9Value); + susBrightness[9] = susConverter.checkSunSensorData(sus9Value); } if (sus10valid) { - sus10valid = susConverter.checkSunSensorData(sus10Value); + susBrightness[10] = susConverter.checkSunSensorData(sus10Value); } if (sus11valid) { - sus11valid = susConverter.checkSunSensorData(sus11Value); + susBrightness[11] = susConverter.checkSunSensorData(sus11Value); } - if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid && - !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) { + bool susValid[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, + sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; + bool allInvalid = + susConverter.checkValidity(susValid, susBrightness, susParameters->susBrightnessThreshold); + + if (allInvalid) { { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - float zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus3vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus4vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus5vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus6vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus7vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus8vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus0vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus1vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus2vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus3vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus4vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus5vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus6vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus7vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus8vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus9vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus10vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus11vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTot.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(susDataProcessed->susVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(double)); susDataProcessed->setValidity(false, true); std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); susDataProcessed->sunIjkModel.setValid(true); @@ -273,118 +275,78 @@ void SensorProcessing::processSus( } return; } - // WARNING: NOT TRANSFORMED IN BODY FRAME YET - // Transformation into Geomtry Frame - float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0}, - sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0}, - sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0}, - sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0}; - if (sus0valid) { - MatrixOperations::multiply( - susParameters->sus0orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha, - susParameters->sus0coeffBeta), - sus0VecBody, 3, 3, 1); - } - if (sus1valid) { - MatrixOperations::multiply( - susParameters->sus1orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha, - susParameters->sus1coeffBeta), - sus1VecBody, 3, 3, 1); - } - if (sus2valid) { - MatrixOperations::multiply( - susParameters->sus2orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha, - susParameters->sus2coeffBeta), - sus2VecBody, 3, 3, 1); - } - if (sus3valid) { - MatrixOperations::multiply( - susParameters->sus3orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha, - susParameters->sus3coeffBeta), - sus3VecBody, 3, 3, 1); - } - if (sus4valid) { - MatrixOperations::multiply( - susParameters->sus4orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha, - susParameters->sus4coeffBeta), - sus4VecBody, 3, 3, 1); - } - if (sus5valid) { - MatrixOperations::multiply( - susParameters->sus5orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha, - susParameters->sus5coeffBeta), - sus5VecBody, 3, 3, 1); - } - if (sus6valid) { - MatrixOperations::multiply( - susParameters->sus6orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha, - susParameters->sus6coeffBeta), - sus6VecBody, 3, 3, 1); - } - if (sus7valid) { - MatrixOperations::multiply( - susParameters->sus7orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha, - susParameters->sus7coeffBeta), - sus7VecBody, 3, 3, 1); - } - if (sus8valid) { - MatrixOperations::multiply( - susParameters->sus8orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha, - susParameters->sus8coeffBeta), - sus8VecBody, 3, 3, 1); - } - if (sus9valid) { - MatrixOperations::multiply( - susParameters->sus9orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha, - susParameters->sus9coeffBeta), - sus9VecBody, 3, 3, 1); - } - if (sus10valid) { - MatrixOperations::multiply( - susParameters->sus10orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha, - susParameters->sus10coeffBeta), - sus10VecBody, 3, 3, 1); - } - if (sus11valid) { - MatrixOperations::multiply( - susParameters->sus11orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha, - susParameters->sus11coeffBeta), - sus11VecBody, 3, 3, 1); - } + float susVecSensor[12][3] = {{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}}; + float susVecBody[12][3] = {{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}}; - /* ------ Mean Value: susDirEst ------ */ - bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, - sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; - float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0], - sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0], - sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]}, - {sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1], - sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1], - sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]}, - {sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2], - sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2], - sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}}; + if (susValid[0]) { + susConverter.calculateSunVector(susVecSensor[0], sus0Value); + MatrixOperations::multiply(susParameters->sus0orientationMatrix[0], susVecSensor[0], + susVecBody[0], 3, 3, 1); + } + if (susValid[1]) { + susConverter.calculateSunVector(susVecSensor[1], sus1Value); + MatrixOperations::multiply(susParameters->sus1orientationMatrix[0], susVecSensor[1], + susVecBody[1], 3, 3, 1); + } + if (susValid[2]) { + susConverter.calculateSunVector(susVecSensor[2], sus2Value); + MatrixOperations::multiply(susParameters->sus2orientationMatrix[0], susVecSensor[2], + susVecBody[2], 3, 3, 1); + } + if (susValid[3]) { + susConverter.calculateSunVector(susVecSensor[3], sus3Value); + MatrixOperations::multiply(susParameters->sus3orientationMatrix[0], susVecSensor[3], + susVecBody[3], 3, 3, 1); + } + if (susValid[4]) { + susConverter.calculateSunVector(susVecSensor[4], sus4Value); + MatrixOperations::multiply(susParameters->sus4orientationMatrix[0], susVecSensor[4], + susVecBody[4], 3, 3, 1); + } + if (susValid[5]) { + susConverter.calculateSunVector(susVecSensor[5], sus5Value); + MatrixOperations::multiply(susParameters->sus5orientationMatrix[0], susVecSensor[5], + susVecBody[5], 3, 3, 1); + } + if (susValid[6]) { + susConverter.calculateSunVector(susVecSensor[6], sus6Value); + MatrixOperations::multiply(susParameters->sus6orientationMatrix[0], susVecSensor[6], + susVecBody[6], 3, 3, 1); + } + if (susValid[7]) { + susConverter.calculateSunVector(susVecSensor[7], sus7Value); + MatrixOperations::multiply(susParameters->sus7orientationMatrix[0], susVecSensor[7], + susVecBody[7], 3, 3, 1); + } + if (susValid[8]) { + susConverter.calculateSunVector(susVecSensor[8], sus8Value); + MatrixOperations::multiply(susParameters->sus8orientationMatrix[0], susVecSensor[8], + susVecBody[8], 3, 3, 1); + } + if (susValid[9]) { + susConverter.calculateSunVector(susVecSensor[9], sus9Value); + MatrixOperations::multiply(susParameters->sus9orientationMatrix[0], susVecSensor[9], + susVecBody[9], 3, 3, 1); + } + if (susValid[10]) { + susConverter.calculateSunVector(susVecSensor[10], sus10Value); + MatrixOperations::multiply(susParameters->sus10orientationMatrix[0], susVecSensor[10], + susVecBody[10], 3, 3, 1); + } + if (susValid[11]) { + susConverter.calculateSunVector(susVecSensor[11], sus11Value); + MatrixOperations::multiply(susParameters->sus11orientationMatrix[0], susVecSensor[11], + susVecBody[11], 3, 3, 1); + } double susMeanValue[3] = {0, 0, 0}; for (uint8_t i = 0; i < 12; i++) { - if (validIds[i]) { - susMeanValue[0] += susVecBody[0][i]; - susMeanValue[1] += susVecBody[1][i]; - susMeanValue[2] += susVecBody[2][i]; - } + susMeanValue[0] += susVecBody[i][0]; + susMeanValue[1] += susVecBody[i][1]; + susMeanValue[2] += susVecBody[i][2]; } double susVecTot[3] = {0.0, 0.0, 0.0}; VectorOperations::normalize(susMeanValue, susVecTot, 3); @@ -405,29 +367,29 @@ void SensorProcessing::processSus( { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus0vec.value, susVecBody[0], 3 * sizeof(float)); susDataProcessed->sus0vec.setValid(sus0valid); - std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus1vec.value, susVecBody[1], 3 * sizeof(float)); susDataProcessed->sus1vec.setValid(sus1valid); - std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus2vec.value, susVecBody[2], 3 * sizeof(float)); susDataProcessed->sus2vec.setValid(sus2valid); - std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus3vec.value, susVecBody[3], 3 * sizeof(float)); susDataProcessed->sus3vec.setValid(sus3valid); - std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus4vec.value, susVecBody[4], 3 * sizeof(float)); susDataProcessed->sus4vec.setValid(sus4valid); - std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus5vec.value, susVecBody[5], 3 * sizeof(float)); susDataProcessed->sus5vec.setValid(sus5valid); - std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus6vec.value, susVecBody[6], 3 * sizeof(float)); susDataProcessed->sus6vec.setValid(sus6valid); - std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus7vec.value, susVecBody[7], 3 * sizeof(float)); susDataProcessed->sus7vec.setValid(sus7valid); - std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus8vec.value, susVecBody[8], 3 * sizeof(float)); susDataProcessed->sus8vec.setValid(sus8valid); - std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus9vec.value, susVecBody[9], 3 * sizeof(float)); susDataProcessed->sus9vec.setValid(sus9valid); - std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus10vec.value, susVecBody[10], 3 * sizeof(float)); susDataProcessed->sus10vec.setValid(sus10valid); - std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus11vec.value, susVecBody[11], 3 * sizeof(float)); susDataProcessed->sus11vec.setValid(sus11valid); std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double)); susDataProcessed->susVecTot.setValid(true); @@ -458,12 +420,11 @@ void SensorProcessing::processGyr( { PoolReadGuard pg(gyrDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - double zeroVector[3] = {0.0, 0.0, 0.0}; - std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyr3vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyrVecTot.value, zeroVector, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr0vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr1vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr2vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr3vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyrVecTot.value, ZERO_VEC_D, 3 * sizeof(double)); gyrDataProcessed->setValidity(false, true); } } @@ -533,7 +494,7 @@ void SensorProcessing::processGyr( } } - if (gyrDataProcessed->gyrVecTot.isValid()) { + if (VectorOperations::norm(gyrVecTot, 3) != 0 and gyrDataProcessed->gyrVecTot.isValid()) { lowPassFilter(gyrVecTot, gyrDataProcessed->gyrVecTot.value, gyrParameters->gyrFilterWeight); } @@ -572,7 +533,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong // Altitude FDIR if (gpsAltitude > gpsParameters->maximumFdirAltitude || - gpsAltitude < gpsParameters->maximumFdirAltitude) { + gpsAltitude < gpsParameters->minimumFdirAltitude) { altitude = gpsParameters->fdirAltitude; } else { altitude = gpsAltitude; diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 47449a75..6dbc5d58 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -23,6 +23,9 @@ class SensorProcessing { acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters); // Will call protected functions private: + static constexpr float ZERO_VEC_F[3] = {0, 0, 0}; + static constexpr double ZERO_VEC_D[3] = {0, 0, 0}; + protected: // short description needed for every function void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 1a645270..31cc0371 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -1,121 +1,64 @@ #include "SusConverter.h" -#include -#include -#include #include -#include - -bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) { - if (susChannel[0] <= susChannelValueCheckLow || susChannel[0] > susChannelValueCheckHigh || +uint64_t SusConverter::checkSunSensorData(const uint16_t susChannel[6]) { + if (susChannel[0] <= SUS_CHANNEL_VALUE_LOW || susChannel[0] > SUS_CHANNEL_VALUE_HIGH || susChannel[0] > susChannel[GNDREF]) { - return false; + return 0; } - if (susChannel[1] <= susChannelValueCheckLow || susChannel[1] > susChannelValueCheckHigh || + if (susChannel[1] <= SUS_CHANNEL_VALUE_LOW || susChannel[1] > SUS_CHANNEL_VALUE_HIGH || susChannel[1] > susChannel[GNDREF]) { - return false; + return 0; }; - if (susChannel[2] <= susChannelValueCheckLow || susChannel[2] > susChannelValueCheckHigh || + if (susChannel[2] <= SUS_CHANNEL_VALUE_LOW || susChannel[2] > SUS_CHANNEL_VALUE_HIGH || susChannel[2] > susChannel[GNDREF]) { - return false; + return 0; }; - if (susChannel[3] <= susChannelValueCheckLow || susChannel[3] > susChannelValueCheckHigh || + if (susChannel[3] <= SUS_CHANNEL_VALUE_LOW || susChannel[3] > SUS_CHANNEL_VALUE_HIGH || susChannel[3] > susChannel[GNDREF]) { - return false; + return 0; }; - susChannelValueSum = + uint64_t susChannelValueSum = 4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]); - if ((susChannelValueSum < susChannelValueSumHigh) && - (susChannelValueSum > susChannelValueSumLow)) { - return false; + if (susChannelValueSum < SUS_ALBEDO_CHECK) { + return 0; }; - return true; + return susChannelValueSum; } -void SusConverter::calcAngle(const uint16_t susChannel[6]) { - float xout, yout; - float s = 0.03; // s=[mm] gap between diodes - uint8_t d = 5; // d=[mm] edge length of the quadratic aperture - uint8_t h = 1; // h=[mm] distance between diodes and aperture - int ch0, ch1, ch2, ch3; +bool SusConverter::checkValidity(bool* susValid, const uint64_t brightness[12], + const float threshold) { + uint8_t maxBrightness = 0; + VectorOperations::maxValue(brightness, 12, &maxBrightness); + if (brightness[maxBrightness] == 0) { + return true; + } + for (uint8_t idx = 0; idx < 12; idx++) { + if ((idx != maxBrightness) and (brightness[idx] < threshold * brightness[maxBrightness])) { + susValid[idx] = false; + continue; + } + susValid[idx] = true; + } + return false; +} + +void SusConverter::calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]) { // Substract measurement values from GNDREF zero current threshold - ch0 = susChannel[GNDREF] - susChannel[0]; - ch1 = susChannel[GNDREF] - susChannel[1]; - ch2 = susChannel[GNDREF] - susChannel[2]; - ch3 = susChannel[GNDREF] - susChannel[3]; + float ch0 = susChannel[GNDREF] - susChannel[0]; + float ch1 = susChannel[GNDREF] - susChannel[1]; + float ch2 = susChannel[GNDREF] - susChannel[2]; + float ch3 = susChannel[GNDREF] - susChannel[3]; // Calculation of x and y - xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] - yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] + float xout = ((D - S) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] + float yout = ((D - S) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] // Calculation of the angles - alphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI); //[°] - alphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI); //[°] -} - -void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) { - uint8_t index, k, l; - - // while loop iterates above all calibration cells to use the different calibration functions in - // each cell - k = 0; - while (k < 3) { - k++; - l = 0; - while (l < 3) { - l++; - // if-condition to check in which cell the data point has to be - if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3.)) - halfCellWidth) && - alphaBetaRaw[0] < ((completeCellWidth * (k / 3.)) - halfCellWidth)) && - (alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3.)) - halfCellWidth) && - alphaBetaRaw[1] < ((completeCellWidth * (l / 3.)) - halfCellWidth))) { - index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell - alphaBetaCalibrated[0] = - coeffAlpha[index][0] + coeffAlpha[index][1] * alphaBetaRaw[0] + - coeffAlpha[index][2] * alphaBetaRaw[1] + - coeffAlpha[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] + - coeffAlpha[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] + - coeffAlpha[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] + - coeffAlpha[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] + - coeffAlpha[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] + - coeffAlpha[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] + - coeffAlpha[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1]; //[°] - alphaBetaCalibrated[1] = - coeffBeta[index][0] + coeffBeta[index][1] * alphaBetaRaw[0] + - coeffBeta[index][2] * alphaBetaRaw[1] + - coeffBeta[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] + - coeffBeta[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] + - coeffBeta[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] + - coeffBeta[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] + - coeffBeta[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] + - coeffBeta[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] + - coeffBeta[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1]; //[°] - } - } - } -} - -float* SusConverter::calculateSunVector() { - // Calculate the normalized Sun Vector - sunVectorSensorFrame[0] = -(tan(alphaBetaCalibrated[0] * (M_PI / 180)) / - (sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) + - powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); - sunVectorSensorFrame[1] = -(tan(alphaBetaCalibrated[1] * (M_PI / 180)) / - (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + - powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); - sunVectorSensorFrame[2] = - -(-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + - powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); - - return sunVectorSensorFrame; -} - -float* SusConverter::getSunVectorSensorFrame(const uint16_t susChannel[6], - const float coeffAlpha[9][10], - const float coeffBeta[9][10]) { - calcAngle(susChannel); - calibration(coeffAlpha, coeffBeta); - return calculateSunVector(); + sunVectorSensorFrame[0] = -xout; + sunVectorSensorFrame[1] = -yout; + sunVectorSensorFrame[2] = H; + VectorOperations::normalize(sunVectorSensorFrame, sunVectorSensorFrame, 3); } diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 046b0ca8..8a6c279b 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -1,8 +1,4 @@ -#ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ -#define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ - -#include -#include +#include #include "AcsParameters.h" @@ -10,41 +6,26 @@ class SusConverter { public: SusConverter() {} - bool checkSunSensorData(const uint16_t susChannel[6]); - - void calcAngle(const uint16_t susChannel[6]); - void calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]); - float* calculateSunVector(); - - float* getSunVectorSensorFrame(const uint16_t susChannel[6], const float coeffAlpha[9][10], - const float coeffBeta[9][10]); + uint64_t checkSunSensorData(const uint16_t susChannel[6]); + bool checkValidity(bool* susValid, const uint64_t brightness[12], const float threshold); + void calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]); private: - float alphaBetaRaw[2]; //[°] - float alphaBetaCalibrated[2]; //[°] - float sunVectorSensorFrame[3]; //[-] - - bool validFlag[12] = {returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, - returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, - returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK}; - static const uint8_t GNDREF = 4; - uint16_t susChannelValueCheckHigh = - 4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check - uint8_t susChannelValueCheckLow = - 0; //[Bit]low borderline for the channel values of one sun sensor for validity Check - uint16_t susChannelValueSumHigh = - 100; // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by - // the reflection of sunlight from the moon/earth - uint8_t susChannelValueSumLow = - 0; //[Bit]low borderline for check if the sun sensor is illuminated - // by the sun or by the reflection of sunlight from the moon/earth - uint8_t completeCellWidth = 140, - halfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in - // which cell a data point should be - uint16_t susChannelValueSum = 0; + // =2^12[Bit]high borderline for the channel values of one sun sensor for validity Check + static constexpr uint16_t SUS_CHANNEL_VALUE_HIGH = 4096; + // [Bit]low borderline for the channel values of one sun sensor for validity Check + static constexpr uint8_t SUS_CHANNEL_VALUE_LOW = 0; + // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by the + // reflection of sunlight from the moon/earth + static constexpr uint16_t SUS_ALBEDO_CHECK = 1000; + // [Bit]low borderline for check if the sun sensor is illuminated by the sun or by the reflection + // of sunlight from the moon/earth + static constexpr uint8_t SUS_CHANNEL_SUM_LOW = 0; + + static constexpr float S = 0.03; // S=[mm] gap between diodes + static constexpr float D = 5; // D=[mm] edge length of the quadratic aperture + static constexpr float H = 2.5; // H=[mm] distance between diodes and aperture AcsParameters acsParameters; }; - -#endif /* MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ */ diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 9ca20862..8f422ec1 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -7,8 +7,10 @@ Detumble::Detumble() {} Detumble::~Detumble() {} -uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, - const bool magFieldRateValid, const bool useFullDetumbleLaw) { +acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid, + const bool satRotRateValid, + const bool magFieldRateValid, + const bool useFullDetumbleLaw) { if (not magFieldValid) { return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (satRotRateValid and useFullDetumbleLaw) { diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 4424896e..9fca77e6 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -11,8 +11,9 @@ class Detumble { Detumble(); virtual ~Detumble(); - uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, - const bool magFieldRateValid, const bool useFullDetumbleLaw); + acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, + const bool magFieldRateValid, + const bool useFullDetumbleLaw); void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB, double gain); diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 1b20efb9..2f5847cc 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -5,9 +5,6 @@ #include #include #include -#include - -#include "../util/MathOperations.h" PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } @@ -32,12 +29,13 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters double qErrorLaw[3] = {0, 0, 0}; for (int i = 0; i < 3; i++) { - if (abs(qError[i]) < qErrorMin) { + if (std::abs(qError[i]) < qErrorMin) { qErrorLaw[i] = qErrorMin; } else { - qErrorLaw[i] = abs(qError[i]); + qErrorLaw[i] = std::abs(qError[i]); } } + double qErrorLawNorm = VectorOperations::norm(qErrorLaw, 3); double gain1 = cInt * omMax / qErrorLawNorm; @@ -73,7 +71,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters double pErrorSign[3] = {0, 0, 0}; for (int i = 0; i < 3; i++) { - if (abs(pError[i]) > 1) { + if (std::abs(pError[i]) > 1) { pErrorSign[i] = sign(pError[i]); } else { pErrorSign[i] = pError[i]; @@ -98,61 +96,92 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); } +void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, + const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, double *rwTrqNs) { + // concentrate RW speeds as vector and convert to double + double speedRws[4] = {static_cast(speedRw0), static_cast(speedRw1), + static_cast(speedRw2), static_cast(speedRw3)}; + VectorOperations::mulScalar(speedRws, 1e-1, speedRws, 4); + VectorOperations::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4); + + // calculate RPM offset utilizing the nullspace + double rpmOffset[4] = {0, 0, 0, 0}; + double rpmOffsetSpeed = pointingLawParameters->nullspaceSpeed / 10 * RPM_TO_RAD_PER_SEC; + VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, rpmOffsetSpeed, + rpmOffset, 4); + + // calculate resulting angular momentum + double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0}; + VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); + VectorOperations::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, + rwAngMomentum, 4); + + // calculate resulting torque + double nullspaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(acsParameters->rwMatrices.nullspaceVector, + acsParameters->rwMatrices.nullspaceVector, *nullspaceMatrix, 4, + 1, 4); + MatrixOperations::multiply(*nullspaceMatrix, rwAngMomentum, rwTrqNs, 4, 4, 1); + VectorOperations::mulScalar(rwTrqNs, -1 * pointingLawParameters->gainNullspace, rwTrqNs, + 4); +} + void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, - double *magFieldEst, bool magFieldEstValid, double *satRate, - int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, - int32_t *speedRw3, double *mgtDpDes) { - if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) { - mgtDpDes[0] = 0; - mgtDpDes[1] = 0; - mgtDpDes[2] = 0; + const double *magFieldB, const bool magFieldBValid, + const double *satRate, const int32_t speedRw0, const int32_t speedRw1, + const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) { + if (not magFieldBValid or not pointingLawParameters->desatOn) { return; } - // 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, 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(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate, - momentumSat, 3, 3, 1); - VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); - // calculating momentum error - double deltaMomentum[3] = {0, 0, 0}; - VectorOperations::subtract(momentumTotal, pointingLawParameters->desatMomentumRef, - deltaMomentum, 3); - // resulting magnetic dipole command - double crossMomentumMagField[3] = {0, 0, 0}; - VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); - double normMag = VectorOperations::norm(magFieldEst, 3), factor = 0; - factor = (pointingLawParameters->deSatGainFactor) / normMag; - VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); -} + // concentrate RW speeds as vector and convert to double + double speedRws[4] = {static_cast(speedRw0), static_cast(speedRw1), + static_cast(speedRw2), static_cast(speedRw3)}; -void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, - const int32_t *speedRw0, const int32_t *speedRw1, - const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { - 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 - 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, 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(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); + // convert magFieldB from uT to T + double magFieldBT[3] = {0, 0, 0}; + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); + + // calculate angular momentum of the satellite + double angMomentumSat[3] = {0, 0, 0}; + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate, + angMomentumSat, 3, 3, 1); + + // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed + // relocate RW speed zero to nullspace RW speed + double refSpeedRws[4] = {0, 0, 0, 0}; + VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, + pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); + VectorOperations::subtract(speedRws, refSpeedRws, speedRws, 4); + // convert speed from 10 RPM to 1 RPM + VectorOperations::mulScalar(speedRws, 1e-1, speedRws, 4); + // convert to rad/s + VectorOperations::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4); + // calculate angular momentum of each RW + double angMomentumRwU[4] = {0, 0, 0, 0}; + VectorOperations::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel, + angMomentumRwU, 4); + // convert RW angular momentum to body RF + double angMomentumRw[3] = {0, 0, 0}; + MatrixOperations::multiply(*(acsParameters->rwMatrices.alignmentMatrix), angMomentumRwU, + angMomentumRw, 3, 4, 1); + + // calculate total angular momentum + double angMomentumTotal[3] = {0, 0, 0}; + VectorOperations::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3); + + // calculating momentum error + double deltaAngMomentum[3] = {0, 0, 0}; + VectorOperations::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef, + deltaAngMomentum, 3); + + // resulting magnetic dipole command + double crossAngMomentumMagField[3] = {0, 0, 0}; + VectorOperations::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField); + double factor = + pointingLawParameters->deSatGainFactor / VectorOperations::norm(magFieldBT, 3); + VectorOperations::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3); } void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) { @@ -169,15 +198,9 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee 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) { + if (rwCmdSpeeds[i] > currRwSpeed[i]) { rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; - } else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) { + } else if (rwCmdSpeeds[i] < currRwSpeed[i]) { rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; } } diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index fad72e6b..5f731e6b 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -1,13 +1,10 @@ #ifndef PTGCTRL_H_ #define PTGCTRL_H_ +#include +#include +#include #include -#include -#include - -#include "../AcsParameters.h" -#include "../SensorValues.h" -#include "eive/resultClassIds.h" class PtgCtrl { /* @@ -29,14 +26,14 @@ class PtgCtrl { void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws); - void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, - double *magFieldEst, bool magFieldEstValid, double *satRate, - int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, - double *mgtDpDes); - void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, - const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, - const int32_t *speedRw3, double *rwTrqNs); + const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, double *rwTrqNs); + + void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, + const double *magFieldB, const bool magFieldBValid, const double *satRate, + const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, double *mgtDpDes); /* @brief: Commands the stiction torque in case wheel speed is to low * torqueCommand modified torque after antistiction @@ -45,6 +42,7 @@ class PtgCtrl { private: const AcsParameters *acsParameters; + static constexpr double RPM_TO_RAD_PER_SEC = (2 * M_PI) / 60; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index ce50f276..43677ccf 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -9,9 +9,10 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter SafeCtrl::~SafeCtrl() {} -uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, - const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, const uint8_t dampingEnabled) { +acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + const bool satRotRateValid, const bool sunDirValid, + const uint8_t mekfEnabled, + const uint8_t dampingEnabled) { if (not magFieldValid) { return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { @@ -95,7 +96,6 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate acsParameters->safeModeControllerParameters.k_orthoNonMekf); // sum of all torques - double cmdTorque[3] = {0, 0, 0}; VectorOperations::add(cmdParallel, cmdOrtho, cmdTorque, 3); // calculate magnetic moment to command diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 91625360..12f9ddb0 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -12,9 +12,9 @@ class SafeCtrl { SafeCtrl(AcsParameters *acsParameters_); virtual ~SafeCtrl(); - uint8_t safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, - const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, const uint8_t dampingEnabled); + acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + const bool satRotRateValid, const bool sunDirValid, + const uint8_t mekfEnabled, const uint8_t dampingEnabled); void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, double *magMomB, diff --git a/mission/controller/tcsDefs.h b/mission/controller/tcsDefs.h index 03aa8ffe..f5d9d180 100644 --- a/mission/controller/tcsDefs.h +++ b/mission/controller/tcsDefs.h @@ -9,6 +9,85 @@ namespace tcsCtrl { +/** + * NOP Limit: Hard limit for device, usually from datasheet. Device damage is possible lif NOP limit + * is exceeded. + * OP Limit: Soft limit. Device should be switched off or TCS controller should take action if the + * limit is exceeded to avoid reaching NOP limit + */ +struct TempLimits { + TempLimits(float nopLowerLimit, float opLowerLimit, float cutOffLimit, float opUpperLimit, + float nopUpperLimit) + : opLowerLimit(opLowerLimit), + opUpperLimit(opUpperLimit), + cutOffLimit(cutOffLimit), + nopLowerLimit(nopLowerLimit), + nopUpperLimit(nopUpperLimit) {} + float opLowerLimit; + float opUpperLimit; + float cutOffLimit; + float nopLowerLimit; + float nopUpperLimit; +}; + +/** + * Abstraction for the state of a single thermal component + */ +struct ThermalState { + uint8_t noSensorAvailableCounter; + // Which sensor is used for this component? + uint8_t sensorIndex = 0; + // Is heating on for that thermal module? + bool heating = false; + // Which switch is being used for heating the component + heater::Switch heaterSwitch = heater::Switch::HEATER_NONE; + // Heater start time and end times as UNIX seconds. Please note that these times will be updated + // when a switch command is sent, with no guarantess that the heater actually went on. + uint32_t heaterStartTime = 0; + uint32_t heaterEndTime = 0; +}; + +/** + * Abstraction for the state of a single heater. + */ +struct HeaterState { + bool switchTransition = false; + heater::SwitchState target = heater::SwitchState::OFF; + uint8_t heaterSwitchControlCycles = 0; + bool trackHeaterMaxBurnTime = false; + Countdown heaterOnMaxBurnTime; +}; + +using HeaterSwitchStates = std::array; + +enum ThermalComponents : uint8_t { + NONE = 0, + ACS_BOARD = 1, + MGT = 2, + RW = 3, + STR = 4, + IF_BOARD = 5, + TCS_BOARD = 6, + OBC = 7, + LEGACY_OBCIF_BOARD = 8, + SBAND_TRANSCEIVER = 9, + PCDUP60_BOARD = 10, + PCDUACU = 11, + PCDUPDU = 12, + PLPCDU_BOARD = 13, + PLOCMISSION_BOARD = 14, + PLOCPROCESSING_BOARD = 15, + DAC = 16, + CAMERA = 17, + DRO = 18, + X8 = 19, + HPA = 20, + TX = 21, + MPA = 22, + SCEX_BOARD = 23, + NUM_THERMAL_COMPONENTS +}; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::TCS_CONTROLLER; static constexpr Event NO_VALID_SENSOR_TEMPERATURE = MAKE_EVENT(0, severity::MEDIUM); static constexpr Event NO_HEALTHY_HEATER_AVAILABLE = MAKE_EVENT(1, severity::MEDIUM); @@ -17,6 +96,13 @@ static constexpr Event OBC_OVERHEATING = MAKE_EVENT(4, severity::HIGH); static constexpr Event CAMERA_OVERHEATING = MAKE_EVENT(5, severity::HIGH); static constexpr Event PCDU_SYSTEM_OVERHEATING = MAKE_EVENT(6, severity::HIGH); static constexpr Event HEATER_NOT_OFF_FOR_OFF_MODE = MAKE_EVENT(7, severity::MEDIUM); +static constexpr Event MGT_OVERHEATING = MAKE_EVENT(8, severity::HIGH); +//! [EXPORT] : [COMMENT] P1: Module index. P2: Heater index +static constexpr Event TCS_SWITCHING_HEATER_ON = MAKE_EVENT(9, severity::INFO); +//! [EXPORT] : [COMMENT] P1: Module index. P2: Heater index +static constexpr Event TCS_SWITCHING_HEATER_OFF = MAKE_EVENT(10, severity::INFO); +//! [EXPORT] : [COMMENT] P1: Heater index. P2: Maximum burn time for heater. +static constexpr Event TCS_HEATER_MAX_BURN_TIME_REACHED = MAKE_EVENT(11, severity::MEDIUM); enum SetId : uint32_t { SENSOR_TEMPERATURES = 0, @@ -24,6 +110,7 @@ enum SetId : uint32_t { SUS_TEMPERATURES = 2, COMPONENT_TEMPERATURES = 3, HEATER_SET = 4, + TCS_CTRL_INFO = 5 }; enum PoolIds : lp_id_t { @@ -91,7 +178,13 @@ enum PoolIds : lp_id_t { TEMP_ADC_PAYLOAD_PCDU, HEATER_SWITCH_LIST, - HEATER_CURRENT + HEATER_CURRENT, + + HEATER_ON_FOR_COMPONENT_VEC, + SENSOR_USED_FOR_TCS_CTRL, + HEATER_IDX_USED_FOR_TCS_CTRL, + HEATER_START_TIME, + HEATER_END_TIME }; static const uint8_t ENTRIES_SENSOR_TEMPERATURE_SET = 25; @@ -231,6 +324,29 @@ class HeaterInfo : public StaticLocalDataSet<3> { lp_var_t heaterCurrent = lp_var_t(sid.objectId, PoolIds::HEATER_CURRENT, this); }; +class TcsCtrlInfo : public StaticLocalDataSet<6> { + public: + explicit TcsCtrlInfo(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, TCS_CTRL_INFO) {} + + explicit TcsCtrlInfo(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, TCS_CTRL_INFO)) {} + + lp_vec_t heatingOnVec = + lp_vec_t( + sid.objectId, PoolIds::HEATER_ON_FOR_COMPONENT_VEC, this); + lp_vec_t sensorIdxUsedForTcsCtrl = + lp_vec_t(sid.objectId, + PoolIds::SENSOR_USED_FOR_TCS_CTRL, this); + lp_vec_t heaterSwitchIdx = + lp_vec_t( + sid.objectId, PoolIds::HEATER_IDX_USED_FOR_TCS_CTRL, this); + lp_vec_t heaterStartTimes = + lp_vec_t(sid.objectId, PoolIds::HEATER_START_TIME, + this); + lp_vec_t heaterEndTimes = + lp_vec_t(sid.objectId, PoolIds::HEATER_END_TIME, + this); +}; + } // namespace tcsCtrl #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_THERMALCONTROLLERDEFINITIONS_H_ */ diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp index 06f1220e..3d9d0c81 100644 --- a/mission/genericFactory.cpp +++ b/mission/genericFactory.cpp @@ -49,6 +49,7 @@ #include "mission/system/acs/acsModeTree.h" #include "mission/system/tcs/tcsModeTree.h" #include "mission/tcs/defs.h" +#include "mission/tmtc/Service15TmStorage.h" #include "mission/tmtc/tmFilters.h" #include "objects/systemObjectList.h" #include "tmtc/pusIds.h" @@ -58,15 +59,13 @@ using persTmStore::PersistentTmStores; #if OBSW_ADD_TCPIP_SERVERS == 1 #if OBSW_ADD_TMTC_UDP_SERVER == 1 // UDP server includes -#include "devices/gpioIds.h" -#include "fsfw/osal/common/UdpTcPollingTask.h" -#include "fsfw/osal/common/UdpTmTcBridge.h" +#include +#include #endif #if OBSW_ADD_TMTC_TCP_SERVER == 1 // TCP server includes -#include "fsfw/osal/common/TcpTmTcBridge.h" -#include "fsfw/osal/common/TcpTmTcServer.h" -#include "mission/tmtc/Service15TmStorage.h" +#include +#include #endif #endif @@ -92,18 +91,21 @@ EiveFaultHandler EIVE_FAULT_HANDLER; } // namespace cfdp std::atomic_bool tcs::TCS_BOARD_SHORTLY_UNAVAILABLE = false; +std::atomic_bool core::SAVE_PUS_SEQUENCE_COUNT = false; +std::atomic_bool core::SAVE_CFDP_SEQUENCE_COUNT = false; void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFunnel** pusFunnel, CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan, StorageManagerIF** ipcStore, StorageManagerIF** tmStore, - PersistentTmStores& stores) { + PersistentTmStores& stores, + uint32_t eventManagerQueueDepth, bool enableHkSets) { // Framework objects - new EventManager(objects::EVENT_MANAGER, 160); + new EventManager(objects::EVENT_MANAGER, eventManagerQueueDepth); auto healthTable = new HealthTable(objects::HEALTH_TABLE); if (healthTable_ != nullptr) { *healthTable_ = healthTable; } - new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER); + new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER, 5, enableHkSets, 120); new VerificationReporter(); auto* timeStamper = new CdsShortTimeStamper(objects::TIME_STAMPER); StorageManagerIF* tcStore; @@ -155,9 +157,11 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun new PusDistributor(config::EIVE_PUS_APID, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib); PusTmFunnel::FunnelCfg pusFunnelCfg(objects::PUS_TM_FUNNEL, "PusTmFunnel", **tmStore, **ipcStore, - config::MAX_PUS_FUNNEL_QUEUE_DEPTH); + config::MAX_PUS_FUNNEL_QUEUE_DEPTH, sdcMan, + config::PUS_SEQUENCE_COUNT_FILE, + core::SAVE_PUS_SEQUENCE_COUNT); // The PUS funnel routes all live TM to the live destinations and to the TM stores. - *pusFunnel = new PusTmFunnel(pusFunnelCfg, *ramToFileStore, *timeStamper, sdcMan); + *pusFunnel = new PusTmFunnel(pusFunnelCfg, *ramToFileStore, *timeStamper); // MISC store and PUS funnel to MISC store routing { @@ -216,7 +220,9 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun stores.cfdpStore->getReportReceptionQueue(0)); } PusTmFunnel::FunnelCfg cfdpFunnelCfg(objects::CFDP_TM_FUNNEL, "CfdpTmFunnel", **tmStore, - **ipcStore, config::MAX_CFDP_FUNNEL_QUEUE_DEPTH); + **ipcStore, config::MAX_CFDP_FUNNEL_QUEUE_DEPTH, sdcMan, + config::CFDP_SEQUENCE_COUNT_FILE, + core::SAVE_CFDP_SEQUENCE_COUNT); *cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, stores.cfdpStore->getReportReceptionQueue(0), *ramToFileStore, config::EIVE_CFDP_APID); @@ -233,16 +239,17 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun // PUS service stack new Service1TelecommandVerification(objects::PUS_SERVICE_1_VERIFICATION, config::EIVE_PUS_APID, - pus::PUS_SERVICE_1, objects::PUS_TM_FUNNEL, 40); + pus::PUS_SERVICE_1, objects::PUS_TM_FUNNEL, + config::VERIFICATION_SERVICE_QUEUE_DEPTH); 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, config::HK_SERVICE_QUEUE_DEPTH); + pus::PUS_SERVICE_3, config::HK_SERVICE_QUEUE_DEPTH, 16); new Service5EventReporting( PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5), 80, 160); new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, config::EIVE_PUS_APID, - pus::PUS_SERVICE_8, 16, 60); + pus::PUS_SERVICE_8, config::ACTION_SERVICE_QUEUE_DEPTH, 16, 60); new Service9TimeManagement( PsbParams(objects::PUS_SERVICE_9_TIME_MGMT, config::EIVE_PUS_APID, pus::PUS_SERVICE_9)); @@ -304,9 +311,9 @@ void ObjectFactory::createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& heaterHandler->connectModeTreeParent(satsystem::tcs::SUBSYSTEM); } -void ObjectFactory::createThermalController(HeaterHandler& heaterHandler) { +void ObjectFactory::createThermalController(HeaterHandler& heaterHandler, bool pollPlPcduTmp1) { auto* tcsCtrl = new ThermalController(objects::THERMAL_CONTROLLER, heaterHandler, - tcs::TCS_BOARD_SHORTLY_UNAVAILABLE); + tcs::TCS_BOARD_SHORTLY_UNAVAILABLE, pollPlPcduTmp1); tcsCtrl->connectModeTreeParent(satsystem::tcs::SUBSYSTEM); } void ObjectFactory::createRwAssy(PowerSwitchIF& pwrSwitcher, power::Switch_t theSwitch, diff --git a/mission/genericFactory.h b/mission/genericFactory.h index 6cd2068d..7845c140 100644 --- a/mission/genericFactory.h +++ b/mission/genericFactory.h @@ -45,11 +45,12 @@ namespace ObjectFactory { void produceGenericObjects(HealthTableIF** healthTable, PusTmFunnel** pusFunnel, CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan, StorageManagerIF** ipcStore, StorageManagerIF** tmStore, - PersistentTmStores& stores); + PersistentTmStores& stores, uint32_t eventManagerQueueDepth, + bool enableHkSets); void createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& pwrSwitcher, HeaterHandler*& heaterHandler); -void createThermalController(HeaterHandler& heaterHandler); +void createThermalController(HeaterHandler& heaterHandler, bool pollPlPcduTmp1); void createRwAssy(PowerSwitchIF& pwrSwitcher, power::Switch_t theSwitch, std::array rws, std::array rwIds); void createSusAssy(PowerSwitchIF& pwrSwitcher, std::array suses); diff --git a/mission/pollingSeqTables.cpp b/mission/pollingSeqTables.cpp index 0367ef27..e18046d5 100644 --- a/mission/pollingSeqTables.cpp +++ b/mission/pollingSeqTables.cpp @@ -39,56 +39,67 @@ ReturnValue_t pst::pstSyrlinks(FixedTimeslotTaskIF *thisSequence) { // I don't think this needs to be in a PST because linux takes care of bus serialization, but // keep it like this for now, it works -ReturnValue_t pst::pstI2cProcessingSystem(FixedTimeslotTaskIF *thisSequence) { +ReturnValue_t pst::pstI2c(TmpSchedConfig schedConf, FixedTimeslotTaskIF *thisSequence) { // Length of a communication cycle uint32_t length = thisSequence->getPeriodMs(); static_cast(length); - // These are actually part of another bus, but this works, so keep it like this for now - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.2, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.2, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.2, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.3, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.3, DeviceHandlerIF::GET_READ); + if (schedConf.scheduleTmpDev0) { + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.1, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.1, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.1, DeviceHandlerIF::GET_READ); + } - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.5, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.5, DeviceHandlerIF::GET_READ); + if (schedConf.scheduleTmpDev1) { + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.2, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.2, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.2, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.3, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.3, DeviceHandlerIF::GET_READ); + } - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.6, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.6, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.6, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.7, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.7, DeviceHandlerIF::GET_READ); - // damaged - /* - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, DeviceHandlerIF::GET_READ); - */ - thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.8, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.8, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.8, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.9, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.9, DeviceHandlerIF::GET_READ); + if (schedConf.schedulePlPcduDev0) { + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.4, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.4, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.4, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.5, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.5, + DeviceHandlerIF::GET_READ); + } + if (schedConf.schedulePlPcduDev1) { + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.6, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.6, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.6, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.7, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.7, + DeviceHandlerIF::GET_READ); + } + + if (schedConf.scheduleIfBoardDev) { + thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.8, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.8, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.8, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.9, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.9, + DeviceHandlerIF::GET_READ); + } static_cast(length); return thisSequence->checkSequence(); } diff --git a/mission/pollingSeqTables.h b/mission/pollingSeqTables.h index e3bc0ad0..5df3c954 100644 --- a/mission/pollingSeqTables.h +++ b/mission/pollingSeqTables.h @@ -39,6 +39,16 @@ struct AcsPstCfg { bool scheduleStr = true; }; +// Default config is for FM. +struct TmpSchedConfig { + bool scheduleTmpDev0 = true; + bool scheduleTmpDev1 = true; + bool schedulePlPcduDev0 = true; + // damaged on FM + bool schedulePlPcduDev1 = false; + bool scheduleIfBoardDev = true; +}; + /** * @brief This function creates the PST for all gomspace devices. * @details @@ -51,7 +61,7 @@ ReturnValue_t pstSyrlinks(FixedTimeslotTaskIF* thisSequence); ReturnValue_t pstTcsAndAcs(FixedTimeslotTaskIF* thisSequence, AcsPstCfg cfg); -ReturnValue_t pstI2cProcessingSystem(FixedTimeslotTaskIF* thisSequence); +ReturnValue_t pstI2c(TmpSchedConfig schedConf, FixedTimeslotTaskIF* thisSequence); /** * Generic test PST diff --git a/mission/power/BpxBatteryHandler.cpp b/mission/power/BpxBatteryHandler.cpp index b4aece40..96253705 100644 --- a/mission/power/BpxBatteryHandler.cpp +++ b/mission/power/BpxBatteryHandler.cpp @@ -1,4 +1,5 @@ #include +#include #include BpxBatteryHandler::BpxBatteryHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, @@ -51,6 +52,9 @@ void BpxBatteryHandler::fillCommandAndReplyMap() { insertInCommandAndReplyMap(bpxBat::RESET_COUNTERS, 1, nullptr, EMPTY_REPLY_LEN); insertInCommandAndReplyMap(bpxBat::CONFIG_CMD, 1, nullptr, EMPTY_REPLY_LEN); insertInCommandAndReplyMap(bpxBat::CONFIG_GET, 1, &cfgSet, CONFIG_GET_REPLY_LEN); + insertInCommandAndReplyMap(bpxBat::CONFIG_SET, 1, nullptr, EMPTY_REPLY_LEN); + insertInCommandAndReplyMap(bpxBat::MAN_HEAT_ON, 1, nullptr, MAN_HEAT_REPLY_LEN); + insertInCommandAndReplyMap(bpxBat::MAN_HEAT_OFF, 1, nullptr, MAN_HEAT_REPLY_LEN); } ReturnValue_t BpxBatteryHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, @@ -155,7 +159,7 @@ ReturnValue_t BpxBatteryHandler::scanForReply(const uint8_t* start, size_t remai case (bpxBat::PING): case (bpxBat::MAN_HEAT_ON): case (bpxBat::MAN_HEAT_OFF): { - if (remainingSize != PING_REPLY_LEN) { + if (remainingSize != MAN_HEAT_REPLY_LEN) { return DeviceHandlerIF::LENGTH_MISSMATCH; } break; diff --git a/mission/power/GomspaceDeviceHandler.cpp b/mission/power/GomspaceDeviceHandler.cpp index 89fca8ee..c20ac4e9 100644 --- a/mission/power/GomspaceDeviceHandler.cpp +++ b/mission/power/GomspaceDeviceHandler.cpp @@ -595,8 +595,8 @@ ReturnValue_t GomspaceDeviceHandler::parsePduHkTable(PDU::PduCoreHk& coreHk, PDU for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) { coreHk.voltages[idx] = as(packet + 0x12 + (idx * 2)); } - auxHk.vcc.value = as(packet + 0x24); - auxHk.vbat.value = as(packet + 0x26); + coreHk.vcc.value = as(packet + 0x24); + coreHk.vbat.value = as(packet + 0x26); coreHk.temperature = as(packet + 0x28) * 0.1; for (uint8_t idx = 0; idx < 3; idx++) { diff --git a/mission/power/P60DockHandler.cpp b/mission/power/P60DockHandler.cpp index d643aef1..77fd9aaf 100644 --- a/mission/power/P60DockHandler.cpp +++ b/mission/power/P60DockHandler.cpp @@ -66,7 +66,7 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) { } coreHk.battMode = newBattMode; - auxHk.heaterOn = *(packet + 0x57); + auxHk.heaterForBp4PackOn = *(packet + 0x57); auxHk.converter5VStatus = *(packet + 0x58); for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) { @@ -111,6 +111,8 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) { } coreHk.setValidity(true, true); auxHk.setValidity(true, true); + // No BP4 pack, no this is always invalid. + auxHk.heaterForBp4PackOn.setValid(false); } ReturnValue_t P60DockHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, diff --git a/mission/power/bpxBattDefs.h b/mission/power/bpxBattDefs.h index 6df87efd..0e167f07 100644 --- a/mission/power/bpxBattDefs.h +++ b/mission/power/bpxBattDefs.h @@ -48,6 +48,7 @@ static constexpr uint32_t CFG_SET_ID = CONFIG_GET; static constexpr size_t GET_HK_REPLY_LEN = 23; static constexpr size_t PING_REPLY_LEN = 3; static constexpr size_t EMPTY_REPLY_LEN = 2; +static constexpr size_t MAN_HEAT_REPLY_LEN = 3; static constexpr size_t CONFIG_GET_REPLY_LEN = 5; static constexpr uint8_t PORT_PING = 1; @@ -219,6 +220,7 @@ class BpxBatteryCfg : public StaticLocalDataSet { if (size < 3) { return SerializeIF::STREAM_TOO_SHORT; } + battheatermode.value = data[0]; battheaterLow.value = data[1]; battheaterHigh.value = data[2]; diff --git a/mission/power/gsDefs.h b/mission/power/gsDefs.h index d42cabd2..1ad000d1 100644 --- a/mission/power/gsDefs.h +++ b/mission/power/gsDefs.h @@ -260,7 +260,8 @@ class HkTableDataset : public StaticLocalDataSet<32> { lp_var_t resetcause = lp_var_t(sid.objectId, pool::P60DOCK_RESETCAUSE, this); /** Battery heater control only possible on BP4 packs */ - lp_var_t heaterOn = lp_var_t(sid.objectId, pool::P60DOCK_HEATER_ON, this); + lp_var_t heaterForBp4PackOn = + lp_var_t(sid.objectId, pool::P60DOCK_HEATER_ON, this); lp_var_t converter5VStatus = lp_var_t(sid.objectId, pool::P60DOCK_CONV_5V_ENABLE_STATUS, this); @@ -404,6 +405,11 @@ class PduCoreHk : public StaticLocalDataSet<9> { /** Battery mode: 1 = Critical, 2 = Safe, 3 = Normal, 4 = Full */ lp_var_t battMode = lp_var_t(sid.objectId, pool::PDU_BATT_MODE, this); lp_var_t temperature = lp_var_t(sid.objectId, pool::PDU_TEMPERATURE, this); + + /** Measured VCC */ + lp_var_t vcc = lp_var_t(sid.objectId, pool::PDU_VCC, this); + /** Measured VBAT */ + lp_var_t vbat = lp_var_t(sid.objectId, pool::PDU_VBAT, this); }; class PduConfig : public StaticLocalDataSet<32> { @@ -451,11 +457,6 @@ class PduAuxHk : public StaticLocalDataSet<36> { PduAuxHk(object_id_t objectId, uint32_t setId) : StaticLocalDataSet(sid_t(objectId, setId)) {} - /** Measured VCC */ - lp_var_t vcc = lp_var_t(sid.objectId, pool::PDU_VCC, this); - /** Measured VBAT */ - lp_var_t vbat = lp_var_t(sid.objectId, pool::PDU_VBAT, this); - /** Output converter enable status */ lp_vec_t converterEnable = lp_vec_t(sid.objectId, pool::PDU_CONV_EN, this); diff --git a/mission/scheduling.cpp b/mission/scheduling.cpp index f89ce711..27a6417c 100644 --- a/mission/scheduling.cpp +++ b/mission/scheduling.cpp @@ -5,10 +5,12 @@ #include "fsfw/tasks/PeriodicTaskIF.h" void scheduling::scheduleTmpTempSensors(PeriodicTaskIF* tmpTask) { - const std::array tmpIds = { - objects::TMP1075_HANDLER_TCS_0, objects::TMP1075_HANDLER_TCS_1, - objects::TMP1075_HANDLER_PLPCDU_0, objects::TMP1075_HANDLER_PLPCDU_1, - objects::TMP1075_HANDLER_IF_BOARD}; + const std::array tmpIds = {objects::TMP1075_HANDLER_TCS_0, + objects::TMP1075_HANDLER_TCS_1, + objects::TMP1075_HANDLER_PLPCDU_0, + // damaged. + // objects::TMP1075_HANDLER_PLPCDU_1, + objects::TMP1075_HANDLER_IF_BOARD}; for (const auto& tmpId : tmpIds) { tmpTask->addComponent(tmpId, DeviceHandlerIF::PERFORM_OPERATION); tmpTask->addComponent(tmpId, DeviceHandlerIF::SEND_WRITE); diff --git a/mission/sysDefs.h b/mission/sysDefs.h index 424b5752..9750f3fd 100644 --- a/mission/sysDefs.h +++ b/mission/sysDefs.h @@ -1,16 +1,28 @@ #ifndef MISSION_SYSDEFS_H_ #define MISSION_SYSDEFS_H_ -#include +#include +#include +#include +#include -#include "acs/defs.h" +#include +#include extern std::atomic_uint16_t I2C_FATAL_ERRORS; namespace satsystem { -enum Mode : Mode_t { BOOT = 5, SAFE = acs::AcsMode::SAFE, PTG_IDLE = acs::AcsMode::PTG_IDLE }; - +enum Mode : Mode_t { + BOOT = 5, + // DO NOT CHANGE THE ORDER starting from here, breaks ACS mode checks. + SAFE = 10, + PTG_IDLE = 11, + PTG_NADIR = 12, + PTG_TARGET = 13, + PTG_TARGET_GS = 14, + PTG_INERTIAL = 15, +}; } namespace xsc { @@ -22,6 +34,9 @@ enum Copy : int { COPY_0, COPY_1, NO_COPY, SELF_COPY, ALL_COPY }; namespace core { +extern std::atomic_bool SAVE_PUS_SEQUENCE_COUNT; +extern std::atomic_bool SAVE_CFDP_SEQUENCE_COUNT; + // TODO: Support for status? Or maybe some command to quickly get information whether a unit // is running. enum SystemctlCmd : uint8_t { START = 0, STOP = 1, RESTART = 2, NUM_CMDS = 3 }; @@ -29,10 +44,13 @@ enum SystemctlCmd : uint8_t { START = 0, STOP = 1, RESTART = 2, NUM_CMDS = 3 }; static constexpr char CONF_FOLDER[] = "conf"; static constexpr char VERSION_FILE_NAME[] = "version.txt"; -static constexpr char REBOOT_FILE_NAME[] = "reboot.txt"; +static constexpr char LEGACY_REBOOT_WATCHDOG_FILE_NAME[] = "reboot.txt"; +static constexpr char REBOOT_WATCHDOG_FILE_NAME[] = "reboot_watchdog.txt"; +static constexpr char REBOOT_COUNTER_FILE_NAME[] = "reboot_counters.txt"; static constexpr char TIME_FILE_NAME[] = "time_backup.txt"; -static constexpr ActionId_t LIST_DIRECTORY_INTO_FILE = 0; +static constexpr uint32_t SYS_ROM_BASE_ADDR = 0x80000000; + static constexpr ActionId_t ANNOUNCE_VERSION = 1; static constexpr ActionId_t ANNOUNCE_CURRENT_IMAGE = 2; static constexpr ActionId_t ANNOUNCE_BOOT_COUNTS = 3; @@ -40,6 +58,7 @@ 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; static constexpr ActionId_t SET_MAX_REBOOT_CNT = 8; +static constexpr ActionId_t READ_REBOOT_MECHANISM_INFO = 9; static constexpr ActionId_t OBSW_UPDATE_FROM_SD_0 = 10; static constexpr ActionId_t OBSW_UPDATE_FROM_SD_1 = 11; @@ -59,6 +78,13 @@ static constexpr ActionId_t EXECUTE_SHELL_CMD_BLOCKING = 40; static constexpr ActionId_t EXECUTE_SHELL_CMD_NON_BLOCKING = 41; static constexpr ActionId_t SYSTEMCTL_CMD_EXECUTOR = 42; +static constexpr ActionId_t LIST_DIRECTORY_INTO_FILE = 50; +static constexpr ActionId_t LIST_DIRECTORY_DUMP_DIRECTLY = 51; +static constexpr ActionId_t CP_HELPER = 52; +static constexpr ActionId_t MV_HELPER = 53; +static constexpr ActionId_t RM_HELPER = 54; +static constexpr ActionId_t MKDIR_HELPER = 55; + static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::CORE; static constexpr Event ALLOC_FAILURE = event::makeEvent(SUBSYSTEM_ID, 0, severity::MEDIUM); @@ -95,6 +121,205 @@ static constexpr Event TRYING_I2C_RECOVERY = event::makeEvent(SUBSYSTEM_ID, 10, static constexpr Event I2C_REBOOT = event::makeEvent(SUBSYSTEM_ID, 11, severity::HIGH); //! [EXPORT] : [COMMENT] PDEC recovery through reset was not possible, performing full reboot. static constexpr Event PDEC_REBOOT = event::makeEvent(SUBSYSTEM_ID, 12, severity::HIGH); +//! [EXPORT] : [COMMENT] Version information of the firmware (not OBSW). +//! 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. +static constexpr Event FIRMWARE_INFO = event::makeEvent(SUBSYSTEM_ID, 13, severity::INFO); + +class ListDirectoryCmdBase { + public: // TODO: Packet definition for clean deserialization + // 3 bytes for a and R flag, at least 5 bytes for minimum valid path /tmp with + // null termination + static constexpr size_t MIN_DATA_LEN = 8; + + ListDirectoryCmdBase(const uint8_t* data, size_t maxSize) : data(data), maxSize(maxSize) {} + virtual ~ListDirectoryCmdBase() = default; + + virtual ReturnValue_t parse() { + if (maxSize < MIN_DATA_LEN) { + return SerializeIF::STREAM_TOO_SHORT; + } + aFlag = data[0]; + rFlag = data[1]; + compressOption = data[2]; + + repoNameLen = strnlen(reinterpret_cast(data + 3), maxSize - 3); + // Last byte MUST be null terminated! + if (repoNameLen >= maxSize - 3) { + return HasActionsIF::INVALID_PARAMETERS; + } + repoName = reinterpret_cast(data + 3); + return returnvalue::OK; + } + + bool aFlagSet() const { return this->aFlag; } + bool rFlagSet() const { return this->rFlag; } + + bool compressionOptionSet() const { return this->compressOption; } + + const char* getRepoName(size_t& repoNameLen) const { + repoNameLen = this->repoNameLen; + return this->repoName; + } + + size_t getBaseSize() { + // Include NULL termination + if (repoName != nullptr) { + return 3 + repoNameLen + 1; + } + return 0; + } + + protected: + const uint8_t* data; + size_t maxSize; + + bool aFlag = false; + bool rFlag = false; + bool compressOption = false; + const char* repoName = nullptr; + size_t repoNameLen = 0; +}; + +class ListDirectoryIntoFile : public ListDirectoryCmdBase { + public: + // TODO: Packet definition for clean deserialization + // 3 bytes for a and R flag, at least 5 bytes for minimum valid path /tmp with + // null termination, at least 7 bytes for minimum target file name /tmp/a with + // null termination. + static constexpr size_t MIN_DATA_LEN = 15; + + ListDirectoryIntoFile(const uint8_t* data, size_t maxSize) + : ListDirectoryCmdBase(data, maxSize) {} + ReturnValue_t parse() override { + if (maxSize < MIN_DATA_LEN) { + return SerializeIF::STREAM_TOO_SHORT; + } + ReturnValue_t result = ListDirectoryCmdBase::parse(); + if (result != returnvalue::OK) { + return result; + } + + targetNameLen = + strnlen(reinterpret_cast(data + getBaseSize()), maxSize - getBaseSize()); + if (targetNameLen >= maxSize - getBaseSize()) { + // Again: String MUST be null terminated. + return HasActionsIF::INVALID_PARAMETERS; + } + targetName = reinterpret_cast(data + getBaseSize()); + return result; + } + const char* getTargetName(size_t& targetNameLen) const { + targetNameLen = this->targetNameLen; + return this->targetName; + } + + private: + const char* targetName = nullptr; + size_t targetNameLen = 0; +}; + +struct SourceTargetPair { + const char* sourceName = nullptr; + size_t sourceNameSize = 0; + const char* targetName = nullptr; + size_t targetNameSize = 0; +}; + +static ReturnValue_t parseDestTargetString(const uint8_t* data, size_t maxLen, + SourceTargetPair& destTgt) { + if (maxLen < 4) { + return SerializeIF::STREAM_TOO_SHORT; + } + destTgt.sourceNameSize = strnlen(reinterpret_cast(data), maxLen); + if (destTgt.sourceNameSize >= maxLen) { + return HasActionsIF::INVALID_PARAMETERS; + } + destTgt.sourceName = reinterpret_cast(data); + size_t remainingLen = maxLen - destTgt.sourceNameSize - 1; + if (remainingLen == 0) { + return HasActionsIF::INVALID_PARAMETERS; + } + destTgt.targetNameSize = + strnlen(reinterpret_cast(data + destTgt.sourceNameSize + 1), remainingLen); + if (destTgt.targetNameSize >= remainingLen) { + return HasActionsIF::INVALID_PARAMETERS; + } + destTgt.targetName = reinterpret_cast(data + destTgt.sourceNameSize + 1); + return returnvalue::OK; +} + +class CpHelperParser { + public: + CpHelperParser(const uint8_t* data, size_t maxLen) : data(data), maxLen(maxLen) {} + + ReturnValue_t parse() { + if (maxLen < 2) { + return SerializeIF::STREAM_TOO_SHORT; + } + recursiveOpt = data[0]; + forceOpt = data[1]; + return parseDestTargetString(data + 2, maxLen - 2, destTgt); + } + const SourceTargetPair& destTgtPair() const { return destTgt; } + bool isRecursiveOptSet() const { return recursiveOpt; } + bool isForceOptSet() const { return forceOpt; } + + private: + const uint8_t* data; + size_t maxLen; + bool recursiveOpt = false; + bool forceOpt = false; + SourceTargetPair destTgt; +}; + +class MvHelperParser { + public: + MvHelperParser(const uint8_t* data, size_t maxLen) : data(data), maxLen(maxLen) {} + + ReturnValue_t parse() { return parseDestTargetString(data, maxLen, destTgt); } + const SourceTargetPair& destTgtPair() const { return destTgt; } + + private: + const uint8_t* data; + size_t maxLen; + SourceTargetPair destTgt; +}; + +class RmHelperParser { + public: + RmHelperParser(const uint8_t* data, size_t maxLen) : data(data), maxLen(maxLen) {} + + ReturnValue_t parse() { + if (maxLen < 2) { + return SerializeIF::STREAM_TOO_SHORT; + } + recursiveOpt = data[0]; + forceOpt = data[1]; + removeTargetSize = strnlen(reinterpret_cast(data + 2), maxLen - 2); + // Must be null-terminated + if (removeTargetSize >= maxLen - 2) { + return HasActionsIF::INVALID_PARAMETERS; + } + removeTarget = reinterpret_cast(data + 2); + return returnvalue::OK; + } + bool isRecursiveOptSet() const { return recursiveOpt; } + bool isForceOptSet() const { return forceOpt; } + + const char* getRemoveTarget(size_t& removeTargetSize) { + removeTargetSize = this->removeTargetSize; + return removeTarget; + } + + private: + const uint8_t* data; + size_t maxLen; + bool recursiveOpt = false; + bool forceOpt = false; + const char* removeTarget = nullptr; + size_t removeTargetSize = 0; +}; } // namespace core diff --git a/mission/system/EiveSystem.cpp b/mission/system/EiveSystem.cpp index eb1c4101..f17b82cb 100644 --- a/mission/system/EiveSystem.cpp +++ b/mission/system/EiveSystem.cpp @@ -39,18 +39,22 @@ void EiveSystem::announceMode(bool recursive) { modeStr = "POINTING IDLE"; break; } - case (acs::AcsMode::PTG_INERTIAL): { - modeStr = "POINTING INERTIAL"; + case (satsystem::Mode::PTG_NADIR): { + modeStr = "POINTING NADIR"; break; } - case (acs::AcsMode::PTG_TARGET): { + case (satsystem::Mode::PTG_TARGET): { modeStr = "POINTING TARGET"; break; } - case (acs::AcsMode::PTG_TARGET_GS): { + case (satsystem::Mode::PTG_TARGET_GS): { modeStr = "POINTING TARGET GS"; break; } + case (satsystem::Mode::PTG_INERTIAL): { + modeStr = "POINTING INERTIAL"; + break; + } } sif::info << "EIVE system is now in " << modeStr << " mode" << std::endl; return Subsystem::announceMode(recursive); @@ -114,6 +118,7 @@ ReturnValue_t EiveSystem::initialize() { manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::PCDU_SYSTEM_OVERHEATING)); manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::OBC_OVERHEATING)); + manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::MGT_OVERHEATING)); manager->subscribeToEvent(eventQueue->getId(), event::getEventId(pdec::INVALID_TC_FRAME)); return Subsystem::initialize(); } @@ -132,6 +137,7 @@ void EiveSystem::handleEventMessages() { break; } case tcsCtrl::OBC_OVERHEATING: + case tcsCtrl::MGT_OVERHEATING: case tcsCtrl::PCDU_SYSTEM_OVERHEATING: { if (isInTransition) { performSafeRecovery = true; diff --git a/mission/system/acs/DualLaneAssemblyBase.cpp b/mission/system/acs/DualLaneAssemblyBase.cpp index 7d22fbd1..dc97908f 100644 --- a/mission/system/acs/DualLaneAssemblyBase.cpp +++ b/mission/system/acs/DualLaneAssemblyBase.cpp @@ -183,11 +183,11 @@ void DualLaneAssemblyBase::handleModeTransitionFailed(ReturnValue_t result) { // transition to dual mode. if (not tryingOtherSide) { triggerEvent(CANT_KEEP_MODE, mode, submode); - startTransition(mode, nextSubmode); + startTransition(targetMode, nextSubmode); tryingOtherSide = true; } else { - triggerEvent(transitionOtherSideFailedEvent, mode, targetSubmode); - startTransition(mode, Submodes::DUAL_MODE); + triggerEvent(transitionOtherSideFailedEvent, targetMode, targetSubmode); + startTransition(targetMode, Submodes::DUAL_MODE); } } @@ -205,7 +205,8 @@ bool DualLaneAssemblyBase::checkAndHandleRecovery() { opCode = pwrStateMachine.fsm(); if (opCode == OpCodes::TO_OFF_DONE or opCode == OpCodes::TIMEOUT_OCCURED) { customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_ON; - pwrStateMachine.start(targetMode, targetSubmode); + // Command power back on in any case. + pwrStateMachine.start(HasModesIF::MODE_ON, targetSubmode); } } if (customRecoveryStates == POWER_SWITCHING_ON) { diff --git a/mission/system/systemTree.cpp b/mission/system/systemTree.cpp index 37adbaf5..10c40617 100644 --- a/mission/system/systemTree.cpp +++ b/mission/system/systemTree.cpp @@ -22,17 +22,21 @@ const auto check = subsystem::checkInsert; void buildBootSequence(Subsystem& ss, ModeListEntry& eh); void buildSafeSequence(Subsystem& ss, ModeListEntry& eh); void buildIdleSequence(Subsystem& ss, ModeListEntry& eh); +void buildPtgNadirSequence(Subsystem& ss, ModeListEntry& eh); +void buildPtgTargetSequence(Subsystem& ss, ModeListEntry& eh); +void buildPtgTargetGsSequence(Subsystem& ss, ModeListEntry& eh); +void buildPtgInertialSequence(Subsystem& ss, ModeListEntry& eh); } // namespace static const auto OFF = HasModesIF::MODE_OFF; static const auto NML = DeviceHandlerIF::MODE_NORMAL; -void satsystem::init() { +void satsystem::init(bool commandPlPcdu1) { auto& acsSubsystem = acs::init(); acsSubsystem.connectModeTreeParent(EIVE_SYSTEM); auto& payloadSubsystem = payload::init(); payloadSubsystem.connectModeTreeParent(EIVE_SYSTEM); - auto& tcsSubsystem = tcs::init(); + auto& tcsSubsystem = tcs::init(commandPlPcdu1); tcsSubsystem.connectModeTreeParent(EIVE_SYSTEM); auto& comSubsystem = com::init(); comSubsystem.connectModeTreeParent(EIVE_SYSTEM); @@ -40,6 +44,10 @@ void satsystem::init() { buildBootSequence(EIVE_SYSTEM, entry); buildSafeSequence(EIVE_SYSTEM, entry); buildIdleSequence(EIVE_SYSTEM, entry); + buildPtgNadirSequence(EIVE_SYSTEM, entry); + buildPtgTargetSequence(EIVE_SYSTEM, entry); + buildPtgTargetGsSequence(EIVE_SYSTEM, entry); + buildPtgInertialSequence(EIVE_SYSTEM, entry); EIVE_SYSTEM.setInitialMode(satsystem::Mode::BOOT, 0); } @@ -68,88 +76,44 @@ auto EIVE_TABLE_IDLE_TRANS_0 = auto EIVE_TABLE_IDLE_TRANS_1 = std::make_pair((satsystem::Mode::PTG_IDLE << 24) | 3, FixedArrayList()); +auto EIVE_SEQUENCE_PTG_NADIR = + std::make_pair(satsystem::Mode::PTG_NADIR, FixedArrayList()); +auto EIVE_TABLE_PTG_NADIR_TGT = + std::make_pair((satsystem::Mode::PTG_NADIR << 24) | 1, FixedArrayList()); +auto EIVE_TABLE_PTG_NADIR_TRANS_0 = + std::make_pair((satsystem::Mode::PTG_NADIR << 24) | 2, FixedArrayList()); +auto EIVE_TABLE_PTG_NADIR_TRANS_1 = + std::make_pair((satsystem::Mode::PTG_NADIR << 24) | 3, FixedArrayList()); + +auto EIVE_SEQUENCE_PTG_TARGET = + std::make_pair(satsystem::Mode::PTG_TARGET, FixedArrayList()); +auto EIVE_TABLE_PTG_TARGET_TGT = + std::make_pair((satsystem::Mode::PTG_TARGET << 24) | 1, FixedArrayList()); +auto EIVE_TABLE_PTG_TARGET_TRANS_0 = + std::make_pair((satsystem::Mode::PTG_TARGET << 24) | 2, FixedArrayList()); +auto EIVE_TABLE_PTG_TARGET_TRANS_1 = + std::make_pair((satsystem::Mode::PTG_TARGET << 24) | 3, FixedArrayList()); + +auto EIVE_SEQUENCE_PTG_TARGET_GS = + std::make_pair(satsystem::Mode::PTG_TARGET_GS, FixedArrayList()); +auto EIVE_TABLE_PTG_TARGET_GS_TGT = + std::make_pair((satsystem::Mode::PTG_TARGET_GS << 24) | 1, FixedArrayList()); +auto EIVE_TABLE_PTG_TARGET_GS_TRANS_0 = + std::make_pair((satsystem::Mode::PTG_TARGET_GS << 24) | 2, FixedArrayList()); +auto EIVE_TABLE_PTG_TARGET_GS_TRANS_1 = + std::make_pair((satsystem::Mode::PTG_TARGET_GS << 24) | 3, FixedArrayList()); + +auto EIVE_SEQUENCE_PTG_INERTIAL = + std::make_pair(satsystem::Mode::PTG_INERTIAL, FixedArrayList()); +auto EIVE_TABLE_PTG_INERTIAL_TGT = + std::make_pair((satsystem::Mode::PTG_INERTIAL << 24) | 1, FixedArrayList()); +auto EIVE_TABLE_PTG_INERTIAL_TRANS_0 = + std::make_pair((satsystem::Mode::PTG_INERTIAL << 24) | 2, FixedArrayList()); +auto EIVE_TABLE_PTG_INERTIAL_TRANS_1 = + std::make_pair((satsystem::Mode::PTG_INERTIAL << 24) | 3, FixedArrayList()); + namespace { -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, - bool allowAllSubmodes = false) { - eh.setObject(obj); - eh.setMode(mode); - eh.setSubmode(submode); - if (allowAllSubmodes) { - eh.allowAllSubmodes(); - } - check(table.insert(eh), ctxc); - }; - // Insert Helper Sequence - auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, - bool checkSuccess) { - eh.setTableId(tableId); - eh.setWaitSeconds(waitSeconds); - eh.setCheckSuccess(checkSuccess); - check(sequence.insert(eh), ctxc); - }; - - // 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); - - // Build SAFE transition 0. - iht(objects::TCS_SUBSYSTEM, NML, 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 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); - check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_SAFE.first, &EIVE_SEQUENCE_SAFE.second, - EIVE_SEQUENCE_SAFE.first)), - ctxc); -} - -void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { - std::string context = "satsystem::buildIdleSequence"; - auto ctxc = context.c_str(); - // Insert Helper Table - auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { - eh.setObject(obj); - eh.setMode(mode); - eh.setSubmode(submode); - check(table.insert(eh), ctxc); - }; - // Insert Helper Sequence - auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, - bool checkSuccess) { - eh.setTableId(tableId); - eh.setWaitSeconds(waitSeconds); - eh.setCheckSuccess(checkSuccess); - check(sequence.insert(eh), ctxc); - }; - - 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 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 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); - check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_IDLE.first, &EIVE_SEQUENCE_IDLE.second, - EIVE_SEQUENCE_SAFE.first)), - ctxc); -} - void buildBootSequence(Subsystem& ss, ModeListEntry& eh) { std::string context = "satsystem::buildBootSequence"; auto ctxc = context.c_str(); @@ -194,4 +158,240 @@ void buildBootSequence(Subsystem& ss, ModeListEntry& eh) { EIVE_SEQUENCE_SAFE.first)), ctxc); } + +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, + bool allowAllSubmodes = false) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + if (allowAllSubmodes) { + eh.allowAllSubmodes(); + } + check(table.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + // Do no track submode to allow transitions to DETUMBLE submode. + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TGT.second, true); + check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TGT.first, &EIVE_TABLE_SAFE_TGT.second)), ctxc); + + // Build SAFE transition 0. + iht(objects::TCS_SUBSYSTEM, NML, 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 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); + check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_SAFE.first, &EIVE_SEQUENCE_SAFE.second, + EIVE_SEQUENCE_SAFE.first)), + ctxc); +} + +void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::buildIdleSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(table.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + 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 IDLE transition 0 + iht(objects::TCS_SUBSYSTEM, NML, 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 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); + check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_IDLE.first, &EIVE_SEQUENCE_IDLE.second, + EIVE_SEQUENCE_SAFE.first)), + ctxc); +} + +void buildPtgNadirSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::buildPtgNadirSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(table.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_NADIR, 0, EIVE_TABLE_PTG_NADIR_TGT.second); + check(ss.addTable(TableEntry(EIVE_TABLE_PTG_NADIR_TGT.first, &EIVE_TABLE_PTG_NADIR_TGT.second)), + ctxc); + + // Build PTG_NADIR transition 0 + iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second); + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_NADIR, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second); + check(ss.addTable( + TableEntry(EIVE_TABLE_PTG_NADIR_TRANS_0.first, &EIVE_TABLE_PTG_NADIR_TRANS_0.second)), + ctxc); + + // Build PTG_NADIR sequence + ihs(EIVE_SEQUENCE_PTG_NADIR.second, EIVE_TABLE_PTG_NADIR_TGT.first, 0, false); + ihs(EIVE_SEQUENCE_PTG_NADIR.second, EIVE_TABLE_PTG_NADIR_TRANS_0.first, 0, false); + check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_PTG_NADIR.first, &EIVE_SEQUENCE_PTG_NADIR.second, + EIVE_SEQUENCE_IDLE.first)), + ctxc); +} + +void buildPtgTargetSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::buildPtgTargetSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(table.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET, 0, EIVE_TABLE_PTG_TARGET_TGT.second); + check(ss.addTable(TableEntry(EIVE_TABLE_PTG_TARGET_TGT.first, &EIVE_TABLE_PTG_TARGET_TGT.second)), + ctxc); + + // Build PTG_TARGET transition 0 + iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second); + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second); + check(ss.addTable( + TableEntry(EIVE_TABLE_PTG_TARGET_TRANS_0.first, &EIVE_TABLE_PTG_TARGET_TRANS_0.second)), + ctxc); + + // Build PTG_TARGET sequence + ihs(EIVE_SEQUENCE_PTG_TARGET.second, EIVE_TABLE_PTG_TARGET_TGT.first, 0, false); + ihs(EIVE_SEQUENCE_PTG_TARGET.second, EIVE_TABLE_PTG_TARGET_TRANS_0.first, 0, false); + check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_PTG_TARGET.first, + &EIVE_SEQUENCE_PTG_TARGET.second, EIVE_SEQUENCE_IDLE.first)), + ctxc); +} + +void buildPtgTargetGsSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::buildPtgTargetGsSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(table.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET_GS, 0, EIVE_TABLE_PTG_TARGET_GS_TGT.second); + check(ss.addTable( + TableEntry(EIVE_TABLE_PTG_TARGET_GS_TGT.first, &EIVE_TABLE_PTG_TARGET_GS_TGT.second)), + ctxc); + + // Build PTG_TARGET_GS transition 0 + iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second); + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET_GS, 0, + EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second); + check(ss.addTable(TableEntry(EIVE_TABLE_PTG_TARGET_GS_TRANS_0.first, + &EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second)), + ctxc); + + // Build PTG_TARGET_GS sequence + ihs(EIVE_SEQUENCE_PTG_TARGET_GS.second, EIVE_TABLE_PTG_TARGET_GS_TGT.first, 0, false); + ihs(EIVE_SEQUENCE_PTG_TARGET_GS.second, EIVE_TABLE_PTG_TARGET_GS_TRANS_0.first, 0, false); + check( + ss.addSequence(SequenceEntry(EIVE_SEQUENCE_PTG_TARGET_GS.first, + &EIVE_SEQUENCE_PTG_TARGET_GS.second, EIVE_SEQUENCE_IDLE.first)), + ctxc); +} + +void buildPtgInertialSequence(Subsystem& ss, ModeListEntry& eh) { + std::string context = "satsystem::buildPtgInertialSequence"; + auto ctxc = context.c_str(); + // Insert Helper Table + auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList& table) { + eh.setObject(obj); + eh.setMode(mode); + eh.setSubmode(submode); + check(table.insert(eh), ctxc); + }; + // Insert Helper Sequence + auto ihs = [&](ArrayList& sequence, Mode_t tableId, uint32_t waitSeconds, + bool checkSuccess) { + eh.setTableId(tableId); + eh.setWaitSeconds(waitSeconds); + eh.setCheckSuccess(checkSuccess); + check(sequence.insert(eh), ctxc); + }; + + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_INERTIAL, 0, EIVE_TABLE_PTG_INERTIAL_TGT.second); + check(ss.addTable( + TableEntry(EIVE_TABLE_PTG_INERTIAL_TGT.first, &EIVE_TABLE_PTG_INERTIAL_TGT.second)), + ctxc); + + // Build PTG_INERTIAL transition 0 + iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_INERTIAL_TRANS_0.second); + iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_INERTIAL, 0, + EIVE_TABLE_PTG_INERTIAL_TRANS_0.second); + check(ss.addTable(TableEntry(EIVE_TABLE_PTG_INERTIAL_TRANS_0.first, + &EIVE_TABLE_PTG_INERTIAL_TRANS_0.second)), + ctxc); + + // Build PTG_INERTIAL sequence + ihs(EIVE_SEQUENCE_PTG_INERTIAL.second, EIVE_TABLE_PTG_INERTIAL_TGT.first, 0, false); + ihs(EIVE_SEQUENCE_PTG_INERTIAL.second, EIVE_TABLE_PTG_INERTIAL_TRANS_0.first, 0, false); + check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_PTG_INERTIAL.first, + &EIVE_SEQUENCE_PTG_INERTIAL.second, EIVE_SEQUENCE_IDLE.first)), + ctxc); +} + } // namespace diff --git a/mission/system/systemTree.h b/mission/system/systemTree.h index 9d769277..0ed73a89 100644 --- a/mission/system/systemTree.h +++ b/mission/system/systemTree.h @@ -5,7 +5,7 @@ namespace satsystem { -void init(); +void init(bool commandPlPcdu1); extern EiveSystem EIVE_SYSTEM; diff --git a/mission/system/tcs/TmpDevFdir.cpp b/mission/system/tcs/TmpDevFdir.cpp index d501dd1a..8c2b1706 100644 --- a/mission/system/tcs/TmpDevFdir.cpp +++ b/mission/system/tcs/TmpDevFdir.cpp @@ -20,7 +20,7 @@ ReturnValue_t TmpDevFdir::eventReceived(EventMessage* event) { // We'll try a recovery as long as defined in MAX_REBOOT. // Might cause some AssemblyBase cycles, so keep number low. // Ignored for TMP device, no way to power cycle it without going to OFF/BOOT mode. - // handleRecovery(event->getEvent()); + setFaulty(event->getEvent()); break; case DeviceHandlerIF::DEVICE_INTERPRETING_REPLY_FAILED: case DeviceHandlerIF::DEVICE_READING_REPLY_FAILED: @@ -28,8 +28,10 @@ ReturnValue_t TmpDevFdir::eventReceived(EventMessage* event) { case DeviceHandlerIF::DEVICE_UNKNOWN_REPLY: // Some DH's generate generic reply-ids. case DeviceHandlerIF::DEVICE_BUILDING_COMMAND_FAILED: // These faults all mean that there were stupid replies from a device. - // With now way to do a recovery, set the device to faulty immediately. - setFaulty(event->getEvent()); + // With no way to do a recovery, set the device to faulty instead of trying a recovery. + if (strangeReplyCount.incrementAndCheck()) { + setFaulty(event->getEvent()); + } break; case DeviceHandlerIF::DEVICE_SENDING_COMMAND_FAILED: case DeviceHandlerIF::DEVICE_REQUESTING_REPLY_FAILED: @@ -40,7 +42,9 @@ ReturnValue_t TmpDevFdir::eventReceived(EventMessage* event) { break; } // else - setFaulty(event->getEvent()); + if (missedReplyCount.incrementAndCheck()) { + setFaulty(event->getEvent()); + } break; case StorageManagerIF::GET_DATA_FAILED: case StorageManagerIF::STORE_DATA_FAILED: diff --git a/mission/system/tcs/tcsModeTree.cpp b/mission/system/tcs/tcsModeTree.cpp index 6cc57c6f..09a4fa61 100644 --- a/mission/system/tcs/tcsModeTree.cpp +++ b/mission/system/tcs/tcsModeTree.cpp @@ -10,8 +10,8 @@ TcsSubsystem satsystem::tcs::SUBSYSTEM(objects::TCS_SUBSYSTEM, 12, 24); namespace { // Alias for checker function const auto check = subsystem::checkInsert; -void buildOffSequence(Subsystem& ss, ModeListEntry& eh); -void buildNormalSequence(Subsystem& ss, ModeListEntry& eh); +void buildOffSequence(Subsystem& ss, ModeListEntry& eh, bool commandPlPcdu1); +void buildNormalSequence(Subsystem& ss, ModeListEntry& eh, bool commandPlPcdu1); } // namespace static const auto OFF = HasModesIF::MODE_OFF; @@ -27,17 +27,17 @@ auto TCS_TABLE_NORMAL_TGT = std::make_pair((NML << 24) | 1, FixedArrayList()); auto TCS_TABLE_NORMAL_TRANS_1 = std::make_pair((NML << 24) | 3, FixedArrayList()); -Subsystem& satsystem::tcs::init() { +Subsystem& satsystem::tcs::init(bool commandPlPcdu1) { ModeListEntry entry; - buildOffSequence(SUBSYSTEM, entry); - buildNormalSequence(SUBSYSTEM, entry); + buildOffSequence(SUBSYSTEM, entry, commandPlPcdu1); + buildNormalSequence(SUBSYSTEM, entry, commandPlPcdu1); SUBSYSTEM.setInitialMode(OFF); return SUBSYSTEM; } namespace { -void buildOffSequence(Subsystem& ss, ModeListEntry& eh) { +void buildOffSequence(Subsystem& ss, ModeListEntry& eh, bool commandPlPcdu1) { std::string context = "satsystem::tcs::buildOffSequence"; auto ctxc = context.c_str(); // Insert Helper Table @@ -67,7 +67,9 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) { 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 + if (commandPlPcdu1) { + iht(objects::TMP1075_HANDLER_PLPCDU_1, OFF, 0, TCS_TABLE_OFF_TRANS_1.second); + } 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); @@ -79,7 +81,7 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) { ctxc); } -void buildNormalSequence(Subsystem& ss, ModeListEntry& eh) { +void buildNormalSequence(Subsystem& ss, ModeListEntry& eh, bool commandPlPcdu1) { std::string context = "satsystem::tcs::buildNormalSequence"; auto ctxc = context.c_str(); // Insert Helper Table @@ -105,7 +107,9 @@ void buildNormalSequence(Subsystem& ss, ModeListEntry& eh) { 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); - // TMP PL PCDU 1 is damaged + if (commandPlPcdu1) { + iht(objects::TMP1075_HANDLER_PLPCDU_1, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second); + } 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); diff --git a/mission/system/tcs/tcsModeTree.h b/mission/system/tcs/tcsModeTree.h index e5973641..4370ae73 100644 --- a/mission/system/tcs/tcsModeTree.h +++ b/mission/system/tcs/tcsModeTree.h @@ -7,7 +7,7 @@ namespace satsystem { namespace tcs { extern TcsSubsystem SUBSYSTEM; -Subsystem& init(); +Subsystem& init(bool commandPlPcdu1); } // namespace tcs } // namespace satsystem diff --git a/mission/tcs/HeaterHandler.cpp b/mission/tcs/HeaterHandler.cpp index 630dc92c..b88dfe75 100644 --- a/mission/tcs/HeaterHandler.cpp +++ b/mission/tcs/HeaterHandler.cpp @@ -51,9 +51,13 @@ ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) { if (mainLineSwitcher->getSwitchState(mainLineSwitch) == SWITCH_OFF) { waitForSwitchOff = false; mode = MODE_OFF; + busyWithSwitchCommanding = false; modeHelper.modeChanged(mode, submode); } } + if (busyWithSwitchCommanding and heaterCmdBusyCd.hasTimedOut()) { + busyWithSwitchCommanding = false; + } } catch (const std::out_of_range& e) { sif::warning << "HeaterHandler::performOperation: " "Out of range error | " @@ -93,7 +97,7 @@ ReturnValue_t HeaterHandler::initialize() { ReturnValue_t HeaterHandler::initializeHeaterMap() { for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) { - heaterVec.push_back(HeaterWrapper(helper.heaters[switchNr], SwitchState::OFF)); + heaterVec.push_back(HeaterWrapper(helper.heaters[switchNr], heater::SwitchState::OFF)); } return returnvalue::OK; } @@ -101,23 +105,23 @@ ReturnValue_t HeaterHandler::initializeHeaterMap() { void HeaterHandler::readCommandQueue() { ReturnValue_t result = returnvalue::OK; CommandMessage command; - do { + if (not busyWithSwitchCommanding) { result = commandQueue->receiveMessage(&command); if (result == MessageQueueIF::EMPTY) { - break; + return; } else if (result != returnvalue::OK) { sif::warning << "HeaterHandler::readCommandQueue: Message reception error" << std::endl; - break; - } - result = actionHelper.handleActionMessage(&command); - if (result == returnvalue::OK) { - continue; + return; } result = modeHelper.handleModeCommand(&command); if (result == returnvalue::OK) { - continue; + return; } - } while (result == returnvalue::OK); + result = actionHelper.handleActionMessage(&command); + if (result == returnvalue::OK) { + return; + } + } } ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, @@ -167,6 +171,8 @@ ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t heater.action = action; heater.cmdActive = true; heater.replyQueue = commandedBy; + busyWithSwitchCommanding = true; + heaterCmdBusyCd.resetTimer(); return returnvalue::OK; } @@ -208,7 +214,7 @@ ReturnValue_t HeaterHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t o void HeaterHandler::handleSwitchHandling() { for (uint8_t idx = 0; idx < heater::NUMBER_OF_SWITCHES; idx++) { auto health = heaterVec[idx].healthDevice->getHealth(); - if (heaterVec[idx].switchState == SwitchState::ON) { + if (heaterVec[idx].switchState == heater::SwitchState::ON) { // If a heater is still on but the device was marked faulty by the operator, the SW // will shut down the heater if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY) { @@ -249,6 +255,7 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) { sif::error << "HeaterHandler::handleSwitchOnCommand: Main switch setting on timeout" << std::endl; heater.cmdActive = false; + busyWithSwitchCommanding = false; heater.waitMainSwitchOn = false; if (heater.replyQueue != commandQueue->getId()) { actionHelper.finish(false, heater.replyQueue, heater.action, MAIN_SWITCH_SET_TIMEOUT); @@ -259,25 +266,25 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) { // Check state of main line switch ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch); if (mainSwitchState == PowerSwitchIF::SWITCH_ON) { - if (getSwitchState(heaterIdx) == SwitchState::OFF) { - gpioId_t gpioId = heater.gpioId; - result = gpioInterface->pullHigh(gpioId); - if (result != returnvalue::OK) { - sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull gpio with id " << gpioId - << " high" << std::endl; - triggerEvent(GPIO_PULL_HIGH_FAILED, result); - } else { - triggerEvent(HEATER_WENT_ON, heaterIdx, 0); - { - MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); - heater.switchState = ON; - } - } - } else { - triggerEvent(SWITCH_ALREADY_ON, heaterIdx); + gpioId_t gpioId = heater.gpioId; + result = gpioInterface->pullHigh(gpioId); + if (result != returnvalue::OK) { + sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull GPIO with ID " << gpioId + << " high" << std::endl; + triggerEvent(GPIO_PULL_HIGH_FAILED, result); + } + if (result == returnvalue::OK) { + triggerEvent(HEATER_WENT_ON, heaterIdx, 0); + { + MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + heater.switchState = heater::SwitchState::ON; + } + EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO, + MODE_ON, 0); + busyWithSwitchCommanding = false; + mode = HasModesIF::MODE_ON; + modeHelper.modeChanged(mode, submode); } - mode = HasModesIF::MODE_ON; - modeHelper.modeChanged(mode, submode); // There is no need to send action finish replies if the sender was the // HeaterHandler itself if (heater.replyQueue != commandQueue->getId()) { @@ -310,28 +317,33 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) { void HeaterHandler::handleSwitchOffCommand(heater::Switch heaterIdx) { ReturnValue_t result = returnvalue::OK; auto& heater = heaterVec.at(heaterIdx); - // Check whether switch is already off - if (getSwitchState(heaterIdx)) { - gpioId_t gpioId = heater.gpioId; - result = gpioInterface->pullLow(gpioId); - if (result != returnvalue::OK) { - sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id" << gpioId - << " low" << std::endl; - triggerEvent(GPIO_PULL_LOW_FAILED, result); - } else { + gpioId_t gpioId = heater.gpioId; + result = gpioInterface->pullLow(gpioId); + if (result != returnvalue::OK) { + sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id" << gpioId + << " low" << std::endl; + triggerEvent(GPIO_PULL_LOW_FAILED, result); + } + if (result == returnvalue::OK) { + // Check whether switch is already off + if (getSwitchState(heaterIdx) == heater::SwitchState::ON) { { MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); - heater.switchState = OFF; + heater.switchState = heater::SwitchState::OFF; } triggerEvent(HEATER_WENT_OFF, heaterIdx, 0); - // When all switches are off, also main line switch will be turned off - if (allSwitchesOff()) { - mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF); - waitForSwitchOff = true; - } + } else { + triggerEvent(SWITCH_ALREADY_OFF, heaterIdx); + } + EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO, + MODE_OFF, 0); + // When all switches are off, also main line switch will be turned off + if (allSwitchesOff()) { + mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF); + waitForSwitchOff = true; + } else { + busyWithSwitchCommanding = false; } - } else { - triggerEvent(SWITCH_ALREADY_OFF, heaterIdx); } if (heater.replyQueue != NO_COMMANDER) { // Report back switch command reply if necessary @@ -344,15 +356,15 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switch heaterIdx) { heater.cmdActive = false; } -HeaterHandler::SwitchState HeaterHandler::getSwitchState(heater::Switch switchNr) const { +heater::SwitchState HeaterHandler::getSwitchState(heater::Switch switchNr) const { MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); return heaterVec.at(switchNr).switchState; } -ReturnValue_t HeaterHandler::switchHeater(heater::Switch heater, SwitchState switchState) { - if (switchState == SwitchState::ON) { +ReturnValue_t HeaterHandler::switchHeater(heater::Switch heater, heater::SwitchState switchState) { + if (switchState == heater::SwitchState::ON) { return sendSwitchCommand(heater, PowerSwitchIF::SWITCH_ON); - } else if (switchState == SwitchState::OFF) { + } else if (switchState == heater::SwitchState::OFF) { return sendSwitchCommand(heater, PowerSwitchIF::SWITCH_OFF); } return returnvalue::FAILED; @@ -361,10 +373,10 @@ ReturnValue_t HeaterHandler::switchHeater(heater::Switch heater, SwitchState swi void HeaterHandler::announceMode(bool recursive) { triggerEvent(MODE_INFO, mode, submode); - std::array states; + std::array states; getAllSwitchStates(states); for (unsigned idx = 0; idx < helper.heaters.size(); idx++) { - if (states[idx] == ON) { + if (states[idx] == heater::SwitchState::ON) { EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_ON, 0); } else { EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_OFF, @@ -393,7 +405,7 @@ ModeTreeChildIF& HeaterHandler::getModeTreeChildIF() { return *this; } object_id_t HeaterHandler::getObjectId() const { return SystemObject::getObjectId(); } -ReturnValue_t HeaterHandler::getAllSwitchStates(std::array& statesBuf) { +ReturnValue_t HeaterHandler::getAllSwitchStates(std::array& statesBuf) { { MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); if (mg.getLockResult() != returnvalue::OK) { @@ -407,13 +419,13 @@ ReturnValue_t HeaterHandler::getAllSwitchStates(std::array& stat } bool HeaterHandler::allSwitchesOff() { - bool allSwitchesOrd = false; 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; + if (heaterVec.at(switchNr).switchState == heater::SwitchState::ON) { + return false; + } } - return !allSwitchesOrd; + return true; } MessageQueueId_t HeaterHandler::getCommandQueue() const { return commandQueue->getId(); } @@ -428,7 +440,7 @@ ReturnValue_t HeaterHandler::getSwitchState(uint8_t switchNr) const { if (switchNr > 7) { return returnvalue::FAILED; } - if (getSwitchState(static_cast(switchNr)) == SwitchState::ON) { + if (getSwitchState(static_cast(switchNr)) == heater::SwitchState::ON) { return PowerSwitchIF::SWITCH_ON; } return PowerSwitchIF::SWITCH_OFF; diff --git a/mission/tcs/HeaterHandler.h b/mission/tcs/HeaterHandler.h index 609ac725..4901c505 100644 --- a/mission/tcs/HeaterHandler.h +++ b/mission/tcs/HeaterHandler.h @@ -60,7 +60,6 @@ class HeaterHandler : public ExecutableObjectIF, static const ReturnValue_t COMMAND_ALREADY_WAITING = MAKE_RETURN_CODE(0xA5); enum CmdSourceParam : uint8_t { INTERNAL = 0, EXTERNAL = 1 }; - enum SwitchState : uint8_t { ON = 1, OFF = 0 }; /** Device command IDs */ static const DeviceCommandId_t SWITCH_HEATER = 0x0; @@ -69,14 +68,14 @@ class HeaterHandler : public ExecutableObjectIF, PowerSwitchIF* mainLineSwitcherObjectId, power::Switch_t mainLineSwitch); ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) override; - ReturnValue_t getAllSwitchStates(std::array& statesBuf); + ReturnValue_t getAllSwitchStates(std::array& statesBuf); virtual ~HeaterHandler(); protected: enum SwitchAction : uint8_t { SET_SWITCH_OFF, SET_SWITCH_ON, NONE }; - ReturnValue_t switchHeater(heater::Switch heater, SwitchState switchState); + ReturnValue_t switchHeater(heater::Switch heater, heater::SwitchState switchState); HasHealthIF::HealthState getHealth(heater::Switch heater); ReturnValue_t performOperation(uint8_t operationCode = 0) override; @@ -121,14 +120,14 @@ class HeaterHandler : public ExecutableObjectIF, * @param mainSwitchCountdown Sets timeout to wait for main switch being set on. */ struct HeaterWrapper { - HeaterWrapper(HeaterPair pair, SwitchState initState) + HeaterWrapper(HeaterPair pair, heater::SwitchState initState) : healthDevice(pair.first), gpioId(pair.second), switchState(initState) {} HealthDevice* healthDevice = nullptr; gpioId_t gpioId = gpio::NO_GPIO; SwitchAction action = SwitchAction::NONE; MessageQueueId_t replyQueue = MessageQueueIF::NO_QUEUE; bool cmdActive = false; - SwitchState switchState = SwitchState::OFF; + heater::SwitchState switchState = heater::SwitchState::OFF; bool waitMainSwitchOn = false; Countdown mainSwitchCountdown; }; @@ -148,6 +147,7 @@ class HeaterHandler : public ExecutableObjectIF, /** Size of command queue */ size_t cmdQueueSize = 20; bool waitForSwitchOff = true; + bool busyWithSwitchCommanding = false; GpioIF* gpioInterface = nullptr; @@ -163,6 +163,7 @@ class HeaterHandler : public ExecutableObjectIF, power::Switch_t mainLineSwitch; ActionHelper actionHelper; + Countdown heaterCmdBusyCd = Countdown(2000); StorageManagerIF* ipcStore = nullptr; @@ -175,7 +176,7 @@ class HeaterHandler : public ExecutableObjectIF, * @brief Returns the state of a switch (ON - true, or OFF - false). * @param switchNr The number of the switch to check. */ - SwitchState getSwitchState(heater::Switch switchNr) const; + heater::SwitchState getSwitchState(heater::Switch switchNr) const; /** * @brief This function runs commands waiting for execution. diff --git a/mission/tcs/Max31865EiveHandler.cpp b/mission/tcs/Max31865EiveHandler.cpp index 599071ef..a3c1dce1 100644 --- a/mission/tcs/Max31865EiveHandler.cpp +++ b/mission/tcs/Max31865EiveHandler.cpp @@ -37,6 +37,8 @@ void Max31865EiveHandler::doShutDown() { transitionOk = false; } if (state == InternalState::INACTIVE and transitionOk) { + sensorDataset.temperatureCelcius = thermal::INVALID_TEMPERATURE; + sensorDataset.setValidity(false, true); updatePeriodicReply(false, EiveMax31855::RtdCommands::EXCHANGE_SET_ID); setMode(MODE_OFF); } diff --git a/mission/tcs/Tmp1075Definitions.h b/mission/tcs/Tmp1075Definitions.h index 946ac82e..345146e6 100644 --- a/mission/tcs/Tmp1075Definitions.h +++ b/mission/tcs/Tmp1075Definitions.h @@ -29,7 +29,7 @@ static const uint8_t MAX_REPLY_LENGTH = GET_TEMP_REPLY_SIZE; enum Tmp1075PoolIds : lp_id_t { TEMPERATURE_C_TMP1075 }; -class Tmp1075Dataset : public StaticLocalDataSet { +class Tmp1075Dataset : public StaticLocalDataSet<2> { public: Tmp1075Dataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, TMP1075_DATA_SET_ID) {} diff --git a/mission/tcs/Tmp1075Handler.cpp b/mission/tcs/Tmp1075Handler.cpp index df57aa8a..7e8569b6 100644 --- a/mission/tcs/Tmp1075Handler.cpp +++ b/mission/tcs/Tmp1075Handler.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -11,14 +12,12 @@ Tmp1075Handler::Tmp1075Handler(object_id_t objectId, object_id_t comIF, CookieIF Tmp1075Handler::~Tmp1075Handler() {} -void Tmp1075Handler::doStartUp() { - if (getMode() == _MODE_START_UP) { - setMode(MODE_ON); - } -} +void Tmp1075Handler::doStartUp() { setMode(MODE_ON); } void Tmp1075Handler::doShutDown() { communicationStep = CommunicationStep::START_ADC_CONVERSION; + PoolReadGuard pg(&dataset); + dataset.setValidity(false, true); setMode(_MODE_POWER_DOWN); } @@ -86,8 +85,11 @@ ReturnValue_t Tmp1075Handler::scanForReply(const uint8_t *start, size_t remainin ReturnValue_t Tmp1075Handler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { switch (id) { case TMP1075::GET_TEMP: { - int16_t tempValueRaw = 0; - tempValueRaw = packet[0] << 4 | packet[1] >> 4; + // Convert 12 bit MSB first raw temperature to 16 bit first. + int16_t tempValueRaw = static_cast((packet[0] << 8) | packet[1]) >> 4; + // Sign extension to 16 bits: If the sign bit is set, fill up with ones on the left. + tempValueRaw = (packet[0] & 0x80) ? (tempValueRaw | 0xF000) : tempValueRaw; + // 0.0625 is the sensor sensitivity. float tempValue = ((static_cast(tempValueRaw)) * 0.0625); #if OBSW_DEBUG_TMP1075 == 1 sif::info << "Tmp1075 with object id: 0x" << std::hex << getObjectId() @@ -131,12 +133,14 @@ ReturnValue_t Tmp1075Handler::initializeLocalDataPool(localpool::DataPool &local return returnvalue::OK; } -void Tmp1075Handler::setModeNormal() { setMode(_MODE_TO_NORMAL); } - ReturnValue_t Tmp1075Handler::setHealth(HealthState health) { if (health != FAULTY and health != PERMANENT_FAULTY and health != HEALTHY and health != EXTERNAL_CONTROL) { return returnvalue::FAILED; } - return returnvalue::OK; + // Required because we do not have an assembly. + if (health == FAULTY or health == PERMANENT_FAULTY) { + setMode(_MODE_SHUT_DOWN); + } + return DeviceHandlerBase::setHealth(health); } diff --git a/mission/tcs/Tmp1075Handler.h b/mission/tcs/Tmp1075Handler.h index 02fd6823..4badca39 100644 --- a/mission/tcs/Tmp1075Handler.h +++ b/mission/tcs/Tmp1075Handler.h @@ -20,8 +20,6 @@ class Tmp1075Handler : public DeviceHandlerBase { Tmp1075Handler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie); virtual ~Tmp1075Handler(); - void setModeNormal(); - protected: void doStartUp() override; void doShutDown() override; diff --git a/mission/tcs/defs.h b/mission/tcs/defs.h index cb1a73e4..e2843570 100644 --- a/mission/tcs/defs.h +++ b/mission/tcs/defs.h @@ -4,18 +4,23 @@ #include namespace heater { + enum Switch : uint8_t { - HEATER_0_OBC_BRD, - HEATER_1_PLOC_PROC_BRD, + HEATER_0_PLOC_PROC_BRD, + HEATER_1_PCDU_PDU, HEATER_2_ACS_BRD, - HEATER_3_PCDU_PDU, + HEATER_3_OBC_BRD, HEATER_4_CAMERA, HEATER_5_STR, HEATER_6_DRO, HEATER_7_S_BAND, - NUMBER_OF_SWITCHES + NUMBER_OF_SWITCHES = 8, + HEATER_NONE = 0xff }; -} + +enum SwitchState : uint8_t { ON = 1, OFF = 0 }; + +} // namespace heater namespace tcs { diff --git a/mission/tmtc/CfdpTmFunnel.cpp b/mission/tmtc/CfdpTmFunnel.cpp index 46915b82..93c294a4 100644 --- a/mission/tmtc/CfdpTmFunnel.cpp +++ b/mission/tmtc/CfdpTmFunnel.cpp @@ -15,8 +15,16 @@ const char* CfdpTmFunnel::getName() const { return "CFDP TM Funnel"; } ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) { TmTcMessage currentMessage; + ReturnValue_t status; unsigned int count = 0; - ReturnValue_t status = tmQueue->receiveMessage(¤tMessage); + if (saveSequenceCount) { + status = saveSequenceCountToFile(); + if (status != returnvalue::OK) { + sif::error << "CfdpTmFunnel: Storing sequence count to file has failed" << std::endl; + } + saveSequenceCount = false; + } + status = tmQueue->receiveMessage(¤tMessage); while (status == returnvalue::OK) { status = handlePacket(currentMessage); if (status != returnvalue::OK) { diff --git a/mission/tmtc/CfdpTmFunnel.h b/mission/tmtc/CfdpTmFunnel.h index a4d1bc70..7b9efd34 100644 --- a/mission/tmtc/CfdpTmFunnel.h +++ b/mission/tmtc/CfdpTmFunnel.h @@ -3,6 +3,7 @@ #include +#include #include #include "fsfw/objectmanager/SystemObject.h" @@ -23,7 +24,6 @@ class CfdpTmFunnel : public TmFunnelBase { MessageQueueId_t fileStoreDest; StorageManagerIF& ramToFileStore; - uint16_t sourceSequenceCount = 0; uint16_t cfdpInCcsdsApid; }; #endif // FSFW_EXAMPLE_COMMON_CFDPTMFUNNEL_H diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index 7c3738fc..2f884dac 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -17,6 +17,8 @@ using namespace returnvalue; +static constexpr bool DEBUG_DUMPS = false; + PersistentTmStore::PersistentTmStore(PersistentTmStoreArgs args) : SystemObject(args.objectId), tmStore(args.tmStore), @@ -32,6 +34,91 @@ ReturnValue_t PersistentTmStore::cancelDump() { return returnvalue::OK; } +ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t upToUnixSeconds) { + using namespace std::filesystem; + std::error_code e; + dumpParams.orderedDumpFilestamps.clear(); + for (auto const& fileOrDir : directory_iterator(basePath)) { + if (not fileOrDir.is_regular_file(e)) { + continue; + } + dumpParams.fileSize = std::filesystem::file_size(fileOrDir.path(), e); + if (e) { + sif::error << "PersistentTmStore: Could not retrieve file size: " << e.message() << std::endl; + continue; + } + + // File empty or 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(fileOrDir.path(), e); + continue; + } + const path& file = fileOrDir.path(); + struct tm fileTime {}; + 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 > dumpParams.fromUnixTime) and + (fileEpoch + rolloverDiffSeconds <= dumpParams.untilUnixTime)) { + std::ifstream ifile(file, std::ios::binary); + if (ifile.bad()) { + sif::error << "PersistentTmStore: File is bad" << std::endl; + // TODO: Consider deleting file here? + continue; + } + + if (DEBUG_DUMPS) { + sif::debug << "Inserting file " << fileOrDir.path() << std::endl; + } + DumpIndex dumpIndex; + dumpIndex.epoch = fileEpoch; + // Multiple files for the same time are supported via a special suffix. We simply count the + // number of copies and later try to dump the same number of files with the additional + // suffixes + auto iter = dumpParams.orderedDumpFilestamps.find(dumpIndex); + if (iter != dumpParams.orderedDumpFilestamps.end()) { + dumpIndex.epoch = iter->epoch; + dumpIndex.additionalFiles = iter->additionalFiles + 1; + dumpParams.orderedDumpFilestamps.erase(dumpIndex); + } else { + dumpIndex.additionalFiles = 0; + } + dumpParams.orderedDumpFilestamps.emplace(dumpIndex); + } + } + return returnvalue::OK; +} + +std::optional PersistentTmStore::extractSuffix(const std::string& pathStr) { + std::string numberStr; + // Find the position of the dot at the end of the file path + size_t dotPos = pathStr.find_last_of('.'); + if ((dotPos < pathStr.length()) and not std::isdigit(pathStr[dotPos + 1])) { + return std::nullopt; + } + // Extract the substring after the dot + numberStr = pathStr.substr(dotPos + 1); + std::optional number; + try { + number = std::stoi(numberStr); + if (number.value() > std::numeric_limits::max()) { + return std::nullopt; + } + + } catch (std::invalid_argument& exception) { + sif::error << "PersistentTmStore::extractSuffix: Exception " << exception.what() + << ", invald input string: " << numberStr << std::endl; + } + return number; +} + ReturnValue_t PersistentTmStore::assignAndOrCreateMostRecentFile() { if (not activeFile.has_value()) { return createMostRecentFile(std::nullopt); @@ -41,6 +128,7 @@ ReturnValue_t PersistentTmStore::assignAndOrCreateMostRecentFile() { ReturnValue_t PersistentTmStore::handleCommandQueue(StorageManagerIF& ipcStore, Command_t& execCmd) { + execCmd = CommandMessageIF::CMD_NONE; CommandMessage cmdMessage; ReturnValue_t result = tcQueue->receiveMessage(&cmdMessage); if (result != returnvalue::OK) { @@ -75,9 +163,9 @@ ReturnValue_t PersistentTmStore::handleCommandQueue(StorageManagerIF& ipcStore, result = startDumpFromUpTo(dumpFromUnixSeconds, dumpUntilUnixSeconds); if (result == BUSY_DUMPING) { triggerEvent(persTmStore::BUSY_DUMPING_EVENT); - } else { - execCmd = cmd; + return result; } + execCmd = cmd; } } return result; @@ -159,6 +247,12 @@ bool PersistentTmStore::updateBaseDir() { if (not exists(basePath, e)) { create_directories(basePath); } + // Each file will have the base name as a prefix again + path preparedFullFilePath = basePath / baseName; + basePathSize = preparedFullFilePath.string().length(); + std::memcpy(filePathBuf.data(), preparedFullFilePath.c_str(), basePathSize); + filePathBuf[basePathSize] = '_'; + basePathSize += 1; baseDirUninitialized = false; return true; } @@ -189,12 +283,20 @@ ReturnValue_t PersistentTmStore::startDumpFromUpTo(uint32_t fromUnixSeconds, if (state == State::DUMPING) { return returnvalue::FAILED; } - dumpParams.dirIter = directory_iterator(basePath); - if (dumpParams.dirIter == directory_iterator()) { + auto dirIter = directory_iterator(basePath); + // Directory empty case. + if (dirIter == directory_iterator()) { return returnvalue::FAILED; } dumpParams.fromUnixTime = fromUnixSeconds; dumpParams.untilUnixTime = upToUnixSeconds; + buildDumpSet(fromUnixSeconds, upToUnixSeconds); + // No files in time window found. + if (dumpParams.orderedDumpFilestamps.empty()) { + return DUMP_DONE; + } + dumpParams.dumpIter = dumpParams.orderedDumpFilestamps.begin(); + dumpParams.currentSameFileIdx = std::nullopt; state = State::DUMPING; return loadNextDumpFile(); } @@ -203,49 +305,54 @@ 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; - - // File empty or 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 (pathToTime(file, fileTime) != returnvalue::OK) { - sif::error << "Time extraction for file " << file << "failed" << std::endl; - continue; - } - auto fileEpoch = static_cast(timegm(&fileTime)); - 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; + // Handle iteration, which does not necessarily have to increment the set iterator + // if there are multiple files for a given timestamp. + auto handleIteration = [&](DumpIndex& dumpIndex) { + if (dumpIndex.additionalFiles > 0) { + if (not dumpParams.currentSameFileIdx.has_value()) { + // Initialize the file index and stay on same file + dumpParams.currentSameFileIdx = 0; + return; + } else if (dumpParams.currentSameFileIdx.value() < dumpIndex.additionalFiles - 1) { + dumpParams.currentSameFileIdx = dumpParams.currentSameFileIdx.value() + 1; + return; } - ifile.read(reinterpret_cast(fileBuf.data()), - static_cast(dumpParams.fileSize)); - // Increment iterator for next cycle. - dumpParams.dirIter++; - return returnvalue::OK; } + // File will change, reset this field for correct state-keeping. + dumpParams.currentSameFileIdx = std::nullopt; + // Increment iterator for next cycle. + dumpParams.dumpIter++; + }; + while (dumpParams.dumpIter != dumpParams.orderedDumpFilestamps.end()) { + DumpIndex dumpIndex = *dumpParams.dumpIter; + timeval tv{}; + tv.tv_sec = dumpIndex.epoch; + size_t fullPathLength = 0; + createFileName(tv, dumpParams.currentSameFileIdx, fullPathLength); + dumpParams.currentFile = + path(std::string(reinterpret_cast(filePathBuf.data()), fullPathLength)); + if (DEBUG_DUMPS) { + sif::debug << baseName << " dump: Loading " << dumpParams.currentFile << std::endl; + } + dumpParams.fileSize = std::filesystem::file_size(dumpParams.currentFile, e); + if (e) { + // TODO: Event? + sif::error << "PersistentTmStore: Could not load next dump file: " << e.message() + << std::endl; + handleIteration(dumpIndex); + continue; + } + std::ifstream ifile(dumpParams.currentFile, std::ios::binary); + if (ifile.bad()) { + sif::error << "PersistentTmStore: File is bad. Loading next file" << std::endl; + handleIteration(dumpIndex); + continue; + } + ifile.read(reinterpret_cast(fileBuf.data()), + static_cast(dumpParams.fileSize)); + // Next file is loaded, exit the loop. + handleIteration(dumpIndex); + return returnvalue::OK; } // Directory iterator was consumed and we are done. state = State::IDLE; @@ -253,9 +360,13 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() { } ReturnValue_t PersistentTmStore::getNextDumpPacket(PusTmReader& reader, bool& fileHasSwapped) { - if (state == State::IDLE or dumpParams.pendingPacketDump) { + if (state == State::IDLE) { + return DUMP_DONE; + } + if (dumpParams.pendingPacketDump) { return returnvalue::FAILED; } + fileHasSwapped = false; reader.setReadOnlyData(fileBuf.data() + dumpParams.currentSize, fileBuf.size() - dumpParams.currentSize); // CRC check to fully ensure this is a valid TM @@ -266,11 +377,14 @@ ReturnValue_t PersistentTmStore::getNextDumpPacket(PusTmReader& reader, bool& fi // Delete the file and load next. Could use better algorithm to partially // restore the file dump, but for now do not trust the file. std::error_code e; - std::filesystem::remove(dumpParams.dirEntry.path().c_str(), e); + std::filesystem::remove(dumpParams.currentFile.c_str(), e); + if (dumpParams.currentFile == activeFile) { + activeFile == std::nullopt; + assignAndOrCreateMostRecentFile(); + } fileHasSwapped = true; return loadNextDumpFile(); } - fileHasSwapped = false; dumpParams.pendingPacketDump = true; return returnvalue::OK; } @@ -298,37 +412,9 @@ ReturnValue_t PersistentTmStore::pathToTime(const std::filesystem::path& path, s ReturnValue_t PersistentTmStore::createMostRecentFile(std::optional suffix) { using namespace std::filesystem; - unsigned currentIdx = 0; - path pathStart = basePath / baseName; - memcpy(fileBuf.data() + currentIdx, pathStart.c_str(), pathStart.string().length()); - currentIdx += pathStart.string().length(); - fileBuf[currentIdx] = '_'; - currentIdx += 1; - time_t epoch = currentTv.tv_sec; - struct tm* time = gmtime(&epoch); - size_t writtenBytes = strftime(reinterpret_cast(fileBuf.data() + currentIdx), - fileBuf.size(), config::FILE_DATE_FORMAT, time); - if (writtenBytes == 0) { - sif::error << "PersistentTmStore::createMostRecentFile: Could not create file timestamp" - << std::endl; - return returnvalue::FAILED; - } - currentIdx += writtenBytes; - char* res = strcpy(reinterpret_cast(fileBuf.data() + currentIdx), ".bin"); - if (res == nullptr) { - return returnvalue::FAILED; - } - currentIdx += 4; - if (suffix.has_value()) { - std::string fullSuffix = "." + std::to_string(suffix.value()); - res = strcpy(reinterpret_cast(fileBuf.data() + currentIdx), fullSuffix.c_str()); - if (res == nullptr) { - return returnvalue::FAILED; - } - currentIdx += fullSuffix.size(); - } - - path newPath(std::string(reinterpret_cast(fileBuf.data()), currentIdx)); + size_t currentIdx; + createFileName(currentTv, suffix, currentIdx); + path newPath(std::string(reinterpret_cast(filePathBuf.data()), currentIdx)); std::ofstream of(newPath, std::ios::binary); activeFile = newPath; activeFileTv = currentTv; @@ -350,3 +436,33 @@ void PersistentTmStore::getStartAndEndTimeCurrentOrLastDump(uint32_t& startTime, startTime = dumpParams.fromUnixTime; endTime = dumpParams.untilUnixTime; } + +ReturnValue_t PersistentTmStore::createFileName(timeval& tv, std::optional suffix, + size_t& fullPathLength) { + unsigned currentIdx = basePathSize; + time_t epoch = tv.tv_sec; + struct tm* time = gmtime(&epoch); + size_t writtenBytes = strftime(reinterpret_cast(filePathBuf.data() + currentIdx), + filePathBuf.size(), config::FILE_DATE_FORMAT, time); + if (writtenBytes == 0) { + sif::error << "PersistentTmStore::createMostRecentFile: Could not create file timestamp" + << std::endl; + return returnvalue::FAILED; + } + currentIdx += writtenBytes; + char* res = strcpy(reinterpret_cast(filePathBuf.data() + currentIdx), ".bin"); + if (res == nullptr) { + return returnvalue::FAILED; + } + currentIdx += 4; + if (suffix.has_value()) { + std::string fullSuffix = "." + std::to_string(suffix.value()); + res = strcpy(reinterpret_cast(filePathBuf.data() + currentIdx), fullSuffix.c_str()); + if (res == nullptr) { + return returnvalue::FAILED; + } + currentIdx += fullSuffix.size(); + } + fullPathLength = currentIdx; + return returnvalue::OK; +} diff --git a/mission/tmtc/PersistentTmStore.h b/mission/tmtc/PersistentTmStore.h index 17d2c9e7..e86acaaf 100644 --- a/mission/tmtc/PersistentTmStore.h +++ b/mission/tmtc/PersistentTmStore.h @@ -10,6 +10,7 @@ #include #include +#include #include "eive/eventSubsystemIds.h" #include "eive/resultClassIds.h" @@ -37,6 +38,14 @@ struct PersistentTmStoreArgs { SdCardMountedIF& sdcMan; }; +struct DumpIndex { + uint32_t epoch = 0; + // Number of additional files with a suffix like .0, .1 etc. + uint8_t additionalFiles = 0; + // Define a custom comparison function based on the epoch variable + bool operator<(const DumpIndex& other) const { return epoch < other.epoch; } +}; + class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { public: enum class State { IDLE, DUMPING }; @@ -96,7 +105,10 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { std::string baseName; uint8_t currentSameSecNumber = 0; std::filesystem::path basePath; + // std::filesystem::path pathStart = basePath / baseName; uint32_t rolloverDiffSeconds = 0; + std::array filePathBuf{}; + size_t basePathSize; std::array fileBuf{}; timeval currentTv; timeval activeFileTv{}; @@ -106,8 +118,10 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { uint32_t fromUnixTime = 0; uint32_t untilUnixTime = 0; uint32_t currentFileUnixStamp = 0; - std::filesystem::directory_iterator dirIter; - std::filesystem::directory_entry dirEntry; + std::filesystem::path currentFile; + std::set orderedDumpFilestamps{}; + std::set::iterator dumpIter; + std::optional currentSameFileIdx = 0; size_t fileSize = 0; size_t currentSize = 0; }; @@ -122,10 +136,13 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject { [[nodiscard]] MessageQueueId_t getCommandQueue() const override; void calcDiffSeconds(RolloverInterval intervalUnit, uint32_t intervalCount); + ReturnValue_t buildDumpSet(uint32_t fromUnixSeconds, uint32_t upToUnixSeconds); ReturnValue_t createMostRecentFile(std::optional suffix); 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(); + ReturnValue_t createFileName(timeval& tv, std::optional suffix, size_t& fullPathLength); + std::optional extractSuffix(const std::string& pathStr); bool updateBaseDir(); ReturnValue_t assignAndOrCreateMostRecentFile(); }; diff --git a/mission/tmtc/PusLiveDemux.cpp b/mission/tmtc/PusLiveDemux.cpp index bbe1be4f..8ba69e07 100644 --- a/mission/tmtc/PusLiveDemux.cpp +++ b/mission/tmtc/PusLiveDemux.cpp @@ -11,23 +11,20 @@ ReturnValue_t PusLiveDemux::demultiplexPackets(StorageManagerIF& tmStore, 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 - } + if ((destinations.size() > 1) and (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 { - message.setStorageId(origStoreId); +#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) { diff --git a/mission/tmtc/PusTmFunnel.cpp b/mission/tmtc/PusTmFunnel.cpp index dbc8f1c4..87648e55 100644 --- a/mission/tmtc/PusTmFunnel.cpp +++ b/mission/tmtc/PusTmFunnel.cpp @@ -1,5 +1,7 @@ #include "PusTmFunnel.h" +#include + #include "eive/definitions.h" #include "eive/objects.h" #include "fsfw/ipc/CommandMessage.h" @@ -11,8 +13,8 @@ #include "tmtc/pusIds.h" PusTmFunnel::PusTmFunnel(TmFunnelBase::FunnelCfg cfg, StorageManagerIF &ramToFileStore, - TimeReaderIF &timeReader, SdCardMountedIF &sdcMan) - : TmFunnelBase(cfg), ramToFileStore(ramToFileStore), timeReader(timeReader), sdcMan(sdcMan) {} + TimeReaderIF &timeReader) + : TmFunnelBase(cfg), ramToFileStore(ramToFileStore), timeReader(timeReader) {} PusTmFunnel::~PusTmFunnel() = default; @@ -21,6 +23,13 @@ ReturnValue_t PusTmFunnel::performOperation(uint8_t) { ReturnValue_t result; TmTcMessage currentMessage; unsigned int count = 0; + if (saveSequenceCount) { + result = saveSequenceCountToFile(); + if (result != returnvalue::OK) { + sif::error << "PusTmFunnel: Storing sequence count to file has failed" << std::endl; + } + saveSequenceCount = false; + } result = tmQueue->receiveMessage(¤tMessage); while (result == returnvalue::OK) { result = handleTmPacket(currentMessage); @@ -59,7 +68,33 @@ ReturnValue_t PusTmFunnel::handleTmPacket(TmTcMessage &message) { return result; } packet.setSequenceCount(sourceSequenceCount++); + // NOTE: This only works because the limit value bit width is below 16 bits! sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT; + + // Message type counter handling. + uint8_t service = packet.getService(); + bool insertionFailed = false; + auto mapIter = msgCounterMap.find(service); + if (mapIter == msgCounterMap.end()) { + auto iterPair = msgCounterMap.emplace(service, 0); + if (iterPair.second) { + mapIter = iterPair.first; + } else { + // Should really never never happen but you never know.. + insertionFailed = true; + } + } + if (not insertionFailed) { + packet.setMessageCount(mapIter->second); + // Sane overflow handling. + if (mapIter->second == std::numeric_limits::max()) { + mapIter->second = 0; + } else { + mapIter->second++; + } + } + + // Re-calculate CRC after changing the fields. This operation HAS to come last! packet.updateErrorControl(); // Send to persistent TM store if the packet matches some filter. diff --git a/mission/tmtc/PusTmFunnel.h b/mission/tmtc/PusTmFunnel.h index ba6e8d3e..3696fdcc 100644 --- a/mission/tmtc/PusTmFunnel.h +++ b/mission/tmtc/PusTmFunnel.h @@ -9,6 +9,8 @@ #include #include +#include +#include #include #include "PersistentTmStore.h" @@ -25,7 +27,7 @@ class PusTmFunnel : public TmFunnelBase { public: PusTmFunnel(TmFunnelBase::FunnelCfg cfg, StorageManagerIF &ramToFileStore, - TimeReaderIF &timeReader, SdCardMountedIF &sdcMan); + TimeReaderIF &timeReader); [[nodiscard]] const char *getName() const override; ~PusTmFunnel() override; @@ -36,11 +38,10 @@ class PusTmFunnel : public TmFunnelBase { // Update TV stamp every 5 minutes static constexpr dur_millis_t TV_UPDATE_INTERVAL_SECS = 60 * 5; - uint16_t sourceSequenceCount = 0; + std::map msgCounterMap; StorageManagerIF &ramToFileStore; TimeReaderIF &timeReader; bool storesInitialized = false; - SdCardMountedIF &sdcMan; PusTmRouteByFilterHelper persistentTmMap; ReturnValue_t handleTmPacket(TmTcMessage &message); diff --git a/mission/tmtc/TmFunnelBase.cpp b/mission/tmtc/TmFunnelBase.cpp index eb480b03..fc2e4b76 100644 --- a/mission/tmtc/TmFunnelBase.cpp +++ b/mission/tmtc/TmFunnelBase.cpp @@ -2,6 +2,10 @@ #include +#include +#include +#include + #include "fsfw/ipc/QueueFactory.h" TmFunnelBase::TmFunnelBase(FunnelCfg cfg) @@ -10,7 +14,10 @@ TmFunnelBase::TmFunnelBase(FunnelCfg cfg) tmStore(cfg.tmStore), ipcStore(cfg.ipcStore), tmQueue(QueueFactory::instance()->createMessageQueue(cfg.tmMsgDepth)), - liveDemux(*tmQueue) {} + liveDemux(*tmQueue), + sdcMan(cfg.sdcMan), + sequenceCounterFilename(cfg.sequenceCounterFilename), + saveSequenceCount(cfg.saveSequenceCount) {} ReturnValue_t TmFunnelBase::demultiplexLivePackets(store_address_t origStoreId, const uint8_t *tmData, size_t tmSize) { @@ -27,3 +34,38 @@ void TmFunnelBase::addLiveDestination(const char *name, const AcceptsTelemetryIF &downlinkDestination, uint8_t vcid) { liveDemux.addDestination(name, downlinkDestination, vcid); } + +ReturnValue_t TmFunnelBase::initialize() { + using namespace std::filesystem; + // The filesystem should always be available at the start.. Let's assume it always is, otherwise + // we just live with a regular 0 initialization. It simplifies a lot. + std::error_code e; + path filePath = + path(path(sdcMan.getCurrentMountPrefix()) / path("conf") / path(sequenceCounterFilename)); + if (exists(filePath, e)) { + std::ifstream ifile(filePath); + if (ifile.bad()) { + sif::error << "TmFunnelBase::initialize: Faulty file open for sequence counter initialization" + << std::endl; + return returnvalue::OK; + } + if (not(ifile >> sourceSequenceCount)) { + // Initialize to 0 in any case if reading a number failed. + sourceSequenceCount = 0; + } + } + return returnvalue::OK; +} + +ReturnValue_t TmFunnelBase::saveSequenceCountToFile() { + using namespace std::filesystem; + std::error_code e; + path filePath = + path(path(sdcMan.getCurrentMountPrefix()) / path("conf") / path(sequenceCounterFilename)); + std::ofstream ofile(filePath); + if (ofile.bad()) { + return returnvalue::FAILED; + } + ofile << sourceSequenceCount << "\n"; + return returnvalue::OK; +} diff --git a/mission/tmtc/TmFunnelBase.h b/mission/tmtc/TmFunnelBase.h index 51d16626..72d91103 100644 --- a/mission/tmtc/TmFunnelBase.h +++ b/mission/tmtc/TmFunnelBase.h @@ -6,25 +6,34 @@ #include #include #include +#include #include +#include #include class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject { public: struct FunnelCfg { FunnelCfg(object_id_t objId, const char* name, StorageManagerIF& tmStore, - StorageManagerIF& ipcStore, uint32_t tmMsgDepth) + StorageManagerIF& ipcStore, uint32_t tmMsgDepth, SdCardMountedIF& sdcMan, + const char* sequenceCounterFilename, std::atomic_bool& saveSequenceCount) : objectId(objId), name(name), tmStore(tmStore), ipcStore(ipcStore), - tmMsgDepth(tmMsgDepth) {} + tmMsgDepth(tmMsgDepth), + sdcMan(sdcMan), + sequenceCounterFilename(sequenceCounterFilename), + saveSequenceCount(saveSequenceCount) {} object_id_t objectId; const char* name; StorageManagerIF& tmStore; StorageManagerIF& ipcStore; uint32_t tmMsgDepth; + SdCardMountedIF& sdcMan; + const char* sequenceCounterFilename; + std::atomic_bool& saveSequenceCount; }; explicit TmFunnelBase(FunnelCfg cfg); [[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override; @@ -32,6 +41,9 @@ class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject { uint8_t vcid = 0); ReturnValue_t demultiplexLivePackets(store_address_t origStoreId, const uint8_t* tmData, size_t tmSize); + ReturnValue_t initialize() override; + + ReturnValue_t saveSequenceCountToFile(); ~TmFunnelBase() override; @@ -41,6 +53,10 @@ class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject { StorageManagerIF& ipcStore; MessageQueueIF* tmQueue = nullptr; PusLiveDemux liveDemux; + SdCardMountedIF& sdcMan; + const char* sequenceCounterFilename; + std::atomic_bool& saveSequenceCount; + uint16_t sourceSequenceCount = 0; }; #endif /* MISSION_TMTC_TMFUNNELBASE_H_ */ diff --git a/q7s-env-em.sh b/q7s-env-em.sh index 86737627..698b6b23 100755 --- a/q7s-env-em.sh +++ b/q7s-env-em.sh @@ -4,10 +4,10 @@ # custom cross-compiler and sysroot path setups # Adapt the following two entries to your need -CROSS_COMPILE_BIN_PATH="/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin" +export CROSS_COMPILE_BIN_PATH="/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin" export ZYNQ_7020_SYSROOT="/opt/xiphos/sdk/ark/sysroots/cortexa9hf-neon-xiphos-linux-gnueabi" -export PATH=$PATH:${CROSS_COMPILE_BIN_PATH} +export PATH=${CROSS_COMPILE_BIN_PATH}:$PATH export CROSS_COMPILE="arm-linux-gnueabihf" export EIVE_Q7S_EM=1 diff --git a/q7s-env.sh b/q7s-env.sh index 5cf608a0..4b752e17 100755 --- a/q7s-env.sh +++ b/q7s-env.sh @@ -4,10 +4,10 @@ # custom cross-compiler and sysroot path setups # Adapt the following two entries to your need -CROSS_COMPILE_BIN_PATH="/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin" +export CROSS_COMPILE_BIN_PATH="/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin" export ZYNQ_7020_SYSROOT="/opt/xiphos/sdk/ark/sysroots/cortexa9hf-neon-xiphos-linux-gnueabi" -export PATH=$PATH:${CROSS_COMPILE_BIN_PATH} +export PATH=${CROSS_COMPILE_BIN_PATH}:$PATH export CROSS_COMPILE="arm-linux-gnueabihf" # export EIVE_Q7S_EM=1 diff --git a/release_checklist.md b/release-checklist.md similarity index 66% rename from release_checklist.md rename to release-checklist.md index 4c8be066..04a0233c 100644 --- a/release_checklist.md +++ b/release-checklist.md @@ -7,7 +7,9 @@ OBSW Release Checklist 2. Re-run the generators with `generators/gen.py all` 3. Re-run the auto-formatters with the `scripts/auto-formatter.sh` script 4. Verify that the Q7S, Q7S EM and Host build are working -5. Wait for CI/CD results +5. Update `CHANGELOG.md`: Add new `unreleased` section, convert old unreleased section to + header containing version number and release date. +6. Wait for CI/CD results # Post-Release diff --git a/thirdparty/sagittactl b/thirdparty/sagittactl index 2c066bfb..20f32a2f 160000 --- a/thirdparty/sagittactl +++ b/thirdparty/sagittactl @@ -1 +1 @@ -Subproject commit 2c066bfbe9ef8d6b00e142b8f48d45988a469618 +Subproject commit 20f32a2f291d00869c693066de02c0257dec7be4 diff --git a/tmtc b/tmtc index d623e83b..9be81f17 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit d623e83be8536f8887f1da00a1f0c4be191db1e8 +Subproject commit 9be81f1725004b55e937718fbaddc4f4e4e74081 diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt index 20b98979..e9a01891 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -4,7 +4,7 @@ add_subdirectory(mocks) target_sources(${UNITTEST_NAME} PRIVATE main.cpp testEnvironment.cpp - testStampInFilename.cpp + testGenericFilesystem.cpp hdlcEncodingRw.cpp printChar.cpp ) \ No newline at end of file diff --git a/unittest/controller/testThermalController.cpp b/unittest/controller/testThermalController.cpp index c2166ca0..287943ce 100644 --- a/unittest/controller/testThermalController.cpp +++ b/unittest/controller/testThermalController.cpp @@ -30,7 +30,8 @@ TEST_CASE("Thermal Controller", "[ThermalController]") { // testEnvironment::initialize(); - ThermalController controller(THERMAL_CONTROLLER_ID, *heaterHandler, tcsBrdShortlyUnavailable); + ThermalController controller(THERMAL_CONTROLLER_ID, *heaterHandler, tcsBrdShortlyUnavailable, + true); ReturnValue_t result = controller.initialize(); REQUIRE(result == returnvalue::OK); diff --git a/unittest/rebootLogic/src/CoreController.cpp b/unittest/rebootLogic/src/CoreController.cpp index 2bc8bbef..23461ea1 100644 --- a/unittest/rebootLogic/src/CoreController.cpp +++ b/unittest/rebootLogic/src/CoreController.cpp @@ -19,116 +19,117 @@ CoreController::CoreController() { setCurrentBootCopy(xsc::CHIP_0, xsc::COPY_0); } -void CoreController::performRebootFileHandling(bool recreateFile) { +void CoreController::performRebootWatchdogHandling(bool recreateFile) { using namespace std; - std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; if (not std::filesystem::exists(path) or recreateFile) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "CoreController::performRebootFileHandling: Recreating reboot file" << std::endl; #endif - rebootFile.enabled = true; - rebootFile.img00Cnt = 0; - rebootFile.img01Cnt = 0; - rebootFile.img10Cnt = 0; - rebootFile.img11Cnt = 0; - rebootFile.lastChip = xsc::Chip::CHIP_0; - rebootFile.lastCopy = xsc::Copy::COPY_0; - rebootFile.img00Lock = false; - rebootFile.img01Lock = false; - rebootFile.img10Lock = false; - rebootFile.img11Lock = false; - rebootFile.mechanismNextChip = xsc::Chip::NO_CHIP; - rebootFile.mechanismNextCopy = xsc::Copy::NO_COPY; - rebootFile.bootFlag = false; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.enabled = true; + rebootWatchdogFile.img00Cnt = 0; + rebootWatchdogFile.img01Cnt = 0; + rebootWatchdogFile.img10Cnt = 0; + rebootWatchdogFile.img11Cnt = 0; + rebootWatchdogFile.lastChip = xsc::Chip::CHIP_0; + rebootWatchdogFile.lastCopy = xsc::Copy::COPY_0; + rebootWatchdogFile.img00Lock = false; + rebootWatchdogFile.img01Lock = false; + rebootWatchdogFile.img10Lock = false; + rebootWatchdogFile.img11Lock = false; + rebootWatchdogFile.mechanismNextChip = xsc::Chip::NO_CHIP; + rebootWatchdogFile.mechanismNextCopy = xsc::Copy::NO_COPY; + rebootWatchdogFile.bootFlag = false; + rewriteRebootWatchdogFile(rebootWatchdogFile); } else { - if (not parseRebootFile(path, rebootFile)) { - performRebootFileHandling(true); + if (not parseRebootWatchdogFile(path, rebootWatchdogFile)) { + performRebootWatchdogHandling(true); } } if (CURRENT_CHIP == xsc::CHIP_0) { if (CURRENT_COPY == xsc::COPY_0) { - rebootFile.img00Cnt++; + rebootWatchdogFile.img00Cnt++; } else { - rebootFile.img01Cnt++; + rebootWatchdogFile.img01Cnt++; } } else { if (CURRENT_COPY == xsc::COPY_0) { - rebootFile.img10Cnt++; + rebootWatchdogFile.img10Cnt++; } else { - rebootFile.img11Cnt++; + rebootWatchdogFile.img11Cnt++; } } - if (rebootFile.bootFlag) { + if (rebootWatchdogFile.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; + uint32_t p1 = rebootWatchdogFile.lastChip << 16 | rebootWatchdogFile.lastCopy; + uint32_t p2 = rebootWatchdogFile.img00Cnt << 24 | rebootWatchdogFile.img01Cnt << 16 | + rebootWatchdogFile.img10Cnt << 8 | rebootWatchdogFile.img11Cnt; triggerEvent(REBOOT_MECHANISM_TRIGGERED, p1, p2); // Clear the boot flag - rebootFile.bootFlag = false; + rebootWatchdogFile.bootFlag = false; } - if (rebootFile.mechanismNextChip != xsc::NO_CHIP and - rebootFile.mechanismNextCopy != xsc::NO_COPY) { - if (CURRENT_CHIP != rebootFile.mechanismNextChip or - CURRENT_COPY != rebootFile.mechanismNextCopy) { - std::string infoString = std::to_string(rebootFile.mechanismNextChip) + " " + - std::to_string(rebootFile.mechanismNextCopy); + if (rebootWatchdogFile.mechanismNextChip != xsc::NO_CHIP and + rebootWatchdogFile.mechanismNextCopy != xsc::NO_COPY) { + if (CURRENT_CHIP != rebootWatchdogFile.mechanismNextChip or + CURRENT_COPY != rebootWatchdogFile.mechanismNextCopy) { + std::string infoString = std::to_string(rebootWatchdogFile.mechanismNextChip) + " " + + std::to_string(rebootWatchdogFile.mechanismNextCopy); sif::warning << "CoreController::performRebootFileHandling: Expected to be on image " << infoString << " but currently on other image. Locking " << infoString << std::endl; // Firmware or other component might be corrupt and we are on another image then the target // image specified by the mechanism. We can't really trust the target image anymore. // Lock it for now - if (rebootFile.mechanismNextChip == xsc::CHIP_0) { - if (rebootFile.mechanismNextCopy == xsc::COPY_0) { - rebootFile.img00Lock = true; + if (rebootWatchdogFile.mechanismNextChip == xsc::CHIP_0) { + if (rebootWatchdogFile.mechanismNextCopy == xsc::COPY_0) { + rebootWatchdogFile.img00Lock = true; } else { - rebootFile.img01Lock = true; + rebootWatchdogFile.img01Lock = true; } } else { - if (rebootFile.mechanismNextCopy == xsc::COPY_0) { - rebootFile.img10Lock = true; + if (rebootWatchdogFile.mechanismNextCopy == xsc::COPY_0) { + rebootWatchdogFile.img10Lock = true; } else { - rebootFile.img11Lock = true; + rebootWatchdogFile.img11Lock = true; } } } } - rebootFile.lastChip = CURRENT_CHIP; - rebootFile.lastCopy = CURRENT_COPY; + rebootWatchdogFile.lastChip = CURRENT_CHIP; + rebootWatchdogFile.lastCopy = CURRENT_COPY; // Only reboot if the reboot functionality is enabled. // The handler will still increment the boot counts - if (rebootFile.enabled and (*rebootFile.relevantBootCnt >= rebootFile.maxCount)) { + if (rebootWatchdogFile.enabled and + (*rebootWatchdogFile.relevantBootCnt >= rebootWatchdogFile.maxCount)) { // Reboot to other image bool doReboot = false; xsc::Chip tgtChip = xsc::NO_CHIP; xsc::Copy tgtCopy = xsc::NO_COPY; - determineAndExecuteReboot(rebootFile, doReboot, tgtChip, tgtCopy); + determineAndExecuteReboot(rebootWatchdogFile, doReboot, tgtChip, tgtCopy); if (doReboot) { - rebootFile.bootFlag = true; + rebootWatchdogFile.bootFlag = true; #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "Boot counter for image " << CURRENT_CHIP << " " << CURRENT_COPY << " too high. Rebooting to " << tgtChip << " " << tgtCopy << std::endl; #endif - rebootFile.mechanismNextChip = tgtChip; - rebootFile.mechanismNextCopy = tgtCopy; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.mechanismNextChip = tgtChip; + rebootWatchdogFile.mechanismNextCopy = tgtCopy; + rewriteRebootWatchdogFile(rebootWatchdogFile); xsc_boot_copy(static_cast(tgtChip), static_cast(tgtCopy)); } } else { - rebootFile.mechanismNextChip = xsc::NO_CHIP; - rebootFile.mechanismNextCopy = xsc::NO_COPY; + rebootWatchdogFile.mechanismNextChip = xsc::NO_CHIP; + rebootWatchdogFile.mechanismNextCopy = xsc::NO_COPY; } - rewriteRebootFile(rebootFile); + rewriteRebootWatchdogFile(rebootWatchdogFile); } -void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot, +void CoreController::determineAndExecuteReboot(RebootWatchdogFile &rf, bool &needsReboot, xsc::Chip &tgtChip, xsc::Copy &tgtCopy) { tgtChip = xsc::CHIP_0; tgtCopy = xsc::COPY_0; @@ -217,7 +218,7 @@ void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot } } -bool CoreController::parseRebootFile(std::string path, RebootFile &rf) { +bool CoreController::parseRebootWatchdogFile(std::string path, RebootWatchdogFile &rf) { using namespace std; std::string selfMatch; if (CURRENT_CHIP == xsc::CHIP_0) { @@ -400,34 +401,34 @@ bool CoreController::parseRebootFile(std::string path, RebootFile &rf) { } void CoreController::resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy) { - std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism - parseRebootFile(path, rebootFile); + parseRebootWatchdogFile(path, rebootWatchdogFile); if (tgtChip == xsc::ALL_CHIP and tgtCopy == xsc::ALL_COPY) { - rebootFile.img00Cnt = 0; - rebootFile.img01Cnt = 0; - rebootFile.img10Cnt = 0; - rebootFile.img11Cnt = 0; + rebootWatchdogFile.img00Cnt = 0; + rebootWatchdogFile.img01Cnt = 0; + rebootWatchdogFile.img10Cnt = 0; + rebootWatchdogFile.img11Cnt = 0; } else { if (tgtChip == xsc::CHIP_0) { if (tgtCopy == xsc::COPY_0) { - rebootFile.img00Cnt = 0; + rebootWatchdogFile.img00Cnt = 0; } else { - rebootFile.img01Cnt = 0; + rebootWatchdogFile.img01Cnt = 0; } } else { if (tgtCopy == xsc::COPY_0) { - rebootFile.img10Cnt = 0; + rebootWatchdogFile.img10Cnt = 0; } else { - rebootFile.img11Cnt = 0; + rebootWatchdogFile.img11Cnt = 0; } } } - rewriteRebootFile(rebootFile); + rewriteRebootWatchdogFile(rebootWatchdogFile); } -void CoreController::rewriteRebootFile(RebootFile file) { - std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_FILE; +void CoreController::rewriteRebootWatchdogFile(RebootWatchdogFile file) { + std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; std::ofstream rebootFile(path); if (rebootFile.is_open()) { // Initiate reboot file first. Reboot handling will be on on initialization @@ -450,15 +451,15 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ if (size < 1) { return HasActionsIF::INVALID_PARAMETERS; } - std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism - parseRebootFile(path, rebootFile); + parseRebootWatchdogFile(path, rebootWatchdogFile); if (data[0] == 0) { - rebootFile.enabled = false; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.enabled = false; + rewriteRebootWatchdogFile(rebootWatchdogFile); } else if (data[0] == 1) { - rebootFile.enabled = true; - rewriteRebootFile(rebootFile); + rebootWatchdogFile.enabled = true; + rewriteRebootWatchdogFile(rebootWatchdogFile); } else { return HasActionsIF::INVALID_PARAMETERS; } @@ -490,11 +491,11 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ if (size < 1) { return HasActionsIF::INVALID_PARAMETERS; } - std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism - parseRebootFile(path, rebootFile); - rebootFile.maxCount = data[0]; - rewriteRebootFile(rebootFile); + parseRebootWatchdogFile(path, rebootWatchdogFile); + rebootWatchdogFile.maxCount = data[0]; + rewriteRebootWatchdogFile(rebootWatchdogFile); return HasActionsIF::EXECUTION_FINISHED; } default: { @@ -504,23 +505,23 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ } void CoreController::setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy) { - std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_FILE; + std::string path = sdcMan->getCurrentMountPrefix(sdInfo.active) + REBOOT_WATCHDOG_FILE; // Disable the reboot file mechanism - parseRebootFile(path, rebootFile); + parseRebootWatchdogFile(path, rebootWatchdogFile); if (tgtChip == xsc::CHIP_0) { if (tgtCopy == xsc::COPY_0) { - rebootFile.img00Lock = lock; + rebootWatchdogFile.img00Lock = lock; } else { - rebootFile.img01Lock = lock; + rebootWatchdogFile.img01Lock = lock; } } else { if (tgtCopy == xsc::COPY_0) { - rebootFile.img10Lock = lock; + rebootWatchdogFile.img10Lock = lock; } else { - rebootFile.img11Lock = lock; + rebootWatchdogFile.img11Lock = lock; } } - rewriteRebootFile(rebootFile); + rewriteRebootWatchdogFile(rebootWatchdogFile); } void CoreController::setCurrentBootCopy(xsc::Chip chip, xsc::Copy copy) { diff --git a/unittest/rebootLogic/src/CoreController.h b/unittest/rebootLogic/src/CoreController.h index 1846c27f..34d2a5f7 100644 --- a/unittest/rebootLogic/src/CoreController.h +++ b/unittest/rebootLogic/src/CoreController.h @@ -14,7 +14,7 @@ enum Copy : int { COPY_0, COPY_1, NO_COPY, SELF_COPY, ALL_COPY }; } // namespace xsc -struct RebootFile { +struct RebootWatchdogFile { static constexpr uint8_t DEFAULT_MAX_BOOT_CNT = 10; bool enabled = true; @@ -39,7 +39,7 @@ class SdCardManager; class CoreController { public: - static constexpr char REBOOT_FILE[] = "/conf/reboot.txt"; + static constexpr char REBOOT_WATCHDOG_FILE[] = "/conf/reboot.txt"; //! [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 @@ -57,13 +57,13 @@ class CoreController { static void setCurrentBootCopy(xsc::Chip chip, xsc::Copy copy); ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size); - void performRebootFileHandling(bool recreateFile); - void determineAndExecuteReboot(RebootFile& rf, bool& needsReboot, xsc::Chip& tgtChip, + void performRebootWatchdogHandling(bool recreateFile); + void determineAndExecuteReboot(RebootWatchdogFile& rf, bool& needsReboot, xsc::Chip& tgtChip, xsc::Copy& tgtCopy); void setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy); void resetRebootCount(xsc::Chip tgtChip, xsc::Copy tgtCopy); - bool parseRebootFile(std::string path, RebootFile& file); - void rewriteRebootFile(RebootFile file); + bool parseRebootWatchdogFile(std::string path, RebootWatchdogFile& file); + void rewriteRebootWatchdogFile(RebootWatchdogFile file); private: struct SdFsmParams { @@ -74,6 +74,6 @@ class CoreController { } sdInfo; SdCardManager* sdcMan = nullptr; - RebootFile rebootFile = {}; + RebootWatchdogFile rebootWatchdogFile = {}; bool doPerformRebootFileHandling = true; }; \ No newline at end of file diff --git a/unittest/testEnvironment.cpp b/unittest/testEnvironment.cpp index 72726f39..4c19c4cf 100644 --- a/unittest/testEnvironment.cpp +++ b/unittest/testEnvironment.cpp @@ -27,7 +27,7 @@ void factory(void* args) { new HouseKeepingMock(); eventManager = new EventManagerMock(); new HealthTable(objects::HEALTH_TABLE); - new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER); + new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER, 5, false, 60.0); new CdsShortTimeStamper(objects::TIME_STAMPER); { diff --git a/unittest/testGenericFilesystem.cpp b/unittest/testGenericFilesystem.cpp new file mode 100644 index 00000000..e5279bab --- /dev/null +++ b/unittest/testGenericFilesystem.cpp @@ -0,0 +1,43 @@ + +#include +#include + +#include "fsfw/timemanager/Clock.h" + +uint8_t extractSuffix(const std::string& pathStr) { + std::string numberStr; + // Find the position of the dot at the end of the file path + size_t dotPos = pathStr.find_last_of('.'); + if (dotPos != std::string::npos && dotPos < pathStr.length() - 1) { + // Extract the substring after the dot + numberStr = pathStr.substr(dotPos + 1); + } + int number = std::stoi(numberStr); + if (number < 0 or number > std::numeric_limits::max()) { + return 0; + } + return static_cast(number); +} + +TEST_CASE("Stamp in Filename", "[Stamp In Filename]") { + Clock::TimeOfDay_t tod; + std::string baseName = "verif"; + std::string pathStr = "verif_2022-05-25T16:55:23Z.bin"; + unsigned int underscorePos = pathStr.find_last_of('_'); + std::string stampStr = pathStr.substr(underscorePos + 1); + float seconds = 0.0; + char* prefix = nullptr; + int count = + sscanf(stampStr.c_str(), + "%4" SCNu32 "-%2" SCNu32 "-%2" SCNu32 "T%2" SCNu32 ":%2" SCNu32 ":%2" SCNu32 "Z", + &tod.year, &tod.month, &tod.day, &tod.hour, &tod.minute, &tod.second); + static_cast(count); + CHECK(count == 6); +} + +TEST_CASE("Suffix Extraction") { + std::string pathStr = "/mnt/sd0/tm/hk/hk-some-stamp.bin.0"; + CHECK(extractSuffix(pathStr) == 0); + pathStr = "/mnt/sd0/tm/hk/hk-some-stamp.bin.2"; + CHECK(extractSuffix(pathStr) == 2); +} diff --git a/unittest/testStampInFilename.cpp b/unittest/testStampInFilename.cpp deleted file mode 100644 index c66bdce6..00000000 --- a/unittest/testStampInFilename.cpp +++ /dev/null @@ -1,21 +0,0 @@ - -#include -#include - -#include "fsfw/timemanager/Clock.h" - -TEST_CASE("Stamp in Filename", "[Stamp In Filename]") { - Clock::TimeOfDay_t tod; - std::string baseName = "verif"; - std::string pathStr = "verif_2022-05-25T16:55:23Z.bin"; - unsigned int underscorePos = pathStr.find_last_of('_'); - std::string stampStr = pathStr.substr(underscorePos + 1); - float seconds = 0.0; - char* prefix = nullptr; - int count = - sscanf(stampStr.c_str(), - "%4" SCNu32 "-%2" SCNu32 "-%2" SCNu32 "T%2" SCNu32 ":%2" SCNu32 ":%2" SCNu32 "Z", - &tod.year, &tod.month, &tod.day, &tod.hour, &tod.minute, &tod.second); - static_cast(count); - CHECK(count == 6); -}