Compare commits
367 Commits
Author | SHA1 | Date |
---|---|---|
Marius Eggert | c1b8e891c5 | |
Robin Müller | 18cf46ea31 | |
Marius Eggert | 2d7356b9ed | |
Marius Eggert | 675dda8e9e | |
Marius Eggert | 5bb1bd8946 | |
Marius Eggert | eff9116aec | |
Marius Eggert | 4ed516e0bc | |
Marius Eggert | e2ee6a492c | |
Marius Eggert | a6422f2d73 | |
Marius Eggert | ceb2130726 | |
Marius Eggert | fcd3b7815c | |
Marius Eggert | 1067abba9a | |
Marius Eggert | 53376fd9ed | |
Marius Eggert | c2d05b2045 | |
Marius Eggert | 48914a2aba | |
Marius Eggert | ef8736bd81 | |
Marius Eggert | 584a16a67c | |
Robin Müller | addfadddb6 | |
Marius Eggert | df392d3319 | |
Robin Müller | b079ce85f2 | |
Marius Eggert | bc9b8efdb8 | |
Robin Müller | 51dcb56583 | |
Robin Müller | df0c8eefab | |
Robin Müller | 5a7df626ab | |
Marius Eggert | 7b53275d61 | |
Marius Eggert | 709c53d533 | |
Marius Eggert | 3a70155105 | |
Marius Eggert | 7a12c1c8fe | |
Robin Müller | 0f1c41e828 | |
Robin Müller | c4340c3515 | |
Robin Müller | eec934e1b0 | |
Robin Müller | b01d4f6363 | |
Robin Müller | d9789a48d8 | |
Robin Müller | c3237cae3c | |
Marius Eggert | 427c53df8c | |
Marius Eggert | c89e81cac9 | |
Robin Müller | fb54d976d2 | |
Robin Müller | 25a1e4ea25 | |
Robin Müller | a63eb331eb | |
Robin Müller | 1793b325ec | |
Marius Eggert | 1c36f36b1f | |
Marius Eggert | 0f3eeb42d6 | |
Marius Eggert | 887f165484 | |
Robin Müller | 6cc1d86018 | |
Marius Eggert | d4eb124cdf | |
Robin Müller | 47cedb90f9 | |
Marius Eggert | f3d4e09487 | |
Marius Eggert | 2057ab9c10 | |
Robin Müller | 1f0e2d99e9 | |
Robin Müller | 35181a2693 | |
Robin Müller | e1b5625086 | |
Robin Müller | 72626582f6 | |
Robin Müller | 3e6d26669a | |
Marius Eggert | 69f378f8b6 | |
Marius Eggert | a46f6f34d6 | |
Marius Eggert | e6855120f3 | |
Marius Eggert | e59235fd75 | |
Robin Müller | 3875ddf92b | |
Robin Müller | 4171b11928 | |
Robin Müller | 43de097812 | |
Robin Müller | 56512fae0d | |
Robin Müller | 76d00ddd37 | |
Marius Eggert | 539221a458 | |
Marius Eggert | 9f03341108 | |
Marius Eggert | 4193a565a4 | |
Marius Eggert | c1c254330b | |
Marius Eggert | c46c6cd28b | |
Marius Eggert | aeb8b92bc4 | |
Marius Eggert | 8011686fbe | |
Marius Eggert | 58be09bd4b | |
Marius Eggert | ad82573a35 | |
Marius Eggert | a1be15e939 | |
Marius Eggert | ba7c9e1c26 | |
Marius Eggert | 46c125d9fe | |
Marius Eggert | 889dd04c6b | |
Marius Eggert | c52746a2df | |
Marius Eggert | f2f856e227 | |
Marius Eggert | 0cbac07f15 | |
Marius Eggert | 18d3c8fa91 | |
Marius Eggert | 3e1ef8bcaf | |
Marius Eggert | d1fc876f03 | |
Marius Eggert | 0278aabee0 | |
Marius Eggert | a9f5b6d2c7 | |
Marius Eggert | 80ea0b341b | |
Marius Eggert | 9740831755 | |
Marius Eggert | c4e18432e2 | |
Marius Eggert | f9f5ba5d46 | |
Marius Eggert | 3b521966a9 | |
Marius Eggert | c58bae5aa5 | |
Marius Eggert | 67e6ccf4ae | |
Marius Eggert | b795beef85 | |
Marius Eggert | 311ecd7fd2 | |
Marius Eggert | 950e86ce4b | |
Marius Eggert | 096328aadc | |
Marius Eggert | 36edd3e324 | |
Marius Eggert | c65e813e94 | |
Marius Eggert | 1c93f51f69 | |
Marius Eggert | 7a43e1bc67 | |
Marius Eggert | dade2d519a | |
Marius Eggert | c4680f85bb | |
Marius Eggert | 6c9a7c3ee5 | |
Marius Eggert | 346f4ff9de | |
Marius Eggert | ad5282ca4a | |
Marius Eggert | 3c869e5215 | |
Marius Eggert | 8374a02ae2 | |
Marius Eggert | f2d1e16697 | |
Marius Eggert | 158927ce5c | |
Marius Eggert | e97105820a | |
Marius Eggert | 88102b26a6 | |
Marius Eggert | 7e689c9f55 | |
Marius Eggert | 97ada32f33 | |
Marius Eggert | 244c364f60 | |
Marius Eggert | c0c3576131 | |
Marius Eggert | c2a4578b81 | |
Marius Eggert | 781feffc90 | |
Marius Eggert | 7efd48c695 | |
Marius Eggert | d0765fdcce | |
Marius Eggert | be0bae58ac | |
Marius Eggert | 616803c6a8 | |
Marius Eggert | 3501763cf0 | |
Marius Eggert | efaf48beff | |
Marius Eggert | e052470cf4 | |
Marius Eggert | 2364f7d5d9 | |
Marius Eggert | c52818a5ce | |
Marius Eggert | fb8a92ecb5 | |
Marius Eggert | 39032249b2 | |
Marius Eggert | aa4bfa8d88 | |
Marius Eggert | c2d8ef9fe4 | |
Marius Eggert | 440d989490 | |
Marius Eggert | 85ed8420fd | |
Marius Eggert | 2a2a173ebd | |
Marius Eggert | cda2a9df33 | |
Marius Eggert | 6da37c0fa4 | |
Marius Eggert | 141f82d0a9 | |
Marius Eggert | b7bf927288 | |
Marius Eggert | 8fe7307a58 | |
Marius Eggert | 7e7d8c249e | |
Marius Eggert | ffce866ad0 | |
Marius Eggert | 236916bf67 | |
Marius Eggert | 9a8c059e05 | |
Marius Eggert | 48d37783dd | |
Marius Eggert | d20c8258da | |
Marius Eggert | bd46a607f0 | |
Marius Eggert | bdb7f29b99 | |
Marius Eggert | c6a308b308 | |
Marius Eggert | b9d4d2f3a3 | |
Marius Eggert | e908ee119e | |
Marius Eggert | f6e99f171a | |
Marius Eggert | 5876ad1f56 | |
Marius Eggert | 9cc991e600 | |
Robin Müller | d0f8b7981f | |
Robin Müller | 6d5f86ff77 | |
Robin Müller | e77e403e38 | |
Robin Müller | 6609ac85e3 | |
Robin Müller | e97970ef64 | |
Marius Eggert | af6acb035c | |
Marius Eggert | dbb5d6d359 | |
Marius Eggert | efd17a971f | |
Marius Eggert | 557051ba37 | |
Marius Eggert | 21ef879cae | |
Marius Eggert | e9a665ee90 | |
Marius Eggert | a2a360c1d7 | |
Robin Müller | d43d1c4f65 | |
Robin Müller | b8a07a3299 | |
Robin Müller | dc730bb6de | |
Marius Eggert | 61555aa5cf | |
Marius Eggert | a7b05f60a7 | |
Marius Eggert | 8749e4cf6e | |
Marius Eggert | 97a54c6691 | |
Marius Eggert | 2d0f13b6e6 | |
Marius Eggert | 352053f1d2 | |
Marius Eggert | f382bd3e61 | |
Marius Eggert | b987b6e70c | |
Marius Eggert | 4cf34cb99a | |
Robin Müller | 2ac9f972da | |
Robin Müller | 69f4b6de06 | |
Robin Müller | c434882e37 | |
Marius Eggert | 9e89482298 | |
Marius Eggert | 9e1e286b97 | |
Marius Eggert | 9c73e89cbb | |
Marius Eggert | 386430b9f2 | |
Marius Eggert | ab71014f67 | |
Marius Eggert | fc40c5cc83 | |
Marius Eggert | 21ecbaaa7b | |
Marius Eggert | 4c589e0c1b | |
Marius Eggert | f4d1dbb5df | |
Marius Eggert | 18a7705d97 | |
Marius Eggert | 3aa3810cac | |
Marius Eggert | 2965c63bf6 | |
Marius Eggert | 7066939d76 | |
Marius Eggert | c04be384a1 | |
Marius Eggert | 64f6f92ce6 | |
Marius Eggert | a1570e94e7 | |
Marius Eggert | 962df7e86f | |
Marius Eggert | f11240deb4 | |
Marius Eggert | 8df720e940 | |
Marius Eggert | 3c389cb069 | |
Marius Eggert | b2faa77ebe | |
Marius Eggert | 96acca4847 | |
Marius Eggert | d14b83fab9 | |
Marius Eggert | 7517b33d83 | |
Marius Eggert | 3313f58905 | |
Marius Eggert | 9adb4af0e5 | |
Marius Eggert | 13aa6f50ff | |
Marius Eggert | 585b8043e6 | |
Marius Eggert | 688943cacb | |
Marius Eggert | a8172e641f | |
Marius Eggert | 2e08f3b393 | |
Marius Eggert | ff3bf4e291 | |
Marius Eggert | 4d5ac727a8 | |
Marius Eggert | 2f43f58792 | |
Marius Eggert | 75654277b2 | |
Marius Eggert | 58a8c5869c | |
Marius Eggert | 5e6b0f5ea2 | |
Marius Eggert | e445f8942a | |
Robin Müller | 1fd6db8138 | |
Robin Müller | 2d92368240 | |
Robin Müller | 9a1d91c261 | |
Robin Müller | dbb530e27b | |
Marius Eggert | 026776c1ec | |
Marius Eggert | 7d4b97d977 | |
Robin Müller | 77527c631c | |
Robin Müller | 43cca9ed0c | |
Marius Eggert | 2b09ec8bc0 | |
Marius Eggert | 9124cb85fa | |
Marius Eggert | 124ae6cc7e | |
Marius Eggert | b9e4c51d82 | |
Marius Eggert | f60fe1ed02 | |
Marius Eggert | 79a659bc39 | |
Marius Eggert | efc0be104c | |
Marius Eggert | 7314c71062 | |
Marius Eggert | 9d6206302a | |
Marius Eggert | d467953c18 | |
Marius Eggert | cf7fbcef97 | |
Marius Eggert | 16255a91dc | |
Marius Eggert | d32f7b31c3 | |
Marius Eggert | 236ca64de3 | |
Marius Eggert | b68bbe64a3 | |
Marius Eggert | f56d1c2b12 | |
Marius Eggert | c064d1f625 | |
Marius Eggert | 3954d4dabe | |
Marius Eggert | 16d909bd5b | |
Marius Eggert | 1b2dd328ca | |
Marius Eggert | 2346866ba2 | |
Marius Eggert | 779bb4a277 | |
Robin Müller | a562a45445 | |
Robin Müller | b5a7db4e66 | |
Robin Müller | b3785628a7 | |
Robin Müller | 36658d07f0 | |
Robin Müller | 617b956ac9 | |
Marius Eggert | ff76f99ad9 | |
Marius Eggert | 13c180902d | |
Marius Eggert | 4929cad198 | |
Marius Eggert | 393d50a54f | |
Marius Eggert | 3399620ba3 | |
Marius Eggert | b7f62ac3e2 | |
Marius Eggert | 6cbd873a97 | |
Marius Eggert | 27c824eaec | |
Marius Eggert | b4ace906bc | |
Marius Eggert | 681aebe011 | |
Marius Eggert | 484dba7eb6 | |
Marius Eggert | 5ae97d7c09 | |
Marius Eggert | 01fde7193d | |
Marius Eggert | db9afe4a62 | |
Marius Eggert | ecc147ca7f | |
Marius Eggert | 6ca1dda807 | |
Marius Eggert | 6f3876d204 | |
Marius Eggert | f0e551fa54 | |
Marius Eggert | 15cddfdeb7 | |
Marius Eggert | e7dc9cddfd | |
Marius Eggert | 84ba6262a8 | |
Marius Eggert | 8be94cf2dc | |
Robin Müller | cc47114891 | |
Robin Müller | 0eebc4b00f | |
Robin Müller | 6da0c8fe1a | |
Robin Müller | 2c9e857060 | |
Marius Eggert | c85bef6de4 | |
Marius Eggert | 8b9c0d3abf | |
Marius Eggert | 4d22f7f889 | |
Marius Eggert | aa521e89f6 | |
Marius Eggert | 467ab39ad9 | |
Marius Eggert | 0d80e2a8ec | |
Marius Eggert | 3cfde3071c | |
Marius Eggert | 7dc69d3473 | |
Marius Eggert | e3f7cda69a | |
Marius Eggert | ef4d3066e9 | |
Marius Eggert | 07e4a6bc5a | |
Marius Eggert | da5890f99a | |
Marius Eggert | 8b91c91a7a | |
Marius Eggert | e301b19aba | |
Marius Eggert | ac06947078 | |
Marius Eggert | 84401cc427 | |
Marius Eggert | 20920fe22f | |
Marius Eggert | 83fc8a633e | |
Marius Eggert | e17ef1bcfd | |
Marius Eggert | e3ad08d987 | |
Marius Eggert | a4d514fbb5 | |
Marius Eggert | 8390a02690 | |
Marius Eggert | d065f6257e | |
Marius Eggert | 96b74574b0 | |
Marius Eggert | 64d105cf87 | |
Marius Eggert | f7c997980c | |
Marius Eggert | 0064cf13cb | |
Marius Eggert | 62c11798e5 | |
Marius Eggert | 79efca3a66 | |
Marius Eggert | 3cc6fce575 | |
Robin Müller | 5c7f712bf2 | |
Marius Eggert | 72b937f223 | |
Robin Müller | c4d0203846 | |
Robin Müller | 16fa3d1e26 | |
Robin Müller | 5ec173f8c9 | |
Marius Eggert | 8dbbb7b9ec | |
Marius Eggert | 87a22abf24 | |
Marius Eggert | 68b0e3b20a | |
Marius Eggert | 01735d79f7 | |
Marius Eggert | 0a7454029d | |
Marius Eggert | c7ac5904f8 | |
Marius Eggert | bec57f98c0 | |
Marius Eggert | 3441365c65 | |
Marius Eggert | c67f65369c | |
Marius Eggert | ba6eac505e | |
Marius Eggert | ec2b026103 | |
Marius Eggert | 060217fbb4 | |
Marius Eggert | 0d7f5d5dca | |
Marius Eggert | 398ddd1a3f | |
Marius Eggert | c4297975ff | |
Marius Eggert | 772e8f1149 | |
Marius Eggert | 315365921e | |
Marius Eggert | 6cf746463b | |
Marius Eggert | 5f388d53a6 | |
Marius Eggert | 26f69d611e | |
Marius Eggert | ec3523e5ad | |
Marius Eggert | 2f296c3c8c | |
Marius Eggert | 588612875d | |
Marius Eggert | 944dfb6f81 | |
Marius Eggert | 291c9ea99b | |
Marius Eggert | c1e7ac7e9a | |
Marius Eggert | 7daeb9a148 | |
Marius Eggert | 10841a01b7 | |
Marius Eggert | 42b94ab581 | |
Marius Eggert | ff5e1cd76b | |
Marius Eggert | 3892276a5c | |
Marius Eggert | edf2f889c1 | |
Marius Eggert | 6c90777e4b | |
Robin Müller | a778daacfd | |
Robin Müller | e4f8f20d78 | |
Robin Müller | 5df5c30e30 | |
Robin Müller | 928dd9e2e0 | |
Robin Müller | 05b88dd294 | |
Robin Müller | 3c3babdd4b | |
Marius Eggert | 82dd505194 | |
Marius Eggert | 54187e47c1 | |
Marius Eggert | 39b9f770ac | |
Marius Eggert | a71c12c9ce | |
Marius Eggert | 4fcb7fc8c6 | |
Marius Eggert | 65502c0107 | |
Marius Eggert | 392a97bb65 | |
Marius Eggert | 367e879d91 | |
Marius Eggert | ef730022a0 | |
Marius Eggert | d8ae45b352 | |
Marius Eggert | 4e8776ff68 | |
Marius Eggert | 883ff4f6de | |
Marius Eggert | 13f3739386 | |
Marius Eggert | f4ed981003 | |
Robin Müller | 0ae8b4e85e | |
Robin Müller | 561fe5aa3c | |
Robin Müller | b000f77f4b |
186
CHANGELOG.md
186
CHANGELOG.md
|
@ -16,6 +16,192 @@ will consitute of a breaking change warranting a new major release:
|
|||
|
||||
# [unreleased]
|
||||
|
||||
# [v7.8.1] 2024-04-11
|
||||
|
||||
- Reverted fix for wrong order in quaternion multiplication for computation of the error quaternion.
|
||||
|
||||
# [v7.8.0] 2024-04-10
|
||||
|
||||
## Changed
|
||||
|
||||
- Reverted lower OP limit of `PLOC` to -10°C.
|
||||
- All pointing laws are now allowed to use the `MEKF` per default.
|
||||
- Changed limits in `PWR Controller`.
|
||||
- PUS time service: Now dumps the time before and after relative timeshift or setting absolute time
|
||||
- The `GPS Controller` does not set itself to `OFF` anymore, if it has not detected a valid fix for
|
||||
some time. Instead it attempts to reset both GNSS devices once.
|
||||
- The maximum time to reach a fix is shortened from 30min to 15min.
|
||||
- The time the reset pin of the GNSS devices is pulled is prolonged from 5ms to 10s.
|
||||
- A `GPS FIX HAS CHANGED` is now only triggered if no fix change has been detected within the past
|
||||
2min. This means, this event might be thrown with a 2min delay. It is instantly thrown, if the mode
|
||||
of the controller is changed. As arguments it now displays the new fix and the numer of fix changes
|
||||
missed.
|
||||
- The number of satellites seen and used is reset to 0, in case they are set to invalid.
|
||||
- Altitude, latitude and longitude messages are not checked anymore, in case the mode message was
|
||||
already invalid.
|
||||
|
||||
## Added
|
||||
|
||||
- PUS timeservice relative timeshift.
|
||||
|
||||
## Fixed
|
||||
|
||||
- Fixed wrong order in quaternion multiplication for computation of the error quaternion.
|
||||
- Re-worked some FDIR logic in the FSFW. The former logic prevented events with a severity
|
||||
higher than INFO if the device was in EXTERNAL CONTROL. The new logic will allow to trigger
|
||||
events but still inhibit FDIR reactions if the device is in EXTERNAL CONTROL.
|
||||
|
||||
# [v7.7.4] 2024-03-21
|
||||
|
||||
## Changed
|
||||
|
||||
- Rotational rate limit for the GS target pointing is now seperated from controller limit. It
|
||||
is also reduced to 0.75°/s now.
|
||||
|
||||
## Fixed
|
||||
|
||||
- Fixed wrong sign in calculation of total current within the `PWR Controller`.
|
||||
|
||||
# [v7.7.3] 2024-03-18
|
||||
|
||||
- Bumped `eive-fsfw`
|
||||
|
||||
## Added
|
||||
|
||||
- Added parameter to disable STR input for MEKF.
|
||||
|
||||
## Changed
|
||||
|
||||
- If a primary heater is set to `EXTERNAL CONTROL` and `ON`, the `TCS Controller` will no
|
||||
try to control the temperature of that object.
|
||||
- Set lower OP limit of `PLOC` to -5°C.
|
||||
|
||||
## Fixed
|
||||
|
||||
- Added prevention of sign jump for target quaternion of GS pointing, which would reduce the
|
||||
performance of the controller.
|
||||
- Heaters set to `EXTERNAL CONTROL` no longer can be switched off by the `TCS Controller`, if
|
||||
they violate the maximum burn duration of the controller.
|
||||
|
||||
# [v7.7.2] 2024-03-06
|
||||
|
||||
## Fixed
|
||||
|
||||
- Camera and E-band antenna now point towards the target instead of away from the target for the
|
||||
pointing target mode.
|
||||
|
||||
# [v7.7.1] 2024-03-06
|
||||
|
||||
- Bumped `eive-tmtc` to v6.1.1
|
||||
- Bumped `eive-fsfw`
|
||||
|
||||
## Added
|
||||
|
||||
- The `CoreController` now sets the leap seconds on initalization. They are stored in a persistent
|
||||
file. If the file does yet not exist, it will be created. The leap seconds can be updated using an
|
||||
action command. This will also update the file.
|
||||
|
||||
## Fixed
|
||||
|
||||
- Fixed wrong dimension of a matrix within the `MEKF`, which would lead to a seg fault, if the
|
||||
star tracker was available.
|
||||
- Fixed case in which control values within the `AcsController` could become NaN.
|
||||
|
||||
# [v7.7.0] 2024-02-29
|
||||
|
||||
- Bumped `eive-tmtc` to v6.1.0
|
||||
- Bumped `eive-fsfw`
|
||||
|
||||
## Fixed
|
||||
|
||||
- PLOC SUPV sets: Added missing `PoolReadGuard` instantiations when reading boot status report
|
||||
and latchup status report.
|
||||
- PLOC SUPV latchup report could not be handled previously.
|
||||
- Bugfix in PLOC SUPV latchup report parsing.
|
||||
- Bugfix in PLOC MPSoC HK set: Set and variables were not set valid.
|
||||
- The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode
|
||||
and is triggered by the `AcsController` now.
|
||||
- Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`.
|
||||
- Fixed calculation of desaturation torque for faulty RWs.
|
||||
- Fixed bugs within the `MEKF` and simplified the code.
|
||||
|
||||
## Changed
|
||||
|
||||
- `FusedRotationRate` now only uses rotation rate from QUEST and STR in higher modes
|
||||
- QUEST and STR rates are now allowed per default
|
||||
- Changed PTG Strat priorities to favor STR before MEKF.
|
||||
- Increased message queue depth and maximum number of handled messages per cycle for
|
||||
`PusServiceBase` based classes (especially PUS scheduler).
|
||||
- `MathOperations` functions were moved to their appropriate classes within the `eive-fsfw`
|
||||
- Changed pointing strategy for target groundstation mode to prevent blinding of the STR. This
|
||||
also limits the rotation for the reference target quaternion to prevent spikes in required
|
||||
rotation rates.
|
||||
- Updated QUEST and Sun Vector Params to new values.
|
||||
- Removed the satellites's angular momentum from desaturation calculation.
|
||||
- Bumped internal `sagittactl` library to v11.11.
|
||||
|
||||
## Added
|
||||
|
||||
- Updated STR handler to unlock and allow using the secondary firmware slot.
|
||||
- STR handling for new BlobStats TM set.
|
||||
- Added new action command to update the standard deviations within the `MEKF` from the
|
||||
`AcsParameters`.
|
||||
|
||||
# [v7.6.1] 2024-02-05
|
||||
|
||||
## Changed
|
||||
|
||||
- Guidance now uses the coordinate functions from the FSFW.
|
||||
- Idle should now point the STR away from the earth
|
||||
|
||||
## Fixed
|
||||
|
||||
- Fixed bugs in `Guidance::comparePtg` and corrected overloading
|
||||
- Detumbling State Machine is now robust to commanded mode changes.
|
||||
|
||||
# [v7.6.0] 2024-01-30
|
||||
|
||||
- Bumped `eive-tmtc` to v5.13.0
|
||||
- Bumped `eive-fsfw`
|
||||
|
||||
## Added
|
||||
|
||||
- Added new parameter for MPSoC which allows to skip SUPV commanding.
|
||||
|
||||
## Changed
|
||||
|
||||
- Increased allowed mode transition time for PLOC SUPV.
|
||||
- Detumbling can now be triggered from all modes of the `AcsController`. In case the
|
||||
current mode is a higher pointing mode, the STR will be set to faulty, to trigger a
|
||||
transition to safe first. Then, in a second step, a transition to detumble is triggered.
|
||||
|
||||
## Fixed
|
||||
|
||||
- If the PCDU handler fails reading data from the IPC store, it will
|
||||
not try to do a deserialization anymore.
|
||||
- All action commands sent by the PLOC SUPV to itself will have no sender now.
|
||||
- RW speed commands get reset to 0 RPM, if the `AcsController` has changed its mode
|
||||
to Safe
|
||||
- Antistiction for RWs will set commanded speed to 0 RPM, if a wheel is detected as not
|
||||
working
|
||||
- Removed parameter to disable antistiction, as deactivating it would result in the
|
||||
`AcsController` being allowed sending invalid speed commands to the RW Handler, which
|
||||
would then trigger FDIR and turning off the functioning device
|
||||
- `RwHandler` returnvalues would use the `INTERFACE_ID` of the `DeviceHandlerBase`
|
||||
- The `AcsController` will reset its stored guidance values on mode change and lost
|
||||
orientation.
|
||||
- The nullspace controller will only be used if all RWs are available.
|
||||
- Calculation of required rotation rate in pointing modes has been fixed to actual
|
||||
calculation of rotation rate from two quaternions.
|
||||
- Fixed alignment matrix and pseudo inverses of RWs, to match the wrong definition of
|
||||
positive rotation.
|
||||
|
||||
# [v7.5.5] 2024-01-22
|
||||
|
||||
## Fixed
|
||||
|
||||
- Calculation of error quaternion was done with inverse of the required target quaternion.
|
||||
|
||||
# [v7.5.4] 2024-01-16
|
||||
|
||||
## Fixed
|
||||
|
|
|
@ -10,8 +10,8 @@
|
|||
cmake_minimum_required(VERSION 3.13)
|
||||
|
||||
set(OBSW_VERSION_MAJOR 7)
|
||||
set(OBSW_VERSION_MINOR 5)
|
||||
set(OBSW_VERSION_REVISION 4)
|
||||
set(OBSW_VERSION_MINOR 8)
|
||||
set(OBSW_VERSION_REVISION 1)
|
||||
|
||||
# set(CMAKE_VERBOSE TRUE)
|
||||
|
||||
|
|
|
@ -1,7 +1,7 @@
|
|||
/**
|
||||
* @brief Auto-generated event translation file. Contains 318 translations.
|
||||
* @brief Auto-generated event translation file. Contains 324 translations.
|
||||
* @details
|
||||
* Generated on: 2023-12-13 11:29:45
|
||||
* Generated on: 2024-04-10 11:49:35
|
||||
*/
|
||||
#include "translateEvents.h"
|
||||
|
||||
|
@ -82,8 +82,11 @@ const char *BIT_LOCK_STRING = "BIT_LOCK";
|
|||
const char *BIT_LOCK_LOST_STRING = "BIT_LOCK_LOST";
|
||||
const char *FRAME_PROCESSING_FAILED_STRING = "FRAME_PROCESSING_FAILED";
|
||||
const char *CLOCK_SET_STRING = "CLOCK_SET";
|
||||
const char *CLOCK_DUMP_STRING = "CLOCK_DUMP";
|
||||
const char *CLOCK_DUMP_LEGACY_STRING = "CLOCK_DUMP_LEGACY";
|
||||
const char *CLOCK_SET_FAILURE_STRING = "CLOCK_SET_FAILURE";
|
||||
const char *CLOCK_DUMP_STRING = "CLOCK_DUMP";
|
||||
const char *CLOCK_DUMP_BEFORE_SETTING_TIME_STRING = "CLOCK_DUMP_BEFORE_SETTING_TIME";
|
||||
const char *CLOCK_DUMP_AFTER_SETTING_TIME_STRING = "CLOCK_DUMP_AFTER_SETTING_TIME";
|
||||
const char *TC_DELETION_FAILED_STRING = "TC_DELETION_FAILED";
|
||||
const char *TEST_STRING = "TEST";
|
||||
const char *CHANGE_OF_SETUP_PARAMETER_STRING = "CHANGE_OF_SETUP_PARAMETER";
|
||||
|
@ -94,7 +97,7 @@ const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR";
|
|||
const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR";
|
||||
const char *HANDLING_CFDP_REQUEST_FAILED_STRING = "HANDLING_CFDP_REQUEST_FAILED";
|
||||
const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
|
||||
const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
|
||||
const char *RATE_RECOVERY_STRING = "RATE_RECOVERY";
|
||||
const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
|
||||
const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
|
||||
const char *MEKF_RECOVERY_STRING = "MEKF_RECOVERY";
|
||||
|
@ -103,6 +106,8 @@ const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFO
|
|||
const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE";
|
||||
const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD";
|
||||
const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED";
|
||||
const char *PTG_RATE_VIOLATION_STRING = "PTG_RATE_VIOLATION";
|
||||
const char *DETUMBLE_TRANSITION_FAILED_STRING = "DETUMBLE_TRANSITION_FAILED";
|
||||
const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
|
||||
const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
|
||||
const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
|
||||
|
@ -239,6 +244,7 @@ const char *SIDE_SWITCH_TRANSITION_NOT_ALLOWED_12903_STRING = "SIDE_SWITCH_TRANS
|
|||
const char *CHILDREN_LOST_MODE_STRING = "CHILDREN_LOST_MODE";
|
||||
const char *GPS_FIX_CHANGE_STRING = "GPS_FIX_CHANGE";
|
||||
const char *CANT_GET_FIX_STRING = "CANT_GET_FIX";
|
||||
const char *RESET_FAIL_STRING = "RESET_FAIL";
|
||||
const char *P60_BOOT_COUNT_STRING = "P60_BOOT_COUNT";
|
||||
const char *BATT_MODE_STRING = "BATT_MODE";
|
||||
const char *BATT_MODE_CHANGED_STRING = "BATT_MODE_CHANGED";
|
||||
|
@ -481,9 +487,15 @@ const char *translateEvents(Event event) {
|
|||
case (8900):
|
||||
return CLOCK_SET_STRING;
|
||||
case (8901):
|
||||
return CLOCK_DUMP_STRING;
|
||||
return CLOCK_DUMP_LEGACY_STRING;
|
||||
case (8902):
|
||||
return CLOCK_SET_FAILURE_STRING;
|
||||
case (8903):
|
||||
return CLOCK_DUMP_STRING;
|
||||
case (8904):
|
||||
return CLOCK_DUMP_BEFORE_SETTING_TIME_STRING;
|
||||
case (8905):
|
||||
return CLOCK_DUMP_AFTER_SETTING_TIME_STRING;
|
||||
case (9100):
|
||||
return TC_DELETION_FAILED_STRING;
|
||||
case (9700):
|
||||
|
@ -505,7 +517,7 @@ const char *translateEvents(Event event) {
|
|||
case (11200):
|
||||
return SAFE_RATE_VIOLATION_STRING;
|
||||
case (11201):
|
||||
return SAFE_RATE_RECOVERY_STRING;
|
||||
return RATE_RECOVERY_STRING;
|
||||
case (11202):
|
||||
return MULTIPLE_RW_INVALID_STRING;
|
||||
case (11203):
|
||||
|
@ -522,6 +534,10 @@ const char *translateEvents(Event event) {
|
|||
return TLE_TOO_OLD_STRING;
|
||||
case (11209):
|
||||
return TLE_FILE_READ_FAILED_STRING;
|
||||
case (11210):
|
||||
return PTG_RATE_VIOLATION_STRING;
|
||||
case (11211):
|
||||
return DETUMBLE_TRANSITION_FAILED_STRING;
|
||||
case (11300):
|
||||
return SWITCH_CMD_SENT_STRING;
|
||||
case (11301):
|
||||
|
@ -794,6 +810,8 @@ const char *translateEvents(Event event) {
|
|||
return GPS_FIX_CHANGE_STRING;
|
||||
case (13101):
|
||||
return CANT_GET_FIX_STRING;
|
||||
case (13102):
|
||||
return RESET_FAIL_STRING;
|
||||
case (13200):
|
||||
return P60_BOOT_COUNT_STRING;
|
||||
case (13201):
|
||||
|
|
|
@ -2,7 +2,7 @@
|
|||
* @brief Auto-generated object translation file.
|
||||
* @details
|
||||
* Contains 175 translations.
|
||||
* Generated on: 2023-12-13 11:29:45
|
||||
* Generated on: 2024-04-10 11:49:35
|
||||
*/
|
||||
#include "translateObjects.h"
|
||||
|
||||
|
|
|
@ -480,6 +480,16 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_
|
|||
successRecipient = commandedBy;
|
||||
return returnvalue::OK;
|
||||
}
|
||||
case (UPDATE_LEAP_SECONDS): {
|
||||
if (size != sizeof(uint16_t)) {
|
||||
return HasActionsIF::INVALID_PARAMETERS;
|
||||
}
|
||||
ReturnValue_t result = actionUpdateLeapSeconds(data);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
return HasActionsIF::EXECUTION_FINISHED;
|
||||
}
|
||||
default: {
|
||||
return HasActionsIF::INVALID_ACTION_ID;
|
||||
}
|
||||
|
@ -1411,6 +1421,9 @@ void CoreController::performMountedSdCardOperations() {
|
|||
if (not timeFileInitDone) {
|
||||
initClockFromTimeFile();
|
||||
}
|
||||
if (not leapSecondsInitDone) {
|
||||
initLeapSeconds();
|
||||
}
|
||||
performRebootWatchdogHandling(false);
|
||||
performRebootCountersHandling(false);
|
||||
}
|
||||
|
@ -2066,14 +2079,78 @@ ReturnValue_t CoreController::backupTimeFileHandler() {
|
|||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
void CoreController::initLeapSeconds() {
|
||||
ReturnValue_t result = initLeapSecondsFromFile();
|
||||
if (result != returnvalue::OK) {
|
||||
Clock::setLeapSeconds(config::LEAP_SECONDS);
|
||||
writeLeapSecondsToFile(config::LEAP_SECONDS);
|
||||
}
|
||||
leapSecondsInitDone = true;
|
||||
}
|
||||
|
||||
ReturnValue_t CoreController::initLeapSecondsFromFile() {
|
||||
std::string fileName = currMntPrefix + LEAP_SECONDS_FILE;
|
||||
std::error_code e;
|
||||
if (sdcMan->isSdCardUsable(std::nullopt) and std::filesystem::exists(fileName, e)) {
|
||||
std::ifstream leapSecondsFile(fileName);
|
||||
std::string nextWord;
|
||||
std::getline(leapSecondsFile, nextWord);
|
||||
std::istringstream iss(nextWord);
|
||||
iss >> nextWord;
|
||||
if (iss.bad() or nextWord != "LEAP") {
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
iss >> nextWord;
|
||||
if (iss.bad() or nextWord != "SECONDS:") {
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
iss >> nextWord;
|
||||
uint16_t leapSeconds = 0;
|
||||
leapSeconds = std::stoi(nextWord.c_str());
|
||||
if (iss.bad()) {
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
Clock::setLeapSeconds(leapSeconds);
|
||||
return returnvalue::OK;
|
||||
}
|
||||
sif::error
|
||||
<< "CoreController::leapSecondsFileHandler: Initalization of leap seconds from file failed"
|
||||
<< std::endl;
|
||||
return returnvalue::FAILED;
|
||||
};
|
||||
|
||||
ReturnValue_t CoreController::writeLeapSecondsToFile(const uint16_t leapSeconds) {
|
||||
std::string fileName = currMntPrefix + LEAP_SECONDS_FILE;
|
||||
if (not sdcMan->isSdCardUsable(std::nullopt)) {
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
std::ofstream leapSecondsFile(fileName.c_str(), std::ofstream::out | std::ofstream::trunc);
|
||||
if (not leapSecondsFile.good()) {
|
||||
sif::error << "CoreController::leapSecondsFileHandler: Error opening leap seconds file: "
|
||||
<< strerror(errno) << std::endl;
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
leapSecondsFile << "LEAP SECONDS: " << leapSeconds << std::endl;
|
||||
return returnvalue::OK;
|
||||
};
|
||||
|
||||
ReturnValue_t CoreController::actionUpdateLeapSeconds(const uint8_t *data) {
|
||||
uint16_t leapSeconds = data[1] | (data[0] << 8);
|
||||
ReturnValue_t result = writeLeapSecondsToFile(leapSeconds);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
Clock::setLeapSeconds(leapSeconds);
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
ReturnValue_t CoreController::initClockFromTimeFile() {
|
||||
using namespace GpsHyperion;
|
||||
using namespace std;
|
||||
std::string fileName = currMntPrefix + BACKUP_TIME_FILE;
|
||||
std::error_code e;
|
||||
if (sdcMan->isSdCardUsable(std::nullopt) and std::filesystem::exists(fileName, e) and
|
||||
((gpsFix == FixMode::UNKNOWN or gpsFix == FixMode::NOT_SEEN) or
|
||||
not utility::timeSanityCheck())) {
|
||||
((gpsFix == FixMode::NOT_SEEN) or not utility::timeSanityCheck())) {
|
||||
ifstream timeFile(fileName);
|
||||
string nextWord;
|
||||
getline(timeFile, nextWord);
|
||||
|
|
|
@ -9,7 +9,7 @@
|
|||
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
|
||||
#include <fsfw_hal/linux/uio/UioMapper.h>
|
||||
#include <libxiphos.h>
|
||||
#include <mission/acs/archive/GPSDefinitions.h>
|
||||
#include <linux/acs/GPSDefinitions.h>
|
||||
#include <mission/utility/trace.h>
|
||||
|
||||
#include <atomic>
|
||||
|
@ -150,6 +150,8 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
|
|||
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 LEAP_SECONDS_FILE =
|
||||
"/" + std::string(core::CONF_FOLDER) + "/" + std::string(core::LEAP_SECONDS_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 =
|
||||
|
@ -209,7 +211,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
|
|||
static constexpr MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING;
|
||||
static constexpr uint32_t MUTEX_TIMEOUT = 20;
|
||||
bool enableHkSet = false;
|
||||
GpsHyperion::FixMode gpsFix = GpsHyperion::FixMode::UNKNOWN;
|
||||
GpsHyperion::FixMode gpsFix = GpsHyperion::FixMode::NOT_SEEN;
|
||||
|
||||
// States for SD state machine, which is used in non-blocking mode
|
||||
enum class SdStates {
|
||||
|
@ -296,6 +298,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
|
|||
|
||||
std::string currMntPrefix;
|
||||
bool timeFileInitDone = false;
|
||||
bool leapSecondsInitDone = false;
|
||||
bool performOneShotSdCardOpsSwitch = false;
|
||||
uint8_t shortSdCardCdCounter = 0;
|
||||
#if OBSW_THREAD_TRACING == 1
|
||||
|
@ -335,7 +338,11 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
|
|||
void performMountedSdCardOperations();
|
||||
ReturnValue_t initVersionFile();
|
||||
|
||||
void initLeapSeconds();
|
||||
ReturnValue_t initLeapSecondsFromFile();
|
||||
ReturnValue_t initClockFromTimeFile();
|
||||
ReturnValue_t actionUpdateLeapSeconds(const uint8_t* data);
|
||||
ReturnValue_t writeLeapSecondsToFile(const uint16_t leapSeconds);
|
||||
ReturnValue_t performSdCardCheck();
|
||||
ReturnValue_t backupTimeFileHandler();
|
||||
ReturnValue_t initBootCopyFile();
|
||||
|
|
|
@ -67,7 +67,7 @@
|
|||
#include "mission/system/acs/acsModeTree.h"
|
||||
#include "mission/system/com/SyrlinksFdir.h"
|
||||
#include "mission/system/com/comModeTree.h"
|
||||
#include "mission/system/payloadModeTree.h"
|
||||
#include "mission/system/payload/payloadModeTree.h"
|
||||
#include "mission/system/power/GomspacePowerFdir.h"
|
||||
#include "mission/system/tcs/RtdFdir.h"
|
||||
#include "mission/system/tcs/TcsBoardAssembly.h"
|
||||
|
@ -510,7 +510,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF*
|
|||
debugGps = true;
|
||||
#endif
|
||||
RESET_ARGS_GNSS.gpioComIF = gpioComIF;
|
||||
RESET_ARGS_GNSS.waitPeriodMs = 5;
|
||||
RESET_ARGS_GNSS.waitPeriodMs = 10 * 1e3;
|
||||
auto gpsCtrl = new GpsHyperionLinuxController(objects::GPS_CONTROLLER, objects::NO_OBJECT,
|
||||
enableHkSets, debugGps);
|
||||
gpsCtrl->setResetPinTriggerFunction(gps::triggerGpioResetPin, &RESET_ARGS_GNSS);
|
||||
|
@ -951,7 +951,7 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher, SdCardManage
|
|||
auto cfgGetter = new StrConfigPathGetter(sdcMan);
|
||||
auto starTracker =
|
||||
new StarTrackerHandler(objects::STAR_TRACKER, objects::STR_COM_IF, starTrackerCookie,
|
||||
strComIF, power::PDU1_CH2_STAR_TRACKER_5V, *cfgGetter);
|
||||
strComIF, power::PDU1_CH2_STAR_TRACKER_5V, *cfgGetter, sdcMan);
|
||||
starTracker->setPowerSwitcher(pwrSwitcher);
|
||||
starTracker->connectModeTreeParent(*strAssy);
|
||||
starTracker->setCustomFdir(strFdir);
|
||||
|
|
|
@ -20,6 +20,9 @@ static constexpr char OBSW_VERSION_FILE_PATH[] = "/usr/share/eive-obsw/obsw_vers
|
|||
// ISO8601 timestamp.
|
||||
static constexpr char FILE_DATE_FORMAT[] = "%FT%H%M%SZ";
|
||||
|
||||
// Leap Seconds as of 2024-03-04
|
||||
static constexpr uint16_t LEAP_SECONDS = 37;
|
||||
|
||||
static constexpr uint16_t EIVE_PUS_APID = 0x65;
|
||||
static constexpr uint16_t EIVE_CFDP_APID = 0x66;
|
||||
static constexpr uint16_t EIVE_LOCAL_CFDP_ENTITY_ID = EIVE_CFDP_APID;
|
||||
|
|
|
@ -2,7 +2,7 @@
|
|||
#define DUMMIES_GPSCTRLDUMMY_H_
|
||||
|
||||
#include <fsfw/controller/ExtendedControllerBase.h>
|
||||
#include <mission/acs/archive/GPSDefinitions.h>
|
||||
#include <linux/acs/GPSDefinitions.h>
|
||||
|
||||
class GpsCtrlDummy : public ExtendedControllerBase {
|
||||
public:
|
||||
|
|
|
@ -1,5 +1,5 @@
|
|||
#include <dummies/GpsDhbDummy.h>
|
||||
#include <mission/acs/archive/GPSDefinitions.h>
|
||||
#include <linux/acs/GPSDefinitions.h>
|
||||
|
||||
GpsDhbDummy::GpsDhbDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
|
||||
: DeviceHandlerBase(objectId, comif, comCookie) {}
|
||||
|
|
|
@ -40,7 +40,7 @@
|
|||
#include "mission/genericFactory.h"
|
||||
#include "mission/system/acs/acsModeTree.h"
|
||||
#include "mission/system/com/comModeTree.h"
|
||||
#include "mission/system/payloadModeTree.h"
|
||||
#include "mission/system/payload/payloadModeTree.h"
|
||||
#include "mission/system/tcs/tcsModeTree.h"
|
||||
#include "mission/tcs/defs.h"
|
||||
|
||||
|
|
2
fsfw
2
fsfw
|
@ -1 +1 @@
|
|||
Subproject commit e64e8b274d436502d5c5b87865b9006e52e4b1aa
|
||||
Subproject commit 8b21dd276de72b46b41bc7080d6646180bfa0662
|
|
@ -75,9 +75,12 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
|
|||
7902;0x1ede;BIT_LOCK;INFO;A Bit Lock signal. Was detected. P1: raw BLO state, P2: 0;fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
7903;0x1edf;BIT_LOCK_LOST;INFO;A previously found Bit Lock signal was lost. P1: raw BLO state, P2: 0;fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
7905;0x1ee1;FRAME_PROCESSING_FAILED;LOW;The CCSDS Board could not interpret a TC;fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
8900;0x22c4;CLOCK_SET;INFO;No description;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8901;0x22c5;CLOCK_DUMP;INFO;No description;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8902;0x22c6;CLOCK_SET_FAILURE;LOW;No description;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8900;0x22c4;CLOCK_SET;INFO;Clock has been set. P1: old timeval seconds. P2: new timeval seconds.;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8901;0x22c5;CLOCK_DUMP_LEGACY;INFO;Clock dump event. P1: timeval seconds P2: timeval milliseconds.;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8902;0x22c6;CLOCK_SET_FAILURE;LOW;Clock could not be set. P1: Returncode.;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8903;0x22c7;CLOCK_DUMP;INFO;Clock dump event. P1: timeval seconds P2: timeval microseconds.;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8904;0x22c8;CLOCK_DUMP_BEFORE_SETTING_TIME;INFO;No description;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8905;0x22c9;CLOCK_DUMP_AFTER_SETTING_TIME;INFO;No description;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
9100;0x238c;TC_DELETION_FAILED;MEDIUM;Deletion of a TC from the map failed. P1: First 32 bit of request ID, P2. Last 32 bit of Request ID;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h
|
||||
9700;0x25e4;TEST;INFO;No description;fsfw/src/fsfw/pus/Service17Test.h
|
||||
10600;0x2968;CHANGE_OF_SETUP_PARAMETER;LOW;No description;fsfw/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h
|
||||
|
@ -88,7 +91,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
|
|||
10804;0x2a34;FILENAME_TOO_LARGE_ERROR;LOW;P1: Transaction step ID, P2: 0 for source file name, 1 for dest file name;fsfw/src/fsfw/cfdp/handler/defs.h
|
||||
10805;0x2a35;HANDLING_CFDP_REQUEST_FAILED;LOW;CFDP request handling failed. P2: Returncode.;fsfw/src/fsfw/cfdp/handler/defs.h
|
||||
11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;The limits for the rotation in safe mode were violated.;mission/acs/defs.h
|
||||
11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;The system has recovered from a safe rate rotation violation.;mission/acs/defs.h
|
||||
11201;0x2bc1;RATE_RECOVERY;MEDIUM;The system has recovered from a rate rotation violation.;mission/acs/defs.h
|
||||
11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;Multiple RWs are invalid, uncommandable and therefore higher ACS modes cannot be maintained.;mission/acs/defs.h
|
||||
11203;0x2bc3;MEKF_INVALID_INFO;INFO;MEKF was not able to compute a solution. P1: MEKF state on exit;mission/acs/defs.h
|
||||
11204;0x2bc4;MEKF_RECOVERY;INFO;MEKF is able to compute a solution again.;mission/acs/defs.h
|
||||
|
@ -97,6 +100,8 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
|
|||
11207;0x2bc7;SAFE_MODE_CONTROLLER_FAILURE;HIGH;The ACS safe mode controller was not able to compute a solution and has failed. P1: Missing information about magnetic field, P2: Missing information about rotational rate;mission/acs/defs.h
|
||||
11208;0x2bc8;TLE_TOO_OLD;INFO;The TLE for the SGP4 Propagator has become too old.;mission/acs/defs.h
|
||||
11209;0x2bc9;TLE_FILE_READ_FAILED;LOW;The TLE could not be read from the filesystem.;mission/acs/defs.h
|
||||
11210;0x2bca;PTG_RATE_VIOLATION;MEDIUM;The limits for the rotation in pointing mode were violated.;mission/acs/defs.h
|
||||
11211;0x2bcb;DETUMBLE_TRANSITION_FAILED;HIGH;The detumble transition has failed. //! P1: Last detumble state before failure.;mission/acs/defs.h
|
||||
11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/power/defs.h
|
||||
11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/power/defs.h
|
||||
11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/power/defs.h
|
||||
|
@ -231,8 +236,9 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
|
|||
12902;0x3266;POWER_STATE_MACHINE_TIMEOUT;MEDIUM;No description;mission/system/acs/SusAssembly.h
|
||||
12903;0x3267;SIDE_SWITCH_TRANSITION_NOT_ALLOWED;LOW;Not implemented, would increase already high complexity. Operator should instead command the assembly off first and then command the assembly on into the desired mode/submode combination;mission/system/acs/SusAssembly.h
|
||||
13000;0x32c8;CHILDREN_LOST_MODE;MEDIUM;No description;mission/system/tcs/TcsBoardAssembly.h
|
||||
13100;0x332c;GPS_FIX_CHANGE;INFO;Fix has changed. P1: Old fix. P2: New fix 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix;mission/acs/archive/GPSDefinitions.h
|
||||
13101;0x332d;CANT_GET_FIX;LOW;Could not get fix in maximum allowed time. P1: Maximum allowed time to get a fix after the GPS was switched on.;mission/acs/archive/GPSDefinitions.h
|
||||
13100;0x332c;GPS_FIX_CHANGE;INFO;Fix has changed. P1: New fix. P2: Missed fix changes 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix;linux/acs/GPSDefinitions.h
|
||||
13101;0x332d;CANT_GET_FIX;MEDIUM;Could not get fix in maximum allowed time. Trying to reset both GNSS devices. P1: Maximum allowed time to get a fix after the GPS was switched on.;linux/acs/GPSDefinitions.h
|
||||
13102;0x332e;RESET_FAIL;HIGH;Failed to reset an GNNS Device. P1: Board-Side.;linux/acs/GPSDefinitions.h
|
||||
13200;0x3390;P60_BOOT_COUNT;INFO;P60 boot count is broadcasted once at SW startup. P1: Boot count;mission/power/P60DockHandler.h
|
||||
13201;0x3391;BATT_MODE;INFO;Battery mode is broadcasted at startup. P1: Mode;mission/power/P60DockHandler.h
|
||||
13202;0x3392;BATT_MODE_CHANGED;MEDIUM;Battery mode has changed. P1: Old mode. P2: New mode;mission/power/P60DockHandler.h
|
||||
|
|
|
|
@ -454,6 +454,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
|
|||
0x5208;IMTQ_CmdErrUnknown;No description;8;IMTQ_HANDLER;mission/acs/imtqHelpers.h
|
||||
0x5209;IMTQ_StartupCfgError;No description;9;IMTQ_HANDLER;mission/acs/imtqHelpers.h
|
||||
0x520a;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;10;IMTQ_HANDLER;mission/acs/imtqHelpers.h
|
||||
0x53a0;RWHA_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;RW_HANDLER;mission/acs/RwHandler.h
|
||||
0x53a1;RWHA_InvalidRampTime;Action Message with invalid ramp time was received.;161;RW_HANDLER;mission/acs/RwHandler.h
|
||||
0x53a2;RWHA_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;RW_HANDLER;mission/acs/RwHandler.h
|
||||
0x53a3;RWHA_ExecutionFailed;Command execution failed;163;RW_HANDLER;mission/acs/RwHandler.h
|
||||
0x53a4;RWHA_CrcError;Reaction wheel reply has invalid crc;164;RW_HANDLER;mission/acs/RwHandler.h
|
||||
0x53a5;RWHA_ValueNotRead;No description;165;RW_HANDLER;mission/acs/RwHandler.h
|
||||
0x53b0;RWHA_SpiWriteFailure;No description;176;RW_HANDLER;mission/acs/rwHelpers.h
|
||||
0x53b1;RWHA_SpiReadFailure;Used by the spi send function to tell a failing read call;177;RW_HANDLER;mission/acs/rwHelpers.h
|
||||
0x53b2;RWHA_MissingStartSign;Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E;178;RW_HANDLER;mission/acs/rwHelpers.h
|
||||
|
@ -487,12 +493,8 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
|
|||
0x54b6;STRH_StartrackerAlreadyBooted;Star tracker is already in firmware mode;182;STR_HANDLER;mission/acs/str/StarTrackerHandler.h
|
||||
0x54b7;STRH_StartrackerNotRunningFirmware;Star tracker must be in firmware mode to run this command;183;STR_HANDLER;mission/acs/str/StarTrackerHandler.h
|
||||
0x54b8;STRH_StartrackerNotRunningBootloader;Star tracker must be in bootloader mode to run this command;184;STR_HANDLER;mission/acs/str/StarTrackerHandler.h
|
||||
0x59a0;SUSS_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;SUS_HANDLER;mission/acs/RwHandler.h
|
||||
0x59a1;SUSS_InvalidRampTime;Action Message with invalid ramp time was received.;161;SUS_HANDLER;mission/acs/RwHandler.h
|
||||
0x59a2;SUSS_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;SUS_HANDLER;mission/acs/RwHandler.h
|
||||
0x59a3;SUSS_ExecutionFailed;Command execution failed;163;SUS_HANDLER;mission/acs/RwHandler.h
|
||||
0x59a4;SUSS_CrcError;Reaction wheel reply has invalid crc;164;SUS_HANDLER;mission/acs/RwHandler.h
|
||||
0x59a5;SUSS_ValueNotRead;No description;165;SUS_HANDLER;mission/acs/RwHandler.h
|
||||
0x59a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/acs/archive/LegacySusHandler.h
|
||||
0x59a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/acs/archive/LegacySusHandler.h
|
||||
0x5e00;GOMS_PacketTooLong;No description;0;GOM_SPACE_HANDLER;mission/power/GomspaceDeviceHandler.h
|
||||
0x5e01;GOMS_InvalidTableId;No description;1;GOM_SPACE_HANDLER;mission/power/GomspaceDeviceHandler.h
|
||||
0x5e02;GOMS_InvalidAddress;No description;2;GOM_SPACE_HANDLER;mission/power/GomspaceDeviceHandler.h
|
||||
|
@ -509,9 +511,11 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
|
|||
0x67a2;SADPL_MainSwitchTimeoutFailure;No description;162;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h
|
||||
0x67a3;SADPL_SwitchingDeplSa1Failed;No description;163;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h
|
||||
0x67a4;SADPL_SwitchingDeplSa2Failed;No description;164;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h
|
||||
0x6a00;ACSCTRL_FileDeletionFailed;File deletion failed and at least one file is still existent.;0;ACS_CTRL;mission/controller/AcsController.h
|
||||
0x6a01;ACSCTRL_WriteFileFailed;Writing the TLE to the file has failed.;1;ACS_CTRL;mission/controller/AcsController.h
|
||||
0x6a02;ACSCTRL_ReadFileFailed;Reading the TLE to the file has failed.;2;ACS_CTRL;mission/controller/AcsController.h
|
||||
0x6aa0;ACSCTRL_FileDeletionFailed;File deletion failed and at least one file is still existent.;160;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
|
||||
0x6aa1;ACSCTRL_WriteFileFailed;Writing the TLE to the file has failed.;161;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
|
||||
0x6aa2;ACSCTRL_ReadFileFailed;Reading the TLE to the file has failed.;162;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
|
||||
0x6aa3;ACSCTRL_SingleRwUnavailable;A single RW has failed.;163;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
|
||||
0x6aa4;ACSCTRL_MultipleRwUnavailable;Multiple RWs have failed.;164;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
|
||||
0x6b02;ACSMEKF_MekfUninitialized;No description;2;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6b03;ACSMEKF_MekfNoGyrData;No description;3;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6b04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
|
|
|
|
@ -75,9 +75,12 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
|
|||
7902;0x1ede;BIT_LOCK;INFO;A Bit Lock signal. Was detected. P1: raw BLO state, P2: 0;fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
7903;0x1edf;BIT_LOCK_LOST;INFO;A previously found Bit Lock signal was lost. P1: raw BLO state, P2: 0;fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
7905;0x1ee1;FRAME_PROCESSING_FAILED;LOW;The CCSDS Board could not interpret a TC;fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
8900;0x22c4;CLOCK_SET;INFO;No description;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8901;0x22c5;CLOCK_DUMP;INFO;No description;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8902;0x22c6;CLOCK_SET_FAILURE;LOW;No description;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8900;0x22c4;CLOCK_SET;INFO;Clock has been set. P1: old timeval seconds. P2: new timeval seconds.;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8901;0x22c5;CLOCK_DUMP_LEGACY;INFO;Clock dump event. P1: timeval seconds P2: timeval milliseconds.;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8902;0x22c6;CLOCK_SET_FAILURE;LOW;Clock could not be set. P1: Returncode.;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8903;0x22c7;CLOCK_DUMP;INFO;Clock dump event. P1: timeval seconds P2: timeval microseconds.;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8904;0x22c8;CLOCK_DUMP_BEFORE_SETTING_TIME;INFO;No description;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8905;0x22c9;CLOCK_DUMP_AFTER_SETTING_TIME;INFO;No description;fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
9100;0x238c;TC_DELETION_FAILED;MEDIUM;Deletion of a TC from the map failed. P1: First 32 bit of request ID, P2. Last 32 bit of Request ID;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h
|
||||
9700;0x25e4;TEST;INFO;No description;fsfw/src/fsfw/pus/Service17Test.h
|
||||
10600;0x2968;CHANGE_OF_SETUP_PARAMETER;LOW;No description;fsfw/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h
|
||||
|
@ -88,7 +91,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
|
|||
10804;0x2a34;FILENAME_TOO_LARGE_ERROR;LOW;P1: Transaction step ID, P2: 0 for source file name, 1 for dest file name;fsfw/src/fsfw/cfdp/handler/defs.h
|
||||
10805;0x2a35;HANDLING_CFDP_REQUEST_FAILED;LOW;CFDP request handling failed. P2: Returncode.;fsfw/src/fsfw/cfdp/handler/defs.h
|
||||
11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;The limits for the rotation in safe mode were violated.;mission/acs/defs.h
|
||||
11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;The system has recovered from a safe rate rotation violation.;mission/acs/defs.h
|
||||
11201;0x2bc1;RATE_RECOVERY;MEDIUM;The system has recovered from a rate rotation violation.;mission/acs/defs.h
|
||||
11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;Multiple RWs are invalid, uncommandable and therefore higher ACS modes cannot be maintained.;mission/acs/defs.h
|
||||
11203;0x2bc3;MEKF_INVALID_INFO;INFO;MEKF was not able to compute a solution. P1: MEKF state on exit;mission/acs/defs.h
|
||||
11204;0x2bc4;MEKF_RECOVERY;INFO;MEKF is able to compute a solution again.;mission/acs/defs.h
|
||||
|
@ -97,6 +100,8 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
|
|||
11207;0x2bc7;SAFE_MODE_CONTROLLER_FAILURE;HIGH;The ACS safe mode controller was not able to compute a solution and has failed. P1: Missing information about magnetic field, P2: Missing information about rotational rate;mission/acs/defs.h
|
||||
11208;0x2bc8;TLE_TOO_OLD;INFO;The TLE for the SGP4 Propagator has become too old.;mission/acs/defs.h
|
||||
11209;0x2bc9;TLE_FILE_READ_FAILED;LOW;The TLE could not be read from the filesystem.;mission/acs/defs.h
|
||||
11210;0x2bca;PTG_RATE_VIOLATION;MEDIUM;The limits for the rotation in pointing mode were violated.;mission/acs/defs.h
|
||||
11211;0x2bcb;DETUMBLE_TRANSITION_FAILED;HIGH;The detumble transition has failed. //! P1: Last detumble state before failure.;mission/acs/defs.h
|
||||
11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/power/defs.h
|
||||
11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/power/defs.h
|
||||
11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/power/defs.h
|
||||
|
@ -231,8 +236,9 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
|
|||
12902;0x3266;POWER_STATE_MACHINE_TIMEOUT;MEDIUM;No description;mission/system/acs/SusAssembly.h
|
||||
12903;0x3267;SIDE_SWITCH_TRANSITION_NOT_ALLOWED;LOW;Not implemented, would increase already high complexity. Operator should instead command the assembly off first and then command the assembly on into the desired mode/submode combination;mission/system/acs/SusAssembly.h
|
||||
13000;0x32c8;CHILDREN_LOST_MODE;MEDIUM;No description;mission/system/tcs/TcsBoardAssembly.h
|
||||
13100;0x332c;GPS_FIX_CHANGE;INFO;Fix has changed. P1: Old fix. P2: New fix 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix;mission/acs/archive/GPSDefinitions.h
|
||||
13101;0x332d;CANT_GET_FIX;LOW;Could not get fix in maximum allowed time. P1: Maximum allowed time to get a fix after the GPS was switched on.;mission/acs/archive/GPSDefinitions.h
|
||||
13100;0x332c;GPS_FIX_CHANGE;INFO;Fix has changed. P1: New fix. P2: Missed fix changes 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix;linux/acs/GPSDefinitions.h
|
||||
13101;0x332d;CANT_GET_FIX;MEDIUM;Could not get fix in maximum allowed time. Trying to reset both GNSS devices. P1: Maximum allowed time to get a fix after the GPS was switched on.;linux/acs/GPSDefinitions.h
|
||||
13102;0x332e;RESET_FAIL;HIGH;Failed to reset an GNNS Device. P1: Board-Side.;linux/acs/GPSDefinitions.h
|
||||
13200;0x3390;P60_BOOT_COUNT;INFO;P60 boot count is broadcasted once at SW startup. P1: Boot count;mission/power/P60DockHandler.h
|
||||
13201;0x3391;BATT_MODE;INFO;Battery mode is broadcasted at startup. P1: Mode;mission/power/P60DockHandler.h
|
||||
13202;0x3392;BATT_MODE_CHANGED;MEDIUM;Battery mode has changed. P1: Old mode. P2: New mode;mission/power/P60DockHandler.h
|
||||
|
|
|
|
@ -454,6 +454,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
|
|||
0x5208;IMTQ_CmdErrUnknown;No description;8;IMTQ_HANDLER;mission/acs/imtqHelpers.h
|
||||
0x5209;IMTQ_StartupCfgError;No description;9;IMTQ_HANDLER;mission/acs/imtqHelpers.h
|
||||
0x520a;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;10;IMTQ_HANDLER;mission/acs/imtqHelpers.h
|
||||
0x53a0;RWHA_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;RW_HANDLER;mission/acs/RwHandler.h
|
||||
0x53a1;RWHA_InvalidRampTime;Action Message with invalid ramp time was received.;161;RW_HANDLER;mission/acs/RwHandler.h
|
||||
0x53a2;RWHA_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;RW_HANDLER;mission/acs/RwHandler.h
|
||||
0x53a3;RWHA_ExecutionFailed;Command execution failed;163;RW_HANDLER;mission/acs/RwHandler.h
|
||||
0x53a4;RWHA_CrcError;Reaction wheel reply has invalid crc;164;RW_HANDLER;mission/acs/RwHandler.h
|
||||
0x53a5;RWHA_ValueNotRead;No description;165;RW_HANDLER;mission/acs/RwHandler.h
|
||||
0x53b0;RWHA_SpiWriteFailure;No description;176;RW_HANDLER;mission/acs/rwHelpers.h
|
||||
0x53b1;RWHA_SpiReadFailure;Used by the spi send function to tell a failing read call;177;RW_HANDLER;mission/acs/rwHelpers.h
|
||||
0x53b2;RWHA_MissingStartSign;Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E;178;RW_HANDLER;mission/acs/rwHelpers.h
|
||||
|
@ -499,12 +505,8 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
|
|||
0x58a1;PLSPVhLP_ProcessTerminated;Process has been terminated by command;161;PLOC_SUPV_HELPER;linux/payload/PlocSupvUartMan.h
|
||||
0x58a2;PLSPVhLP_PathNotExists;Received command with invalid pathname;162;PLOC_SUPV_HELPER;linux/payload/PlocSupvUartMan.h
|
||||
0x58a3;PLSPVhLP_EventBufferReplyInvalidApid;Expected event buffer TM but received space packet with other APID;163;PLOC_SUPV_HELPER;linux/payload/PlocSupvUartMan.h
|
||||
0x59a0;SUSS_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;SUS_HANDLER;mission/acs/RwHandler.h
|
||||
0x59a1;SUSS_InvalidRampTime;Action Message with invalid ramp time was received.;161;SUS_HANDLER;mission/acs/RwHandler.h
|
||||
0x59a2;SUSS_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;SUS_HANDLER;mission/acs/RwHandler.h
|
||||
0x59a3;SUSS_ExecutionFailed;Command execution failed;163;SUS_HANDLER;mission/acs/RwHandler.h
|
||||
0x59a4;SUSS_CrcError;Reaction wheel reply has invalid crc;164;SUS_HANDLER;mission/acs/RwHandler.h
|
||||
0x59a5;SUSS_ValueNotRead;No description;165;SUS_HANDLER;mission/acs/RwHandler.h
|
||||
0x59a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/acs/archive/LegacySusHandler.h
|
||||
0x59a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/acs/archive/LegacySusHandler.h
|
||||
0x5aa0;IPCI_PapbBusy;No description;160;CCSDS_IP_CORE_BRIDGE;linux/ipcore/PapbVcInterface.h
|
||||
0x5ba0;PTME_UnknownVcId;No description;160;PTME;linux/ipcore/Ptme.h
|
||||
0x5d01;STRHLP_SdNotMounted;SD card specified in path string not mounted;1;STR_HELPER;linux/acs/StrComHandler.h
|
||||
|
@ -593,9 +595,11 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
|
|||
0x69b5;SPVRTVIF_SupvHelperExecuting;Supervisor helper task ist currently executing a command (wait until helper tas has finished or interrupt by sending the terminate command);181;SUPV_RETURN_VALUES_IF;linux/payload/plocSupvDefs.h
|
||||
0x69c0;SPVRTVIF_BufTooSmall;No description;192;SUPV_RETURN_VALUES_IF;linux/payload/plocSupvDefs.h
|
||||
0x69c1;SPVRTVIF_NoReplyTimeout;No description;193;SUPV_RETURN_VALUES_IF;linux/payload/plocSupvDefs.h
|
||||
0x6a00;ACSCTRL_FileDeletionFailed;File deletion failed and at least one file is still existent.;0;ACS_CTRL;mission/controller/AcsController.h
|
||||
0x6a01;ACSCTRL_WriteFileFailed;Writing the TLE to the file has failed.;1;ACS_CTRL;mission/controller/AcsController.h
|
||||
0x6a02;ACSCTRL_ReadFileFailed;Reading the TLE to the file has failed.;2;ACS_CTRL;mission/controller/AcsController.h
|
||||
0x6aa0;ACSCTRL_FileDeletionFailed;File deletion failed and at least one file is still existent.;160;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
|
||||
0x6aa1;ACSCTRL_WriteFileFailed;Writing the TLE to the file has failed.;161;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
|
||||
0x6aa2;ACSCTRL_ReadFileFailed;Reading the TLE to the file has failed.;162;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
|
||||
0x6aa3;ACSCTRL_SingleRwUnavailable;A single RW has failed.;163;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
|
||||
0x6aa4;ACSCTRL_MultipleRwUnavailable;Multiple RWs have failed.;164;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
|
||||
0x6b02;ACSMEKF_MekfUninitialized;No description;2;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6b03;ACSMEKF_MekfNoGyrData;No description;3;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6b04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
|
|
|
|
@ -1,7 +1,7 @@
|
|||
/**
|
||||
* @brief Auto-generated event translation file. Contains 318 translations.
|
||||
* @brief Auto-generated event translation file. Contains 324 translations.
|
||||
* @details
|
||||
* Generated on: 2023-12-13 11:29:45
|
||||
* Generated on: 2024-04-10 11:49:35
|
||||
*/
|
||||
#include "translateEvents.h"
|
||||
|
||||
|
@ -82,8 +82,11 @@ const char *BIT_LOCK_STRING = "BIT_LOCK";
|
|||
const char *BIT_LOCK_LOST_STRING = "BIT_LOCK_LOST";
|
||||
const char *FRAME_PROCESSING_FAILED_STRING = "FRAME_PROCESSING_FAILED";
|
||||
const char *CLOCK_SET_STRING = "CLOCK_SET";
|
||||
const char *CLOCK_DUMP_STRING = "CLOCK_DUMP";
|
||||
const char *CLOCK_DUMP_LEGACY_STRING = "CLOCK_DUMP_LEGACY";
|
||||
const char *CLOCK_SET_FAILURE_STRING = "CLOCK_SET_FAILURE";
|
||||
const char *CLOCK_DUMP_STRING = "CLOCK_DUMP";
|
||||
const char *CLOCK_DUMP_BEFORE_SETTING_TIME_STRING = "CLOCK_DUMP_BEFORE_SETTING_TIME";
|
||||
const char *CLOCK_DUMP_AFTER_SETTING_TIME_STRING = "CLOCK_DUMP_AFTER_SETTING_TIME";
|
||||
const char *TC_DELETION_FAILED_STRING = "TC_DELETION_FAILED";
|
||||
const char *TEST_STRING = "TEST";
|
||||
const char *CHANGE_OF_SETUP_PARAMETER_STRING = "CHANGE_OF_SETUP_PARAMETER";
|
||||
|
@ -94,7 +97,7 @@ const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR";
|
|||
const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR";
|
||||
const char *HANDLING_CFDP_REQUEST_FAILED_STRING = "HANDLING_CFDP_REQUEST_FAILED";
|
||||
const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
|
||||
const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
|
||||
const char *RATE_RECOVERY_STRING = "RATE_RECOVERY";
|
||||
const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
|
||||
const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
|
||||
const char *MEKF_RECOVERY_STRING = "MEKF_RECOVERY";
|
||||
|
@ -103,6 +106,8 @@ const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFO
|
|||
const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE";
|
||||
const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD";
|
||||
const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED";
|
||||
const char *PTG_RATE_VIOLATION_STRING = "PTG_RATE_VIOLATION";
|
||||
const char *DETUMBLE_TRANSITION_FAILED_STRING = "DETUMBLE_TRANSITION_FAILED";
|
||||
const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
|
||||
const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
|
||||
const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
|
||||
|
@ -239,6 +244,7 @@ const char *SIDE_SWITCH_TRANSITION_NOT_ALLOWED_12903_STRING = "SIDE_SWITCH_TRANS
|
|||
const char *CHILDREN_LOST_MODE_STRING = "CHILDREN_LOST_MODE";
|
||||
const char *GPS_FIX_CHANGE_STRING = "GPS_FIX_CHANGE";
|
||||
const char *CANT_GET_FIX_STRING = "CANT_GET_FIX";
|
||||
const char *RESET_FAIL_STRING = "RESET_FAIL";
|
||||
const char *P60_BOOT_COUNT_STRING = "P60_BOOT_COUNT";
|
||||
const char *BATT_MODE_STRING = "BATT_MODE";
|
||||
const char *BATT_MODE_CHANGED_STRING = "BATT_MODE_CHANGED";
|
||||
|
@ -481,9 +487,15 @@ const char *translateEvents(Event event) {
|
|||
case (8900):
|
||||
return CLOCK_SET_STRING;
|
||||
case (8901):
|
||||
return CLOCK_DUMP_STRING;
|
||||
return CLOCK_DUMP_LEGACY_STRING;
|
||||
case (8902):
|
||||
return CLOCK_SET_FAILURE_STRING;
|
||||
case (8903):
|
||||
return CLOCK_DUMP_STRING;
|
||||
case (8904):
|
||||
return CLOCK_DUMP_BEFORE_SETTING_TIME_STRING;
|
||||
case (8905):
|
||||
return CLOCK_DUMP_AFTER_SETTING_TIME_STRING;
|
||||
case (9100):
|
||||
return TC_DELETION_FAILED_STRING;
|
||||
case (9700):
|
||||
|
@ -505,7 +517,7 @@ const char *translateEvents(Event event) {
|
|||
case (11200):
|
||||
return SAFE_RATE_VIOLATION_STRING;
|
||||
case (11201):
|
||||
return SAFE_RATE_RECOVERY_STRING;
|
||||
return RATE_RECOVERY_STRING;
|
||||
case (11202):
|
||||
return MULTIPLE_RW_INVALID_STRING;
|
||||
case (11203):
|
||||
|
@ -522,6 +534,10 @@ const char *translateEvents(Event event) {
|
|||
return TLE_TOO_OLD_STRING;
|
||||
case (11209):
|
||||
return TLE_FILE_READ_FAILED_STRING;
|
||||
case (11210):
|
||||
return PTG_RATE_VIOLATION_STRING;
|
||||
case (11211):
|
||||
return DETUMBLE_TRANSITION_FAILED_STRING;
|
||||
case (11300):
|
||||
return SWITCH_CMD_SENT_STRING;
|
||||
case (11301):
|
||||
|
@ -794,6 +810,8 @@ const char *translateEvents(Event event) {
|
|||
return GPS_FIX_CHANGE_STRING;
|
||||
case (13101):
|
||||
return CANT_GET_FIX_STRING;
|
||||
case (13102):
|
||||
return RESET_FAIL_STRING;
|
||||
case (13200):
|
||||
return P60_BOOT_COUNT_STRING;
|
||||
case (13201):
|
||||
|
|
|
@ -2,7 +2,7 @@
|
|||
* @brief Auto-generated object translation file.
|
||||
* @details
|
||||
* Contains 179 translations.
|
||||
* Generated on: 2023-12-13 11:29:45
|
||||
* Generated on: 2024-04-10 11:49:35
|
||||
*/
|
||||
#include "translateObjects.h"
|
||||
|
||||
|
|
|
@ -26,7 +26,7 @@
|
|||
#include "devConf.h"
|
||||
#include "devices/gpioIds.h"
|
||||
#include "mission/system/acs/acsModeTree.h"
|
||||
#include "mission/system/payloadModeTree.h"
|
||||
#include "mission/system/payload/payloadModeTree.h"
|
||||
#include "mission/system/power/epsModeTree.h"
|
||||
|
||||
void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiComIF,
|
||||
|
|
|
@ -7,15 +7,19 @@
|
|||
|
||||
namespace GpsHyperion {
|
||||
|
||||
enum class FixMode : uint8_t { NOT_SEEN = 0, NO_FIX = 1, FIX_2D = 2, FIX_3D = 3, UNKNOWN = 4 };
|
||||
enum FixMode : uint8_t { NOT_SEEN = 0, NO_FIX = 1, FIX_2D = 2, FIX_3D = 3 };
|
||||
|
||||
enum GnssChip : uint8_t { A_SIDE = 0, B_SIDE = 1 };
|
||||
|
||||
static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::GPS_HANDLER;
|
||||
//! [EXPORT] : [COMMENT] Fix has changed. P1: Old fix. P2: New fix
|
||||
//! [EXPORT] : [COMMENT] Fix has changed. P1: New fix. P2: Missed fix changes
|
||||
//! 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix
|
||||
static constexpr Event GPS_FIX_CHANGE = event::makeEvent(SUBSYSTEM_ID, 0, severity::INFO);
|
||||
//! [EXPORT] : [COMMENT] Could not get fix in maximum allowed time. P1: Maximum allowed time
|
||||
//! to get a fix after the GPS was switched on.
|
||||
static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 1, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Could not get fix in maximum allowed time. Trying to reset both GNSS
|
||||
//! devices. P1: Maximum allowed time to get a fix after the GPS was switched on.
|
||||
static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 1, severity::MEDIUM);
|
||||
//! [EXPORT] : [COMMENT] Failed to reset an GNNS Device. P1: Board-Side.
|
||||
static constexpr Event RESET_FAIL = event::makeEvent(SUBSYSTEM_ID, 2, severity::HIGH);
|
||||
|
||||
static constexpr DeviceCommandId_t GPS_REPLY = 0;
|
||||
static constexpr DeviceCommandId_t TRIGGER_RESET_PIN_GNSS = 5;
|
||||
|
@ -53,8 +57,6 @@ static constexpr uint8_t SKYVIEW_ENTRIES = 6;
|
|||
|
||||
static constexpr uint8_t MAX_SATELLITES = 30;
|
||||
|
||||
enum GpsFixModes : uint8_t { INVALID = 0, NO_FIX = 1, FIX_2D = 2, FIX_3D = 3 };
|
||||
|
||||
} // namespace GpsHyperion
|
||||
|
||||
class GpsPrimaryDataset : public StaticLocalDataSet<GpsHyperion::CORE_DATASET_ENTRIES> {
|
|
@ -44,24 +44,21 @@ LocalPoolDataSetBase *GpsHyperionLinuxController::getDataSetHandle(sid_t sid) {
|
|||
|
||||
ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode,
|
||||
uint32_t *msToReachTheMode) {
|
||||
if (not modeCommanded) {
|
||||
if (mode == MODE_ON or mode == MODE_OFF) {
|
||||
// 5h time to reach fix
|
||||
*msToReachTheMode = MAX_SECONDS_TO_REACH_FIX;
|
||||
maxTimeToReachFix.resetTimer();
|
||||
modeCommanded = true;
|
||||
} else if (mode == MODE_NORMAL) {
|
||||
return HasModesIF::INVALID_MODE;
|
||||
}
|
||||
if (mode == MODE_ON) {
|
||||
maxTimeToReachFix.resetTimer();
|
||||
gainedNewFix.timeOut();
|
||||
} else if (mode == MODE_NORMAL) {
|
||||
return HasModesIF::INVALID_MODE;
|
||||
}
|
||||
if (mode == MODE_OFF) {
|
||||
maxTimeToReachFix.timeOut();
|
||||
gainedNewFix.timeOut();
|
||||
PoolReadGuard pg(&gpsSet);
|
||||
gpsSet.setValidity(false, true);
|
||||
// There can't be a fix with a device that is off.
|
||||
triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, 0);
|
||||
gpsSet.fixMode.value = 0;
|
||||
// The ctrl is off, so it cannot detect the data from the devices.
|
||||
handleFixChangedEvent(GpsHyperion::FixMode::NOT_SEEN);
|
||||
gpsSet.fixMode.value = GpsHyperion::FixMode::NOT_SEEN;
|
||||
oneShotSwitches.reset();
|
||||
modeCommanded = false;
|
||||
}
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
@ -75,13 +72,16 @@ ReturnValue_t GpsHyperionLinuxController::executeAction(ActionId_t actionId,
|
|||
PoolReadGuard pg(&gpsSet);
|
||||
// Set HK entries invalid
|
||||
gpsSet.setValidity(false, true);
|
||||
resetCallback(data, size, resetCallbackArgs);
|
||||
ReturnValue_t result = resetCallback(data, size, resetCallbackArgs);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
return HasActionsIF::EXECUTION_FINISHED;
|
||||
}
|
||||
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
||||
}
|
||||
}
|
||||
return returnvalue::OK;
|
||||
return HasActionsIF::INVALID_ACTION_ID;
|
||||
}
|
||||
|
||||
ReturnValue_t GpsHyperionLinuxController::initializeLocalDataPool(
|
||||
|
@ -216,15 +216,9 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
|
|||
bool modeIsSet = true;
|
||||
if (MODE_SET != (MODE_SET & gps.set)) {
|
||||
if (mode != MODE_OFF) {
|
||||
if (maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) {
|
||||
sif::warning << "GpsHyperionLinuxController: No mode could be set in allowed "
|
||||
<< maxTimeToReachFix.getTimeoutMs() / 1000 << " seconds" << std::endl;
|
||||
triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs());
|
||||
oneShotSwitches.cantGetFixSwitch = false;
|
||||
}
|
||||
modeIsSet = false;
|
||||
} else {
|
||||
// GPS device is off anyway, so do other handling
|
||||
// GPS ctrl is off anyway, so do other handling
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
}
|
||||
|
@ -249,27 +243,44 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
|
|||
uint8_t newFix = 0;
|
||||
if (modeIsSet) {
|
||||
// 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
|
||||
if (gps.fix.mode == 2 or gps.fix.mode == 3) {
|
||||
if (gps.fix.mode == GpsHyperion::FixMode::FIX_2D or
|
||||
gps.fix.mode == GpsHyperion::FixMode::FIX_3D) {
|
||||
validFix = true;
|
||||
maxTimeToReachFix.resetTimer();
|
||||
}
|
||||
newFix = gps.fix.mode;
|
||||
if (newFix == 0 or newFix == 1) {
|
||||
if (modeCommanded and maxTimeToReachFix.hasTimedOut()) {
|
||||
// We are supposed to be on and functioning, but no fix was found
|
||||
if (mode == MODE_ON or mode == MODE_NORMAL) {
|
||||
mode = MODE_OFF;
|
||||
}
|
||||
modeCommanded = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (gpsSet.fixMode.value != newFix) {
|
||||
#if OBSW_Q7S_EM != 1
|
||||
triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFix);
|
||||
#endif
|
||||
handleFixChangedEvent(newFix);
|
||||
}
|
||||
gpsSet.fixMode = newFix;
|
||||
gpsSet.fixMode.setValid(modeIsSet);
|
||||
// We are supposed to be on and functioning, but no fix was found
|
||||
if (not validFix) {
|
||||
if (maxTimeToReachFix.hasTimedOut()) {
|
||||
// Set HK entries invalid
|
||||
gpsSet.setValidity(false, true);
|
||||
if (oneShotSwitches.cantGetFixSwitch) {
|
||||
sif::warning << "GpsHyperionLinuxController: No fix detected in allowed "
|
||||
<< maxTimeToReachFix.getTimeoutMs() / 1000 << " seconds" << std::endl;
|
||||
triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs());
|
||||
oneShotSwitches.cantGetFixSwitch = false;
|
||||
// Try resetting the devices
|
||||
if (resetCallback != nullptr) {
|
||||
uint8_t chip = GpsHyperion::GnssChip::A_SIDE;
|
||||
ReturnValue_t result = resetCallback(&chip, 1, resetCallbackArgs);
|
||||
if (result != returnvalue::OK) {
|
||||
triggerEvent(GpsHyperion::RESET_FAIL, chip);
|
||||
}
|
||||
chip = GpsHyperion::GnssChip::B_SIDE;
|
||||
result = resetCallback(&chip, 1, resetCallbackArgs);
|
||||
if (result != returnvalue::OK) {
|
||||
triggerEvent(GpsHyperion::RESET_FAIL, chip);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Only set on specific messages, so only set a valid flag to invalid
|
||||
// if not set for more than a full message set (10 messages here)
|
||||
|
@ -282,9 +293,12 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
|
|||
}
|
||||
satNotSetCounter = 0;
|
||||
} else {
|
||||
satNotSetCounter++;
|
||||
if (gpsSet.satInUse.isValid() and satNotSetCounter >= 10) {
|
||||
if (satNotSetCounter < 10) {
|
||||
satNotSetCounter++;
|
||||
} else {
|
||||
gpsSet.satInUse.value = 0;
|
||||
gpsSet.satInUse.setValid(false);
|
||||
gpsSet.satInView.value = 0;
|
||||
gpsSet.satInView.setValid(false);
|
||||
}
|
||||
}
|
||||
|
@ -292,22 +306,24 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
|
|||
// LATLON is set for every message, no need for a counter
|
||||
bool latValid = false;
|
||||
bool longValid = false;
|
||||
if (LATLON_SET == (LATLON_SET & gps.set)) {
|
||||
if (std::isfinite(gps.fix.latitude)) {
|
||||
// Negative latitude -> South direction
|
||||
gpsSet.latitude.value = gps.fix.latitude;
|
||||
// As specified in gps.h: Only valid if mode >= 2
|
||||
if (gps.fix.mode >= 2) {
|
||||
latValid = true;
|
||||
if (modeIsSet) {
|
||||
if (LATLON_SET == (LATLON_SET & gps.set)) {
|
||||
if (std::isfinite(gps.fix.latitude)) {
|
||||
// Negative latitude -> South direction
|
||||
gpsSet.latitude.value = gps.fix.latitude;
|
||||
// As specified in gps.h: Only valid if mode >= 2
|
||||
if (gps.fix.mode >= GpsHyperion::FixMode::FIX_2D) {
|
||||
latValid = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (std::isfinite(gps.fix.longitude)) {
|
||||
// Negative longitude -> West direction
|
||||
gpsSet.longitude.value = gps.fix.longitude;
|
||||
// As specified in gps.h: Only valid if mode >= 2
|
||||
if (gps.fix.mode >= 2) {
|
||||
longValid = true;
|
||||
if (std::isfinite(gps.fix.longitude)) {
|
||||
// Negative longitude -> West direction
|
||||
gpsSet.longitude.value = gps.fix.longitude;
|
||||
// As specified in gps.h: Only valid if mode >= 2
|
||||
if (gps.fix.mode >= GpsHyperion::FixMode::FIX_2D) {
|
||||
longValid = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -316,20 +332,24 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
|
|||
|
||||
// ALTITUDE is set for every message, no need for a counter
|
||||
bool altitudeValid = false;
|
||||
if (ALTITUDE_SET == (ALTITUDE_SET & gps.set) && std::isfinite(gps.fix.altitude)) {
|
||||
gpsSet.altitude.value = gps.fix.altitude;
|
||||
// As specified in gps.h: Only valid if mode == 3
|
||||
if (gps.fix.mode == 3) {
|
||||
altitudeValid = true;
|
||||
if (modeIsSet) {
|
||||
if (ALTITUDE_SET == (ALTITUDE_SET & gps.set) && std::isfinite(gps.fix.altitude)) {
|
||||
gpsSet.altitude.value = gps.fix.altitude;
|
||||
// As specified in gps.h: Only valid if mode == 3
|
||||
if (gps.fix.mode == GpsHyperion::FixMode::FIX_3D) {
|
||||
altitudeValid = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
gpsSet.altitude.setValid(altitudeValid);
|
||||
|
||||
// SPEED is set for every message, no need for a counter
|
||||
bool speedValid = false;
|
||||
if (SPEED_SET == (SPEED_SET & gps.set) && std::isfinite(gps.fix.speed)) {
|
||||
gpsSet.speed.value = gps.fix.speed;
|
||||
speedValid = true;
|
||||
if (modeIsSet) {
|
||||
if (SPEED_SET == (SPEED_SET & gps.set) && std::isfinite(gps.fix.speed)) {
|
||||
gpsSet.speed.value = gps.fix.speed;
|
||||
speedValid = true;
|
||||
}
|
||||
}
|
||||
gpsSet.speed.setValid(speedValid);
|
||||
|
||||
|
@ -430,3 +450,14 @@ void GpsHyperionLinuxController::overwriteTimeIfNotSane(timeval time, bool valid
|
|||
timeInit = true;
|
||||
}
|
||||
}
|
||||
|
||||
void GpsHyperionLinuxController::handleFixChangedEvent(uint8_t newFix) {
|
||||
if (gainedNewFix.hasTimedOut()) {
|
||||
triggerEvent(GpsHyperion::GPS_FIX_CHANGE, newFix, fixChangeCounter);
|
||||
fixChangeCounter = 0;
|
||||
gainedNewFix.resetTimer();
|
||||
return;
|
||||
}
|
||||
fixChangeCounter++;
|
||||
gainedNewFix.resetTimer();
|
||||
}
|
||||
|
|
|
@ -1,14 +1,13 @@
|
|||
#ifndef MISSION_DEVICES_GPSHYPERIONHANDLER_H_
|
||||
#define MISSION_DEVICES_GPSHYPERIONHANDLER_H_
|
||||
|
||||
#include <mission/acs/archive/GPSDefinitions.h>
|
||||
#include <common/config/eive/eventSubsystemIds.h>
|
||||
#include <fsfw/FSFW.h>
|
||||
#include <fsfw/controller/ExtendedControllerBase.h>
|
||||
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
|
||||
#include <linux/acs/GPSDefinitions.h>
|
||||
#include <mission/utility/trace.h>
|
||||
|
||||
#include "eive/eventSubsystemIds.h"
|
||||
#include "fsfw/FSFW.h"
|
||||
#include "fsfw/controller/ExtendedControllerBase.h"
|
||||
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
|
||||
|
||||
#ifdef FSFW_OSAL_LINUX
|
||||
#include <gps.h>
|
||||
#include <libgpsmm.h>
|
||||
|
@ -24,8 +23,8 @@
|
|||
*/
|
||||
class GpsHyperionLinuxController : public ExtendedControllerBase {
|
||||
public:
|
||||
// 30 minutes
|
||||
static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 30;
|
||||
// 15 minutes
|
||||
static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 15;
|
||||
|
||||
enum ReadModes { SHM = 0, SOCKET = 1 };
|
||||
|
||||
|
@ -65,7 +64,8 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
|
|||
const char* currentClientBuf = nullptr;
|
||||
ReadModes readMode = ReadModes::SOCKET;
|
||||
Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000);
|
||||
bool modeCommanded = false;
|
||||
Countdown gainedNewFix = Countdown(60 * 2 * 1000);
|
||||
uint32_t fixChangeCounter = 0;
|
||||
bool timeInit = false;
|
||||
uint8_t satNotSetCounter = 0;
|
||||
|
||||
|
@ -92,6 +92,8 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
|
|||
// we set it with the roughly valid time from the GPS. For some reason, NTP might only work
|
||||
// if the time difference between sys time and current time is not too large
|
||||
void overwriteTimeIfNotSane(timeval time, bool validFix);
|
||||
|
||||
void handleFixChangedEvent(uint8_t newFix);
|
||||
};
|
||||
|
||||
#endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */
|
||||
|
|
|
@ -175,7 +175,8 @@ void StrComHandler::setDownloadImageName(std::string filename) {
|
|||
|
||||
void StrComHandler::setFlashReadFilename(std::string filename) { flashRead.filename = filename; }
|
||||
|
||||
ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname) {
|
||||
ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname,
|
||||
startracker::FirmwareTarget target) {
|
||||
{
|
||||
MutexGuard mg(lock);
|
||||
if (state != InternalState::SLEEPING) {
|
||||
|
@ -192,8 +193,13 @@ ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname) {
|
|||
if (not std::filesystem::exists(flashWrite.fullname)) {
|
||||
return FILE_NOT_EXISTS;
|
||||
}
|
||||
flashWrite.firstRegion = static_cast<uint8_t>(startracker::FirmwareRegions::FIRST);
|
||||
flashWrite.lastRegion = static_cast<uint8_t>(startracker::FirmwareRegions::LAST);
|
||||
if (target == startracker::FirmwareTarget::MAIN) {
|
||||
flashWrite.firstRegion = static_cast<uint8_t>(startracker::FirmwareRegions::FIRST_MAIN);
|
||||
flashWrite.lastRegion = static_cast<uint8_t>(startracker::FirmwareRegions::LAST_MAIN);
|
||||
} else if (target == startracker::FirmwareTarget::BACKUP) {
|
||||
flashWrite.firstRegion = static_cast<uint8_t>(startracker::FirmwareRegions::FIRST_BACKUP);
|
||||
flashWrite.lastRegion = static_cast<uint8_t>(startracker::FirmwareRegions::LAST_BACKUP);
|
||||
}
|
||||
{
|
||||
MutexGuard mg(lock);
|
||||
replyWasReceived = false;
|
||||
|
@ -264,7 +270,7 @@ ReturnValue_t StrComHandler::performImageDownload() {
|
|||
file.close();
|
||||
return returnvalue::OK;
|
||||
}
|
||||
arc_pack_download_action_req(&downloadReq, cmdBuf.data(), &size);
|
||||
prv_arc_pack_download_action_req(&downloadReq, cmdBuf.data(), &size);
|
||||
result = sendAndRead(size, downloadReq.position);
|
||||
if (result != returnvalue::OK) {
|
||||
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
|
||||
|
@ -275,7 +281,7 @@ ReturnValue_t StrComHandler::performImageDownload() {
|
|||
file.close();
|
||||
return result;
|
||||
}
|
||||
result = checkActionReply(replySize);
|
||||
result = checkActionReply(replySize, "downloading image");
|
||||
if (result != returnvalue::OK) {
|
||||
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
|
||||
serial::flushRxBuf(serialPort);
|
||||
|
@ -343,12 +349,12 @@ ReturnValue_t StrComHandler::performImageUpload() {
|
|||
}
|
||||
file.seekg(uploadReq.position * SIZE_IMAGE_PART, file.beg);
|
||||
file.read(reinterpret_cast<char*>(uploadReq.data), SIZE_IMAGE_PART);
|
||||
arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size);
|
||||
prv_arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size);
|
||||
result = sendAndRead(size, uploadReq.position);
|
||||
if (result != returnvalue::OK) {
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
result = checkActionReply(replyLen);
|
||||
result = checkActionReply(replyLen, "sky image upload");
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
|
@ -369,12 +375,12 @@ ReturnValue_t StrComHandler::performImageUpload() {
|
|||
file.seekg(fullChunks * SIZE_IMAGE_PART, file.beg);
|
||||
file.read(reinterpret_cast<char*>(uploadReq.data), remainder);
|
||||
file.close();
|
||||
arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size);
|
||||
prv_arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size);
|
||||
result = sendAndRead(size, uploadReq.position);
|
||||
if (result != returnvalue::OK) {
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
result = checkActionReply(replyLen);
|
||||
result = checkActionReply(replyLen, "sky image upload");
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
|
@ -388,8 +394,7 @@ ReturnValue_t StrComHandler::performImageUpload() {
|
|||
ReturnValue_t StrComHandler::performFirmwareUpdate() {
|
||||
using namespace startracker;
|
||||
ReturnValue_t result = returnvalue::OK;
|
||||
result = unlockAndEraseRegions(static_cast<uint32_t>(startracker::FirmwareRegions::FIRST),
|
||||
static_cast<uint32_t>(startracker::FirmwareRegions::LAST));
|
||||
result = unlockAndEraseRegions(flashWrite.firstRegion, flashWrite.lastRegion);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
|
@ -440,12 +445,12 @@ ReturnValue_t StrComHandler::performFlashWrite() {
|
|||
bytesWrittenInRegion = 0;
|
||||
}
|
||||
req.address = bytesWrittenInRegion;
|
||||
arc_pack_write_action_req(&req, cmdBuf.data(), &size);
|
||||
prv_arc_pack_write_action_req(&req, cmdBuf.data(), &size);
|
||||
result = sendAndRead(size, req.address);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = checkActionReply(replyLen);
|
||||
result = checkActionReply(replyLen, "firmware image upload");
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
|
@ -483,12 +488,12 @@ ReturnValue_t StrComHandler::performFlashWrite() {
|
|||
req.length = remainingBytes;
|
||||
totalBytesWritten += CHUNK_SIZE;
|
||||
bytesWrittenInRegion += remainingBytes;
|
||||
arc_pack_write_action_req(&req, cmdBuf.data(), &size);
|
||||
prv_arc_pack_write_action_req(&req, cmdBuf.data(), &size);
|
||||
result = sendAndRead(size, req.address);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = checkActionReply(replyLen);
|
||||
result = checkActionReply(replyLen, "flash write");
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
|
@ -531,7 +536,7 @@ ReturnValue_t StrComHandler::performFlashRead() {
|
|||
} else {
|
||||
req.length = CHUNK_SIZE;
|
||||
}
|
||||
arc_pack_read_action_req(&req, cmdBuf.data(), &size);
|
||||
prv_arc_pack_read_action_req(&req, cmdBuf.data(), &size);
|
||||
result = sendAndRead(size, req.address);
|
||||
if (result != returnvalue::OK) {
|
||||
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
|
||||
|
@ -542,7 +547,7 @@ ReturnValue_t StrComHandler::performFlashRead() {
|
|||
file.close();
|
||||
return result;
|
||||
}
|
||||
result = checkActionReply(replyLen);
|
||||
result = checkActionReply(replyLen, "flash read");
|
||||
if (result != returnvalue::OK) {
|
||||
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
|
||||
serial::flushRxBuf(serialPort);
|
||||
|
@ -584,7 +589,7 @@ ReturnValue_t StrComHandler::sendAndRead(size_t size, uint32_t failParameter) {
|
|||
return readOneReply(failParameter);
|
||||
}
|
||||
|
||||
ReturnValue_t StrComHandler::checkActionReply(size_t replySize) {
|
||||
ReturnValue_t StrComHandler::checkActionReply(size_t replySize, const char* context) {
|
||||
uint8_t type = startracker::getReplyFrameType(replyPtr);
|
||||
if (type != TMTC_ACTIONREPLY) {
|
||||
sif::warning << "StrHelper::checkActionReply: Received reply with invalid type ID" << std::endl;
|
||||
|
@ -592,7 +597,7 @@ ReturnValue_t StrComHandler::checkActionReply(size_t replySize) {
|
|||
}
|
||||
uint8_t status = startracker::getStatusField(replyPtr);
|
||||
if (status != ArcsecDatalinkLayer::STATUS_OK) {
|
||||
sif::warning << "StrHelper::checkActionReply: Status failure: "
|
||||
sif::warning << "StrHelper::checkActionReply: Status failure for " << context << ": "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
return STATUS_ERROR;
|
||||
}
|
||||
|
@ -744,23 +749,26 @@ ReturnValue_t StrComHandler::unlockAndEraseRegions(uint32_t from, uint32_t to) {
|
|||
struct UnlockActionRequest unlockReq;
|
||||
struct EraseActionRequest eraseReq;
|
||||
uint32_t size = 0;
|
||||
for (uint32_t idx = from; idx <= to; idx++) {
|
||||
for (uint32_t idx = from; idx < to; idx++) {
|
||||
unlockReq.region = idx;
|
||||
unlockReq.code = startracker::region_secrets::secret[idx];
|
||||
arc_pack_unlock_action_req(&unlockReq, cmdBuf.data(), &size);
|
||||
unlockReq.code = startracker::region_secrets::SECRETS[idx];
|
||||
prv_arc_pack_unlock_action_req(&unlockReq, cmdBuf.data(), &size);
|
||||
result = sendAndRead(size, unlockReq.region);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = checkActionReply(replyLen);
|
||||
result = checkActionReply(replyLen, "unlocking region");
|
||||
if (result != returnvalue::OK) {
|
||||
sif::warning << "StrHelper::unlockAndEraseRegions: Failed to unlock region with id "
|
||||
<< static_cast<unsigned int>(unlockReq.region) << std::endl;
|
||||
return result;
|
||||
}
|
||||
eraseReq.region = idx;
|
||||
arc_pack_erase_action_req(&eraseReq, cmdBuf.data(), &size);
|
||||
prv_arc_pack_erase_action_req(&eraseReq, cmdBuf.data(), &size);
|
||||
result = sendAndRead(size, eraseReq.region);
|
||||
if (result != returnvalue::OK) {
|
||||
}
|
||||
result = checkActionReply(replyLen, "erasing region");
|
||||
if (result != returnvalue::OK) {
|
||||
sif::warning << "StrHelper::unlockAndEraseRegions: Failed to erase region with id "
|
||||
<< static_cast<unsigned int>(eraseReq.region) << std::endl;
|
||||
|
|
|
@ -6,6 +6,7 @@
|
|||
#include <string>
|
||||
|
||||
#include "OBSWConfig.h"
|
||||
#include "mission/acs/str/strHelpers.h"
|
||||
|
||||
#ifdef XIPHOS_Q7S
|
||||
#include "bsp_q7s/fs/SdCardManager.h"
|
||||
|
@ -127,7 +128,7 @@ class StrComHandler : public SystemObject, public DeviceCommunicationIF, public
|
|||
* @param fullname Full name including absolute path of file containing firmware
|
||||
* update.
|
||||
*/
|
||||
ReturnValue_t startFirmwareUpdate(std::string fullname);
|
||||
ReturnValue_t startFirmwareUpdate(std::string fullname, startracker::FirmwareTarget target);
|
||||
|
||||
/**
|
||||
* @brief Starts the flash read procedure
|
||||
|
@ -334,7 +335,7 @@ class StrComHandler : public SystemObject, public DeviceCommunicationIF, public
|
|||
*
|
||||
* @return returnvalue::OK if reply confirms success of packet transfer, otherwise REUTRN_FAILED
|
||||
*/
|
||||
ReturnValue_t checkActionReply(size_t replySize);
|
||||
ReturnValue_t checkActionReply(size_t replySize, const char *context);
|
||||
|
||||
/**
|
||||
* @brief Checks the position field in a star tracker upload/download reply.
|
||||
|
|
|
@ -1,7 +1,7 @@
|
|||
/**
|
||||
* @brief Auto-generated event translation file. Contains 318 translations.
|
||||
* @brief Auto-generated event translation file. Contains 324 translations.
|
||||
* @details
|
||||
* Generated on: 2023-12-13 11:29:45
|
||||
* Generated on: 2024-04-10 11:49:35
|
||||
*/
|
||||
#include "translateEvents.h"
|
||||
|
||||
|
@ -82,8 +82,11 @@ const char *BIT_LOCK_STRING = "BIT_LOCK";
|
|||
const char *BIT_LOCK_LOST_STRING = "BIT_LOCK_LOST";
|
||||
const char *FRAME_PROCESSING_FAILED_STRING = "FRAME_PROCESSING_FAILED";
|
||||
const char *CLOCK_SET_STRING = "CLOCK_SET";
|
||||
const char *CLOCK_DUMP_STRING = "CLOCK_DUMP";
|
||||
const char *CLOCK_DUMP_LEGACY_STRING = "CLOCK_DUMP_LEGACY";
|
||||
const char *CLOCK_SET_FAILURE_STRING = "CLOCK_SET_FAILURE";
|
||||
const char *CLOCK_DUMP_STRING = "CLOCK_DUMP";
|
||||
const char *CLOCK_DUMP_BEFORE_SETTING_TIME_STRING = "CLOCK_DUMP_BEFORE_SETTING_TIME";
|
||||
const char *CLOCK_DUMP_AFTER_SETTING_TIME_STRING = "CLOCK_DUMP_AFTER_SETTING_TIME";
|
||||
const char *TC_DELETION_FAILED_STRING = "TC_DELETION_FAILED";
|
||||
const char *TEST_STRING = "TEST";
|
||||
const char *CHANGE_OF_SETUP_PARAMETER_STRING = "CHANGE_OF_SETUP_PARAMETER";
|
||||
|
@ -94,7 +97,7 @@ const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR";
|
|||
const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR";
|
||||
const char *HANDLING_CFDP_REQUEST_FAILED_STRING = "HANDLING_CFDP_REQUEST_FAILED";
|
||||
const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
|
||||
const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
|
||||
const char *RATE_RECOVERY_STRING = "RATE_RECOVERY";
|
||||
const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
|
||||
const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
|
||||
const char *MEKF_RECOVERY_STRING = "MEKF_RECOVERY";
|
||||
|
@ -103,6 +106,8 @@ const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFO
|
|||
const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE";
|
||||
const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD";
|
||||
const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED";
|
||||
const char *PTG_RATE_VIOLATION_STRING = "PTG_RATE_VIOLATION";
|
||||
const char *DETUMBLE_TRANSITION_FAILED_STRING = "DETUMBLE_TRANSITION_FAILED";
|
||||
const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
|
||||
const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
|
||||
const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
|
||||
|
@ -239,6 +244,7 @@ const char *SIDE_SWITCH_TRANSITION_NOT_ALLOWED_12903_STRING = "SIDE_SWITCH_TRANS
|
|||
const char *CHILDREN_LOST_MODE_STRING = "CHILDREN_LOST_MODE";
|
||||
const char *GPS_FIX_CHANGE_STRING = "GPS_FIX_CHANGE";
|
||||
const char *CANT_GET_FIX_STRING = "CANT_GET_FIX";
|
||||
const char *RESET_FAIL_STRING = "RESET_FAIL";
|
||||
const char *P60_BOOT_COUNT_STRING = "P60_BOOT_COUNT";
|
||||
const char *BATT_MODE_STRING = "BATT_MODE";
|
||||
const char *BATT_MODE_CHANGED_STRING = "BATT_MODE_CHANGED";
|
||||
|
@ -481,9 +487,15 @@ const char *translateEvents(Event event) {
|
|||
case (8900):
|
||||
return CLOCK_SET_STRING;
|
||||
case (8901):
|
||||
return CLOCK_DUMP_STRING;
|
||||
return CLOCK_DUMP_LEGACY_STRING;
|
||||
case (8902):
|
||||
return CLOCK_SET_FAILURE_STRING;
|
||||
case (8903):
|
||||
return CLOCK_DUMP_STRING;
|
||||
case (8904):
|
||||
return CLOCK_DUMP_BEFORE_SETTING_TIME_STRING;
|
||||
case (8905):
|
||||
return CLOCK_DUMP_AFTER_SETTING_TIME_STRING;
|
||||
case (9100):
|
||||
return TC_DELETION_FAILED_STRING;
|
||||
case (9700):
|
||||
|
@ -505,7 +517,7 @@ const char *translateEvents(Event event) {
|
|||
case (11200):
|
||||
return SAFE_RATE_VIOLATION_STRING;
|
||||
case (11201):
|
||||
return SAFE_RATE_RECOVERY_STRING;
|
||||
return RATE_RECOVERY_STRING;
|
||||
case (11202):
|
||||
return MULTIPLE_RW_INVALID_STRING;
|
||||
case (11203):
|
||||
|
@ -522,6 +534,10 @@ const char *translateEvents(Event event) {
|
|||
return TLE_TOO_OLD_STRING;
|
||||
case (11209):
|
||||
return TLE_FILE_READ_FAILED_STRING;
|
||||
case (11210):
|
||||
return PTG_RATE_VIOLATION_STRING;
|
||||
case (11211):
|
||||
return DETUMBLE_TRANSITION_FAILED_STRING;
|
||||
case (11300):
|
||||
return SWITCH_CMD_SENT_STRING;
|
||||
case (11301):
|
||||
|
@ -794,6 +810,8 @@ const char *translateEvents(Event event) {
|
|||
return GPS_FIX_CHANGE_STRING;
|
||||
case (13101):
|
||||
return CANT_GET_FIX_STRING;
|
||||
case (13102):
|
||||
return RESET_FAIL_STRING;
|
||||
case (13200):
|
||||
return P60_BOOT_COUNT_STRING;
|
||||
case (13201):
|
||||
|
|
|
@ -2,7 +2,7 @@
|
|||
* @brief Auto-generated object translation file.
|
||||
* @details
|
||||
* Contains 179 translations.
|
||||
* Generated on: 2023-12-13 11:29:45
|
||||
* Generated on: 2024-04-10 11:49:35
|
||||
*/
|
||||
#include "translateObjects.h"
|
||||
|
||||
|
|
|
@ -900,6 +900,14 @@ ReturnValue_t FreshSupvHandler::parseTmPackets() {
|
|||
}
|
||||
break;
|
||||
}
|
||||
case (Apid::LATCHUP_MON): {
|
||||
if (tmReader.getServiceId() ==
|
||||
static_cast<uint8_t>(supv::tm::LatchupMonId::LATCHUP_STATUS_REPORT)) {
|
||||
handleLatchupStatusReport(receivedData);
|
||||
continue;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case (Apid::ADC_MON): {
|
||||
if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::AdcMonId::ADC_REPORT)) {
|
||||
genericHandleTm("ADC", receivedData, adcReport, supv::Apid::ADC_MON,
|
||||
|
@ -1115,7 +1123,8 @@ void FreshSupvHandler::handleEvent(EventMessage* eventMessage) {
|
|||
if (not isCommandAlreadyActive(supv::SHUTDOWN_MPSOC)) {
|
||||
CommandMessage actionMsg;
|
||||
ActionMessage::setCommand(&actionMsg, supv::SHUTDOWN_MPSOC, store_address_t::invalid());
|
||||
result = messageQueue->sendMessage(getCommandQueue(), &actionMsg);
|
||||
result = messageQueue->sendMessageFrom(getCommandQueue(), &actionMsg,
|
||||
MessageQueueIF::NO_QUEUE);
|
||||
if (result != returnvalue::OK) {
|
||||
triggerEvent(supv::SUPV_MPSOC_SHUTDOWN_BUILD_FAILED);
|
||||
sif::warning << "PlocSupervisorHandler::handleEvent: Failed to build MPSoC shutdown "
|
||||
|
@ -1390,15 +1399,17 @@ ReturnValue_t FreshSupvHandler::verifyPacket(const uint8_t* start, size_t foundL
|
|||
|
||||
ReturnValue_t FreshSupvHandler::handleBootStatusReport(const uint8_t* data) {
|
||||
ReturnValue_t result = returnvalue::OK;
|
||||
|
||||
result = verifyPacket(data, tmReader.getFullPacketLen());
|
||||
|
||||
if (result == result::CRC_FAILURE) {
|
||||
sif::error << "PlocSupervisorHandler::handleBootStatusReport: Boot status report has invalid"
|
||||
" crc"
|
||||
<< std::endl;
|
||||
return result;
|
||||
}
|
||||
PoolReadGuard pg(&bootStatusReport);
|
||||
if (pg.getReadResult() != returnvalue::OK) {
|
||||
return pg.getReadResult();
|
||||
}
|
||||
|
||||
const uint8_t* payloadStart = tmReader.getPayloadStart();
|
||||
uint16_t offset = 0;
|
||||
|
@ -1462,13 +1473,17 @@ ReturnValue_t FreshSupvHandler::handleLatchupStatusReport(const uint8_t* data) {
|
|||
ReturnValue_t result = returnvalue::OK;
|
||||
|
||||
result = verifyPacket(data, tmReader.getFullPacketLen());
|
||||
|
||||
if (result == result::CRC_FAILURE) {
|
||||
sif::error << "PlocSupervisorHandler::handleLatchupStatusReport: Latchup status report has "
|
||||
<< "invalid crc" << std::endl;
|
||||
return result;
|
||||
}
|
||||
|
||||
PoolReadGuard pg(&latchupStatusReport);
|
||||
if (pg.getReadResult() != returnvalue::OK) {
|
||||
return pg.getReadResult();
|
||||
}
|
||||
|
||||
const uint8_t* payloadData = tmReader.getPayloadStart();
|
||||
uint16_t offset = 0;
|
||||
latchupStatusReport.id = *(payloadData + offset);
|
||||
|
@ -1485,10 +1500,10 @@ ReturnValue_t FreshSupvHandler::handleLatchupStatusReport(const uint8_t* data) {
|
|||
offset += 2;
|
||||
latchupStatusReport.cnt5 = *(payloadData + offset) << 8 | *(payloadData + offset + 1);
|
||||
offset += 2;
|
||||
latchupStatusReport.cnt6 = *(payloadData + offset) << 8 | *(data + offset + 1);
|
||||
latchupStatusReport.cnt6 = *(payloadData + offset) << 8 | *(payloadData + offset + 1);
|
||||
offset += 2;
|
||||
uint16_t msec = *(payloadData + offset) << 8 | *(payloadData + offset + 1);
|
||||
latchupStatusReport.isSet = msec >> supv::LatchupStatusReport::IS_SET_BIT_POS;
|
||||
latchupStatusReport.isSynced = msec >> supv::LatchupStatusReport::IS_SET_BIT_POS;
|
||||
latchupStatusReport.timeMsec = msec & (~(1 << latchupStatusReport.IS_SET_BIT_POS));
|
||||
offset += 2;
|
||||
latchupStatusReport.timeSec = *(payloadData + offset);
|
||||
|
|
|
@ -7,6 +7,7 @@
|
|||
#include "OBSWConfig.h"
|
||||
#include "fsfw/datapool/PoolReadGuard.h"
|
||||
#include "fsfw/globalfunctions/CRC.h"
|
||||
#include "fsfw/parameters/HasParametersIF.h"
|
||||
|
||||
PlocMpsocHandler::PlocMpsocHandler(object_id_t objectId, object_id_t uartComIFid,
|
||||
CookieIF* comCookie, PlocMpsocSpecialComHelper* plocMPSoCHelper,
|
||||
|
@ -997,6 +998,7 @@ ReturnValue_t PlocMpsocHandler::handleGetHkReport(const uint8_t* data) {
|
|||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
hkReport.setValidity(true, true);
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
|
@ -1395,14 +1397,18 @@ bool PlocMpsocHandler::handleHwStartup() {
|
|||
return true;
|
||||
#endif
|
||||
if (powerState == PowerState::IDLE) {
|
||||
if (supv::SUPV_ON) {
|
||||
commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC);
|
||||
supvTransitionCd.resetTimer();
|
||||
powerState = PowerState::PENDING_STARTUP;
|
||||
if (skipSupvCommandingToOn) {
|
||||
powerState = PowerState::DONE;
|
||||
} else {
|
||||
triggerEvent(SUPV_NOT_ON, 1);
|
||||
// Set back to OFF for now, failing the transition.
|
||||
setMode(MODE_OFF);
|
||||
if (supv::SUPV_ON) {
|
||||
commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC);
|
||||
supvTransitionCd.resetTimer();
|
||||
powerState = PowerState::PENDING_STARTUP;
|
||||
} else {
|
||||
triggerEvent(SUPV_NOT_ON, 1);
|
||||
// Set back to OFF for now, failing the transition.
|
||||
setMode(MODE_OFF);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (powerState == PowerState::SUPV_FAILED) {
|
||||
|
@ -1532,3 +1538,20 @@ ReturnValue_t PlocMpsocHandler::checkModeCommand(Mode_t commandedMode, Submode_t
|
|||
}
|
||||
return DeviceHandlerBase::checkModeCommand(commandedMode, commandedSubmode, msToReachTheMode);
|
||||
}
|
||||
|
||||
ReturnValue_t PlocMpsocHandler::getParameter(uint8_t domainId, uint8_t uniqueId,
|
||||
ParameterWrapper* parameterWrapper,
|
||||
const ParameterWrapper* newValues,
|
||||
uint16_t startAtIndex) {
|
||||
if (uniqueId == mpsoc::ParamId::SKIP_SUPV_ON_COMMANDING) {
|
||||
uint8_t value = 0;
|
||||
newValues->getElement(&value);
|
||||
if (value > 1) {
|
||||
return HasParametersIF::INVALID_VALUE;
|
||||
}
|
||||
parameterWrapper->set(skipSupvCommandingToOn);
|
||||
return returnvalue::OK;
|
||||
}
|
||||
return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues,
|
||||
startAtIndex);
|
||||
}
|
||||
|
|
|
@ -201,6 +201,8 @@ class PlocMpsocHandler : public DeviceHandlerBase, public CommandsActionsIF {
|
|||
|
||||
PowerState powerState = PowerState::IDLE;
|
||||
|
||||
uint8_t skipSupvCommandingToOn = false;
|
||||
|
||||
/**
|
||||
* @brief Handles events received from the PLOC MPSoC helper
|
||||
*/
|
||||
|
@ -316,6 +318,9 @@ class PlocMpsocHandler : public DeviceHandlerBase, public CommandsActionsIF {
|
|||
pwrctrl::EnablePl enablePl = pwrctrl::EnablePl(objects::POWER_CONTROLLER);
|
||||
ReturnValue_t checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
|
||||
uint32_t* msToReachTheMode) override;
|
||||
|
||||
ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, ParameterWrapper* parameterWrapper,
|
||||
const ParameterWrapper* newValues, uint16_t startAtIndex) override;
|
||||
};
|
||||
|
||||
#endif /* BSP_Q7S_DEVICES_PLOC_PLOCMPSOCHANDLER_H_ */
|
||||
|
|
|
@ -945,15 +945,7 @@ ReturnValue_t PlocSupvUartManager::handleRunningLongerRequest() {
|
|||
break;
|
||||
}
|
||||
case Request::REQUEST_EVENT_BUFFER: {
|
||||
// result = performEventBufferRequest();
|
||||
// if (result == returnvalue::OK) {
|
||||
// triggerEvent(SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL, result);
|
||||
// } else if (result == PROCESS_TERMINATED) {
|
||||
// // Event already triggered
|
||||
// break;
|
||||
// } else {
|
||||
// triggerEvent(SUPV_EVENT_BUFFER_REQUEST_FAILED, result);
|
||||
// }
|
||||
sif::error << "Requesting event buffer is not implemented" << std::endl;
|
||||
break;
|
||||
}
|
||||
case Request::DEFAULT: {
|
||||
|
|
|
@ -13,6 +13,8 @@
|
|||
|
||||
namespace mpsoc {
|
||||
|
||||
enum ParamId : uint8_t { SKIP_SUPV_ON_COMMANDING = 0x01 };
|
||||
|
||||
enum FileAccessModes : uint8_t {
|
||||
// Opens a file, fails if the file does not exist.
|
||||
OPEN_EXISTING = 0x00,
|
||||
|
|
|
@ -49,7 +49,7 @@ static const Event SUPV_EXE_ACK_UNKNOWN_COMMAND = MAKE_EVENT(10, severity::LOW);
|
|||
extern std::atomic_bool SUPV_ON;
|
||||
static constexpr uint32_t INTER_COMMAND_DELAY = 20;
|
||||
static constexpr uint32_t BOOT_TIMEOUT_MS = 4000;
|
||||
static constexpr uint32_t MAX_TRANSITION_TIME_TO_ON_MS = BOOT_TIMEOUT_MS + 2000;
|
||||
static constexpr uint32_t MAX_TRANSITION_TIME_TO_ON_MS = BOOT_TIMEOUT_MS + 3000;
|
||||
static constexpr uint32_t MAX_TRANSITION_TIME_TO_OFF_MS = 1000;
|
||||
|
||||
namespace result {
|
||||
|
@ -1805,7 +1805,7 @@ class LatchupStatusReport : public StaticLocalDataSet<LATCHUP_RPT_SET_ENTRIES> {
|
|||
lp_var_t<uint8_t> timeMon = lp_var_t<uint8_t>(sid.objectId, PoolIds::LATCHUP_RPT_TIME_MON, this);
|
||||
lp_var_t<uint8_t> timeYear =
|
||||
lp_var_t<uint8_t>(sid.objectId, PoolIds::LATCHUP_RPT_TIME_YEAR, this);
|
||||
lp_var_t<uint8_t> isSet = lp_var_t<uint8_t>(sid.objectId, PoolIds::LATCHUP_RPT_IS_SET, this);
|
||||
lp_var_t<uint8_t> isSynced = lp_var_t<uint8_t>(sid.objectId, PoolIds::LATCHUP_RPT_IS_SET, this);
|
||||
|
||||
static const uint8_t IS_SET_BIT_POS = 15;
|
||||
};
|
||||
|
|
|
@ -59,6 +59,7 @@ class RwHandler : public DeviceHandlerBase {
|
|||
LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
|
||||
|
||||
private:
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER;
|
||||
//! [EXPORT] : [COMMENT] Action Message with invalid speed was received. Valid speeds must be in
|
||||
//! the range of [-65000; 1000] or [1000; 65000]
|
||||
static const ReturnValue_t INVALID_SPEED = MAKE_RETURN_CODE(0xA0);
|
||||
|
|
|
@ -66,8 +66,13 @@ enum Source : uint8_t {
|
|||
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
|
||||
//! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
|
||||
static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
|
||||
//! [EXPORT] : [COMMENT] The system has recovered from a safe rate rotation violation.
|
||||
static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
|
||||
//! [EXPORT] : [COMMENT] The limits for the rotation in pointing mode were violated.
|
||||
static constexpr Event PTG_RATE_VIOLATION = MAKE_EVENT(10, severity::MEDIUM);
|
||||
//! [EXPORT] : [COMMENT] The system has recovered from a rate rotation violation.
|
||||
static constexpr Event RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
|
||||
//! [EXPORT] : [COMMENT] The detumble transition has failed.
|
||||
//! //! P1: Last detumble state before failure.
|
||||
static constexpr Event DETUMBLE_TRANSITION_FAILED = MAKE_EVENT(11, severity::HIGH);
|
||||
//! [EXPORT] : [COMMENT] Multiple RWs are invalid, uncommandable and therefore higher ACS modes
|
||||
//! cannot be maintained.
|
||||
static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH);
|
||||
|
|
|
@ -32,7 +32,7 @@ ReturnValue_t ArcsecDatalinkLayer::checkRingBufForFrame(const uint8_t** decodedF
|
|||
size_t encodedDataSize = 0;
|
||||
slip_error_t slipError =
|
||||
slip_decode_frame(decodedRxFrame, &rxFrameSize, rxAnalysisBuffer + startIdx,
|
||||
idx - startIdx + 1, &encodedDataSize, ARC_DEF_SAGITTA_SLIP_ID);
|
||||
idx - startIdx + 1, &encodedDataSize, ARC_DEF_SAGITTA_SLIP_ID, 0);
|
||||
decodeRingBuf.deleteData(idx + 1);
|
||||
switch (slipError) {
|
||||
case (SLIP_OK): {
|
||||
|
@ -76,7 +76,7 @@ void ArcsecDatalinkLayer::reset() { decodeRingBuf.clear(); }
|
|||
|
||||
void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, size_t length, const uint8_t** txFrame,
|
||||
size_t& size) {
|
||||
slip_encode_frame(data, length, txEncoded, &size, ARC_DEF_SAGITTA_SLIP_ID);
|
||||
slip_encode_frame(data, length, txEncoded, sizeof(txEncoded), &size, ARC_DEF_SAGITTA_SLIP_ID);
|
||||
if (txFrame != nullptr) {
|
||||
*txFrame = txEncoded;
|
||||
}
|
||||
|
|
|
@ -5,7 +5,12 @@
|
|||
#include <mission/acs/str/strHelpers.h>
|
||||
#include <mission/acs/str/strJsonCommands.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "fsfw/filesystem/HasFileSystemIF.h"
|
||||
#include "fsfw/ipc/MessageQueueIF.h"
|
||||
#include "fsfw/returnvalues/returnvalue.h"
|
||||
#include "mission/memory/SdCardMountedIF.h"
|
||||
|
||||
extern "C" {
|
||||
#include <sagitta/client/actionreq.h>
|
||||
|
@ -16,7 +21,6 @@ extern "C" {
|
|||
}
|
||||
|
||||
#include <atomic>
|
||||
#include <fstream>
|
||||
#include <thread>
|
||||
|
||||
#include "OBSWConfig.h"
|
||||
|
@ -27,7 +31,8 @@ std::atomic_bool JCFG_DONE(false);
|
|||
|
||||
StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
|
||||
StrComHandler* strHelper, power::Switch_t powerSwitch,
|
||||
startracker::SdCardConfigPathGetter& cfgPathGetter)
|
||||
startracker::SdCardConfigPathGetter& cfgPathGetter,
|
||||
SdCardMountedIF& sdCardIF)
|
||||
: DeviceHandlerBase(objectId, comIF, comCookie),
|
||||
temperatureSet(this),
|
||||
versionSet(this),
|
||||
|
@ -58,8 +63,10 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
|
|||
centroidSet(this),
|
||||
centroidsSet(this),
|
||||
contrastSet(this),
|
||||
blobStatsSet(this),
|
||||
strHelper(strHelper),
|
||||
powerSwitch(powerSwitch),
|
||||
sdCardIF(sdCardIF),
|
||||
cfgPathGetter(cfgPathGetter) {
|
||||
if (comCookie == nullptr) {
|
||||
sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl;
|
||||
|
@ -138,6 +145,9 @@ ReturnValue_t StarTrackerHandler::initialize() {
|
|||
// delay whole satellite boot process.
|
||||
reloadJsonCfgFile();
|
||||
|
||||
// Default firmware target is always initialized from persistent file.
|
||||
loadTargetFirmwareFromPersistentCfg();
|
||||
|
||||
EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
|
||||
if (manager == nullptr) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
|
@ -165,6 +175,19 @@ ReturnValue_t StarTrackerHandler::initialize() {
|
|||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::loadTargetFirmwareFromPersistentCfg() {
|
||||
const char* prefix = sdCardIF.getCurrentMountPrefix();
|
||||
std::filesystem::path path = std::filesystem::path(prefix) / startracker::FW_TARGET_CFG_PATH;
|
||||
std::ifstream ifile(path);
|
||||
if (ifile.is_open() and !ifile.bad()) {
|
||||
std::string targetStr;
|
||||
std::getline(ifile, targetStr);
|
||||
if (targetStr == "backup") {
|
||||
firmwareTargetRaw = static_cast<uint8_t>(startracker::FirmwareTarget::BACKUP);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool StarTrackerHandler::reloadJsonCfgFile() {
|
||||
jcfgCountdown.resetTimer();
|
||||
auto strCfgPath = cfgPathGetter.getCfgPath();
|
||||
|
@ -307,21 +330,11 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu
|
|||
strHelper->setFlashReadFilename(std::string(reinterpret_cast<const char*>(data), size));
|
||||
return EXECUTION_FINISHED;
|
||||
}
|
||||
case (startracker::FIRMWARE_UPDATE): {
|
||||
result = DeviceHandlerBase::acceptExternalDeviceCommands();
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) {
|
||||
return FILE_PATH_TOO_LONG;
|
||||
}
|
||||
result =
|
||||
strHelper->startFirmwareUpdate(std::string(reinterpret_cast<const char*>(data), size));
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
strHelperHandlingSpecialRequest = true;
|
||||
return EXECUTION_FINISHED;
|
||||
case (startracker::FIRMWARE_UPDATE_MAIN): {
|
||||
return handleFirmwareUpdateCommand(data, size, startracker::FirmwareTarget::MAIN);
|
||||
}
|
||||
case (startracker::FIRMWARE_UPDATE_BACKUP): {
|
||||
return handleFirmwareUpdateCommand(data, size, startracker::FirmwareTarget::BACKUP);
|
||||
}
|
||||
default:
|
||||
break;
|
||||
|
@ -330,6 +343,23 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu
|
|||
reinitNextSetParam = true;
|
||||
return DeviceHandlerBase::executeAction(actionId, commandedBy, data, size);
|
||||
}
|
||||
ReturnValue_t StarTrackerHandler::handleFirmwareUpdateCommand(const uint8_t* data, size_t size,
|
||||
startracker::FirmwareTarget target) {
|
||||
ReturnValue_t result = DeviceHandlerBase::acceptExternalDeviceCommands();
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) {
|
||||
return FILE_PATH_TOO_LONG;
|
||||
}
|
||||
result = strHelper->startFirmwareUpdate(std::string(reinterpret_cast<const char*>(data), size),
|
||||
target);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
strHelperHandlingSpecialRequest = true;
|
||||
return EXECUTION_FINISHED;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::performOperationHook() {
|
||||
EventMessage event;
|
||||
|
@ -543,7 +573,7 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
|
|||
Clock::getClock(&tv);
|
||||
setTimeRequest.unixTime =
|
||||
(static_cast<uint64_t>(tv.tv_sec) * 1000 * 1000) + (static_cast<uint64_t>(tv.tv_usec));
|
||||
arc_pack_settime_action_req(&setTimeRequest, commandBuffer, &rawPacketLen);
|
||||
prv_arc_pack_settime_action_req(&setTimeRequest, commandBuffer, &rawPacketLen);
|
||||
size_t serLen = 0;
|
||||
// Time in milliseconds. Manual serialization because arcsec API ignores endianness.
|
||||
SerializeAdapter::serialize(&setTimeRequest.unixTime, commandBuffer + 2, &serLen,
|
||||
|
@ -568,8 +598,12 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
|
|||
prepareRequestContrastTm();
|
||||
return returnvalue::OK;
|
||||
}
|
||||
case (startracker::REQ_BLOB_STATS): {
|
||||
prepareRequestBlobStatsTm();
|
||||
return returnvalue::OK;
|
||||
}
|
||||
case (startracker::BOOT): {
|
||||
prepareBootCommand();
|
||||
prepareBootCommand(static_cast<startracker::FirmwareTarget>(firmwareTargetRaw));
|
||||
return returnvalue::OK;
|
||||
}
|
||||
case (startracker::REQ_VERSION): {
|
||||
|
@ -856,6 +890,8 @@ void StarTrackerHandler::fillCommandAndReplyMap() {
|
|||
startracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
this->insertInCommandAndReplyMap(startracker::REQ_CONTRAST, 3, &contrastSet,
|
||||
startracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
this->insertInCommandAndReplyMap(startracker::REQ_BLOB_STATS, 3, &blobStatsSet,
|
||||
startracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
|
||||
|
@ -1053,6 +1089,12 @@ void StarTrackerHandler::resetSecondaryTmSet() {
|
|||
histogramSet.setValidity(false, true);
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&blobStatsSet);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
histogramSet.setValidity(false, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void StarTrackerHandler::bootBootloader() {
|
||||
|
@ -1201,6 +1243,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
|
|||
result = handleTm(packet, histogramSet, "REQ_HISTO");
|
||||
break;
|
||||
}
|
||||
case (startracker::REQ_BLOB_STATS): {
|
||||
result = handleTm(packet, blobStatsSet, "REQ_BLOB_STATS");
|
||||
break;
|
||||
}
|
||||
case (startracker::SUBSCRIPTION):
|
||||
case (startracker::LOGLEVEL):
|
||||
case (startracker::LOGSUBSCRIPTION):
|
||||
|
@ -1602,6 +1648,13 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
|
|||
localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_C, new PoolEntry<uint32_t>(9));
|
||||
localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_D, new PoolEntry<uint32_t>(9));
|
||||
|
||||
localDataPoolMap.emplace(startracker::TICKS_BLOB_STATS, new PoolEntry<uint32_t>());
|
||||
localDataPoolMap.emplace(startracker::TIME_BLOB_STATS, new PoolEntry<uint64_t>());
|
||||
localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_NOISE, new PoolEntry<uint8_t>(16));
|
||||
localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_THOLD, new PoolEntry<uint8_t>(16));
|
||||
localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_LVALID, new PoolEntry<uint8_t>(16));
|
||||
localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_OFLOW, new PoolEntry<uint8_t>(16));
|
||||
|
||||
poolManager.subscribeForDiagPeriodicPacket(
|
||||
subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0));
|
||||
poolManager.subscribeForRegularPeriodicPacket(
|
||||
|
@ -1630,6 +1683,8 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
|
|||
subdp::RegularHkPeriodicParams(centroidsSet.getSid(), false, 10.0));
|
||||
poolManager.subscribeForRegularPeriodicPacket(
|
||||
subdp::RegularHkPeriodicParams(contrastSet.getSid(), false, 10.0));
|
||||
poolManager.subscribeForRegularPeriodicPacket(
|
||||
subdp::RegularHkPeriodicParams(blobStatsSet.getSid(), false, 10.0));
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
|
@ -1659,7 +1714,8 @@ ReturnValue_t StarTrackerHandler::checkMode(ActionId_t actionId) {
|
|||
case startracker::UPLOAD_IMAGE:
|
||||
case startracker::DOWNLOAD_IMAGE:
|
||||
case startracker::FLASH_READ:
|
||||
case startracker::FIRMWARE_UPDATE: {
|
||||
case startracker::FIRMWARE_UPDATE_BACKUP:
|
||||
case startracker::FIRMWARE_UPDATE_MAIN: {
|
||||
return DeviceHandlerBase::acceptExternalDeviceCommands();
|
||||
default:
|
||||
break;
|
||||
|
@ -1875,6 +1931,10 @@ ReturnValue_t StarTrackerHandler::scanForTmReply(uint8_t replyId, DeviceCommandI
|
|||
*foundId = startracker::REQ_BLOB;
|
||||
break;
|
||||
}
|
||||
case (startracker::ID::BLOB_STATS): {
|
||||
*foundId = startracker::REQ_BLOB_STATS;
|
||||
break;
|
||||
}
|
||||
case (startracker::ID::BLOBS): {
|
||||
*foundId = startracker::REQ_BLOBS;
|
||||
break;
|
||||
|
@ -1956,10 +2016,10 @@ ReturnValue_t StarTrackerHandler::executeFlashReadCommand(const uint8_t* command
|
|||
return result;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::prepareBootCommand() {
|
||||
void StarTrackerHandler::prepareBootCommand(startracker::FirmwareTarget target) {
|
||||
uint32_t length = 0;
|
||||
struct BootActionRequest bootRequest = {BOOT_REGION_ID};
|
||||
arc_pack_boot_action_req(&bootRequest, commandBuffer, &length);
|
||||
struct BootActionRequest bootRequest = {static_cast<uint8_t>(target)};
|
||||
prv_arc_pack_boot_action_req(&bootRequest, commandBuffer, &length);
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = length;
|
||||
}
|
||||
|
@ -1992,7 +2052,7 @@ ReturnValue_t StarTrackerHandler::prepareChecksumCommand(const uint8_t* commandD
|
|||
return result;
|
||||
}
|
||||
uint32_t rawCmdLength = 0;
|
||||
arc_pack_checksum_action_req(&req, commandBuffer, &rawCmdLength);
|
||||
prv_arc_pack_checksum_action_req(&req, commandBuffer, &rawCmdLength);
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = rawCmdLength;
|
||||
checksumCmd.rememberRegion = req.region;
|
||||
|
@ -2011,7 +2071,7 @@ void StarTrackerHandler::prepareTimeRequest() {
|
|||
void StarTrackerHandler::preparePingRequest() {
|
||||
uint32_t length = 0;
|
||||
struct PingActionRequest pingRequest = {PING_ID};
|
||||
arc_pack_ping_action_req(&pingRequest, commandBuffer, &length);
|
||||
prv_arc_pack_ping_action_req(&pingRequest, commandBuffer, &length);
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = length;
|
||||
}
|
||||
|
@ -2040,7 +2100,7 @@ void StarTrackerHandler::preparePowerRequest() {
|
|||
void StarTrackerHandler::prepareSwitchToBootloaderCmd() {
|
||||
uint32_t length = 0;
|
||||
struct RebootActionRequest rebootReq {};
|
||||
arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length);
|
||||
prv_arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length);
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = length;
|
||||
}
|
||||
|
@ -2049,7 +2109,7 @@ void StarTrackerHandler::prepareTakeImageCommand(const uint8_t* commandData) {
|
|||
uint32_t length = 0;
|
||||
struct CameraActionRequest camReq;
|
||||
camReq.actionid = *commandData;
|
||||
arc_pack_camera_action_req(&camReq, commandBuffer, &length);
|
||||
prv_arc_pack_camera_action_req(&camReq, commandBuffer, &length);
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = length;
|
||||
}
|
||||
|
@ -2115,6 +2175,14 @@ ReturnValue_t StarTrackerHandler::prepareRequestCentroidTm() {
|
|||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::prepareRequestBlobStatsTm() {
|
||||
uint32_t length = 0;
|
||||
arc_tm_pack_blobstats_req(commandBuffer, &length);
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = length;
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::prepareRequestCentroidsTm() {
|
||||
uint32_t length = 0;
|
||||
arc_tm_pack_centroids_req(commandBuffer, &length);
|
||||
|
@ -2389,7 +2457,8 @@ ReturnValue_t StarTrackerHandler::checkProgram() {
|
|||
internalState = InternalState::DONE;
|
||||
}
|
||||
break;
|
||||
case startracker::Program::FIRMWARE:
|
||||
case startracker::Program::FIRMWARE_BACKUP:
|
||||
case startracker::Program::FIRMWARE_MAIN: {
|
||||
if (startupState == StartupState::WAIT_CHECK_PROGRAM) {
|
||||
startupState = StartupState::BOOT_BOOTLOADER;
|
||||
}
|
||||
|
@ -2400,9 +2469,10 @@ ReturnValue_t StarTrackerHandler::checkProgram() {
|
|||
internalState = InternalState::FAILED_BOOTLOADER_BOOT;
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID"
|
||||
<< std::endl;
|
||||
sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID "
|
||||
<< static_cast<int>(versionSet.program.value) << std::endl;
|
||||
return INVALID_PROGRAM;
|
||||
}
|
||||
return returnvalue::OK;
|
||||
|
@ -2862,17 +2932,19 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
|
|||
case startracker::REQ_MATCHED_CENTROIDS:
|
||||
case startracker::REQ_BLOB:
|
||||
case startracker::REQ_BLOBS:
|
||||
case startracker::REQ_BLOB_STATS:
|
||||
case startracker::REQ_CENTROID:
|
||||
case startracker::REQ_CENTROIDS:
|
||||
case startracker::REQ_CONTRAST: {
|
||||
if (getMode() == MODE_ON and getSubmode() != startracker::Program::FIRMWARE) {
|
||||
if (getMode() == MODE_ON and getSubmode() != startracker::SUBMODE_FIRMWARE) {
|
||||
return STARTRACKER_NOT_RUNNING_FIRMWARE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case startracker::FIRMWARE_UPDATE:
|
||||
case startracker::FIRMWARE_UPDATE_MAIN:
|
||||
case startracker::FIRMWARE_UPDATE_BACKUP:
|
||||
case startracker::FLASH_READ:
|
||||
if (getMode() != MODE_ON or getSubmode() != startracker::Program::BOOTLOADER) {
|
||||
if (getMode() != MODE_ON or getSubmode() != startracker::SUBMODE_BOOTLOADER) {
|
||||
return STARTRACKER_NOT_RUNNING_BOOTLOADER;
|
||||
}
|
||||
break;
|
||||
|
@ -2883,3 +2955,42 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
|
|||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::acceptExternalDeviceCommands() { return returnvalue::OK; }
|
||||
|
||||
ReturnValue_t StarTrackerHandler::getParameter(uint8_t domainId, uint8_t uniqueId,
|
||||
ParameterWrapper* parameterWrapper,
|
||||
const ParameterWrapper* newValues,
|
||||
uint16_t startAtIndex) {
|
||||
auto firmwareTargetUpdate = [&](bool persistent) {
|
||||
uint8_t value = 0;
|
||||
newValues->getElement(&value);
|
||||
if (value != static_cast<uint8_t>(startracker::FirmwareTarget::MAIN) &&
|
||||
value != static_cast<uint8_t>(startracker::FirmwareTarget::BACKUP)) {
|
||||
return HasParametersIF::INVALID_VALUE;
|
||||
}
|
||||
parameterWrapper->set(firmwareTargetRaw);
|
||||
if (persistent) {
|
||||
if (sdCardIF.isSdCardUsable(std::nullopt)) {
|
||||
const char* prefix = sdCardIF.getCurrentMountPrefix();
|
||||
std::filesystem::path path =
|
||||
std::filesystem::path(prefix) / startracker::FW_TARGET_CFG_PATH;
|
||||
std::ofstream of(path, std::ofstream::out | std::ofstream::trunc);
|
||||
if (value == static_cast<uint8_t>(startracker::FirmwareTarget::MAIN)) {
|
||||
of << "main\n";
|
||||
} else {
|
||||
of << "backup\n";
|
||||
}
|
||||
} else {
|
||||
return HasFileSystemIF::FILESYSTEM_INACTIVE;
|
||||
}
|
||||
};
|
||||
return returnvalue::OK;
|
||||
};
|
||||
if (uniqueId == startracker::ParamId::FIRMWARE_TARGET) {
|
||||
return firmwareTargetUpdate(false);
|
||||
}
|
||||
if (uniqueId == startracker::ParamId::FIRMWARE_TARGET_PERSISTENT) {
|
||||
return firmwareTargetUpdate(true);
|
||||
}
|
||||
return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues,
|
||||
startAtIndex);
|
||||
}
|
||||
|
|
|
@ -11,10 +11,7 @@
|
|||
#include <set>
|
||||
#include <thread>
|
||||
|
||||
#include "OBSWConfig.h"
|
||||
#include "devices/powerSwitcherList.h"
|
||||
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
|
||||
#include "fsfw/src/fsfw/serialize/SerializeAdapter.h"
|
||||
#include "fsfw/timemanager/Countdown.h"
|
||||
|
||||
extern "C" {
|
||||
|
@ -45,7 +42,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
|
|||
*/
|
||||
StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
|
||||
StrComHandler* strHelper, power::Switch_t powerSwitch,
|
||||
startracker::SdCardConfigPathGetter& cfgPathGetter);
|
||||
startracker::SdCardConfigPathGetter& cfgPathGetter, SdCardMountedIF& sdCardIF);
|
||||
virtual ~StarTrackerHandler();
|
||||
|
||||
ReturnValue_t initialize() override;
|
||||
|
@ -61,6 +58,9 @@ class StarTrackerHandler : public DeviceHandlerBase {
|
|||
|
||||
Submode_t getInitialSubmode() override;
|
||||
|
||||
ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, ParameterWrapper* parameterWrapper,
|
||||
const ParameterWrapper* newValues, uint16_t startAtIndex) override;
|
||||
|
||||
protected:
|
||||
void doStartUp() override;
|
||||
void doShutDown() override;
|
||||
|
@ -161,7 +161,8 @@ class StarTrackerHandler : public DeviceHandlerBase {
|
|||
|
||||
// Ping request will reply ping with this ID (data field)
|
||||
static const uint32_t PING_ID = 0x55;
|
||||
static const uint32_t BOOT_REGION_ID = 1;
|
||||
uint8_t firmwareTargetRaw = static_cast<uint8_t>(startracker::FirmwareTarget::MAIN);
|
||||
|
||||
static const MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING;
|
||||
static const uint32_t MUTEX_TIMEOUT = 20;
|
||||
static const uint32_t BOOT_TIMEOUT = 1000;
|
||||
|
@ -215,6 +216,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
|
|||
startracker::CentroidSet centroidSet;
|
||||
startracker::CentroidsSet centroidsSet;
|
||||
startracker::ContrastSet contrastSet;
|
||||
startracker::BlobStatsSet blobStatsSet;
|
||||
|
||||
// Pointer to object responsible for uploading and downloading images to/from the star tracker
|
||||
StrComHandler* strHelper = nullptr;
|
||||
|
@ -314,12 +316,14 @@ class StarTrackerHandler : public DeviceHandlerBase {
|
|||
std::set<DeviceCommandId_t> additionalRequestedTm{};
|
||||
std::set<DeviceCommandId_t>::iterator currentSecondaryTmIter;
|
||||
|
||||
SdCardMountedIF& sdCardIF;
|
||||
startracker::SdCardConfigPathGetter& cfgPathGetter;
|
||||
|
||||
/**
|
||||
* @brief Handles internal state
|
||||
*/
|
||||
void handleInternalState();
|
||||
void loadTargetFirmwareFromPersistentCfg();
|
||||
|
||||
/**
|
||||
* @brief Checks mode for commands requiring MODE_ON of MODE_NORMAL for execution.
|
||||
|
@ -380,7 +384,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
|
|||
* @brief Fills command buffer with data to boot image (works only when star tracker is
|
||||
* in bootloader mode).
|
||||
*/
|
||||
void prepareBootCommand();
|
||||
void prepareBootCommand(startracker::FirmwareTarget target);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with command to get the checksum of a flash part
|
||||
|
@ -467,6 +471,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
|
|||
ReturnValue_t prepareRequestCentroidTm();
|
||||
ReturnValue_t prepareRequestCentroidsTm();
|
||||
ReturnValue_t prepareRequestContrastTm();
|
||||
ReturnValue_t prepareRequestBlobStatsTm();
|
||||
ReturnValue_t prepareRequestTrackingParams();
|
||||
ReturnValue_t prepareRequestValidationParams();
|
||||
ReturnValue_t prepareRequestAlgoParams();
|
||||
|
@ -550,6 +555,9 @@ class StarTrackerHandler : public DeviceHandlerBase {
|
|||
void bootBootloader();
|
||||
bool reloadJsonCfgFile();
|
||||
ReturnValue_t acceptExternalDeviceCommands() override;
|
||||
|
||||
ReturnValue_t handleFirmwareUpdateCommand(const uint8_t* data, size_t size,
|
||||
startracker::FirmwareTarget target);
|
||||
};
|
||||
|
||||
#endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */
|
||||
|
|
|
@ -14,6 +14,12 @@ namespace startracker {
|
|||
static const Submode_t SUBMODE_BOOTLOADER = 1;
|
||||
static const Submode_t SUBMODE_FIRMWARE = 2;
|
||||
|
||||
enum class FirmwareTarget : uint8_t { MAIN = 1, BACKUP = 10 };
|
||||
|
||||
static constexpr char FW_TARGET_CFG_PATH[] = "startracker/fw-target.txt";
|
||||
|
||||
enum ParamId : uint32_t { FIRMWARE_TARGET = 1, FIRMWARE_TARGET_PERSISTENT = 2 };
|
||||
|
||||
class SdCardConfigPathGetter {
|
||||
public:
|
||||
virtual ~SdCardConfigPathGetter() = default;
|
||||
|
@ -322,6 +328,13 @@ enum PoolIds : lp_id_t {
|
|||
CONTRAST_B,
|
||||
CONTRAST_C,
|
||||
CONTRAST_D,
|
||||
|
||||
TICKS_BLOB_STATS,
|
||||
TIME_BLOB_STATS,
|
||||
BLOB_STATS_NOISE,
|
||||
BLOB_STATS_THOLD,
|
||||
BLOB_STATS_LVALID,
|
||||
BLOB_STATS_OFLOW,
|
||||
};
|
||||
|
||||
static const DeviceCommandId_t PING_REQUEST = 0;
|
||||
|
@ -373,7 +386,7 @@ static const DeviceCommandId_t REQ_DEBUG_CAMERA = 80;
|
|||
static const DeviceCommandId_t LOGLEVEL = 81;
|
||||
static const DeviceCommandId_t LOGSUBSCRIPTION = 82;
|
||||
static const DeviceCommandId_t DEBUG_CAMERA = 83;
|
||||
static const DeviceCommandId_t FIRMWARE_UPDATE = 84;
|
||||
static const DeviceCommandId_t FIRMWARE_UPDATE_MAIN = 84;
|
||||
static const DeviceCommandId_t DISABLE_TIMESTAMP_GENERATION = 85;
|
||||
static const DeviceCommandId_t ENABLE_TIMESTAMP_GENERATION = 86;
|
||||
static constexpr DeviceCommandId_t SET_TIME_FROM_SYS_TIME = 87;
|
||||
|
@ -388,6 +401,9 @@ static constexpr DeviceCommandId_t ADD_SECONDARY_TM_TO_NORMAL_MODE = 95;
|
|||
static constexpr DeviceCommandId_t RESET_SECONDARY_TM_SET = 96;
|
||||
static constexpr DeviceCommandId_t READ_SECONDARY_TM_SET = 97;
|
||||
static constexpr DeviceCommandId_t RELOAD_JSON_CFG_FILE = 100;
|
||||
static constexpr DeviceCommandId_t FIRMWARE_UPDATE_BACKUP = 101;
|
||||
static constexpr DeviceCommandId_t REQ_BLOB_STATS = 102;
|
||||
|
||||
static const DeviceCommandId_t NONE = 0xFFFFFFFF;
|
||||
|
||||
static const uint32_t VERSION_SET_ID = REQ_VERSION;
|
||||
|
@ -419,6 +435,7 @@ static const uint32_t BLOBS_SET_ID = REQ_BLOBS;
|
|||
static const uint32_t CENTROID_SET_ID = REQ_CENTROID;
|
||||
static const uint32_t CENTROIDS_SET_ID = REQ_CENTROIDS;
|
||||
static const uint32_t CONTRAST_SET_ID = REQ_CONTRAST;
|
||||
static const uint32_t BLOB_STATS_SET_ID = REQ_BLOB_STATS;
|
||||
|
||||
/** Max size of unencoded frame */
|
||||
static const size_t MAX_FRAME_SIZE = 1200;
|
||||
|
@ -485,11 +502,13 @@ static constexpr uint8_t CENTROID = 26;
|
|||
static constexpr uint8_t CENTROIDS = 37;
|
||||
static constexpr uint8_t AUTO_BLOB = 39;
|
||||
static constexpr uint8_t MATCHED_CENTROIDS = 40;
|
||||
static constexpr uint8_t BLOB_STATS = 49;
|
||||
} // namespace ID
|
||||
|
||||
namespace Program {
|
||||
static const uint8_t BOOTLOADER = 1;
|
||||
static const uint8_t FIRMWARE = 2;
|
||||
static const uint8_t FIRMWARE_MAIN = 2;
|
||||
static const uint8_t FIRMWARE_BACKUP = 3;
|
||||
} // namespace Program
|
||||
|
||||
namespace region_secrets {
|
||||
|
@ -509,7 +528,7 @@ static const uint32_t REGION_12_SECRET = 0x42fedef6;
|
|||
static const uint32_t REGION_13_SECRET = 0xe53cf10d;
|
||||
static const uint32_t REGION_14_SECRET = 0xe862b70b;
|
||||
static const uint32_t REGION_15_SECRET = 0x79b537ca;
|
||||
static const uint32_t secret[16]{
|
||||
static const uint32_t SECRETS[16]{
|
||||
REGION_0_SECRET, REGION_1_SECRET, REGION_2_SECRET, REGION_3_SECRET,
|
||||
REGION_4_SECRET, REGION_5_SECRET, REGION_6_SECRET, REGION_7_SECRET,
|
||||
REGION_8_SECRET, REGION_9_SECRET, REGION_10_SECRET, REGION_11_SECRET,
|
||||
|
@ -538,7 +557,12 @@ enum class FlashSections : uint8_t {
|
|||
};
|
||||
|
||||
// Flash region IDs of firmware partition
|
||||
enum class FirmwareRegions : uint32_t { FIRST = 1, LAST = 8 };
|
||||
enum class FirmwareRegions : uint32_t {
|
||||
FIRST_MAIN = 1,
|
||||
LAST_MAIN = 8,
|
||||
FIRST_BACKUP = 10,
|
||||
LAST_BACKUP = 16
|
||||
};
|
||||
|
||||
static const uint32_t FLASH_REGION_SIZE = 0x20000;
|
||||
|
||||
|
@ -1545,6 +1569,23 @@ class CentroidsSet : public StaticLocalDataSet<10> {
|
|||
lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::CENTROIDS_MAGNITUDES, this);
|
||||
};
|
||||
|
||||
class BlobStatsSet : public StaticLocalDataSet<6> {
|
||||
public:
|
||||
BlobStatsSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, BLOB_STATS_SET_ID) {}
|
||||
|
||||
// Data received from the Centroids Telemetry Set (ID 49)
|
||||
lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::TICKS_BLOB_STATS, this);
|
||||
lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId, PoolIds::TIME_BLOB_STATS, this);
|
||||
lp_vec_t<uint8_t, 16> noise =
|
||||
lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_NOISE, this);
|
||||
lp_vec_t<uint8_t, 16> thold =
|
||||
lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_THOLD, this);
|
||||
lp_vec_t<uint8_t, 16> lvalid =
|
||||
lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_LVALID, this);
|
||||
lp_vec_t<uint8_t, 16> oflow =
|
||||
lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_OFLOW, this);
|
||||
};
|
||||
|
||||
class ContrastSet : public StaticLocalDataSet<8> {
|
||||
public:
|
||||
ContrastSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CONTRAST_SET_ID) {}
|
||||
|
|
|
@ -6,6 +6,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMoun
|
|||
sdcMan(sdcMan),
|
||||
attitudeEstimation(&acsParameters),
|
||||
fusedRotationEstimation(&acsParameters),
|
||||
navigation(&acsParameters),
|
||||
guidance(&acsParameters),
|
||||
safeCtrl(&acsParameters),
|
||||
ptgCtrl(&acsParameters),
|
||||
|
@ -49,7 +50,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
|
|||
case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: {
|
||||
ReturnValue_t result = guidance.solarArrayDeploymentComplete();
|
||||
if (result == returnvalue::FAILED) {
|
||||
return FILE_DELETION_FAILED;
|
||||
return acsctrl::FILE_DELETION_FAILED;
|
||||
}
|
||||
return HasActionsIF::EXECUTION_FINISHED;
|
||||
}
|
||||
|
@ -63,7 +64,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
|
|||
}
|
||||
case UPDATE_TLE: {
|
||||
if (size != 69 * 2) {
|
||||
return INVALID_PARAMETERS;
|
||||
return HasActionsIF::INVALID_PARAMETERS;
|
||||
}
|
||||
ReturnValue_t result = updateTle(data, data + 69, false);
|
||||
if (result != returnvalue::OK) {
|
||||
|
@ -84,8 +85,11 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
|
|||
}
|
||||
std::memcpy(tle + 69, line2, 69);
|
||||
actionHelper.reportData(commandedBy, actionId, tle, 69 * 2);
|
||||
return EXECUTION_FINISHED;
|
||||
return HasActionsIF::EXECUTION_FINISHED;
|
||||
}
|
||||
case (UPDATE_MEKF_STANDARD_DEVIATIONS):
|
||||
navigation.updateMekfStandardDeviations(&acsParameters);
|
||||
return HasActionsIF::EXECUTION_FINISHED;
|
||||
default: {
|
||||
return HasActionsIF::INVALID_ACTION_ID;
|
||||
}
|
||||
|
@ -173,10 +177,11 @@ void AcsController::performAttitudeControl() {
|
|||
&susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData);
|
||||
fusedRotationEstimation.estimateFusedRotationRate(
|
||||
&susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
|
||||
mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
|
||||
&attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
|
||||
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||
&susDataProcessed, &attitudeEstimationData, &acsParameters);
|
||||
&susDataProcessed, timeDelta, &attitudeEstimationData,
|
||||
acsParameters.kalmanFilterParameters.allowStr);
|
||||
|
||||
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
|
||||
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
||||
|
@ -195,6 +200,8 @@ void AcsController::performAttitudeControl() {
|
|||
mekfInvalidFlag = false;
|
||||
}
|
||||
|
||||
handleDetumbling();
|
||||
|
||||
switch (mode) {
|
||||
case acs::SAFE:
|
||||
switch (submode) {
|
||||
|
@ -284,33 +291,6 @@ void AcsController::performSafe() {
|
|||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
|
||||
|
||||
// detumble check and switch
|
||||
if (acsParameters.safeModeControllerParameters.useMekf) {
|
||||
if (attitudeEstimationData.satRotRateMekf.isValid() and
|
||||
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) >
|
||||
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
||||
detumbleCounter++;
|
||||
}
|
||||
} else if (acsParameters.safeModeControllerParameters.useGyr) {
|
||||
if (gyrDataProcessed.gyrVecTot.isValid() and
|
||||
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
|
||||
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
||||
detumbleCounter++;
|
||||
}
|
||||
} else if (fusedRotRateData.rotRateTotal.isValid() and
|
||||
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
|
||||
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
||||
detumbleCounter++;
|
||||
} else if (detumbleCounter > 0) {
|
||||
detumbleCounter -= 1;
|
||||
}
|
||||
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
|
||||
detumbleCounter = 0;
|
||||
// Triggers detumble mode transition in subsystem
|
||||
triggerEvent(acs::SAFE_RATE_VIOLATION);
|
||||
startTransition(mode, acs::SafeSubmode::DETUMBLE);
|
||||
}
|
||||
|
||||
updateCtrlValData(errAng, safeCtrlStrat);
|
||||
updateActuatorCmdData(cmdDipoleMtqs);
|
||||
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
||||
|
@ -346,33 +326,6 @@ void AcsController::performDetumble() {
|
|||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
|
||||
|
||||
if (acsParameters.safeModeControllerParameters.useMekf) {
|
||||
if (attitudeEstimationData.satRotRateMekf.isValid() and
|
||||
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) <
|
||||
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
||||
detumbleCounter++;
|
||||
}
|
||||
} else if (acsParameters.safeModeControllerParameters.useGyr) {
|
||||
if (gyrDataProcessed.gyrVecTot.isValid() and
|
||||
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
|
||||
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
||||
detumbleCounter++;
|
||||
}
|
||||
} else if (fusedRotRateData.rotRateTotal.isValid() and
|
||||
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
|
||||
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
||||
detumbleCounter++;
|
||||
} else if (detumbleCounter > 0) {
|
||||
detumbleCounter -= 1;
|
||||
}
|
||||
|
||||
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
|
||||
detumbleCounter = 0;
|
||||
// Triggers safe mode transition in subsystem
|
||||
triggerEvent(acs::SAFE_RATE_RECOVERY);
|
||||
startTransition(mode, acs::SafeSubmode::DEFAULT);
|
||||
}
|
||||
|
||||
updateCtrlValData(safeCtrlStrat);
|
||||
updateActuatorCmdData(cmdDipoleMtqs);
|
||||
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
||||
|
@ -412,6 +365,7 @@ void AcsController::performPointingCtrl() {
|
|||
triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
|
||||
ptgCtrlLostCounter = 0;
|
||||
}
|
||||
guidance.resetValues();
|
||||
updateCtrlValData(ptgCtrlStrat);
|
||||
updateActuatorCmdData(ZERO_VEC4, cmdSpeedRws, ZERO_VEC3_INT16);
|
||||
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
|
||||
|
@ -445,10 +399,10 @@ void AcsController::performPointingCtrl() {
|
|||
break;
|
||||
}
|
||||
|
||||
uint8_t enableAntiStiction = true;
|
||||
bool allRwAvailable = true;
|
||||
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
|
||||
if (result == returnvalue::FAILED) {
|
||||
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv, &rwAvail);
|
||||
if (result == acsctrl::MULTIPLE_RW_UNAVAILABLE) {
|
||||
if (multipleRwUnavailableCounter >=
|
||||
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
|
||||
triggerEvent(acs::MULTIPLE_RW_INVALID);
|
||||
|
@ -456,8 +410,10 @@ void AcsController::performPointingCtrl() {
|
|||
}
|
||||
multipleRwUnavailableCounter++;
|
||||
return;
|
||||
} else {
|
||||
multipleRwUnavailableCounter = 0;
|
||||
}
|
||||
multipleRwUnavailableCounter = 0;
|
||||
if (result == acsctrl::SINGLE_RW_UNAVAILABLE) {
|
||||
allRwAvailable = false;
|
||||
}
|
||||
|
||||
// Variables required for guidance
|
||||
|
@ -469,48 +425,46 @@ void AcsController::performPointingCtrl() {
|
|||
|
||||
switch (mode) {
|
||||
case acs::PTG_IDLE:
|
||||
guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat,
|
||||
targetSatRotRate);
|
||||
guidance.targetQuatPtgIdle(timeAbsolute, timeDelta, susDataProcessed.sunIjkModel.value,
|
||||
gpsDataProcessed.gpsPosition.value, targetQuat, targetSatRotRate);
|
||||
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
|
||||
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.idleModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
||||
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
|
||||
allRwAvailable, &rwAvail, &acsParameters.idleModeControllerParameters,
|
||||
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
break;
|
||||
|
||||
case acs::PTG_TARGET:
|
||||
guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
|
||||
gpsDataProcessed.gpsVelocity.value, targetQuat,
|
||||
targetSatRotRate);
|
||||
guidance.targetQuatPtgTarget(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
|
||||
gpsDataProcessed.gpsVelocity.value, targetQuat,
|
||||
targetSatRotRate);
|
||||
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
|
||||
acsParameters.targetModeControllerParameters.quatRef,
|
||||
acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
|
||||
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.targetModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
||||
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
allRwAvailable, &rwAvail, &acsParameters.targetModeControllerParameters,
|
||||
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
break;
|
||||
|
||||
case acs::PTG_TARGET_GS:
|
||||
|
@ -520,42 +474,39 @@ void AcsController::performPointingCtrl() {
|
|||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
|
||||
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.gsTargetModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
||||
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
|
||||
allRwAvailable, &rwAvail, &acsParameters.gsTargetModeControllerParameters,
|
||||
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
break;
|
||||
|
||||
case acs::PTG_NADIR:
|
||||
guidance.targetQuatPtgNadirThreeAxes(
|
||||
timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
|
||||
gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate);
|
||||
guidance.targetQuatPtgNadir(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
|
||||
gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate);
|
||||
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
|
||||
acsParameters.nadirModeControllerParameters.quatRef,
|
||||
acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
|
||||
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.nadirModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
||||
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
|
||||
allRwAvailable, &rwAvail, &acsParameters.nadirModeControllerParameters,
|
||||
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
break;
|
||||
|
||||
case acs::PTG_INERTIAL:
|
||||
|
@ -567,18 +518,17 @@ void AcsController::performPointingCtrl() {
|
|||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
|
||||
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.inertialModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
||||
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
|
||||
allRwAvailable, &rwAvail, &acsParameters.inertialModeControllerParameters,
|
||||
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
break;
|
||||
default:
|
||||
sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
|
||||
|
@ -590,9 +540,7 @@ void AcsController::performPointingCtrl() {
|
|||
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);
|
||||
}
|
||||
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
|
||||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
|
||||
|
||||
|
@ -604,6 +552,74 @@ void AcsController::performPointingCtrl() {
|
|||
acsParameters.rwHandlingParameters.rampTime);
|
||||
}
|
||||
|
||||
void AcsController::handleDetumbling() {
|
||||
switch (detumbleState) {
|
||||
case DetumbleState::NO_DETUMBLE:
|
||||
if (fusedRotRateData.rotRateTotal.isValid() and
|
||||
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
|
||||
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
||||
detumbleCounter++;
|
||||
} else if (detumbleCounter > 0) {
|
||||
detumbleCounter -= 1;
|
||||
}
|
||||
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
|
||||
if (mode == acs::AcsMode::SAFE) {
|
||||
detumbleState = DetumbleState::DETUMBLE_FROM_SAFE;
|
||||
break;
|
||||
}
|
||||
detumbleState = DetumbleState::DETUMBLE_FROM_PTG;
|
||||
}
|
||||
break;
|
||||
case DetumbleState::DETUMBLE_FROM_PTG:
|
||||
triggerEvent(acs::PTG_RATE_VIOLATION);
|
||||
detumbleTransitionCountdow.resetTimer();
|
||||
detumbleState = DetumbleState::PTG_TO_SAFE_TRANSITION;
|
||||
break;
|
||||
case DetumbleState::PTG_TO_SAFE_TRANSITION:
|
||||
if (detumbleTransitionCountdow.hasTimedOut()) {
|
||||
triggerEvent(acs::DETUMBLE_TRANSITION_FAILED, 2);
|
||||
detumbleCounter = 0;
|
||||
detumbleState = DetumbleState::NO_DETUMBLE;
|
||||
break;
|
||||
}
|
||||
if (mode == acs::AcsMode::SAFE) {
|
||||
detumbleState = DetumbleState::DETUMBLE_FROM_SAFE;
|
||||
}
|
||||
break;
|
||||
case DetumbleState::DETUMBLE_FROM_SAFE:
|
||||
detumbleCounter = 0;
|
||||
// Triggers detumble mode transition in subsystem
|
||||
if (mode == acs::AcsMode::SAFE) {
|
||||
triggerEvent(acs::SAFE_RATE_VIOLATION);
|
||||
startTransition(mode, acs::SafeSubmode::DETUMBLE);
|
||||
detumbleState = DetumbleState::IN_DETUMBLE;
|
||||
break;
|
||||
}
|
||||
triggerEvent(acs::DETUMBLE_TRANSITION_FAILED, 3);
|
||||
detumbleState = DetumbleState::NO_DETUMBLE;
|
||||
break;
|
||||
case DetumbleState::IN_DETUMBLE:
|
||||
if (fusedRotRateData.rotRateTotal.isValid() and
|
||||
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
|
||||
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
||||
detumbleCounter++;
|
||||
} else if (detumbleCounter > 0) {
|
||||
detumbleCounter -= 1;
|
||||
}
|
||||
|
||||
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
|
||||
detumbleCounter = 0;
|
||||
// Triggers safe mode transition in subsystem
|
||||
triggerEvent(acs::RATE_RECOVERY);
|
||||
startTransition(mode, acs::SafeSubmode::DEFAULT);
|
||||
detumbleState = DetumbleState::NO_DETUMBLE;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
sif::error << "AcsController: Invalid DetumbleState" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) {
|
||||
if (not safeCtrlFailureFlag) {
|
||||
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure);
|
||||
|
@ -885,6 +901,31 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
|
|||
}
|
||||
|
||||
void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
|
||||
guidance.resetValues();
|
||||
if (mode == acs::AcsMode::SAFE) {
|
||||
{
|
||||
PoolReadGuard pg(&rw1SpeedSet);
|
||||
rw1SpeedSet.setRwSpeed(0, 10);
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&rw2SpeedSet);
|
||||
rw2SpeedSet.setRwSpeed(0, 10);
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&rw3SpeedSet);
|
||||
rw3SpeedSet.setRwSpeed(0, 10);
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&rw4SpeedSet);
|
||||
rw4SpeedSet.setRwSpeed(0, 10);
|
||||
}
|
||||
}
|
||||
if (submode == acs::SafeSubmode::DETUMBLE) {
|
||||
detumbleState = DetumbleState::IN_DETUMBLE;
|
||||
}
|
||||
if (detumbleState == DetumbleState::IN_DETUMBLE and submode != acs::SafeSubmode::DETUMBLE) {
|
||||
detumbleState = DetumbleState::NO_DETUMBLE;
|
||||
}
|
||||
return ExtendedControllerBase::modeChanged(mode, submode);
|
||||
}
|
||||
|
||||
|
@ -1081,7 +1122,7 @@ ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) {
|
|||
tleFile << "\n";
|
||||
tleFile.write(reinterpret_cast<const char *>(tle + 69), 69);
|
||||
} else {
|
||||
return WRITE_FILE_FAILED;
|
||||
return acsctrl::WRITE_FILE_FAILED;
|
||||
}
|
||||
tleFile.close();
|
||||
return returnvalue::OK;
|
||||
|
@ -1105,12 +1146,12 @@ ReturnValue_t AcsController::readTleFromFs(uint8_t *line1, uint8_t *line2) {
|
|||
std::memcpy(line2, tleLine2.c_str(), 69);
|
||||
} else {
|
||||
triggerEvent(acs::TLE_FILE_READ_FAILED);
|
||||
return READ_FILE_FAILED;
|
||||
return acsctrl::READ_FILE_FAILED;
|
||||
}
|
||||
tleFile.close();
|
||||
} else {
|
||||
triggerEvent(acs::TLE_FILE_READ_FAILED);
|
||||
return READ_FILE_FAILED;
|
||||
return acsctrl::READ_FILE_FAILED;
|
||||
}
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
|
|
@ -46,11 +46,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
|||
uint16_t startAtIndex) override;
|
||||
|
||||
protected:
|
||||
void performAttitudeControl();
|
||||
void performSafe();
|
||||
void performDetumble();
|
||||
void performPointingCtrl();
|
||||
|
||||
private:
|
||||
static constexpr int16_t ZERO_VEC3_INT16[3] = {0, 0, 0};
|
||||
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
|
||||
|
@ -94,6 +89,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
|||
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
|
||||
int16_t cmdDipoleMtqs[3] = {0, 0, 0};
|
||||
|
||||
acsctrl::RwAvail rwAvail;
|
||||
|
||||
#if OBSW_THREAD_TRACING == 1
|
||||
uint32_t opCounter = 0;
|
||||
#endif
|
||||
|
@ -101,20 +98,22 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
|||
enum class InternalState { STARTUP, INITIAL_DELAY, READY };
|
||||
InternalState internalState = InternalState::STARTUP;
|
||||
|
||||
enum class DetumbleState {
|
||||
NO_DETUMBLE,
|
||||
DETUMBLE_FROM_PTG,
|
||||
PTG_TO_SAFE_TRANSITION,
|
||||
DETUMBLE_FROM_SAFE,
|
||||
IN_DETUMBLE
|
||||
};
|
||||
DetumbleState detumbleState = DetumbleState::NO_DETUMBLE;
|
||||
|
||||
/** Device command IDs */
|
||||
static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
|
||||
static const DeviceCommandId_t RESET_MEKF = 0x1;
|
||||
static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;
|
||||
static const DeviceCommandId_t UPDATE_TLE = 0x3;
|
||||
static const DeviceCommandId_t READ_TLE = 0x4;
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
|
||||
//! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent.
|
||||
static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0);
|
||||
//! [EXPORT] : [COMMENT] Writing the TLE to the file has failed.
|
||||
static constexpr ReturnValue_t WRITE_FILE_FAILED = MAKE_RETURN_CODE(1);
|
||||
//! [EXPORT] : [COMMENT] Reading the TLE to the file has failed.
|
||||
static constexpr ReturnValue_t READ_FILE_FAILED = MAKE_RETURN_CODE(2);
|
||||
static const DeviceCommandId_t UPDATE_MEKF_STANDARD_DEVIATIONS = 0x5;
|
||||
|
||||
ReturnValue_t initialize() override;
|
||||
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
|
||||
|
@ -134,6 +133,13 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
|||
void modeChanged(Mode_t mode, Submode_t submode);
|
||||
void announceMode(bool recursive);
|
||||
|
||||
void performAttitudeControl();
|
||||
void performSafe();
|
||||
void performDetumble();
|
||||
void performPointingCtrl();
|
||||
|
||||
void handleDetumbling();
|
||||
|
||||
void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure);
|
||||
|
||||
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
|
||||
|
@ -280,6 +286,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
|||
|
||||
// Initial delay to make sure all pool variables have been initialized their owners
|
||||
Countdown initialCountdown = Countdown(INIT_DELAY);
|
||||
|
||||
// Countdown after which the detumbling mode change should have been finished
|
||||
static constexpr dur_millis_t MAX_DURATION = 60 * 1e3;
|
||||
Countdown detumbleTransitionCountdow = Countdown(MAX_DURATION);
|
||||
};
|
||||
|
||||
#endif /* MISSION_CONTROLLER_ACSCONTROLLER_H_ */
|
||||
|
|
|
@ -182,7 +182,7 @@ void PowerController::calculateStateOfCharge() {
|
|||
}
|
||||
|
||||
// calculate total battery current
|
||||
iBat = p60CoreHk.batteryCurrent.value + bpxBatteryHk.dischargeCurrent.value;
|
||||
iBat = p60CoreHk.batteryCurrent.value - bpxBatteryHk.dischargeCurrent.value;
|
||||
|
||||
result = calculateOpenCircuitVoltageCharge();
|
||||
if (result != returnvalue::OK) {
|
||||
|
|
|
@ -57,9 +57,9 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
|
|||
float batteryMaximumCapacity = 2.6 * 2; // [Ah]
|
||||
float coulombCounterVoltageUpperThreshold = 16.2; // [V]
|
||||
double maxAllowedTimeDiff = 1.5; // [s]
|
||||
float payloadOpLimitOn = 0.90; // [%]
|
||||
float payloadOpLimitLow = 0.75; // [%]
|
||||
float higherModesLimit = 0.6; // [%]
|
||||
float payloadOpLimitOn = 0.80; // [%]
|
||||
float payloadOpLimitLow = 0.65; // [%]
|
||||
float higherModesLimit = 0.60; // [%]
|
||||
|
||||
// OCV Look-up-Table {[Ah],[V]}
|
||||
static constexpr uint8_t LOOK_UP_TABLE_MAX_IDX = 99;
|
||||
|
|
|
@ -1641,7 +1641,11 @@ bool ThermalController::chooseHeater(heater::Switch& switchNr, heater::Switch re
|
|||
bool heaterAvailable = true;
|
||||
|
||||
HasHealthIF::HealthState mainHealth = heaterHandler.getHealth(switchNr);
|
||||
heater::SwitchState mainState = heaterHandler.getSwitchState(switchNr);
|
||||
HasHealthIF::HealthState redHealth = heaterHandler.getHealth(redSwitchNr);
|
||||
if (mainHealth == HasHealthIF::EXTERNAL_CONTROL and mainState == heater::SwitchState::ON) {
|
||||
return false;
|
||||
}
|
||||
if (mainHealth != HasHealthIF::HEALTHY) {
|
||||
if (redHealth == HasHealthIF::HEALTHY) {
|
||||
switchNr = redSwitchNr;
|
||||
|
@ -1656,6 +1660,7 @@ bool ThermalController::chooseHeater(heater::Switch& switchNr, heater::Switch re
|
|||
} else {
|
||||
ctrlCtx.redSwitchNrInUse = false;
|
||||
}
|
||||
|
||||
return heaterAvailable;
|
||||
}
|
||||
|
||||
|
@ -1792,7 +1797,8 @@ void ThermalController::heaterMaxDurationControl(
|
|||
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
|
||||
if (heaterHandler.getHealth(static_cast<heater::Switch>(i)) != HasHealthIF::EXTERNAL_CONTROL and
|
||||
currentHeaterStates[i] == heater::SwitchState::ON and
|
||||
heaterStates[i].trackHeaterMaxBurnTime and
|
||||
heaterStates[i].heaterOnMaxBurnTime.hasTimedOut()) {
|
||||
heaterStates[i].switchTransition = false;
|
||||
|
|
|
@ -333,16 +333,16 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
|||
parameterWrapper->setMatrix(rwMatrices.pseudoInverse);
|
||||
break;
|
||||
case 0x2:
|
||||
parameterWrapper->setMatrix(rwMatrices.without1);
|
||||
parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW1);
|
||||
break;
|
||||
case 0x3:
|
||||
parameterWrapper->setMatrix(rwMatrices.without2);
|
||||
parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW2);
|
||||
break;
|
||||
case 0x4:
|
||||
parameterWrapper->setMatrix(rwMatrices.without3);
|
||||
parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW3);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->setMatrix(rwMatrices.without4);
|
||||
parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW4);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->setVector(rwMatrices.nullspaceVector);
|
||||
|
@ -432,9 +432,6 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
|||
parameterWrapper->set(idleModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
case 0xA:
|
||||
parameterWrapper->set(idleModeControllerParameters.useMekf);
|
||||
break;
|
||||
default:
|
||||
|
@ -471,42 +468,39 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
|||
parameterWrapper->set(targetModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
case 0xA:
|
||||
parameterWrapper->set(targetModeControllerParameters.useMekf);
|
||||
break;
|
||||
case 0xB:
|
||||
case 0xA:
|
||||
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
|
||||
break;
|
||||
case 0xC:
|
||||
case 0xB:
|
||||
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
|
||||
break;
|
||||
case 0xD:
|
||||
case 0xC:
|
||||
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
|
||||
break;
|
||||
case 0xE:
|
||||
case 0xD:
|
||||
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
|
||||
break;
|
||||
case 0xF:
|
||||
case 0xE:
|
||||
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
|
||||
break;
|
||||
case 0x10:
|
||||
case 0xF:
|
||||
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
|
||||
break;
|
||||
case 0x11:
|
||||
case 0x10:
|
||||
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
|
||||
break;
|
||||
case 0x12:
|
||||
case 0x11:
|
||||
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
|
||||
break;
|
||||
case 0x13:
|
||||
case 0x12:
|
||||
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
|
||||
break;
|
||||
case 0x14:
|
||||
case 0x13:
|
||||
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
|
||||
break;
|
||||
case 0x15:
|
||||
case 0x14:
|
||||
parameterWrapper->set(targetModeControllerParameters.blindRotRate);
|
||||
break;
|
||||
default:
|
||||
|
@ -543,26 +537,26 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
|||
parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
case 0xA:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.useMekf);
|
||||
break;
|
||||
case 0xB:
|
||||
case 0xA:
|
||||
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
|
||||
break;
|
||||
case 0xC:
|
||||
case 0xB:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
|
||||
break;
|
||||
case 0xD:
|
||||
case 0xC:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
|
||||
break;
|
||||
case 0xE:
|
||||
case 0xD:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
|
||||
break;
|
||||
case 0xF:
|
||||
case 0xE:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
|
||||
break;
|
||||
case 0xF:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.rotRateLimit);
|
||||
break;
|
||||
default:
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
}
|
||||
|
@ -597,21 +591,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
|||
parameterWrapper->set(nadirModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
case 0xA:
|
||||
parameterWrapper->set(nadirModeControllerParameters.useMekf);
|
||||
break;
|
||||
case 0xB:
|
||||
case 0xA:
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
|
||||
break;
|
||||
case 0xC:
|
||||
case 0xB:
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
|
||||
break;
|
||||
case 0xD:
|
||||
case 0xC:
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
|
||||
break;
|
||||
case 0xE:
|
||||
case 0xD:
|
||||
parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
|
||||
break;
|
||||
default:
|
||||
|
@ -648,18 +639,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
|||
parameterWrapper->set(inertialModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
case 0xA:
|
||||
parameterWrapper->set(inertialModeControllerParameters.useMekf);
|
||||
break;
|
||||
case 0xB:
|
||||
case 0xA:
|
||||
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
|
||||
break;
|
||||
case 0xC:
|
||||
case 0xB:
|
||||
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
|
||||
break;
|
||||
case 0xD:
|
||||
case 0xC:
|
||||
parameterWrapper->setVector(inertialModeControllerParameters.quatRef);
|
||||
break;
|
||||
default:
|
||||
|
@ -732,22 +720,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
|||
case (0x11): // KalmanFilterParameters
|
||||
switch (parameterId) {
|
||||
case 0x0:
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseStr);
|
||||
break;
|
||||
case 0x1:
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS);
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSus);
|
||||
break;
|
||||
case 0x2:
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG);
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseMgm);
|
||||
break;
|
||||
case 0x3:
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR);
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyr);
|
||||
break;
|
||||
case 0x4:
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR);
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrArw);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR);
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrBs);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(kalmanFilterParameters.allowStr);
|
||||
break;
|
||||
default:
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
|
|
|
@ -8,6 +8,9 @@
|
|||
typedef unsigned char uint8_t;
|
||||
|
||||
class AcsParameters : public HasParametersIF {
|
||||
private:
|
||||
static constexpr double DEG2RAD = M_PI / 180.;
|
||||
|
||||
public:
|
||||
AcsParameters();
|
||||
virtual ~AcsParameters();
|
||||
|
@ -20,9 +23,9 @@ class AcsParameters : public HasParametersIF {
|
|||
double sampleTime = 0.4; // [s]
|
||||
uint16_t ptgCtrlLostTimer = 750;
|
||||
uint8_t fusedRateSafeDuringEclipse = true;
|
||||
uint8_t fusedRateFromStr = false;
|
||||
uint8_t fusedRateFromQuest = false;
|
||||
double questFilterWeight = 0.0;
|
||||
uint8_t fusedRateFromStr = true;
|
||||
uint8_t fusedRateFromQuest = true;
|
||||
double questFilterWeight = 0.9;
|
||||
} onBoardParams;
|
||||
|
||||
struct InertiaEIVE {
|
||||
|
@ -773,7 +776,7 @@ class AcsParameters : public HasParametersIF {
|
|||
0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513,
|
||||
-0.000889232196185857, -0.00168429567131815}};
|
||||
float susBrightnessThreshold = 0.7;
|
||||
float susVectorFilterWeight = .85;
|
||||
float susVectorFilterWeight = .95;
|
||||
float susRateFilterWeight = .99;
|
||||
} susHandlingParameters;
|
||||
|
||||
|
@ -812,19 +815,19 @@ class AcsParameters : public HasParametersIF {
|
|||
} rwHandlingParameters;
|
||||
|
||||
struct RwMatrices {
|
||||
double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000},
|
||||
{0.0000, -0.9205, 0.0000, 0.9205},
|
||||
{0.3907, 0.3907, 0.3907, 0.3907}};
|
||||
double alignmentMatrix[3][4] = {{-0.9205, 0.0000, 0.9205, 0.0000},
|
||||
{0.0000, 0.9205, 0.0000, -0.9205},
|
||||
{-0.3907, -0.3907, -0.3907, -0.3907}};
|
||||
double pseudoInverse[4][3] = {
|
||||
{0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}};
|
||||
double without1[4][3] = {
|
||||
{0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}};
|
||||
double without2[4][3] = {
|
||||
{0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}};
|
||||
double without3[4][3] = {
|
||||
{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}};
|
||||
{-0.5432, 0, -0.6399}, {0, 0.5432, -0.6399}, {0.5432, 0, -0.6399}, {0, -0.5432, -0.6399}};
|
||||
double pseudoInverseWithoutRW1[4][3] = {
|
||||
{0, 0, 0}, {-0.5432, 0.5432, -1.2798}, {1.0864, 0, 0}, {-0.5432, -0.5432, -1.2798}};
|
||||
double pseudoInverseWithoutRW2[4][3] = {
|
||||
{-0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, -1.0864, 0}};
|
||||
double pseudoInverseWithoutRW3[4][3] = {
|
||||
{-1.0864, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, -0.5432, -1.2798}};
|
||||
double pseudoInverseWithoutRW4[4][3] = {
|
||||
{-0.5432, -0.5432, -1.2798}, {0, 1.0864, 0}, {0.5432, -0.5432, -1.2798}, {0, 0, 0}};
|
||||
double nullspaceVector[4] = {-1, 1, -1, 1};
|
||||
} rwMatrices;
|
||||
|
||||
|
@ -854,7 +857,7 @@ class AcsParameters : public HasParametersIF {
|
|||
struct PointingLawParameters {
|
||||
double zeta = 0.3;
|
||||
double om = 0.3;
|
||||
double omMax = 1 * M_PI / 180;
|
||||
double omMax = 1 * DEG2RAD;
|
||||
double qiMin = 0.1;
|
||||
|
||||
double gainNullspace = 0.01;
|
||||
|
@ -863,8 +866,7 @@ class AcsParameters : public HasParametersIF {
|
|||
double desatMomentumRef[3] = {0, 0, 0};
|
||||
double deSatGainFactor = 1000;
|
||||
uint8_t desatOn = true;
|
||||
uint8_t enableAntiStiction = true;
|
||||
uint8_t useMekf = false;
|
||||
uint8_t useMekf = true;
|
||||
} pointingLawParameters;
|
||||
|
||||
struct IdleModeControllerParameters : PointingLawParameters {
|
||||
|
@ -877,15 +879,15 @@ class AcsParameters : public HasParametersIF {
|
|||
uint8_t timeElapsedMax = 10; // rot rate calculations
|
||||
|
||||
// Default is Stuttgart GS
|
||||
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
|
||||
double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude
|
||||
double altitudeTgt = 500; // [m]
|
||||
double latitudeTgt = 48.7495 * DEG2RAD; // [rad] Latitude
|
||||
double longitudeTgt = 9.10384 * DEG2RAD; // [rad] Longitude
|
||||
double altitudeTgt = 500; // [m]
|
||||
|
||||
// For one-axis control:
|
||||
uint8_t avoidBlindStr = true;
|
||||
double blindAvoidStart = 1.5;
|
||||
double blindAvoidStop = 2.5;
|
||||
double blindRotRate = 1 * M_PI / 180;
|
||||
double blindRotRate = 1. * DEG2RAD;
|
||||
} targetModeControllerParameters;
|
||||
|
||||
struct GsTargetModeControllerParameters : PointingLawParameters {
|
||||
|
@ -893,9 +895,10 @@ class AcsParameters : public HasParametersIF {
|
|||
uint8_t timeElapsedMax = 10; // rot rate calculations
|
||||
|
||||
// Default is Stuttgart GS
|
||||
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
|
||||
double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude
|
||||
double altitudeTgt = 500; // [m]
|
||||
double latitudeTgt = 48.7495 * DEG2RAD; // [rad] Latitude
|
||||
double longitudeTgt = 9.10384 * DEG2RAD; // [rad] Longitude
|
||||
double altitudeTgt = 500; // [m]
|
||||
double rotRateLimit = .75 * DEG2RAD;
|
||||
} gsTargetModeControllerParameters;
|
||||
|
||||
struct NadirModeControllerParameters : PointingLawParameters {
|
||||
|
@ -912,8 +915,8 @@ class AcsParameters : public HasParametersIF {
|
|||
} inertialModeControllerParameters;
|
||||
|
||||
struct StrParameters {
|
||||
double exclusionAngle = 20 * M_PI / 180;
|
||||
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame
|
||||
double exclusionAngle = 20. * DEG2RAD;
|
||||
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // body rf
|
||||
} strParameters;
|
||||
|
||||
struct GpsParameters {
|
||||
|
@ -926,25 +929,27 @@ class AcsParameters : public HasParametersIF {
|
|||
|
||||
struct SunModelParameters {
|
||||
float domega = 36000.771;
|
||||
float omega_0 = 280.46 * M_PI / 180.; // RAAN plus argument of
|
||||
// perigee
|
||||
float m_0 = 357.5277; // coefficients for mean anomaly
|
||||
float dm = 35999.049; // coefficients for mean anomaly
|
||||
float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis
|
||||
float e1 = 0.74508 * M_PI / 180.;
|
||||
float omega_0 = 280.46 * DEG2RAD; // RAAN plus argument of
|
||||
// perigee
|
||||
float m_0 = 357.5277; // coefficients for mean anomaly
|
||||
float dm = 35999.049; // coefficients for mean anomaly
|
||||
float e = 23.4392911 * DEG2RAD; // angle of earth's rotation axis
|
||||
float e1 = 0.74508 * DEG2RAD;
|
||||
|
||||
float p1 = 6892. / 3600. * M_PI / 180.; // some parameter
|
||||
float p2 = 72. / 3600. * M_PI / 180.; // some parameter
|
||||
float p1 = 6892. / 3600. * DEG2RAD; // some parameter
|
||||
float p2 = 72. / 3600. * DEG2RAD; // some parameter
|
||||
} sunModelParameters;
|
||||
|
||||
struct KalmanFilterParameters {
|
||||
double sensorNoiseSTR = 0.1 * M_PI / 180;
|
||||
double sensorNoiseSS = 8 * M_PI / 180;
|
||||
double sensorNoiseMAG = 4 * M_PI / 180;
|
||||
double sensorNoiseGYR = 0.1 * M_PI / 180;
|
||||
double sensorNoiseStr = 0.1 * DEG2RAD;
|
||||
double sensorNoiseSus = 8. * DEG2RAD;
|
||||
double sensorNoiseMgm = 4. * DEG2RAD;
|
||||
double sensorNoiseGyr = 0.1 * DEG2RAD;
|
||||
|
||||
double sensorNoiseArwGYR = 3 * 0.0043 * M_PI / sqrt(10) / 180; // Angular Random Walk
|
||||
double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability
|
||||
double sensorNoiseGyrArw = 3. * 0.0043 / sqrt(10) * DEG2RAD; // Angular Random Walk
|
||||
double sensorNoiseGyrBs = 3. / 3600. * DEG2RAD; // Bias Stability
|
||||
|
||||
uint8_t allowStr = false;
|
||||
} kalmanFilterParameters;
|
||||
|
||||
struct MagnetorquerParameter {
|
||||
|
@ -960,8 +965,8 @@ class AcsParameters : public HasParametersIF {
|
|||
|
||||
struct DetumbleParameter {
|
||||
uint8_t detumblecounter = 75; // 30 s
|
||||
double omegaDetumbleStart = 2 * M_PI / 180;
|
||||
double omegaDetumbleEnd = 1 * M_PI / 180;
|
||||
double omegaDetumbleStart = 2 * DEG2RAD;
|
||||
double omegaDetumbleEnd = 1 * DEG2RAD;
|
||||
double gainBdot = pow(10.0, -3.3);
|
||||
double gainFull = pow(10.0, -2.3);
|
||||
uint8_t useFullDetumbleLaw = false;
|
||||
|
|
|
@ -41,8 +41,8 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
|
|||
|
||||
// Sensor Weights
|
||||
double kSus = 0, kMgm = 0;
|
||||
kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2);
|
||||
kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2);
|
||||
kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSus, -2);
|
||||
kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMgm, -2);
|
||||
|
||||
// Weighted Vectors
|
||||
double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},
|
||||
|
|
|
@ -5,18 +5,18 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_)
|
|||
}
|
||||
|
||||
void FusedRotationEstimation::estimateFusedRotationRate(
|
||||
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
|
||||
acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues,
|
||||
acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
|
||||
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
|
||||
const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||
ACS::SensorValues *sensorValues, acsctrl::AttitudeEstimationData *attitudeEstimationData,
|
||||
const double timeDelta, acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
|
||||
acsctrl::FusedRotRateData *fusedRotRateData) {
|
||||
estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData);
|
||||
estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData);
|
||||
estimateFusedRotationRateSusMgm(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
|
||||
fusedRotRateSourcesData);
|
||||
|
||||
if (fusedRotRateSourcesData->rotRateTotalStr.isValid() and
|
||||
acsParameters->onBoardParams.fusedRateFromStr) {
|
||||
if (not(mode == acs::AcsMode::SAFE) and (fusedRotRateSourcesData->rotRateTotalStr.isValid() and
|
||||
acsParameters->onBoardParams.fusedRateFromStr)) {
|
||||
PoolReadGuard pg(fusedRotRateData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
|
||||
|
@ -29,8 +29,9 @@ void FusedRotationEstimation::estimateFusedRotationRate(
|
|||
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
|
||||
fusedRotRateData->rotRateSource.setValid(true);
|
||||
}
|
||||
} else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and
|
||||
acsParameters->onBoardParams.fusedRateFromQuest) {
|
||||
} else if (not(mode == acs::AcsMode::SAFE) and
|
||||
(fusedRotRateSourcesData->rotRateTotalQuest.isValid() and
|
||||
acsParameters->onBoardParams.fusedRateFromQuest)) {
|
||||
PoolReadGuard pg(fusedRotRateData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
|
||||
|
|
|
@ -12,7 +12,7 @@ class FusedRotationEstimation {
|
|||
public:
|
||||
FusedRotationEstimation(AcsParameters *acsParameters_);
|
||||
|
||||
void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed,
|
||||
void estimateFusedRotationRate(const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
||||
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||
ACS::SensorValues *sensorValues,
|
||||
|
|
|
@ -1,426 +1,311 @@
|
|||
#include "Guidance.h"
|
||||
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <mission/controller/acs/util/MathOperations.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <filesystem>
|
||||
#include <string>
|
||||
|
||||
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
|
||||
|
||||
Guidance::~Guidance() {}
|
||||
|
||||
[[deprecated]] void Guidance::targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3],
|
||||
double velSatE[3], double sunDirI[3],
|
||||
double refDirB[3], double quatBI[4],
|
||||
double targetQuat[4],
|
||||
double targetSatRotRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion to groundstation or given latitude, longitude and altitude
|
||||
//-------------------------------------------------------------------------------------
|
||||
// transform longitude, latitude and altitude to ECEF
|
||||
double targetE[3] = {0, 0, 0};
|
||||
void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta,
|
||||
const double sunDirI[3], const double posSatF[4],
|
||||
double targetQuat[4], double targetSatRotRate[3]) {
|
||||
// positive z-Axis of EIVE in direction of sun
|
||||
double zAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(sunDirI, zAxisIX, 3);
|
||||
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters->targetModeControllerParameters.latitudeTgt,
|
||||
acsParameters->targetModeControllerParameters.longitudeTgt,
|
||||
acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
|
||||
// determine helper vector to point x-Axis and therefore the STR away from Earth
|
||||
double helperXI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0};
|
||||
CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
|
||||
VectorOperations<double>::normalize(posSatI, helperXI, 3);
|
||||
|
||||
// target direction in the ECEF frame
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
|
||||
// construct y-axis from helper vector and z-axis
|
||||
double yAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxisIX, helperXI, yAxisIX);
|
||||
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
|
||||
|
||||
// transformation between ECEF and ECI frame
|
||||
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
// x-axis completes RHS
|
||||
double xAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(yAxisIX, zAxisIX, xAxisIX);
|
||||
VectorOperations<double>::normalize(xAxisIX, xAxisIX, 3);
|
||||
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
// join transformation matrix
|
||||
double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
|
||||
{xAxisIX[1], yAxisIX[1], zAxisIX[1]},
|
||||
{xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
|
||||
QuaternionOperations::fromDcm(dcmIX, targetQuat);
|
||||
|
||||
// transformation between ECEF and Body frame
|
||||
double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
|
||||
QuaternionOperations::toDcm(quatBI, dcmBI);
|
||||
MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
|
||||
|
||||
// target Direction in the body frame
|
||||
double targetDirB[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
|
||||
|
||||
// rotation quaternion from two vectors
|
||||
double refDir[3] = {0, 0, 0};
|
||||
refDir[0] = acsParameters->targetModeControllerParameters.refDirection[0];
|
||||
refDir[1] = acsParameters->targetModeControllerParameters.refDirection[1];
|
||||
refDir[2] = acsParameters->targetModeControllerParameters.refDirection[2];
|
||||
double noramlizedTargetDirB[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
|
||||
VectorOperations<double>::normalize(refDir, refDir, 3);
|
||||
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
|
||||
double normRefDir = VectorOperations<double>::norm(refDir, 3);
|
||||
double crossDir[3] = {0, 0, 0};
|
||||
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
|
||||
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
|
||||
targetQuat[0] = crossDir[0];
|
||||
targetQuat[1] = crossDir[1];
|
||||
targetQuat[2] = crossDir[2];
|
||||
targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
|
||||
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// calculation of reference rotation rate
|
||||
//-------------------------------------------------------------------------------------
|
||||
double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0};
|
||||
// velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
|
||||
MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
|
||||
double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MatrixOperations<double>::multiply(*dcmBI, *dcmIEDot, *dcmBEDot, 3, 3, 3);
|
||||
MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1);
|
||||
VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3);
|
||||
|
||||
double normVelSatB = VectorOperations<double>::norm(velSatB, 3);
|
||||
double normRefSatRate = normVelSatB / normTargetDirB;
|
||||
|
||||
double satRateDir[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(velSatB, targetDirB, satRateDir);
|
||||
VectorOperations<double>::normalize(satRateDir, satRateDir, 3);
|
||||
VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, targetSatRotRate, 3);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate in case of star tracker blinding
|
||||
//-------------------------------------------------------------------------------------
|
||||
if (acsParameters->targetModeControllerParameters.avoidBlindStr) {
|
||||
double sunDirB[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1);
|
||||
|
||||
double exclAngle = acsParameters->strParameters.exclusionAngle,
|
||||
blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart,
|
||||
blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop;
|
||||
double sightAngleSun =
|
||||
VectorOperations<double>::dot(acsParameters->strParameters.boresightAxis, sunDirB);
|
||||
|
||||
if (!(strBlindAvoidFlag)) {
|
||||
double critSightAngle = blindStart * exclAngle;
|
||||
if (sightAngleSun < critSightAngle) {
|
||||
strBlindAvoidFlag = true;
|
||||
}
|
||||
} else {
|
||||
if (sightAngleSun < blindEnd * exclAngle) {
|
||||
double normBlindRefRate = acsParameters->targetModeControllerParameters.blindRotRate;
|
||||
double blindRefRate[3] = {0, 0, 0};
|
||||
if (sunDirB[1] < 0) {
|
||||
blindRefRate[0] = normBlindRefRate;
|
||||
blindRefRate[1] = 0;
|
||||
blindRefRate[2] = 0;
|
||||
} else {
|
||||
blindRefRate[0] = -normBlindRefRate;
|
||||
blindRefRate[1] = 0;
|
||||
blindRefRate[2] = 0;
|
||||
}
|
||||
VectorOperations<double>::add(blindRefRate, targetSatRotRate, targetSatRotRate, 3);
|
||||
} else {
|
||||
strBlindAvoidFlag = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
// revert calculated quaternion from qBX to qIX
|
||||
double quatIB[4] = {0, 0, 0, 1};
|
||||
QuaternionOperations::inverse(quatBI, quatIB);
|
||||
QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
|
||||
// calculate of reference rotation rate
|
||||
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta,
|
||||
double posSatE[3], double velSatE[3], double targetQuat[4],
|
||||
double targetSatRotRate[3]) {
|
||||
void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
|
||||
const double posSatF[3], const double velSatF[3],
|
||||
double targetQuat[4], double targetSatRotRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for target pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
|
||||
double targetE[3] = {0, 0, 0};
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
double targetF[3] = {0, 0, 0};
|
||||
CoordinateTransformations::cartesianFromLatLongAlt(
|
||||
acsParameters->targetModeControllerParameters.latitudeTgt,
|
||||
acsParameters->targetModeControllerParameters.longitudeTgt,
|
||||
acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
|
||||
|
||||
// transformation between ECEF and ECI frame
|
||||
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
acsParameters->targetModeControllerParameters.altitudeTgt, targetF);
|
||||
|
||||
// target direction in the ECI frame
|
||||
double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 3, 3, 1);
|
||||
CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
|
||||
CoordinateTransformations::positionEcfToEci(targetF, targetI, &timeAbsolute);
|
||||
VectorOperations<double>::subtract(targetI, posSatI, targetDirI, 3);
|
||||
|
||||
// x-axis aligned with target direction
|
||||
// this aligns with the camera, E- and S-band antennas
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirI, xAxis, 3);
|
||||
double xAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirI, xAxisIX, 3);
|
||||
VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
|
||||
|
||||
// transform velocity into inertial frame
|
||||
double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
|
||||
VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
|
||||
double velSatI[3] = {0, 0, 0};
|
||||
CoordinateTransformations::velocityEcfToEci(velSatF, posSatF, velSatI, &timeAbsolute);
|
||||
|
||||
// orbital normal vector of target and velocity vector
|
||||
double orbitalNormalI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(posSatI, velocityI, orbitalNormalI);
|
||||
VectorOperations<double>::cross(posSatI, velSatI, orbitalNormalI);
|
||||
VectorOperations<double>::normalize(orbitalNormalI, orbitalNormalI, 3);
|
||||
|
||||
// y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture
|
||||
// resolution
|
||||
double yAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(orbitalNormalI, xAxis, yAxis);
|
||||
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
||||
double yAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(orbitalNormalI, xAxisIX, yAxisIX);
|
||||
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
|
||||
|
||||
// z-axis completes RHS
|
||||
double zAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(xAxis, yAxis, zAxis);
|
||||
double zAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(xAxisIX, yAxisIX, zAxisIX);
|
||||
|
||||
// join transformation matrix
|
||||
double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
|
||||
{xAxisIX[1], yAxisIX[1], zAxisIX[1]},
|
||||
{xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
|
||||
QuaternionOperations::fromDcm(dcmIX, targetQuat);
|
||||
|
||||
int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax;
|
||||
targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
|
||||
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta,
|
||||
double posSatE[3], double sunDirI[3], double targetQuat[4],
|
||||
double targetSatRotRate[3]) {
|
||||
void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
|
||||
const double posSatF[3], const double sunDirI[3],
|
||||
double targetQuat[4], double targetSatRotRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for ground station pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
|
||||
double groundStationE[3] = {0, 0, 0};
|
||||
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
double posGroundStationF[3] = {0, 0, 0};
|
||||
CoordinateTransformations::cartesianFromLatLongAlt(
|
||||
acsParameters->gsTargetModeControllerParameters.latitudeTgt,
|
||||
acsParameters->gsTargetModeControllerParameters.longitudeTgt,
|
||||
acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE);
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(groundStationE, posSatE, targetDirE, 3);
|
||||
|
||||
// transformation between ECEF and ECI frame
|
||||
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF);
|
||||
|
||||
// target direction in the ECI frame
|
||||
double posSatI[3] = {0, 0, 0}, groundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1);
|
||||
VectorOperations<double>::subtract(groundStationI, posSatI, groundStationDirI, 3);
|
||||
double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
|
||||
CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
|
||||
CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, &timeAbsolute);
|
||||
VectorOperations<double>::subtract(posGroundStationI, posSatI, groundStationDirI, 3);
|
||||
|
||||
// negative x-axis aligned with target direction
|
||||
// this aligns with the camera, E- and S-band antennas
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(groundStationDirI, xAxis, 3);
|
||||
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
|
||||
double xAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(groundStationDirI, xAxisIX, 3);
|
||||
VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
|
||||
|
||||
// get sun vector model in ECI
|
||||
VectorOperations<double>::normalize(sunDirI, sunDirI, 3);
|
||||
// get earth vector in ECI
|
||||
double earthDirI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(posSatI, earthDirI, 3);
|
||||
VectorOperations<double>::mulScalar(earthDirI, -1, earthDirI, 3);
|
||||
|
||||
// calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
|
||||
// z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x
|
||||
double xDotS = VectorOperations<double>::dot(xAxis, sunDirI);
|
||||
xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
|
||||
double sunParallel[3], zAxis[3];
|
||||
VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
|
||||
VectorOperations<double>::subtract(sunDirI, sunParallel, zAxis, 3);
|
||||
VectorOperations<double>::normalize(zAxis, zAxis, 3);
|
||||
// sun avoidance calculations
|
||||
double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0}, zAxisSun[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirI),
|
||||
sunPerpendicularX, 3);
|
||||
VectorOperations<double>::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3);
|
||||
VectorOperations<double>::normalize(sunFloorYZ, sunFloorYZ, 3);
|
||||
VectorOperations<double>::mulScalar(sunFloorYZ, -1, zAxisSun, 3);
|
||||
double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0},
|
||||
strVecSunZ[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
|
||||
strVecSunX, 3);
|
||||
VectorOperations<double>::mulScalar(zAxisSun, acsParameters->strParameters.boresightAxis[2],
|
||||
strVecSunZ, 3);
|
||||
VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3);
|
||||
VectorOperations<double>::normalize(strVecSun, strVecSun, 3);
|
||||
sunWeight = VectorOperations<double>::dot(strVecSun, sunDirI);
|
||||
|
||||
// y-axis completes RHS
|
||||
double yAxis[3];
|
||||
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
|
||||
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
||||
// earth avoidance calculations
|
||||
double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, earthDirI),
|
||||
earthPerpendicularX, 3);
|
||||
VectorOperations<double>::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3);
|
||||
VectorOperations<double>::normalize(earthFloorYZ, earthFloorYZ, 3);
|
||||
VectorOperations<double>::mulScalar(earthFloorYZ, -1, zAxisEarth, 3);
|
||||
double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0},
|
||||
strVecEarthZ[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
|
||||
strVecEarthX, 3);
|
||||
VectorOperations<double>::mulScalar(zAxisEarth, acsParameters->strParameters.boresightAxis[2],
|
||||
strVecEarthZ, 3);
|
||||
VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3);
|
||||
VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3);
|
||||
earthWeight = VectorOperations<double>::dot(strVecEarth, earthDirI);
|
||||
|
||||
if ((sunWeight == 0.0) and (earthWeight == 0.0)) {
|
||||
// if this actually ever happens i will eat a broom
|
||||
sunWeight = 0.5;
|
||||
earthWeight = 0.5;
|
||||
}
|
||||
|
||||
// normalize weights for convenience
|
||||
double normFactor = 1. / (std::abs(sunWeight) + std::abs(earthWeight));
|
||||
sunWeight *= normFactor;
|
||||
earthWeight *= normFactor;
|
||||
|
||||
// calculate z-axis for str blinding avoidance
|
||||
double zAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(zAxisSun, sunWeight, zAxisSun, 3);
|
||||
VectorOperations<double>::mulScalar(zAxisEarth, earthWeight, zAxisEarth, 3);
|
||||
VectorOperations<double>::add(zAxisSun, zAxisEarth, zAxisIX, 3);
|
||||
VectorOperations<double>::mulScalar(zAxisIX, -1, zAxisIX, 3);
|
||||
VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
|
||||
|
||||
// calculate y-axis
|
||||
double yAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
|
||||
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
|
||||
|
||||
// join transformation matrix
|
||||
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
|
||||
double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
|
||||
{xAxisIX[1], yAxisIX[1], zAxisIX[1]},
|
||||
{xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
|
||||
QuaternionOperations::fromDcm(dcmIX, targetQuat);
|
||||
|
||||
int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
|
||||
targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
|
||||
limitReferenceRotation(xAxisIX, targetQuat);
|
||||
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
|
||||
|
||||
std::memcpy(xAxisIXprev, xAxisIX, sizeof(xAxisIXprev));
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgSun(double timeDelta, double sunDirI[3], double targetQuat[4],
|
||||
double targetSatRotRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion to sun
|
||||
//-------------------------------------------------------------------------------------
|
||||
// positive z-Axis of EIVE in direction of sun
|
||||
double zAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(sunDirI, zAxis, 3);
|
||||
|
||||
// assign helper vector (north pole inertial)
|
||||
double helperVec[3] = {0, 0, 1};
|
||||
|
||||
// construct y-axis from helper vector and z-axis
|
||||
double yAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxis, helperVec, yAxis);
|
||||
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
||||
|
||||
// x-axis completes RHS
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(yAxis, zAxis, xAxis);
|
||||
VectorOperations<double>::normalize(xAxis, xAxis, 3);
|
||||
|
||||
// join transformation matrix
|
||||
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate
|
||||
//----------------------------------------------------------------------------
|
||||
int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
|
||||
targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
|
||||
}
|
||||
|
||||
[[deprecated]] void Guidance::targetQuatPtgNadirSingleAxis(const timeval timeAbsolute,
|
||||
double posSatE[3], double quatBI[4],
|
||||
double targetQuat[4], double refDirB[3],
|
||||
double refSatRate[3]) {
|
||||
void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta,
|
||||
const double posSatE[3], const double velSatE[3],
|
||||
double targetQuat[4], double refSatRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for Nadir pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
|
||||
|
||||
// transformation between ECEF and ECI frame
|
||||
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
|
||||
// transformation between ECEF and Body frame
|
||||
double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
QuaternionOperations::toDcm(quatBI, dcmBI);
|
||||
MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
|
||||
|
||||
// target Direction in the body frame
|
||||
double targetDirB[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
|
||||
|
||||
// rotation quaternion from two vectors
|
||||
double refDir[3] = {0, 0, 0};
|
||||
refDir[0] = acsParameters->nadirModeControllerParameters.refDirection[0];
|
||||
refDir[1] = acsParameters->nadirModeControllerParameters.refDirection[1];
|
||||
refDir[2] = acsParameters->nadirModeControllerParameters.refDirection[2];
|
||||
double noramlizedTargetDirB[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
|
||||
VectorOperations<double>::normalize(refDir, refDir, 3);
|
||||
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
|
||||
double normRefDir = VectorOperations<double>::norm(refDir, 3);
|
||||
double crossDir[3] = {0, 0, 0};
|
||||
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
|
||||
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
|
||||
targetQuat[0] = crossDir[0];
|
||||
targetQuat[1] = crossDir[1];
|
||||
targetQuat[2] = crossDir[2];
|
||||
targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
|
||||
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate
|
||||
//-------------------------------------------------------------------------------------
|
||||
refSatRate[0] = 0;
|
||||
refSatRate[1] = 0;
|
||||
refSatRate[2] = 0;
|
||||
|
||||
// revert calculated quaternion from qBX to qIX
|
||||
double quatIB[4] = {0, 0, 0, 1};
|
||||
QuaternionOperations::inverse(quatBI, quatIB);
|
||||
QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta,
|
||||
double posSatE[3], double velSatE[3],
|
||||
double targetQuat[4], double refSatRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for Nadir pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// transformation between ECEF and ECI frame
|
||||
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
|
||||
// satellite position in inertial reference frame
|
||||
double posSatI[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
|
||||
CoordinateTransformations::positionEcfToEci(posSatE, posSatI, &timeAbsolute);
|
||||
|
||||
// negative x-axis aligned with position vector
|
||||
// this aligns with the camera, E- and S-band antennas
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(posSatI, xAxis, 3);
|
||||
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
|
||||
double xAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(posSatI, xAxisIX, 3);
|
||||
VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
|
||||
|
||||
// make z-Axis parallel to major part of camera resolution
|
||||
double zAxis[3] = {0, 0, 0};
|
||||
double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
|
||||
VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
|
||||
VectorOperations<double>::cross(xAxis, velocityI, zAxis);
|
||||
VectorOperations<double>::normalize(zAxis, zAxis, 3);
|
||||
double zAxisIX[3] = {0, 0, 0};
|
||||
double velSatI[3] = {0, 0, 0};
|
||||
CoordinateTransformations::velocityEcfToEci(velSatE, posSatE, velSatI, &timeAbsolute);
|
||||
VectorOperations<double>::cross(xAxisIX, velSatI, zAxisIX);
|
||||
VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
|
||||
|
||||
// y-Axis completes RHS
|
||||
double yAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
|
||||
double yAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
|
||||
|
||||
// join transformation matrix
|
||||
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
|
||||
double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
|
||||
{xAxisIX[1], yAxisIX[1], zAxisIX[1]},
|
||||
{xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
|
||||
QuaternionOperations::fromDcm(dcmIX, targetQuat);
|
||||
|
||||
int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax;
|
||||
targetRotationRate(timeElapsedMax, timeDelta, targetQuat, refSatRate);
|
||||
targetRotationRate(timeDelta, targetQuat, refSatRate);
|
||||
}
|
||||
|
||||
void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], double *refSatRate) {
|
||||
if (VectorOperations<double>::norm(quatIXprev, 4) == 0) {
|
||||
std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
|
||||
}
|
||||
if (timeDelta != 0.0) {
|
||||
QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate);
|
||||
} else {
|
||||
std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double));
|
||||
}
|
||||
std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
|
||||
}
|
||||
|
||||
void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) {
|
||||
if ((VectorOperations<double>::norm(quatIXprev, 4) == 0) or
|
||||
(VectorOperations<double>::norm(xAxisIXprev, 3) == 0)) {
|
||||
return;
|
||||
}
|
||||
|
||||
QuaternionOperations::preventSignJump(quatIX, quatIXprev);
|
||||
|
||||
// check required rotation and return if below limit
|
||||
double quatXprevX[4] = {0, 0, 0, 0}, quatXprevI[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::inverse(quatIXprev, quatXprevI);
|
||||
QuaternionOperations::multiply(quatIX, quatXprevI, quatXprevX);
|
||||
QuaternionOperations::normalize(quatXprevX);
|
||||
double phiMax = acsParameters->gsTargetModeControllerParameters.rotRateLimit *
|
||||
acsParameters->onBoardParams.sampleTime;
|
||||
if (2 * std::acos(quatXprevX[3]) < phiMax) {
|
||||
return;
|
||||
}
|
||||
|
||||
// x-axis always needs full rotation
|
||||
double phiX = 0, phiXvec[3] = {0, 0, 0};
|
||||
phiX = std::acos(VectorOperations<double>::dot(xAxisIXprev, xAxisIX));
|
||||
VectorOperations<double>::cross(xAxisIXprev, xAxisIX, phiXvec);
|
||||
VectorOperations<double>::normalize(phiXvec, phiXvec, 3);
|
||||
|
||||
double quatXprevXtilde[4] = {0, 0, 0, 0}, quatIXtilde[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(phiXvec, -std::sin(phiX / 2.), phiXvec, 3);
|
||||
std::memcpy(quatXprevXtilde, phiXvec, sizeof(phiXvec));
|
||||
quatXprevXtilde[3] = cos(phiX / 2.);
|
||||
QuaternionOperations::normalize(quatXprevXtilde);
|
||||
QuaternionOperations::multiply(quatXprevXtilde, quatIXprev, quatIXtilde);
|
||||
|
||||
// use the residual rotation up to the maximum
|
||||
double quatXXtilde[4] = {0, 0, 0, 0}, quatXI[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::inverse(quatIX, quatXI);
|
||||
QuaternionOperations::multiply(quatIXtilde, quatXI, quatXXtilde);
|
||||
|
||||
double phiResidual = 0, phiResidualVec[3] = {0, 0, 0};
|
||||
if ((phiX * phiX) > (phiMax * phiMax)) {
|
||||
phiResidual = 0;
|
||||
} else {
|
||||
phiResidual = std::sqrt((phiMax * phiMax) - (phiX * phiX));
|
||||
}
|
||||
std::memcpy(phiResidualVec, quatXXtilde, sizeof(phiResidualVec));
|
||||
VectorOperations<double>::normalize(phiResidualVec, phiResidualVec, 3);
|
||||
|
||||
double quatXhatXTilde[4] = {0, 0, 0, 0}, quatXTildeXhat[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(phiResidualVec, std::sin(phiResidual / 2.), phiResidualVec,
|
||||
3);
|
||||
std::memcpy(quatXhatXTilde, phiResidualVec, sizeof(phiResidualVec));
|
||||
quatXhatXTilde[3] = std::cos(phiResidual / 2.);
|
||||
QuaternionOperations::normalize(quatXhatXTilde);
|
||||
|
||||
// calculate final quaternion
|
||||
QuaternionOperations::inverse(quatXhatXTilde, quatXTildeXhat);
|
||||
QuaternionOperations::multiply(quatXTildeXhat, quatIXtilde, quatIX);
|
||||
QuaternionOperations::normalize(quatIX);
|
||||
}
|
||||
|
||||
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) {
|
||||
// First calculate error quaternion between current and target orientation
|
||||
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
|
||||
// Last calculate add rotation from reference quaternion
|
||||
QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
|
||||
// First calculate error quaternion between current and target orientation without reference
|
||||
// quaternion
|
||||
double errorQuatWoRef[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef);
|
||||
// Then add rotation from reference quaternion
|
||||
QuaternionOperations::multiply(refQuat, errorQuatWoRef, errorQuat);
|
||||
// Keep scalar part of quaternion positive
|
||||
if (errorQuat[3] < 0) {
|
||||
VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
|
||||
|
@ -429,7 +314,11 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
|
|||
errorAngle = QuaternionOperations::getAngle(errorQuat, true);
|
||||
|
||||
// Calculate error satellite rotational rate
|
||||
// First combine the target and reference satellite rotational rates
|
||||
// Convert target rotational rate into body RF
|
||||
double errorQuatInv[4] = {0, 0, 0, 0}, targetSatRotRateB[3] = {0, 0, 0};
|
||||
QuaternionOperations::inverse(errorQuat, errorQuatInv);
|
||||
QuaternionOperations::multiplyVector(errorQuatInv, targetSatRotRate, targetSatRotRateB);
|
||||
// Combine the target and reference satellite rotational rates
|
||||
double combinedRefSatRotRate[3] = {0, 0, 0};
|
||||
VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
|
||||
// Then subtract the combined required satellite rotational rates from the actual rate
|
||||
|
@ -439,85 +328,48 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
|
|||
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
|
||||
double targetSatRotRate[3], double errorQuat[4],
|
||||
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
|
||||
if (errorQuat[3] < 0) {
|
||||
VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
|
||||
}
|
||||
// Calculate error angle
|
||||
errorAngle = QuaternionOperations::getAngle(errorQuat, true);
|
||||
|
||||
// Calculate error satellite rotational rate
|
||||
VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
|
||||
}
|
||||
|
||||
void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
|
||||
double quatInertialTarget[4], double *refSatRate) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target rotation rate
|
||||
//-------------------------------------------------------------------------------------
|
||||
if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
|
||||
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
|
||||
}
|
||||
if (timeDelta < timeElapsedMax and timeDelta != 0.0) {
|
||||
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<double>::subtract(q, qS, qDiff, 4);
|
||||
VectorOperations<double>::mulScalar(qDiff, 1. / timeDelta, qDiff, 4);
|
||||
|
||||
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<double>::cross(tgtQuatVec, qDiffVec, sum1);
|
||||
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
|
||||
VectorOperations<double>::mulScalar(qDiffVec, q[3], sum3, 3);
|
||||
VectorOperations<double>::add(sum1, sum2, sum, 3);
|
||||
VectorOperations<double>::subtract(sum, sum3, sum, 3);
|
||||
double omegaRefNew[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
|
||||
|
||||
VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
|
||||
VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
|
||||
omegaRefSaved[0] = omegaRefNew[0];
|
||||
omegaRefSaved[1] = omegaRefNew[1];
|
||||
omegaRefSaved[2] = omegaRefNew[2];
|
||||
} else {
|
||||
refSatRate[0] = 0;
|
||||
refSatRate[1] = 0;
|
||||
refSatRate[2] = 0;
|
||||
}
|
||||
|
||||
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
|
||||
double refQuat[4] = {0, 0, 0, 1}, refSatRotRate[3] = {0, 0, 0};
|
||||
comparePtg(currentQuat, currentSatRotRate, targetQuat, targetSatRotRate, refQuat, refSatRotRate,
|
||||
errorQuat, errorSatRotRate, errorAngle);
|
||||
}
|
||||
|
||||
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
|
||||
double *rwPseudoInv) {
|
||||
bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
|
||||
bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
|
||||
bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
|
||||
bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
|
||||
double *rwPseudoInv, acsctrl::RwAvail *rwAvail) {
|
||||
rwAvail->rw1avail = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
|
||||
rwAvail->rw2avail = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
|
||||
rwAvail->rw3avail = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
|
||||
rwAvail->rw4avail = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
|
||||
|
||||
if (rw1valid and rw2valid and rw3valid and rw4valid) {
|
||||
if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and rwAvail->rw4avail) {
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
|
||||
return returnvalue::OK;
|
||||
} else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double));
|
||||
return returnvalue::OK;
|
||||
} else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double));
|
||||
return returnvalue::OK;
|
||||
} else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double));
|
||||
return returnvalue::OK;
|
||||
} else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
|
||||
return returnvalue::OK;
|
||||
} else {
|
||||
return returnvalue::FAILED;
|
||||
} else if (not rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and
|
||||
rwAvail->rw4avail) {
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1,
|
||||
12 * sizeof(double));
|
||||
return acsctrl::SINGLE_RW_UNAVAILABLE;
|
||||
} else if (rwAvail->rw1avail and not rwAvail->rw2avail and rwAvail->rw3avail and
|
||||
rwAvail->rw4avail) {
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2,
|
||||
12 * sizeof(double));
|
||||
return acsctrl::SINGLE_RW_UNAVAILABLE;
|
||||
} else if (rwAvail->rw1avail and rwAvail->rw2avail and not rwAvail->rw3avail and
|
||||
rwAvail->rw4avail) {
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3,
|
||||
12 * sizeof(double));
|
||||
return acsctrl::SINGLE_RW_UNAVAILABLE;
|
||||
} else if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and
|
||||
not rwAvail->rw4avail) {
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4,
|
||||
12 * sizeof(double));
|
||||
return acsctrl::SINGLE_RW_UNAVAILABLE;
|
||||
}
|
||||
return acsctrl::MULTIPLE_RW_UNAVAILABLE;
|
||||
}
|
||||
|
||||
void Guidance::resetValues() {
|
||||
std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev));
|
||||
std::memcpy(xAxisIXprev, ZERO_VEC3, sizeof(xAxisIXprev));
|
||||
}
|
||||
|
||||
void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) {
|
||||
|
|
|
@ -1,11 +1,18 @@
|
|||
#ifndef GUIDANCE_H_
|
||||
#define GUIDANCE_H_
|
||||
|
||||
#include <time.h>
|
||||
#include <fsfw/coordinates/CoordinateTransformations.h>
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <mission/controller/acs/AcsParameters.h>
|
||||
#include <mission/controller/acs/SensorValues.h>
|
||||
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
||||
|
||||
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
|
||||
#include "AcsParameters.h"
|
||||
#include "SensorValues.h"
|
||||
#include <cmath>
|
||||
#include <filesystem>
|
||||
#include <string>
|
||||
|
||||
class Guidance {
|
||||
public:
|
||||
|
@ -14,34 +21,22 @@ class Guidance {
|
|||
|
||||
void getTargetParamsSafe(double sunTargetSafe[3]);
|
||||
ReturnValue_t solarArrayDeploymentComplete();
|
||||
void resetValues();
|
||||
|
||||
// Function to get the target quaternion and reference rotation rate from gps position and
|
||||
// position of the ground station
|
||||
void targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3], double velSatE[3],
|
||||
double sunDirI[3], double refDirB[3], double quatBI[4],
|
||||
double targetQuat[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta, double posSatE[3],
|
||||
double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta, double posSatE[3],
|
||||
double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, const double sunDirI[3],
|
||||
const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
|
||||
const double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
|
||||
const double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
|
||||
const double velSatF[3], double targetQuat[4], double refSatRate[3]);
|
||||
|
||||
// Function to get the target quaternion and reference rotation rate for sun pointing after ground
|
||||
// station
|
||||
void targetQuatPtgSun(const double timeDelta, double sunDirI[3], double targetQuat[4],
|
||||
double targetSatRotRate[3]);
|
||||
void targetRotationRate(const double timeDelta, double quatInertialTarget[4],
|
||||
double *targetSatRotRate);
|
||||
|
||||
// Function to get the target quaternion and refence rotation rate from gps position for Nadir
|
||||
// pointing
|
||||
void targetQuatPtgNadirSingleAxis(const timeval timeAbsolute, double posSatE[3], double quatBI[4],
|
||||
double targetQuat[4], double refDirB[3], double refSatRate[3]);
|
||||
void targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta,
|
||||
double posSatE[3], double velSatE[3], double targetQuat[4],
|
||||
double refSatRate[3]);
|
||||
void limitReferenceRotation(const double xAxisIX[3], double quatIX[4]);
|
||||
|
||||
// @note: Calculates the error quaternion between the current orientation and the target
|
||||
// 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);
|
||||
|
@ -49,19 +44,18 @@ class Guidance {
|
|||
double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
|
||||
double &errorAngle);
|
||||
|
||||
void targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
|
||||
double quatInertialTarget[4], double *targetSatRotRate);
|
||||
|
||||
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
|
||||
// reation wheel maybe can be done in "commanding.h"
|
||||
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
|
||||
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv,
|
||||
acsctrl::RwAvail *rwAvail);
|
||||
|
||||
private:
|
||||
const AcsParameters *acsParameters;
|
||||
|
||||
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
|
||||
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
|
||||
|
||||
bool strBlindAvoidFlag = false;
|
||||
double savedQuaternion[4] = {0, 0, 0, 0};
|
||||
double omegaRefSaved[3] = {0, 0, 0};
|
||||
double quatIXprev[4] = {0, 0, 0, 0};
|
||||
double xAxisIXprev[3] = {0, 0, 0};
|
||||
|
||||
static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
|
||||
static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";
|
||||
|
|
|
@ -1,19 +1,5 @@
|
|||
#include "Igrf13Model.h"
|
||||
|
||||
#include <fsfw/src/fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "util/MathOperations.h"
|
||||
|
||||
using namespace Math;
|
||||
|
||||
Igrf13Model::Igrf13Model() {}
|
||||
Igrf13Model::~Igrf13Model() {}
|
||||
|
||||
|
@ -23,7 +9,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
|
|||
double magFieldModel[3] = {0, 0, 0};
|
||||
double phi = longitude, theta = gcLatitude; // geocentric
|
||||
/* Here is the co-latitude needed*/
|
||||
theta -= 90 * PI / 180;
|
||||
theta -= 90. * M_PI / 180.;
|
||||
theta *= (-1);
|
||||
|
||||
double rE = 6371200.0; // radius earth [m]
|
||||
|
@ -83,13 +69,14 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
|
|||
magFieldModel[1] *= -1;
|
||||
magFieldModel[2] *= (-1 / sin(theta));
|
||||
|
||||
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
|
||||
double JD2000 = 0;
|
||||
Clock::convertTimevalToJD2000(timeOfMagMeasurement, &JD2000);
|
||||
double UT1 = JD2000 / 36525.;
|
||||
|
||||
double gst =
|
||||
280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3);
|
||||
gst = std::fmod(gst, 360.);
|
||||
gst *= PI / 180.;
|
||||
gst *= M_PI / 180.;
|
||||
double lst = gst + longitude; // local sidereal time [rad]
|
||||
|
||||
magFieldModelInertial[0] =
|
||||
|
@ -107,7 +94,8 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
|
|||
|
||||
void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
|
||||
double JD2000Igrf = (2458850.0 - 2451545); // Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000
|
||||
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
|
||||
double JD2000 = 0;
|
||||
Clock::convertTimevalToJD2000(timeOfMagMeasurement, &JD2000);
|
||||
double days = ceil(JD2000 - JD2000Igrf);
|
||||
for (int i = 0; i <= igrfOrder; i++) {
|
||||
for (int j = 0; j <= (igrfOrder - 1); j++) {
|
||||
|
|
|
@ -16,10 +16,11 @@
|
|||
#ifndef IGRF13MODEL_H_
|
||||
#define IGRF13MODEL_H_
|
||||
|
||||
#include <fsfw/parameters/HasParametersIF.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <fsfw/src/fsfw/timemanager/Clock.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -1,14 +1,16 @@
|
|||
#ifndef MULTIPLICATIVEKALMANFILTER_H_
|
||||
#define MULTIPLICATIVEKALMANFILTER_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
|
||||
#include "AcsParameters.h"
|
||||
#include "eive/resultClassIds.h"
|
||||
#include <common/config/eive/resultClassIds.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <mission/controller/acs/AcsParameters.h>
|
||||
#include <mission/controller/acs/SensorValues.h>
|
||||
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
||||
|
||||
class MultiplicativeKalmanFilter {
|
||||
/* @brief: This class handles the calculation of an estimated quaternion and the gyro bias by
|
||||
/* @brief: This class handles the calculation of an estimated quaternion and the gyroscope bias by
|
||||
* means of the spacecraft attitude sensors
|
||||
*
|
||||
* @note: A description of the used algorithms can be found in the bachelor thesis of Robin
|
||||
|
@ -18,56 +20,26 @@ class MultiplicativeKalmanFilter {
|
|||
public:
|
||||
/* @brief: Constructor
|
||||
*/
|
||||
MultiplicativeKalmanFilter();
|
||||
MultiplicativeKalmanFilter(AcsParameters *acsParameters);
|
||||
virtual ~MultiplicativeKalmanFilter();
|
||||
|
||||
ReturnValue_t reset(acsctrl::AttitudeEstimationData *mekfData);
|
||||
ReturnValue_t reset(acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
||||
|
||||
/* @brief: init() - This function initializes the Kalman Filter and will provide the first
|
||||
* quaternion through the QUEST algorithm
|
||||
* @param: magneticField_ magnetic field vector in the body frame
|
||||
* sunDir_ sun direction vector in the body frame
|
||||
* sunDirJ sun direction vector in the ECI frame
|
||||
* magFieldJ magnetic field vector in the ECI frame
|
||||
*/
|
||||
ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
|
||||
const bool validSS, const double *sunDirJ, const bool validSSModel,
|
||||
const double *magFieldJ, const bool validMagModel,
|
||||
acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters);
|
||||
ReturnValue_t init(const acsctrl::SusDataProcessed *susData,
|
||||
const acsctrl::MgmDataProcessed *mgmData,
|
||||
const acsctrl::GyrDataProcessed *gyrData,
|
||||
acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
||||
|
||||
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
|
||||
* for the current step after the initalization
|
||||
* @param: quaternionSTR Star Tracker Quaternion between from body to ECI frame
|
||||
* rateGYRs_ Estimated satellite rotation rate from the
|
||||
* Gyroscopes [rad/s] magneticField_ magnetic field vector in the body frame sunDir_
|
||||
* sun direction vector in the body frame sunDirJ sun direction vector in the ECI
|
||||
* frame magFieldJ magnetic field vector in the ECI frame
|
||||
* outputQuat Stores the calculated quaternion
|
||||
* outputSatRate Stores the adjusted satellite rate
|
||||
* @return ReturnValue_t Feedback of this class, KALMAN_NO_GYR_MEAS if no satellite rate from
|
||||
* the sensors was provided, KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model
|
||||
* calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible,
|
||||
* RETURN_OK else
|
||||
*/
|
||||
ReturnValue_t mekfEst(const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
|
||||
const bool validGYRs_, const double *magneticField_,
|
||||
const bool validMagField_, const double *sunDir_, const bool validSS,
|
||||
const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
|
||||
const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
|
||||
AcsParameters *acsParameters);
|
||||
ReturnValue_t mekfEst(const acsctrl::SusDataProcessed *susData,
|
||||
const acsctrl::MgmDataProcessed *mgmData,
|
||||
const acsctrl::GyrDataProcessed *gyrData, const double timeDelta,
|
||||
acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
||||
|
||||
enum MekfStatus : uint8_t {
|
||||
UNINITIALIZED = 0,
|
||||
NO_GYR_DATA = 1,
|
||||
NO_MODEL_VECTORS = 2,
|
||||
NO_SUS_MGM_STR_DATA = 3,
|
||||
COVARIANCE_INVERSION_FAILED = 4,
|
||||
NOT_FINITE = 5,
|
||||
INITIALIZED = 10,
|
||||
RUNNING = 11,
|
||||
};
|
||||
void updateStandardDeviations(const AcsParameters *acsParameters);
|
||||
|
||||
void setStrData(const double qX, const double qY, const double qZ, const double qW,
|
||||
const bool valid, const bool allowStr);
|
||||
|
||||
// resetting Mekf
|
||||
static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_MEKF;
|
||||
static constexpr ReturnValue_t MEKF_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2);
|
||||
static constexpr ReturnValue_t MEKF_NO_GYR_DATA = returnvalue::makeCode(IF_MEKF_ID, 3);
|
||||
|
@ -82,26 +54,93 @@ class MultiplicativeKalmanFilter {
|
|||
private:
|
||||
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
|
||||
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
|
||||
static constexpr double ZERO_MAT66[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
||||
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
||||
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
|
||||
static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
|
||||
static constexpr double EYE3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
|
||||
static constexpr double EYE6[6][6] = {{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0},
|
||||
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
|
||||
|
||||
/*Parameters*/
|
||||
double quaternion_STR_SB[4];
|
||||
enum MekfStatus : uint8_t {
|
||||
UNINITIALIZED = 0,
|
||||
NO_GYR_DATA = 1,
|
||||
NO_MODEL_VECTORS = 2,
|
||||
NO_SUS_MGM_STR_DATA = 3,
|
||||
COVARIANCE_INVERSION_FAILED = 4,
|
||||
NOT_FINITE = 5,
|
||||
INITIALIZED = 10,
|
||||
RUNNING = 11,
|
||||
};
|
||||
|
||||
/*States*/
|
||||
double initialQuaternion[4] = {0, 0, 0, 1}; /*after reset?QUEST*/
|
||||
double initialCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
||||
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
||||
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
|
||||
double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
|
||||
uint8_t sensorsAvail = 0;
|
||||
enum SensorAvailability : uint8_t {
|
||||
NONE = 0,
|
||||
SUS_MGM_STR = 1,
|
||||
SUS_MGM = 2,
|
||||
SUS_STR = 3,
|
||||
MGM_STR = 4,
|
||||
SUS = 5,
|
||||
MGM = 6,
|
||||
STR = 7,
|
||||
};
|
||||
|
||||
/*Outputs*/
|
||||
double quatBJ[4]; /* Output Quaternion */
|
||||
double biasGYR[3]; /*Between measured and estimated sat Rate*/
|
||||
/*Parameter INIT*/
|
||||
/*Functions*/
|
||||
void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus);
|
||||
void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus,
|
||||
double quat[4], double satRotRate[3]);
|
||||
MekfStatus mekfStatus = MekfStatus::UNINITIALIZED;
|
||||
|
||||
struct StrData {
|
||||
struct StrQuat {
|
||||
double value[4] = {0, 0, 0, 0};
|
||||
bool valid = false;
|
||||
} strQuat;
|
||||
} strData;
|
||||
|
||||
// Standard Deviations
|
||||
double sigmaSus = 0;
|
||||
double sigmaMgm = 0;
|
||||
double sigmaStr = 0;
|
||||
double sigmaGyr = 0;
|
||||
// sigmaV
|
||||
double sigmaGyrArw = 0;
|
||||
// sigmaU
|
||||
double sigmaGyrBs = 0;
|
||||
|
||||
// Covariance Matrices
|
||||
double covSus[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double covMgm[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double covStr[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
|
||||
double covAposteriori[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
||||
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
|
||||
|
||||
// Sensor Availability
|
||||
SensorAvailability sensorsAvailable = SensorAvailability::NONE;
|
||||
uint8_t matrixDimensionFactor = 0;
|
||||
|
||||
// Estimated States
|
||||
double estimatedQuaternionBI[4] = {0, 0, 0, 1};
|
||||
double estimatedBiasGyr[3] = {0, 0, 0};
|
||||
double estimatedRotRate[3] = {0, 0, 0};
|
||||
double estimatedCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
||||
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
||||
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
|
||||
|
||||
// Functions
|
||||
ReturnValue_t checkAvailableSensors(const acsctrl::SusDataProcessed *susData,
|
||||
const acsctrl::MgmDataProcessed *mgmData,
|
||||
const acsctrl::GyrDataProcessed *gyrData,
|
||||
acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
||||
|
||||
void kfUpdate(const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData,
|
||||
double *measSensMatrix, double *measCovMatrix, double *measVec, double *measEstVec);
|
||||
ReturnValue_t kfGain(double *measSensMatrix, double *measCovMatrix, double *kalmanGain,
|
||||
acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
||||
void kfCovAposteriori(double *kalmanGain, double *measSensMatrix);
|
||||
void kfStateAposteriori(double *kalmanGain, double *measVec, double *estVec);
|
||||
void kfPropagate(const acsctrl::GyrDataProcessed *gyrData, const double timeDiff);
|
||||
|
||||
ReturnValue_t kfFiniteCheck(acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
||||
|
||||
void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
||||
void updateDataSet(acsctrl::AttitudeEstimationData *attitudeEstimationData);
|
||||
};
|
||||
|
||||
#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */
|
||||
|
|
|
@ -1,43 +1,28 @@
|
|||
#include "Navigation.h"
|
||||
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "util/CholeskyDecomposition.h"
|
||||
#include "util/MathOperations.h"
|
||||
|
||||
Navigation::Navigation() {}
|
||||
Navigation::Navigation(AcsParameters *acsParameters) : multiplicativeKalmanFilter(acsParameters) {}
|
||||
|
||||
Navigation::~Navigation() {}
|
||||
|
||||
ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
|
||||
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::AttitudeEstimationData *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.isTrustWorthy.value;
|
||||
ReturnValue_t Navigation::useMekf(const ACS::SensorValues *sensorValues,
|
||||
const acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||
const acsctrl::MgmDataProcessed *mgmDataProcessed,
|
||||
const acsctrl::SusDataProcessed *susDataProcessed,
|
||||
const double timeDelta,
|
||||
acsctrl::AttitudeEstimationData *attitudeEstimationData,
|
||||
const bool allowStr) {
|
||||
multiplicativeKalmanFilter.setStrData(
|
||||
sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
|
||||
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value,
|
||||
sensorValues->strSet.caliQx.isValid(), allowStr);
|
||||
|
||||
if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
|
||||
mekfStatus = multiplicativeKalmanFilter.init(
|
||||
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
|
||||
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
|
||||
susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
|
||||
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData,
|
||||
acsParameters);
|
||||
mekfStatus = multiplicativeKalmanFilter.init(susDataProcessed, mgmDataProcessed,
|
||||
gyrDataProcessed, attitudeEstimationData);
|
||||
return mekfStatus;
|
||||
} else {
|
||||
mekfStatus = multiplicativeKalmanFilter.mekfEst(
|
||||
quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value,
|
||||
gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
|
||||
mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
|
||||
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
|
||||
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
|
||||
mgmDataProcessed->magIgrfModel.isValid(), mekfData, acsParameters);
|
||||
susDataProcessed, mgmDataProcessed, gyrDataProcessed, timeDelta, attitudeEstimationData);
|
||||
return mekfStatus;
|
||||
}
|
||||
}
|
||||
|
@ -82,3 +67,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
|
|||
ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {
|
||||
return sgp4Propagator.initialize(line1, line2);
|
||||
}
|
||||
|
||||
void Navigation::updateMekfStandardDeviations(const AcsParameters *acsParameters) {
|
||||
multiplicativeKalmanFilter.updateStandardDeviations(acsParameters);
|
||||
}
|
||||
|
|
|
@ -5,24 +5,25 @@
|
|||
#include <mission/acs/defs.h>
|
||||
#include <mission/controller/acs/AcsParameters.h>
|
||||
#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
|
||||
#include <mission/controller/acs/SensorProcessing.h>
|
||||
#include <mission/controller/acs/SensorValues.h>
|
||||
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
||||
|
||||
class Navigation {
|
||||
public:
|
||||
Navigation();
|
||||
Navigation(AcsParameters *acsParameters);
|
||||
virtual ~Navigation();
|
||||
|
||||
ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
|
||||
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters);
|
||||
ReturnValue_t useMekf(const ACS::SensorValues *sensorValues,
|
||||
const acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||
const acsctrl::MgmDataProcessed *mgmDataProcessed,
|
||||
const acsctrl::SusDataProcessed *susDataProcessed, const double timeDelta,
|
||||
acsctrl::AttitudeEstimationData *attitudeEstimationData,
|
||||
const bool allowStr);
|
||||
void resetMekf(acsctrl::AttitudeEstimationData *mekfData);
|
||||
|
||||
ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
|
||||
ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);
|
||||
void updateMekfStandardDeviations(const AcsParameters *acsParameters);
|
||||
|
||||
protected:
|
||||
private:
|
||||
|
|
|
@ -180,7 +180,8 @@ void SensorProcessing::processSus(
|
|||
const AcsParameters::SunModelParameters *sunModelParameters,
|
||||
acsctrl::SusDataProcessed *susDataProcessed) {
|
||||
/* -------- Sun Model Direction (IJK frame) ------- */
|
||||
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeAbsolute);
|
||||
double JD2000 = 0;
|
||||
Clock::convertTimevalToJD2000(timeAbsolute, &JD2000);
|
||||
|
||||
// Julean Centuries
|
||||
double sunIjkModel[3] = {0.0, 0.0, 0.0};
|
||||
|
@ -198,6 +199,7 @@ void SensorProcessing::processSus(
|
|||
sunIjkModel[0] = cos(eclipticLongitude);
|
||||
sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
|
||||
sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
|
||||
VectorOperations<double>::normalize(sunIjkModel, sunIjkModel, 3);
|
||||
|
||||
uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
||||
if (sus0valid) {
|
||||
|
@ -528,8 +530,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
|
|||
uint8_t gpsSource = acs::gps::Source::NONE;
|
||||
// We do not trust the GPS and therefore it shall die here if SPG4 is running
|
||||
if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) {
|
||||
MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude,
|
||||
gdLongitude, altitude);
|
||||
CoordinateTransformations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value,
|
||||
gdLatitude, gdLongitude, altitude);
|
||||
double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
|
||||
gcLatitude = atan(factor * tan(gdLatitude));
|
||||
{
|
||||
|
@ -559,7 +561,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
|
|||
|
||||
// Calculation of the satellite velocity in earth fixed frame
|
||||
double deltaDistance[3] = {0, 0, 0};
|
||||
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
|
||||
CoordinateTransformations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
|
||||
if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) {
|
||||
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
|
||||
VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3);
|
||||
|
|
|
@ -2,6 +2,7 @@
|
|||
#define SENSORPROCESSING_H_
|
||||
|
||||
#include <common/config/eive/resultClassIds.h>
|
||||
#include <fsfw/coordinates/CoordinateTransformations.h>
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include <fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
|
@ -9,12 +10,12 @@
|
|||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <fsfw/globalfunctions/timevalOperations.h>
|
||||
#include <fsfw/returnvalues/returnvalue.h>
|
||||
#include <fsfw/timemanager/Clock.h>
|
||||
#include <mission/acs/defs.h>
|
||||
#include <mission/controller/acs/AcsParameters.h>
|
||||
#include <mission/controller/acs/Igrf13Model.h>
|
||||
#include <mission/controller/acs/SensorValues.h>
|
||||
#include <mission/controller/acs/SusConverter.h>
|
||||
#include <mission/controller/acs/util/MathOperations.h>
|
||||
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
||||
|
||||
#include <cmath>
|
||||
|
|
|
@ -4,7 +4,7 @@
|
|||
#include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h>
|
||||
#include <fsfw_hal/devicehandlers/MgmRM3100Handler.h>
|
||||
#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
|
||||
#include <mission/acs/archive/GPSDefinitions.h>
|
||||
#include <linux/acs/GPSDefinitions.h>
|
||||
#include <mission/acs/gyroAdisHelpers.h>
|
||||
#include <mission/acs/imtqHelpers.h>
|
||||
#include <mission/acs/rwHelpers.h>
|
||||
|
|
|
@ -1,11 +1,5 @@
|
|||
#include "PtgCtrl.h"
|
||||
|
||||
#include <fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <fsfw/globalfunctions/sign.h>
|
||||
|
||||
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
|
||||
|
||||
PtgCtrl::~PtgCtrl() {}
|
||||
|
@ -15,10 +9,10 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
|
|||
const bool fusedRateValid, const uint8_t rotRateSource, const uint8_t mekfEnabled) {
|
||||
if (not magFieldValid) {
|
||||
return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
|
||||
} else if (mekfEnabled and mekfValid) {
|
||||
return acs::ControlModeStrategy::PTGCTRL_MEKF;
|
||||
} else if (strValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
|
||||
return acs::ControlModeStrategy::PTGCTRL_STR;
|
||||
} else if (mekfEnabled and mekfValid) {
|
||||
return acs::ControlModeStrategy::PTGCTRL_MEKF;
|
||||
} else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
|
||||
return acs::ControlModeStrategy::PTGCTRL_QUEST;
|
||||
}
|
||||
|
@ -39,7 +33,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
|
|||
double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]};
|
||||
|
||||
double cInt = 2 * om * zeta;
|
||||
double kInt = 2 * pow(om, 2);
|
||||
double kInt = 2 * om * om;
|
||||
|
||||
double qErrorLaw[3] = {0, 0, 0};
|
||||
|
||||
|
@ -68,9 +62,9 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
|
|||
|
||||
// Inverse of gainMatrix
|
||||
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
|
||||
gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
|
||||
gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
|
||||
gainMatrixInverse[0][0] = 1. / gainMatrix[0][0];
|
||||
gainMatrixInverse[1][1] = 1. / gainMatrix[1][1];
|
||||
gainMatrixInverse[2][2] = 1. / gainMatrix[2][2];
|
||||
|
||||
double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MatrixOperations<double>::multiply(
|
||||
|
@ -111,9 +105,13 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
|
|||
VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
|
||||
}
|
||||
|
||||
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
void PtgCtrl::ptgNullspace(const bool allRwAvabilable,
|
||||
AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
|
||||
const int32_t speedRw3, double *rwTrqNs) {
|
||||
if (not allRwAvabilable) {
|
||||
return;
|
||||
}
|
||||
// concentrate RW speeds as vector and convert to double
|
||||
double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
|
||||
static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
|
||||
|
@ -142,9 +140,10 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara
|
|||
4);
|
||||
}
|
||||
|
||||
void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
void PtgCtrl::ptgDesaturation(const bool allRwAvailable, const acsctrl::RwAvail *rwAvail,
|
||||
AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
const double *magFieldB, const bool magFieldBValid,
|
||||
const double *satRate, const int32_t speedRw0, const int32_t speedRw1,
|
||||
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;
|
||||
|
@ -158,17 +157,24 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
|
|||
double magFieldBT[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
|
||||
|
||||
// calculate angular momentum of the satellite
|
||||
double angMomentumSat[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::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<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
|
||||
pointingLawParameters->nullspaceSpeed, refSpeedRws, 4);
|
||||
if (not allRwAvailable) {
|
||||
if (not rwAvail->rw1avail) {
|
||||
refSpeedRws[0] = 0.0;
|
||||
} else if (not rwAvail->rw2avail) {
|
||||
refSpeedRws[1] = 0.0;
|
||||
} else if (not rwAvail->rw3avail) {
|
||||
refSpeedRws[2] = 0.0;
|
||||
} else if (not rwAvail->rw4avail) {
|
||||
refSpeedRws[3] = 0.0;
|
||||
}
|
||||
}
|
||||
VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
|
||||
|
||||
// convert speed from 10 RPM to 1 RPM
|
||||
VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
|
||||
// convert to rad/s
|
||||
|
@ -184,16 +190,12 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
|
|||
|
||||
// calculate total angular momentum
|
||||
double angMomentumTotal[3] = {0, 0, 0};
|
||||
VectorOperations<double>::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3);
|
||||
|
||||
// calculating momentum error
|
||||
double deltaAngMomentum[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef,
|
||||
deltaAngMomentum, 3);
|
||||
VectorOperations<double>::subtract(angMomentumRw, pointingLawParameters->desatMomentumRef,
|
||||
angMomentumTotal, 3);
|
||||
|
||||
// resulting magnetic dipole command
|
||||
double crossAngMomentumMagField[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField);
|
||||
VectorOperations<double>::cross(angMomentumTotal, magFieldBT, crossAngMomentumMagField);
|
||||
double factor =
|
||||
pointingLawParameters->deSatGainFactor / VectorOperations<double>::norm(magFieldBT, 3);
|
||||
VectorOperations<double>::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3);
|
||||
|
@ -217,9 +219,13 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee
|
|||
rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
|
||||
} else if (rwCmdSpeeds[i] < currRwSpeed[i]) {
|
||||
rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
|
||||
} else {
|
||||
rwCmdSpeeds[i] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
rwCmdSpeeds[i] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -1,11 +1,16 @@
|
|||
#ifndef PTGCTRL_H_
|
||||
#define PTGCTRL_H_
|
||||
|
||||
#include <math.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <fsfw/globalfunctions/sign.h>
|
||||
#include <mission/acs/defs.h>
|
||||
#include <mission/controller/acs/AcsParameters.h>
|
||||
#include <mission/controller/acs/SensorValues.h>
|
||||
#include <stdio.h>
|
||||
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
class PtgCtrl {
|
||||
/*
|
||||
|
@ -33,14 +38,16 @@ class PtgCtrl {
|
|||
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
|
||||
const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
|
||||
|
||||
void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
void ptgNullspace(const bool allRwAvabilable,
|
||||
AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
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);
|
||||
void ptgDesaturation(const bool allRwAvabilable, const acsctrl::RwAvail *rwAvail,
|
||||
AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
const double *magFieldB, const bool magFieldBValid, 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 anti-stiction
|
||||
|
|
|
@ -1,98 +0,0 @@
|
|||
/*
|
||||
* TinyEKF: Extended Kalman Filter for embedded processors
|
||||
*
|
||||
* Copyright (C) 2015 Simon D. Levy
|
||||
*
|
||||
* MIT License
|
||||
*/
|
||||
#ifndef CHOLESKYDECOMPOSITION_H_
|
||||
#define CHOLESKYDECOMPOSITION_H_
|
||||
#include <math.h>
|
||||
// typedef unsigned int uint8_t;
|
||||
|
||||
template <typename T1, typename T2 = T1, typename T3 = T2>
|
||||
class CholeskyDecomposition {
|
||||
public:
|
||||
static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) {
|
||||
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
|
||||
return cholsl(matrix, result, tempMatrix, dimension);
|
||||
}
|
||||
|
||||
private:
|
||||
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
|
||||
static uint8_t choldc1(double *a, double *p, uint8_t n) {
|
||||
int8_t i, j, k;
|
||||
double sum;
|
||||
|
||||
for (i = 0; i < n; i++) {
|
||||
for (j = i; j < n; j++) {
|
||||
sum = a[i * n + j];
|
||||
for (k = i - 1; k >= 0; k--) {
|
||||
sum -= a[i * n + k] * a[j * n + k];
|
||||
}
|
||||
if (i == j) {
|
||||
if (sum <= 0) {
|
||||
return 1; /* error */
|
||||
}
|
||||
p[i] = sqrt(sum);
|
||||
} else {
|
||||
a[j * n + i] = sum / p[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0; /* success */
|
||||
}
|
||||
|
||||
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
|
||||
static uint8_t choldcsl(double *A, double *a, double *p, uint8_t n) {
|
||||
uint8_t i, j, k;
|
||||
double sum;
|
||||
for (i = 0; i < n; i++)
|
||||
for (j = 0; j < n; j++) a[i * n + j] = A[i * n + j];
|
||||
if (choldc1(a, p, n)) return 1;
|
||||
for (i = 0; i < n; i++) {
|
||||
a[i * n + i] = 1 / p[i];
|
||||
for (j = i + 1; j < n; j++) {
|
||||
sum = 0;
|
||||
for (k = i; k < j; k++) {
|
||||
sum -= a[j * n + k] * a[k * n + i];
|
||||
}
|
||||
a[j * n + i] = sum / p[j];
|
||||
}
|
||||
}
|
||||
|
||||
return 0; /* success */
|
||||
}
|
||||
|
||||
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
|
||||
static uint8_t cholsl(double *A, double *a, double *p, uint8_t n) {
|
||||
uint8_t i, j, k;
|
||||
if (choldcsl(A, a, p, n)) return 1;
|
||||
for (i = 0; i < n; i++) {
|
||||
for (j = i + 1; j < n; j++) {
|
||||
a[i * n + j] = 0.0;
|
||||
}
|
||||
}
|
||||
for (i = 0; i < n; i++) {
|
||||
a[i * n + i] *= a[i * n + i];
|
||||
for (k = i + 1; k < n; k++) {
|
||||
a[i * n + i] += a[k * n + i] * a[k * n + i];
|
||||
}
|
||||
for (j = i + 1; j < n; j++) {
|
||||
for (k = j; k < n; k++) {
|
||||
a[i * n + j] += a[k * n + i] * a[k * n + j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (i = 0; i < n; i++) {
|
||||
for (j = 0; j < i; j++) {
|
||||
a[i * n + j] = a[j * n + i];
|
||||
}
|
||||
}
|
||||
|
||||
return 0; /* success */
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* CONTRIB_MATH_CHOLESKYDECOMPOSITION_H_ */
|
|
@ -1,468 +0,0 @@
|
|||
#ifndef MATH_MATHOPERATIONS_H_
|
||||
#define MATH_MATHOPERATIONS_H_
|
||||
|
||||
#include <fsfw/src/fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/sign.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
|
||||
#include "fsfw/serviceinterface.h"
|
||||
|
||||
template <typename T1, typename T2 = T1>
|
||||
class MathOperations {
|
||||
public:
|
||||
static void skewMatrix(const T1 vector[], T2 *result) {
|
||||
// Input Dimension [3], Output [3][3]
|
||||
result[0] = 0;
|
||||
result[1] = -vector[2];
|
||||
result[2] = vector[1];
|
||||
result[3] = vector[2];
|
||||
result[4] = 0;
|
||||
result[5] = -vector[0];
|
||||
result[6] = -vector[1];
|
||||
result[7] = vector[0];
|
||||
result[8] = 0;
|
||||
}
|
||||
static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], T2 *result,
|
||||
uint8_t size = 3) {
|
||||
// Looks like MatrixOpertions::multiply is able to do the same thing
|
||||
for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) {
|
||||
for (uint8_t resultRow = 0; resultRow < size; resultRow++) {
|
||||
result[resultColumn + size * resultRow] =
|
||||
vector1[resultRow] * transposeVector2[resultColumn];
|
||||
}
|
||||
}
|
||||
/*matrixSun[i][j] = sunEstB[i] * sunEstB[j];
|
||||
matrixMag[i][j] = magEstB[i] * magEstB[j];
|
||||
matrixSunMag[i][j] = sunEstB[i] * magEstB[j];
|
||||
matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/
|
||||
}
|
||||
|
||||
static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) {
|
||||
int min_idx;
|
||||
T1 temp;
|
||||
memcpy(result, matrix, rowSize * colSize * sizeof(*result));
|
||||
// One by one move boundary of unsorted subarray
|
||||
for (int k = 0; k < rowSize; k++) {
|
||||
for (int i = 0; i < colSize - 1; i++) {
|
||||
// Find the minimum element in unsorted array
|
||||
min_idx = i;
|
||||
for (int j = i + 1; j < colSize; j++) {
|
||||
if (result[j + k * colSize] < result[min_idx + k * colSize]) {
|
||||
min_idx = j;
|
||||
}
|
||||
}
|
||||
// Swap the found minimum element with the first element
|
||||
temp = result[i + k * colSize];
|
||||
result[i + k * colSize] = result[min_idx + k * colSize];
|
||||
result[min_idx + k * colSize] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void convertDateToJD2000(const T1 time, T2 julianDate) {
|
||||
// time = { Y, M, D, h, m,s}
|
||||
// time in sec and microsec -> The Epoch (unixtime)
|
||||
julianDate = 1721013.5 + 367 * time[0] - floor(7 / 4 * (time[0] + (time[1] + 9) / 12)) +
|
||||
floor(275 * time[1] / 9) + time[2] +
|
||||
(60 * time[3] + time[4] + (time(5) / 60)) / 1440;
|
||||
}
|
||||
|
||||
static T1 convertUnixToJD2000(timeval time) {
|
||||
// time = {{s},{us}}
|
||||
T1 julianDate2000;
|
||||
julianDate2000 = (time.tv_sec / 86400.0) + 2440587.5 - 2451545;
|
||||
return julianDate2000;
|
||||
}
|
||||
|
||||
static void dcmFromQuat(const T1 vector[], T1 *outputDcm) {
|
||||
// convention q = [qx,qy,qz, qw]
|
||||
outputDcm[0] = pow(vector[0], 2) - pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
|
||||
outputDcm[1] = 2 * (vector[0] * vector[1] + vector[2] * vector[3]);
|
||||
outputDcm[2] = 2 * (vector[0] * vector[2] - vector[1] * vector[3]);
|
||||
|
||||
outputDcm[3] = 2 * (vector[1] * vector[0] - vector[2] * vector[3]);
|
||||
outputDcm[4] = -pow(vector[0], 2) + pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
|
||||
outputDcm[5] = 2 * (vector[1] * vector[2] + vector[0] * vector[3]);
|
||||
|
||||
outputDcm[6] = 2 * (vector[2] * vector[0] + vector[1] * vector[3]);
|
||||
outputDcm[7] = 2 * (vector[2] * vector[1] - vector[0] * vector[3]);
|
||||
outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2);
|
||||
}
|
||||
|
||||
static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt,
|
||||
T2 *cartesianOutput) {
|
||||
/* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
|
||||
* longitude and altitude
|
||||
* @param: lat geodetic latitude [rad]
|
||||
* longi longitude [rad]
|
||||
* alt altitude [m]
|
||||
* cartesianOutput Cartesian Coordinates in ECEF (3x1)
|
||||
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
|
||||
* Landis Markley and John L. Crassidis*/
|
||||
double radiusPolar = 6356752.314;
|
||||
double radiusEqua = 6378137;
|
||||
|
||||
double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
|
||||
double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2));
|
||||
|
||||
cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi);
|
||||
cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
|
||||
cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
|
||||
}
|
||||
|
||||
static void latLongAltFromCartesian(const T1 *vector, T1 &latitude, T1 &longitude, T1 &altitude) {
|
||||
/* @brief: latLongAltFromCartesian() - calculates latitude, longitude and altitude from
|
||||
* cartesian coordinates in ECEF
|
||||
* @param: x x-value of position vector [m]
|
||||
* y y-value of position vector [m]
|
||||
* z z-value of position vector [m]
|
||||
* latitude geodetic latitude [rad]
|
||||
* longitude longitude [rad]
|
||||
* altitude altitude [m]
|
||||
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f
|
||||
* Landis Markley and John L. Crassidis*/
|
||||
// From World Geodetic System the Earth Radii
|
||||
double a = 6378137.0; // semimajor axis [m]
|
||||
double b = 6356752.3142; // semiminor axis [m]
|
||||
|
||||
// Calculation
|
||||
double e2 = 1 - pow(b, 2) / pow(a, 2);
|
||||
double epsilon2 = pow(a, 2) / pow(b, 2) - 1;
|
||||
double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2));
|
||||
double p = std::abs(vector[2]) / epsilon2;
|
||||
double s = pow(rho, 2) / (e2 * epsilon2);
|
||||
double q = pow(p, 2) - pow(b, 2) + s;
|
||||
double u = p / sqrt(q);
|
||||
double v = pow(b, 2) * pow(u, 2) / q;
|
||||
double P = 27 * v * s / q;
|
||||
double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.);
|
||||
double t = (1 + Q + 1 / Q) / 6;
|
||||
double c = sqrt(pow(u, 2) - 1 + 2 * t);
|
||||
double w = (c - u) / 2;
|
||||
double d =
|
||||
sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.));
|
||||
double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2));
|
||||
latitude = asin((epsilon2 + 1) * d / N);
|
||||
altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N;
|
||||
longitude = atan2(vector[1], vector[0]);
|
||||
}
|
||||
|
||||
static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
|
||||
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
|
||||
* @param: time Current time
|
||||
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
|
||||
* outputDotDcmEJ Derivative of transformation matrix [3][3]
|
||||
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
|
||||
* Landis Markley and John L. Crassidis*/
|
||||
double JD2000Floor = 0;
|
||||
double JD2000 = convertUnixToJD2000(time);
|
||||
// Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
|
||||
JD2000Floor = floor(JD2000);
|
||||
if ((JD2000 - JD2000Floor) < 0.5) {
|
||||
JD2000Floor -= 0.5;
|
||||
} else {
|
||||
JD2000Floor += 0.5;
|
||||
}
|
||||
|
||||
double JC2000 = JD2000Floor / 36525;
|
||||
double sec = (JD2000 - JD2000Floor) * 86400;
|
||||
double gmst = 0; // greenwich mean sidereal time
|
||||
gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000, 2) -
|
||||
0.0000062 * pow(JC2000, 3) + 1.002737909350795 * sec;
|
||||
double rest = gmst / 86400;
|
||||
double FloorRest = floor(rest);
|
||||
double secOfDay = rest - FloorRest;
|
||||
secOfDay *= 86400;
|
||||
gmst = secOfDay / 240 * M_PI / 180;
|
||||
|
||||
outputDcmEJ[0] = cos(gmst);
|
||||
outputDcmEJ[1] = sin(gmst);
|
||||
outputDcmEJ[2] = 0;
|
||||
outputDcmEJ[3] = -sin(gmst);
|
||||
outputDcmEJ[4] = cos(gmst);
|
||||
outputDcmEJ[5] = 0;
|
||||
outputDcmEJ[6] = 0;
|
||||
outputDcmEJ[7] = 0;
|
||||
outputDcmEJ[8] = 1;
|
||||
|
||||
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
|
||||
double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
|
||||
{outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]},
|
||||
{outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}};
|
||||
double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
|
||||
double omegaEarth = 0.000072921158553;
|
||||
double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
|
||||
MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
|
||||
}
|
||||
|
||||
/* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
|
||||
* give also the back the derivative of this matrix
|
||||
* @param: unixTime Current time in Unix format
|
||||
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
|
||||
* outputDotDcmEJ Derivative of transformation matrix [3][3]
|
||||
* @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und
|
||||
Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff
|
||||
* Oliver Zeile
|
||||
*
|
||||
https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/
|
||||
static void ecfToEciWithNutPre(timeval unixTime, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
|
||||
// TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10
|
||||
//(initial Offset) International Atomic Time (TAI)
|
||||
|
||||
double JD2000UTC1 = convertUnixToJD2000(unixTime);
|
||||
|
||||
// Julian Date / century from TT
|
||||
timeval terestrialTime = unixTime;
|
||||
terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37;
|
||||
double JD2000TT = convertUnixToJD2000(terestrialTime);
|
||||
double JC2000TT = JD2000TT / 36525;
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Transformation from earth rotation Theta
|
||||
//-------------------------------------------------------------------------------------
|
||||
double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
// Earth Rotation angle
|
||||
double era = 0;
|
||||
era = 2 * M_PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1);
|
||||
// Greenwich Mean Sidereal Time
|
||||
double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) -
|
||||
0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4);
|
||||
double arcsecFactor = 1 * M_PI / (180 * 3600);
|
||||
gmst2000 *= arcsecFactor;
|
||||
gmst2000 += era;
|
||||
|
||||
theta[0][0] = cos(gmst2000);
|
||||
theta[0][1] = sin(gmst2000);
|
||||
theta[0][2] = 0;
|
||||
theta[1][0] = -sin(gmst2000);
|
||||
theta[1][1] = cos(gmst2000);
|
||||
theta[1][2] = 0;
|
||||
theta[2][0] = 0;
|
||||
theta[2][1] = 0;
|
||||
theta[2][2] = 1;
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Transformation from earth Precession P
|
||||
//-------------------------------------------------------------------------------------
|
||||
double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
|
||||
double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT, 2) + 0.017998 * pow(JC2000TT, 3);
|
||||
double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT, 2) - 0.041833 * pow(JC2000TT, 3);
|
||||
double ze = zeta + 0.79280 * pow(JC2000TT, 2) + 0.000205 * pow(JC2000TT, 3);
|
||||
|
||||
zeta *= arcsecFactor;
|
||||
theta2 *= arcsecFactor;
|
||||
ze *= arcsecFactor;
|
||||
|
||||
precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta);
|
||||
precession[1][0] = cos(ze) * sin(zeta) + sin(ze) * cos(theta2) * cos(zeta);
|
||||
precession[2][0] = sin(theta2) * cos(zeta);
|
||||
precession[0][1] = -sin(ze) * cos(zeta) - cos(ze) * cos(theta2) * sin(zeta);
|
||||
precession[1][1] = cos(ze) * cos(zeta) - sin(ze) * cos(theta2) * sin(zeta);
|
||||
precession[2][1] = -sin(theta2) * sin(zeta);
|
||||
precession[0][2] = -cos(ze) * sin(theta2);
|
||||
precession[1][2] = -sin(ze) * sin(theta2);
|
||||
precession[2][2] = cos(theta2);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Transformation from earth Nutation N
|
||||
//-------------------------------------------------------------------------------------
|
||||
double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
// lunar asc node
|
||||
double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT +
|
||||
7.455 * pow(JC2000TT, 2) + 0.008 * pow(JC2000TT, 3);
|
||||
Om *= arcsecFactor;
|
||||
// delta psi approx
|
||||
double dp = -17.2 * arcsecFactor * sin(Om);
|
||||
|
||||
// delta eps approx
|
||||
double de = 9.203 * arcsecFactor * cos(Om);
|
||||
|
||||
// % true obliquity of the ecliptic eps p.71 (simplified)
|
||||
double e = 23.43929111 * M_PI / 180 - 46.8150 / 3600 * JC2000TT * M_PI / 180;
|
||||
|
||||
nutation[0][0] = cos(dp);
|
||||
nutation[1][0] = cos(e + de) * sin(dp);
|
||||
nutation[2][0] = sin(e + de) * sin(dp);
|
||||
nutation[0][1] = -cos(e) * sin(dp);
|
||||
nutation[1][1] = cos(e) * cos(e + de) * cos(dp) + sin(e) * sin(e + de);
|
||||
nutation[2][1] = cos(e) * sin(e + de) * cos(dp) - sin(e) * cos(e + de);
|
||||
nutation[0][2] = -sin(e) * sin(dp);
|
||||
nutation[1][2] = sin(e) * cos(e + de) * cos(dp) - cos(e) * sin(e + de);
|
||||
nutation[2][2] = sin(e) * sin(e + de) * cos(dp) + cos(e) * cos(e + de);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Derivative of rotation matrix from earth
|
||||
//-------------------------------------------------------------------------------------
|
||||
double thetaDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dotMatrix[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
|
||||
double omegaEarth = 0.000072921158553;
|
||||
MatrixOperations<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3);
|
||||
MatrixOperations<double>::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of transformation matrix and Derivative of transformation matrix
|
||||
//-------------------------------------------------------------------------------------
|
||||
double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3);
|
||||
MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3);
|
||||
|
||||
MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
|
||||
}
|
||||
static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
|
||||
int i, j;
|
||||
double determinant = 0;
|
||||
double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
|
||||
{matrix[3], matrix[4], matrix[5]},
|
||||
{matrix[6], matrix[7], matrix[8]}};
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] -
|
||||
mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
|
||||
}
|
||||
// cout<<"\n\ndeterminant: "<<determinant;
|
||||
// cout<<"\n\nInverse of matrix is: \n";
|
||||
for (i = 0; i < 3; i++) {
|
||||
for (j = 0; j < 3; j++) {
|
||||
output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -
|
||||
(mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) /
|
||||
determinant;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
|
||||
/* do not use this. takes 300ms */
|
||||
float det = 0;
|
||||
T1 matrix[size][size], submatrix[size - 1][size - 1];
|
||||
for (uint8_t row = 0; row < size; row++) {
|
||||
for (uint8_t col = 0; col < size; col++) {
|
||||
matrix[row][col] = inputMatrix[row * size + col];
|
||||
}
|
||||
}
|
||||
if (size == 2)
|
||||
return ((matrix[0][0] * matrix[1][1]) - (matrix[1][0] * matrix[0][1]));
|
||||
else {
|
||||
for (uint8_t col = 0; col < size; col++) {
|
||||
int subRow = 0;
|
||||
for (uint8_t rowIndex = 1; rowIndex < size; rowIndex++) {
|
||||
int subCol = 0;
|
||||
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
|
||||
if (colIndex == col) continue;
|
||||
submatrix[subRow][subCol] = matrix[rowIndex][colIndex];
|
||||
subCol++;
|
||||
}
|
||||
subRow++;
|
||||
}
|
||||
det += (pow(-1, col) * matrix[0][col] *
|
||||
MathOperations<T1>::matrixDeterminant(*submatrix, size - 1));
|
||||
}
|
||||
}
|
||||
return det;
|
||||
}
|
||||
|
||||
static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
|
||||
// Stopwatch stopwatch;
|
||||
T1 matrix[size][size], identity[size][size];
|
||||
// reformat array to matrix
|
||||
for (uint8_t row = 0; row < size; row++) {
|
||||
for (uint8_t col = 0; col < size; col++) {
|
||||
matrix[row][col] = inputMatrix[row * size + col];
|
||||
}
|
||||
}
|
||||
// init identity matrix
|
||||
std::memset(identity, 0.0, sizeof(identity));
|
||||
for (uint8_t diag = 0; diag < size; diag++) {
|
||||
identity[diag][diag] = 1;
|
||||
}
|
||||
// gauss-jordan algo
|
||||
// sort matrix such as no diag entry shall be 0
|
||||
for (uint8_t row = 0; row < size; row++) {
|
||||
if (matrix[row][row] == 0.0) {
|
||||
bool swaped = false;
|
||||
uint8_t rowIndex = 0;
|
||||
while ((rowIndex < size) && !swaped) {
|
||||
if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) {
|
||||
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
|
||||
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
|
||||
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
|
||||
}
|
||||
swaped = true;
|
||||
}
|
||||
rowIndex++;
|
||||
}
|
||||
if (!swaped) {
|
||||
return 1; // matrix not invertible
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int row = 0; row < size; row++) {
|
||||
if (matrix[row][row] == 0.0) {
|
||||
uint8_t rowIndex;
|
||||
if (row == 0) {
|
||||
rowIndex = size - 1;
|
||||
} else {
|
||||
rowIndex = row - 1;
|
||||
}
|
||||
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
|
||||
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
|
||||
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
|
||||
}
|
||||
row--;
|
||||
if (row < 0) {
|
||||
return 1; // Matrix is not invertible
|
||||
}
|
||||
}
|
||||
}
|
||||
// remove non diag elements in matrix (jordan)
|
||||
for (int row = 0; row < size; row++) {
|
||||
for (int rowIndex = 0; rowIndex < size; rowIndex++) {
|
||||
if (row != rowIndex) {
|
||||
double ratio = matrix[rowIndex][row] / matrix[row][row];
|
||||
for (int colIndex = 0; colIndex < size; colIndex++) {
|
||||
matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex];
|
||||
identity[rowIndex][colIndex] -= ratio * identity[row][colIndex];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// normalize rows in matrix (gauss)
|
||||
for (int row = 0; row < size; row++) {
|
||||
for (int col = 0; col < size; col++) {
|
||||
identity[row][col] = identity[row][col] / matrix[row][row];
|
||||
}
|
||||
}
|
||||
std::memcpy(inverse, identity, sizeof(identity));
|
||||
return 0; // successful inversion
|
||||
}
|
||||
|
||||
static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
|
||||
for (uint8_t i = 0; i < size; i++) {
|
||||
if (not isfinite(inputVector[i])) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
|
||||
for (uint8_t col = 0; col < cols; col++) {
|
||||
for (uint8_t row = 0; row < rows; row++) {
|
||||
if (not isfinite(inputMatrix[row * cols + cols])) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* ACS_MATH_MATHOPERATIONS_H_ */
|
|
@ -1,6 +1,7 @@
|
|||
#ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_
|
||||
#define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_
|
||||
|
||||
#include <common/config/eive/resultClassIds.h>
|
||||
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
|
||||
#include <fsfw/datapoollocal/localPoolDefinitions.h>
|
||||
|
||||
|
@ -8,6 +9,25 @@
|
|||
|
||||
namespace acsctrl {
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
|
||||
//! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent.
|
||||
static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0xA0);
|
||||
//! [EXPORT] : [COMMENT] Writing the TLE to the file has failed.
|
||||
static constexpr ReturnValue_t WRITE_FILE_FAILED = MAKE_RETURN_CODE(0xA1);
|
||||
//! [EXPORT] : [COMMENT] Reading the TLE to the file has failed.
|
||||
static constexpr ReturnValue_t READ_FILE_FAILED = MAKE_RETURN_CODE(0xA2);
|
||||
//! [EXPORT] : [COMMENT] A single RW has failed.
|
||||
static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA3);
|
||||
//! [EXPORT] : [COMMENT] Multiple RWs have failed.
|
||||
static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA4);
|
||||
|
||||
struct RwAvail {
|
||||
bool rw1avail = false;
|
||||
bool rw2avail = false;
|
||||
bool rw3avail = false;
|
||||
bool rw4avail = false;
|
||||
};
|
||||
|
||||
enum SetIds : uint32_t {
|
||||
MGM_SENSOR_DATA,
|
||||
MGM_PROCESSED_DATA,
|
||||
|
|
|
@ -250,20 +250,30 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
|
|||
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, 16);
|
||||
new Service5EventReporting(
|
||||
PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5),
|
||||
80, 160);
|
||||
|
||||
auto psbParamsService5 =
|
||||
PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5);
|
||||
psbParamsService5.requestQueueDepth = 50;
|
||||
psbParamsService5.maxPacketsPerCycle = 50;
|
||||
new Service5EventReporting(psbParamsService5, 80, 160);
|
||||
new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, config::EIVE_PUS_APID,
|
||||
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));
|
||||
|
||||
new Service11TelecommandScheduling<common::OBSW_MAX_SCHEDULED_TCS>(
|
||||
PsbParams(objects::PUS_SERVICE_11_TC_SCHEDULER, config::EIVE_PUS_APID, pus::PUS_SERVICE_11),
|
||||
ccsdsDistrib);
|
||||
auto psbParamsService11 =
|
||||
PsbParams(objects::PUS_SERVICE_11_TC_SCHEDULER, config::EIVE_PUS_APID, pus::PUS_SERVICE_11);
|
||||
psbParamsService11.requestQueueDepth = 100;
|
||||
psbParamsService11.maxPacketsPerCycle = 100;
|
||||
new Service11TelecommandScheduling<common::OBSW_MAX_SCHEDULED_TCS>(psbParamsService11,
|
||||
ccsdsDistrib);
|
||||
|
||||
new Service15TmStorage(objects::PUS_SERVICE_15_TM_STORAGE, config::EIVE_PUS_APID, 10);
|
||||
new Service17Test(
|
||||
PsbParams(objects::PUS_SERVICE_17_TEST, config::EIVE_PUS_APID, pus::PUS_SERVICE_17));
|
||||
auto psbParamsService17 =
|
||||
PsbParams(objects::PUS_SERVICE_17_TEST, config::EIVE_PUS_APID, pus::PUS_SERVICE_17);
|
||||
psbParamsService17.requestQueueDepth = 50;
|
||||
psbParamsService17.maxPacketsPerCycle = 50;
|
||||
new Service17Test(psbParamsService17);
|
||||
new Service20ParameterManagement(objects::PUS_SERVICE_20_PARAMETERS, config::EIVE_PUS_APID,
|
||||
pus::PUS_SERVICE_20);
|
||||
new CService200ModeCommanding(objects::PUS_SERVICE_200_MODE_MGMT, config::EIVE_PUS_APID,
|
||||
|
|
|
@ -65,8 +65,8 @@ ReturnValue_t PcduHandler::performOperation(uint8_t counter) {
|
|||
ReturnValue_t PcduHandler::initialize() {
|
||||
ReturnValue_t result;
|
||||
|
||||
IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
|
||||
if (IPCStore == nullptr) {
|
||||
ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
|
||||
if (ipcStore == nullptr) {
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
|
||||
|
@ -162,10 +162,13 @@ void PcduHandler::updateHkTableDataset(store_address_t storeId, LocalPoolDataSet
|
|||
sizeof(CCSDSTime::CDS_short), dataset);
|
||||
const uint8_t* packet_ptr = nullptr;
|
||||
size_t size = 0;
|
||||
result = IPCStore->getData(storeId, &packet_ptr, &size);
|
||||
result = ipcStore->getData(storeId, &packet_ptr, &size);
|
||||
if (result != returnvalue::OK) {
|
||||
sif::error << "PCDUHandler::updateHkTableDataset: Failed to get data from IPCStore."
|
||||
<< std::endl;
|
||||
sif::error << "PCDUHandler::updateHkTableDataset: Failed to get data from IPC store, result 0x"
|
||||
<< std::hex << std::setw(4) << std::setfill('0') << result << std::dec
|
||||
<< std::setfill(' ') << std::endl;
|
||||
result = ipcStore->deleteData(storeId);
|
||||
return;
|
||||
}
|
||||
result = packetUpdate.deSerialize(&packet_ptr, &size, SerializeIF::Endianness::MACHINE);
|
||||
if (result != returnvalue::OK) {
|
||||
|
@ -173,7 +176,7 @@ void PcduHandler::updateHkTableDataset(store_address_t storeId, LocalPoolDataSet
|
|||
"in hk table dataset"
|
||||
<< std::endl;
|
||||
}
|
||||
result = IPCStore->deleteData(storeId);
|
||||
result = ipcStore->deleteData(storeId);
|
||||
if (result != returnvalue::OK) {
|
||||
sif::error << "PCDUHandler::updateHkTableDataset: Failed to delete data in IPCStore"
|
||||
<< std::endl;
|
||||
|
@ -396,7 +399,7 @@ ReturnValue_t PcduHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onO
|
|||
setParamMessage.serialize(&commandPtr, &serializedLength, maxSize, SerializeIF::Endianness::BIG);
|
||||
|
||||
store_address_t storeAddress;
|
||||
result = IPCStore->addData(&storeAddress, command, sizeof(command));
|
||||
result = ipcStore->addData(&storeAddress, command, sizeof(command));
|
||||
|
||||
CommandMessage message;
|
||||
ActionMessage::setCommand(&message, GOMSPACE::PARAM_SET, storeAddress);
|
||||
|
|
|
@ -94,7 +94,7 @@ class PcduHandler : public PowerSwitchIF,
|
|||
* Pointer to the IPCStore.
|
||||
* This caches the pointer received from the objectManager in the constructor.
|
||||
*/
|
||||
StorageManagerIF* IPCStore = nullptr;
|
||||
StorageManagerIF* ipcStore = nullptr;
|
||||
|
||||
/**
|
||||
* Message queue to communicate with other objetcs. Used for example to receive
|
||||
|
|
|
@ -55,6 +55,7 @@ static constexpr char VERSION_FILE_NAME[] = "version.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 LEAP_SECONDS_FILE_NAME[] = "leapseconds.txt";
|
||||
static constexpr char TIME_FILE_NAME[] = "time_backup.txt";
|
||||
|
||||
static constexpr uint32_t SYS_ROM_BASE_ADDR = 0x80000000;
|
||||
|
@ -93,6 +94,8 @@ static constexpr ActionId_t MV_HELPER = 53;
|
|||
static constexpr ActionId_t RM_HELPER = 54;
|
||||
static constexpr ActionId_t MKDIR_HELPER = 55;
|
||||
|
||||
static constexpr ActionId_t UPDATE_LEAP_SECONDS = 60;
|
||||
|
||||
static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::CORE;
|
||||
|
||||
static constexpr Event ALLOC_FAILURE = event::makeEvent(SUBSYSTEM_ID, 0, severity::MEDIUM);
|
||||
|
|
|
@ -3,8 +3,9 @@ add_subdirectory(acs)
|
|||
add_subdirectory(tcs)
|
||||
add_subdirectory(com)
|
||||
add_subdirectory(power)
|
||||
add_subdirectory(payload)
|
||||
|
||||
target_sources(
|
||||
${LIB_EIVE_MISSION}
|
||||
PRIVATE systemTree.cpp DualLanePowerStateMachine.cpp EiveSystem.cpp
|
||||
treeUtil.cpp SharedPowerAssemblyBase.cpp payloadModeTree.cpp)
|
||||
treeUtil.cpp SharedPowerAssemblyBase.cpp)
|
||||
|
|
|
@ -174,6 +174,7 @@ ReturnValue_t EiveSystem::initialize() {
|
|||
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(pdec::INVALID_TC_FRAME));
|
||||
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_LOW));
|
||||
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_CRITICAL));
|
||||
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::PTG_RATE_VIOLATION));
|
||||
return Subsystem::initialize();
|
||||
}
|
||||
|
||||
|
@ -224,6 +225,16 @@ void EiveSystem::handleEventMessages() {
|
|||
}
|
||||
break;
|
||||
}
|
||||
case acs::PTG_RATE_VIOLATION: {
|
||||
CommandMessage msg;
|
||||
HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HasHealthIF::FAULTY);
|
||||
ReturnValue_t result = MessageQueueSenderIF::sendMessage(
|
||||
strQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
|
||||
if (result != returnvalue::OK) {
|
||||
sif::error << "EIVE System: Sending FAULTY command to STR Assembly failed"
|
||||
<< std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
default:
|
||||
|
|
|
@ -1,6 +1,8 @@
|
|||
#include "StrFdir.h"
|
||||
|
||||
#include "mission/acs/defs.h"
|
||||
#include <eive/objects.h>
|
||||
#include <fsfw/events/EventManagerIF.h>
|
||||
#include <mission/acs/defs.h>
|
||||
|
||||
StrFdir::StrFdir(object_id_t strObject)
|
||||
: DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {}
|
||||
|
@ -12,3 +14,13 @@ ReturnValue_t StrFdir::eventReceived(EventMessage* event) {
|
|||
}
|
||||
return DeviceHandlerFailureIsolation::eventReceived(event);
|
||||
}
|
||||
|
||||
ReturnValue_t StrFdir::initialize() {
|
||||
ReturnValue_t result = DeviceHandlerFailureIsolation::initialize();
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
|
||||
return manager->subscribeToEvent(eventQueue->getId(),
|
||||
event::getEventId(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION));
|
||||
}
|
||||
|
|
|
@ -6,6 +6,7 @@
|
|||
class StrFdir : public DeviceHandlerFailureIsolation {
|
||||
public:
|
||||
StrFdir(object_id_t strObject);
|
||||
ReturnValue_t initialize() override;
|
||||
ReturnValue_t eventReceived(EventMessage* event) override;
|
||||
};
|
||||
|
||||
|
|
|
@ -0,0 +1 @@
|
|||
target_sources(${LIB_EIVE_MISSION} PRIVATE payloadModeTree.cpp)
|
|
@ -10,7 +10,7 @@
|
|||
#include "mission/com/defs.h"
|
||||
#include "mission/sysDefs.h"
|
||||
#include "mission/system/acs/acsModeTree.h"
|
||||
#include "mission/system/payloadModeTree.h"
|
||||
#include "mission/system/payload/payloadModeTree.h"
|
||||
#include "mission/system/power/epsModeTree.h"
|
||||
#include "mission/system/tcs/tcsModeTree.h"
|
||||
#include "treeUtil.h"
|
||||
|
|
|
@ -1 +1 @@
|
|||
Subproject commit 20f32a2f291d00869c693066de02c0257dec7be4
|
||||
Subproject commit a558693675dae09aff7ef74242c25f826584c46a
|
2
tmtc
2
tmtc
|
@ -1 +1 @@
|
|||
Subproject commit 747ad34eec5baa5199de49a1330687508c991550
|
||||
Subproject commit a8d0143b1ed9a14f7e071ee3344dc4e8f1937c55
|
Loading…
Reference in New Issue