v7.5.0 #828

Merged
muellerr merged 96 commits from dev-7.5.0 into main 2023-12-06 17:44:23 +01:00
47 changed files with 1099 additions and 488 deletions

View File

@ -16,6 +16,31 @@ will consitute of a breaking change warranting a new major release:
# [unreleased]
# [v7.5.0] 2023-12-06
- `eive-tmtc` v5.12.0
## Changed
- ACS-Board default side changed to B-Side
- The TLE uploaded now gets stored in a file on the filesystem. It will always be stored on
the current active SD Card. After a reboot, the TLE will be read from the filesystem.
A filesystem change via `prefSD` on bootup, can lead to the TLE not being read, even
though it is there.
- Added action cmd to read the currently stored TLE.
- Both the `AcsController` and the `PwrController` now use the monotonic clock to calculate
the time difference.
- `ACS Controller` now has the function `performAttitudeControl` which is called prior to passing
on to the relevant mode functions. It handles all telemetry relevant functions, which were
always called, regardless of the mode.
## Added
- Higher ACS modes can now be entered without a running `MEKF`. Higher modes will collect their
quaternion and rotational rate depending on the available sources.
- `QUEST` attitude estimation was added to the `AcsController`.
- The fused rotational rate can now be estimated from `QUEST` and the `STR`.
# [v7.4.1] 2023-12-06
## Fixed

View File

@ -10,8 +10,8 @@
cmake_minimum_required(VERSION 3.13)
set(OBSW_VERSION_MAJOR 7)
set(OBSW_VERSION_MINOR 4)
set(OBSW_VERSION_REVISION 1)
set(OBSW_VERSION_MINOR 5)
set(OBSW_VERSION_REVISION 0)
# set(CMAKE_VERBOSE TRUE)

View File

@ -1,7 +1,7 @@
/**
* @brief Auto-generated event translation file. Contains 317 translations.
* @brief Auto-generated event translation file. Contains 318 translations.
* @details
* Generated on: 2023-11-29 15:14:46
* Generated on: 2023-12-06 17:19:38
*/
#include "translateEvents.h"
@ -99,9 +99,10 @@ 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";
const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET";
const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFORMATION";
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 *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";
@ -514,11 +515,13 @@ const char *translateEvents(Event event) {
case (11205):
return MEKF_AUTOMATIC_RESET_STRING;
case (11206):
return MEKF_INVALID_MODE_VIOLATION_STRING;
return PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING;
case (11207):
return SAFE_MODE_CONTROLLER_FAILURE_STRING;
case (11208):
return TLE_TOO_OLD_STRING;
case (11209):
return TLE_FILE_READ_FAILED_STRING;
case (11300):
return SWITCH_CMD_SENT_STRING;
case (11301):

View File

@ -2,7 +2,7 @@
* @brief Auto-generated object translation file.
* @details
* Contains 175 translations.
* Generated on: 2023-11-29 15:14:46
* Generated on: 2023-12-06 17:19:38
*/
#include "translateObjects.h"

View File

@ -175,7 +175,7 @@ void ObjectFactory::produce(void* args) {
createScexComponents(q7s::UART_SCEX_DEV, pwrSwitcher, *SdCardManager::instance(), false,
power::Switches::PDU1_CH5_SOLAR_CELL_EXP_5V);
#endif
createAcsController(true, enableHkSets);
createAcsController(true, enableHkSets, *SdCardManager::instance());
HeaterHandler* heaterHandler;
createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler);
createThermalController(*heaterHandler, true);

View File

@ -130,6 +130,6 @@ void ObjectFactory::produce(void* args) {
createMiscComponents();
createThermalController(*heaterHandler, false);
createAcsController(true, enableHkSets);
createAcsController(true, enableHkSets, *SdCardManager::instance());
satsystem::init(false);
}

2
fsfw

@ -1 +1 @@
Subproject commit b28174db249cb33b541f665270fd6af14c382351
Subproject commit 7105e199c650303ac1a48e75aebc44182630931e

View File

@ -93,9 +93,10 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
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
11205;0x2bc5;MEKF_AUTOMATIC_RESET;INFO;MEKF performed an automatic reset after detection of nonfinite values.;mission/acs/defs.h
11206;0x2bc6;MEKF_INVALID_MODE_VIOLATION;HIGH;MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.;mission/acs/defs.h
11206;0x2bc6;PTG_CTRL_NO_ATTITUDE_INFORMATION;HIGH;For a prolonged time, no attitude information was available for the Pointing Controller. Falling back to Safe Mode.;mission/acs/defs.h
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
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

1 Event ID (dec) Event ID (hex) Name Severity Description File Path
93 11203 0x2bc3 MEKF_INVALID_INFO INFO MEKF was not able to compute a solution. P1: MEKF state on exit mission/acs/defs.h
94 11204 0x2bc4 MEKF_RECOVERY INFO MEKF is able to compute a solution again. mission/acs/defs.h
95 11205 0x2bc5 MEKF_AUTOMATIC_RESET INFO MEKF performed an automatic reset after detection of nonfinite values. mission/acs/defs.h
96 11206 0x2bc6 MEKF_INVALID_MODE_VIOLATION PTG_CTRL_NO_ATTITUDE_INFORMATION HIGH MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time. For a prolonged time, no attitude information was available for the Pointing Controller. Falling back to Safe Mode. mission/acs/defs.h
97 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
98 11208 0x2bc8 TLE_TOO_OLD INFO The TLE for the SGP4 Propagator has become too old. mission/acs/defs.h
99 11209 0x2bc9 TLE_FILE_READ_FAILED LOW The TLE could not be read from the filesystem. mission/acs/defs.h
100 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
101 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
102 11302 0x2c26 SWITCHING_Q7S_DENIED MEDIUM No description mission/power/defs.h

View File

@ -509,6 +509,8 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
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
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 Full ID (hex) Name Description Unique ID Subsytem Name File Path
509 0x67a3 SADPL_SwitchingDeplSa1Failed No description 163 SA_DEPL_HANDLER mission/SolarArrayDeploymentHandler.h
510 0x67a4 SADPL_SwitchingDeplSa2Failed No description 164 SA_DEPL_HANDLER mission/SolarArrayDeploymentHandler.h
511 0x6a00 ACSCTRL_FileDeletionFailed File deletion failed and at least one file is still existent. 0 ACS_CTRL mission/controller/AcsController.h
512 0x6a01 ACSCTRL_WriteFileFailed Writing the TLE to the file has failed. 1 ACS_CTRL mission/controller/AcsController.h
513 0x6a02 ACSCTRL_ReadFileFailed Reading the TLE to the file has failed. 2 ACS_CTRL mission/controller/AcsController.h
514 0x6b02 ACSMEKF_MekfUninitialized No description 2 ACS_MEKF mission/controller/acs/MultiplicativeKalmanFilter.h
515 0x6b03 ACSMEKF_MekfNoGyrData No description 3 ACS_MEKF mission/controller/acs/MultiplicativeKalmanFilter.h
516 0x6b04 ACSMEKF_MekfNoModelVectors No description 4 ACS_MEKF mission/controller/acs/MultiplicativeKalmanFilter.h

View File

@ -93,9 +93,10 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
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
11205;0x2bc5;MEKF_AUTOMATIC_RESET;INFO;MEKF performed an automatic reset after detection of nonfinite values.;mission/acs/defs.h
11206;0x2bc6;MEKF_INVALID_MODE_VIOLATION;HIGH;MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.;mission/acs/defs.h
11206;0x2bc6;PTG_CTRL_NO_ATTITUDE_INFORMATION;HIGH;For a prolonged time, no attitude information was available for the Pointing Controller. Falling back to Safe Mode.;mission/acs/defs.h
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
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

1 Event ID (dec) Event ID (hex) Name Severity Description File Path
93 11203 0x2bc3 MEKF_INVALID_INFO INFO MEKF was not able to compute a solution. P1: MEKF state on exit mission/acs/defs.h
94 11204 0x2bc4 MEKF_RECOVERY INFO MEKF is able to compute a solution again. mission/acs/defs.h
95 11205 0x2bc5 MEKF_AUTOMATIC_RESET INFO MEKF performed an automatic reset after detection of nonfinite values. mission/acs/defs.h
96 11206 0x2bc6 MEKF_INVALID_MODE_VIOLATION PTG_CTRL_NO_ATTITUDE_INFORMATION HIGH MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time. For a prolonged time, no attitude information was available for the Pointing Controller. Falling back to Safe Mode. mission/acs/defs.h
97 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
98 11208 0x2bc8 TLE_TOO_OLD INFO The TLE for the SGP4 Propagator has become too old. mission/acs/defs.h
99 11209 0x2bc9 TLE_FILE_READ_FAILED LOW The TLE could not be read from the filesystem. mission/acs/defs.h
100 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
101 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
102 11302 0x2c26 SWITCHING_Q7S_DENIED MEDIUM No description mission/power/defs.h

View File

@ -593,6 +593,8 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
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
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 Full ID (hex) Name Description Unique ID Subsytem Name File Path
593 0x69c0 SPVRTVIF_BufTooSmall No description 192 SUPV_RETURN_VALUES_IF linux/payload/plocSupvDefs.h
594 0x69c1 SPVRTVIF_NoReplyTimeout No description 193 SUPV_RETURN_VALUES_IF linux/payload/plocSupvDefs.h
595 0x6a00 ACSCTRL_FileDeletionFailed File deletion failed and at least one file is still existent. 0 ACS_CTRL mission/controller/AcsController.h
596 0x6a01 ACSCTRL_WriteFileFailed Writing the TLE to the file has failed. 1 ACS_CTRL mission/controller/AcsController.h
597 0x6a02 ACSCTRL_ReadFileFailed Reading the TLE to the file has failed. 2 ACS_CTRL mission/controller/AcsController.h
598 0x6b02 ACSMEKF_MekfUninitialized No description 2 ACS_MEKF mission/controller/acs/MultiplicativeKalmanFilter.h
599 0x6b03 ACSMEKF_MekfNoGyrData No description 3 ACS_MEKF mission/controller/acs/MultiplicativeKalmanFilter.h
600 0x6b04 ACSMEKF_MekfNoModelVectors No description 4 ACS_MEKF mission/controller/acs/MultiplicativeKalmanFilter.h

View File

@ -1,7 +1,7 @@
/**
* @brief Auto-generated event translation file. Contains 317 translations.
* @brief Auto-generated event translation file. Contains 318 translations.
* @details
* Generated on: 2023-11-29 15:14:46
* Generated on: 2023-12-06 17:19:38
*/
#include "translateEvents.h"
@ -99,9 +99,10 @@ 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";
const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET";
const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFORMATION";
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 *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";
@ -514,11 +515,13 @@ const char *translateEvents(Event event) {
case (11205):
return MEKF_AUTOMATIC_RESET_STRING;
case (11206):
return MEKF_INVALID_MODE_VIOLATION_STRING;
return PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING;
case (11207):
return SAFE_MODE_CONTROLLER_FAILURE_STRING;
case (11208):
return TLE_TOO_OLD_STRING;
case (11209):
return TLE_FILE_READ_FAILED_STRING;
case (11300):
return SWITCH_CMD_SENT_STRING;
case (11301):

View File

@ -2,7 +2,7 @@
* @brief Auto-generated object translation file.
* @details
* Contains 179 translations.
* Generated on: 2023-11-29 15:14:46
* Generated on: 2023-12-06 17:19:38
*/
#include "translateObjects.h"

View File

@ -331,8 +331,9 @@ void ObjectFactory::createScexComponents(std::string uartDev, PowerSwitchIF* pwr
scexHandler->connectModeTreeParent(satsystem::payload::SUBSYSTEM);
}
AcsController* ObjectFactory::createAcsController(bool connectSubsystem, bool enableHkSets) {
auto acsCtrl = new AcsController(objects::ACS_CONTROLLER, enableHkSets);
AcsController* ObjectFactory::createAcsController(bool connectSubsystem, bool enableHkSets,
SdCardMountedIF& mountedIF) {
auto acsCtrl = new AcsController(objects::ACS_CONTROLLER, enableHkSets, mountedIF);
if (connectSubsystem) {
acsCtrl->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM);
}

View File

@ -31,7 +31,8 @@ void createScexComponents(std::string uartDev, PowerSwitchIF* pwrSwitcher,
void gpioChecker(ReturnValue_t result, std::string output);
AcsController* createAcsController(bool connectSubsystem, bool enableHkSets);
AcsController* createAcsController(bool connectSubsystem, bool enableHkSets,
SdCardMountedIF& mountedIF);
PowerController* createPowerController(bool connectSubsystem, bool enableHkSets);
} // namespace ObjectFactory

View File

@ -1,7 +1,7 @@
/**
* @brief Auto-generated event translation file. Contains 317 translations.
* @brief Auto-generated event translation file. Contains 318 translations.
* @details
* Generated on: 2023-11-29 15:14:46
* Generated on: 2023-12-06 17:19:38
*/
#include "translateEvents.h"
@ -99,9 +99,10 @@ 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";
const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET";
const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFORMATION";
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 *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";
@ -514,11 +515,13 @@ const char *translateEvents(Event event) {
case (11205):
return MEKF_AUTOMATIC_RESET_STRING;
case (11206):
return MEKF_INVALID_MODE_VIOLATION_STRING;
return PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING;
case (11207):
return SAFE_MODE_CONTROLLER_FAILURE_STRING;
case (11208):
return TLE_TOO_OLD_STRING;
case (11209):
return TLE_FILE_READ_FAILED_STRING;
case (11300):
return SWITCH_CMD_SENT_STRING;
case (11301):

View File

@ -2,7 +2,7 @@
* @brief Auto-generated object translation file.
* @details
* Contains 179 translations.
* Generated on: 2023-11-29 15:14:46
* Generated on: 2023-12-06 17:19:38
*/
#include "translateObjects.h"

View File

@ -22,10 +22,10 @@ enum AcsMode : Mode_t {
enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 };
enum SafeModeStrategy : uint8_t {
SAFECTRL_OFF = 0,
SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
SAFECTRL_NO_SENSORS_FOR_CONTROL = 2,
enum ControlModeStrategy : uint8_t {
CTRL_OFF = 0,
CTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
CTRL_NO_SENSORS_FOR_CONTROL = 2,
// OBSW version <= v6.1.0
LEGACY_SAFECTRL_ACTIVE_MEKF = 10,
LEGACY_SAFECTRL_WITHOUT_MEKF = 11,
@ -40,14 +40,28 @@ enum SafeModeStrategy : uint8_t {
SAFECTRL_ECLIPSE_IDELING = 19,
SAFECTRL_DETUMBLE_FULL = 20,
SAFECTRL_DETUMBLE_DETERIORATED = 21,
// Added in vNext
PTGCTRL_MEKF = 100,
PTGCTRL_STR = 101,
PTGCTRL_QUEST = 102,
};
enum GpsSource : uint8_t {
namespace gps {
enum Source : uint8_t {
NONE,
GPS,
GPS_EXTRAPOLATED,
SPG4,
};
}
namespace rotrate {
enum Source : uint8_t {
NONE,
SUSMGM,
QUEST,
STR,
};
}
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
//! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
@ -64,15 +78,17 @@ static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO);
static constexpr Event MEKF_RECOVERY = MAKE_EVENT(4, severity::INFO);
//! [EXPORT] : [COMMENT] MEKF performed an automatic reset after detection of nonfinite values.
static constexpr Event MEKF_AUTOMATIC_RESET = MAKE_EVENT(5, severity::INFO);
//! [EXPORT] : [COMMENT] MEKF was not able to compute a solution during any pointing ACS mode for a
//! prolonged time.
static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIGH);
//! [EXPORT] : [COMMENT] For a prolonged time, no attitude information was available for the
//! Pointing Controller. Falling back to Safe Mode.
static constexpr Event PTG_CTRL_NO_ATTITUDE_INFORMATION = MAKE_EVENT(6, severity::HIGH);
//! [EXPORT] : [COMMENT] 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
static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(7, severity::HIGH);
//! [EXPORT] : [COMMENT] The TLE for the SGP4 Propagator has become too old.
static constexpr Event TLE_TOO_OLD = MAKE_EVENT(8, severity::INFO);
//! [EXPORT] : [COMMENT] The TLE could not be read from the filesystem.
static constexpr Event TLE_FILE_READ_FAILED = MAKE_EVENT(9, severity::LOW);
extern const char* getModeStr(AcsMode mode);

View File

@ -1,12 +1,10 @@
#include "AcsController.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <mission/acs/defs.h>
#include <mission/config/torquer.h>
AcsController::AcsController(object_id_t objectId, bool enableHkSets)
AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMountedIF &sdcMan)
: ExtendedControllerBase(objectId),
enableHkSets(enableHkSets),
sdcMan(sdcMan),
attitudeEstimation(&acsParameters),
fusedRotationEstimation(&acsParameters),
guidance(&acsParameters),
safeCtrl(&acsParameters),
@ -19,11 +17,11 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets)
gyrDataRaw(this),
gyrDataProcessed(this),
gpsDataProcessed(this),
mekfData(this),
attitudeEstimationData(this),
ctrlValData(this),
actuatorCmdData(this),
fusedRotRateData(this),
tleData(this) {}
fusedRotRateSourcesData(this) {}
ReturnValue_t AcsController::initialize() {
ReturnValue_t result = parameterHelper.initialize();
@ -56,7 +54,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
return HasActionsIF::EXECUTION_FINISHED;
}
case RESET_MEKF: {
navigation.resetMekf(&mekfData);
navigation.resetMekf(&attitudeEstimationData);
return HasActionsIF::EXECUTION_FINISHED;
}
case RESTORE_MEKF_NONFINITE_RECOVERY: {
@ -67,22 +65,27 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
if (size != 69 * 2) {
return INVALID_PARAMETERS;
}
ReturnValue_t result = navigation.updateTle(data, data + 69);
ReturnValue_t result = updateTle(data, data + 69, false);
if (result != returnvalue::OK) {
PoolReadGuard pg(&tleData);
navigation.updateTle(tleData.line1.value, tleData.line2.value);
return result;
}
{
PoolReadGuard pg(&tleData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(tleData.line1.value, data, 69);
std::memcpy(tleData.line2.value, data + 69, 69);
tleData.setValidity(true, true);
}
result = writeTleToFs(data);
if (result != returnvalue::OK) {
return result;
}
return HasActionsIF::EXECUTION_FINISHED;
}
case (READ_TLE): {
uint8_t tle[69 * 2] = {};
uint8_t line2[69] = {};
ReturnValue_t result = readTleFromFs(tle, line2);
if (result != returnvalue::OK) {
return result;
}
std::memcpy(tle + 69, line2, 69);
actionHelper.reportData(commandedBy, actionId, tle, 69 * 2);
return EXECUTION_FINISHED;
}
default: {
return HasActionsIF::INVALID_ACTION_ID;
}
@ -130,31 +133,17 @@ void AcsController::performControlOperation() {
}
case InternalState::INITIAL_DELAY: {
if (initialCountdown.hasTimedOut()) {
uint8_t line1[69] = {};
uint8_t line2[69] = {};
readTleFromFs(line1, line2);
updateTle(line1, line2, true);
internalState = InternalState::READY;
}
return;
}
case InternalState::READY: {
if (mode != MODE_OFF) {
switch (mode) {
case acs::SAFE:
switch (submode) {
case SUBMODE_NONE:
performSafe();
break;
case acs::DETUMBLE:
performDetumble();
break;
}
break;
case acs::PTG_IDLE:
case acs::PTG_TARGET:
case acs::PTG_TARGET_GS:
case acs::PTG_NADIR:
case acs::PTG_INERTIAL:
performPointingCtrl();
break;
}
performAttitudeControl();
}
break;
}
@ -163,32 +152,42 @@ void AcsController::performControlOperation() {
}
}
void AcsController::performSafe() {
timeval now;
Clock::getClock_timeval(&now);
void AcsController::performAttitudeControl() {
Clock::getClock_timeval(&timeAbsolute);
Clock::getClockMonotonic(&timeRelative);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
if (timeRelative.tv_sec != 0 and oldTimeRelative.tv_sec != 0) {
timeDelta = timevalOperations::toDouble(timeRelative - oldTimeRelative);
}
oldTimeRelative = timeRelative;
ReturnValue_t result = navigation.useSpg4(timeAbsolute, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else if (result != Sgp4Propagator::TLE_TOO_OLD) {
tleTooOldFlag = false;
}
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
sensorProcessing.process(timeAbsolute, timeDelta, &sensorValues, &mgmDataProcessed,
&susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData);
fusedRotationEstimation.estimateFusedRotationRate(
&susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
&attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
&susDataProcessed, &attitudeEstimationData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
triggerEvent(acs::MEKF_INVALID_INFO,
static_cast<uint32_t>(attitudeEstimationData.mekfStatus.value));
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
navigation.resetMekf(&attitudeEstimationData);
mekfLost = true;
}
} else if (mekfInvalidFlag) {
@ -196,32 +195,55 @@ void AcsController::performSafe() {
mekfInvalidFlag = false;
}
switch (mode) {
case acs::SAFE:
switch (submode) {
case SUBMODE_NONE:
performSafe();
break;
case acs::DETUMBLE:
performDetumble();
break;
}
break;
case acs::PTG_IDLE:
case acs::PTG_TARGET:
case acs::PTG_TARGET_GS:
case acs::PTG_NADIR:
case acs::PTG_INERTIAL:
performPointingCtrl();
break;
}
}
void AcsController::performSafe() {
// get desired satellite rate, sun direction to align to and inertia
double sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir);
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
acsParameters.safeModeControllerParameters.useGyr,
acsParameters.safeModeControllerParameters.dampingDuringEclipse);
switch (safeCtrlStrat) {
case (acs::SafeModeStrategy::SAFECTRL_MEKF):
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
magMomMtq, errAng);
case (acs::ControlModeStrategy::SAFECTRL_MEKF):
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value,
attitudeEstimationData.satRotRateMekf.value,
susDataProcessed.sunIjkModel.value, attitudeEstimationData.quatMekf.value,
sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_GYR):
case (acs::ControlModeStrategy::SAFECTRL_GYR):
safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
case (acs::ControlModeStrategy::SAFECTRL_SUSMGM):
safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
fusedRotRateData.rotRateParallel.value,
fusedRotRateData.rotRateOrthogonal.value,
@ -229,29 +251,29 @@ void AcsController::performSafe() {
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value,
gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq,
errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq,
errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING):
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING):
errAng = NAN;
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL):
safeCtrlFailure(1, 0);
break;
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL):
safeCtrlFailure(0, 1);
break;
default:
@ -264,8 +286,8 @@ void AcsController::performSafe() {
// detumble check and switch
if (acsParameters.safeModeControllerParameters.useMekf) {
if (mekfData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
if (attitudeEstimationData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
}
@ -296,55 +318,24 @@ void AcsController::performSafe() {
}
void AcsController::performDetumble() {
timeval now;
Clock::getClock_timeval(&now);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else {
tleTooOldFlag = false;
}
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
mekfLost = true;
}
} else if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false;
}
acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
acs::ControlModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.isValid(),
acsParameters.detumbleParameter.useFullDetumbleLaw);
double magMomMtq[3] = {0, 0, 0};
switch (safeCtrlStrat) {
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL):
case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL):
detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
magMomMtq, acsParameters.detumbleParameter.gainFull);
break;
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
magMomMtq, acsParameters.detumbleParameter.gainBdot);
break;
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL):
safeCtrlFailure(1, 0);
break;
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL):
safeCtrlFailure(0, 1);
break;
default:
@ -356,8 +347,8 @@ void AcsController::performDetumble() {
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
if (acsParameters.safeModeControllerParameters.useMekf) {
if (mekfData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
if (attitudeEstimationData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
}
@ -389,51 +380,71 @@ void AcsController::performDetumble() {
}
void AcsController::performPointingCtrl() {
timeval now;
Clock::getClock_timeval(&now);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else {
tleTooOldFlag = false;
bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and
sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid());
uint8_t useMekf = false;
switch (mode) {
case acs::PTG_IDLE:
useMekf = acsParameters.idleModeControllerParameters.useMekf;
break;
case acs::PTG_TARGET:
useMekf = acsParameters.targetModeControllerParameters.useMekf;
break;
case acs::PTG_TARGET_GS:
useMekf = acsParameters.gsTargetModeControllerParameters.useMekf;
break;
case acs::PTG_NADIR:
useMekf = acsParameters.nadirModeControllerParameters.useMekf;
break;
case acs::PTG_INERTIAL:
useMekf = acsParameters.inertialModeControllerParameters.useMekf;
break;
}
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
mekfInvalidCounter++;
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
mekfLost = true;
}
if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
// Trigger this so STR FDIR can set the device faulty.
EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0);
mekfInvalidCounter = 0;
acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
fusedRotRateData.rotRateSource.isValid(), useMekf);
if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
ptgCtrlLostCounter++;
if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
ptgCtrlLostCounter = 0;
}
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
return;
} else {
if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false;
}
mekfInvalidCounter = 0;
ptgCtrlLostCounter = 0;
}
double quatBI[4] = {0, 0, 0, 0}, rotRateB[3] = {0, 0, 0};
switch (ptgCtrlStrat) {
case acs::ControlModeStrategy::PTGCTRL_MEKF:
std::memcpy(quatBI, attitudeEstimationData.quatMekf.value, sizeof(quatBI));
std::memcpy(rotRateB, attitudeEstimationData.satRotRateMekf.value, sizeof(rotRateB));
break;
case acs::ControlModeStrategy::PTGCTRL_STR:
quatBI[0] = sensorValues.strSet.caliQx.value;
quatBI[1] = sensorValues.strSet.caliQy.value;
quatBI[2] = sensorValues.strSet.caliQz.value;
quatBI[3] = sensorValues.strSet.caliQw.value;
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
break;
case acs::ControlModeStrategy::PTGCTRL_QUEST:
std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI));
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
break;
default:
sif::error << "AcsController: Invalid pointing mode strategy for performDetumble"
<< std::endl;
break;
}
uint8_t enableAntiStiction = true;
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
if (result == returnvalue::FAILED) {
if (multipleRwUnavailableCounter >=
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
@ -455,10 +466,10 @@ void AcsController::performPointingCtrl() {
switch (mode) {
case acs::PTG_IDLE:
guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat,
guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
@ -469,18 +480,18 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
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;
break;
case acs::PTG_TARGET:
guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef,
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
acsParameters.targetModeControllerParameters.quatRef,
acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
@ -493,17 +504,17 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
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;
break;
case acs::PTG_TARGET_GS:
guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
@ -514,18 +525,18 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
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;
break;
case acs::PTG_NADIR:
guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
guidance.targetQuatPtgNadirThreeAxes(
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,
@ -538,17 +549,17 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
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;
break;
case acs::PTG_INERTIAL:
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
sizeof(targetQuat));
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef,
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
acsParameters.inertialModeControllerParameters.quatRef,
acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
@ -561,9 +572,9 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
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;
break;
default:
@ -702,7 +713,7 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
ctrlValData.safeStrat.value = acs::SafeModeStrategy::SAFECTRL_OFF;
ctrlValData.safeStrat.value = acs::ControlModeStrategy::CTRL_OFF;
ctrlValData.setValidity(true, true);
}
}
@ -779,11 +790,12 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource);
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
// MEKF
// Attitude Estimation
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_QUEST, &quatQuest);
poolManager.subscribeForDiagPeriodicPacket({attitudeEstimationData.getSid(), enableHkSets, 10.0});
// Ctrl Values
localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
@ -800,10 +812,15 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
// TLE Data
localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_1, &line1);
localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_2, &line2);
// Fused Rot Rate Sources
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL_SUSMGM, &rotRateOrthogonalSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL_SUSMGM, &rotRateParallelSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SUSMGM, &rotRateTotalSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_QUEST, &rotRateTotalQuest);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_STR, &rotRateTotalStr);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateSourcesData.getSid(), false, 10.0});
return returnvalue::OK;
}
@ -824,13 +841,15 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
case acsctrl::GPS_PROCESSED_DATA:
return &gpsDataProcessed;
case acsctrl::MEKF_DATA:
return &mekfData;
return &attitudeEstimationData;
case acsctrl::CTRL_VAL_DATA:
return &ctrlValData;
case acsctrl::ACTUATOR_CMD_DATA:
return &actuatorCmdData;
case acsctrl::FUSED_ROTATION_RATE_DATA:
return &fusedRotRateData;
case acsctrl::FUSED_ROTATION_RATE_SOURCES_DATA:
return &fusedRotRateSourcesData;
default:
return nullptr;
}
@ -1031,6 +1050,67 @@ void AcsController::copySusData() {
}
}
ReturnValue_t AcsController::updateTle(const uint8_t *line1, const uint8_t *line2, bool fromFile) {
ReturnValue_t result = navigation.updateTle(line1, line2);
if (result != returnvalue::OK) {
if (not fromFile) {
uint8_t fileLine1[69] = {};
uint8_t fileLine2[69] = {};
readTleFromFs(fileLine1, fileLine2);
navigation.updateTle(fileLine1, fileLine2);
}
return result;
}
return returnvalue::OK;
}
ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) {
auto mntPrefix = sdcMan.getCurrentMountPrefix();
if (mntPrefix == nullptr or !sdcMan.isSdCardUsable(std::nullopt)) {
return returnvalue::FAILED;
}
std::string path = mntPrefix + TLE_FILE;
// Clear existing TLE from file
std::ofstream tleFile(path.c_str(), std::ofstream::out | std::ofstream::trunc);
if (tleFile.is_open()) {
tleFile.write(reinterpret_cast<const char *>(tle), 69);
tleFile << "\n";
tleFile.write(reinterpret_cast<const char *>(tle + 69), 69);
} else {
return WRITE_FILE_FAILED;
}
tleFile.close();
return returnvalue::OK;
}
ReturnValue_t AcsController::readTleFromFs(uint8_t *line1, uint8_t *line2) {
auto mntPrefix = sdcMan.getCurrentMountPrefix();
if (mntPrefix == nullptr or !sdcMan.isSdCardUsable(std::nullopt)) {
return returnvalue::FAILED;
}
std::string path = mntPrefix + TLE_FILE;
std::error_code e;
if (std::filesystem::exists(path, e)) {
// Read existing TLE from file
std::fstream tleFile = std::fstream(path.c_str(), std::fstream::in);
if (tleFile.is_open()) {
std::string tleLine1, tleLine2;
getline(tleFile, tleLine1);
std::memcpy(line1, tleLine1.c_str(), 69);
getline(tleFile, tleLine2);
std::memcpy(line2, tleLine2.c_str(), 69);
} else {
triggerEvent(acs::TLE_FILE_READ_FAILED);
return READ_FILE_FAILED;
}
tleFile.close();
} else {
triggerEvent(acs::TLE_FILE_READ_FAILED);
return READ_FILE_FAILED;
}
return returnvalue::OK;
}
void AcsController::copyGyrData() {
{
PoolReadGuard pg(&sensorValues.gyr0AdisSet);

View File

@ -4,16 +4,20 @@
#include <eive/objects.h>
#include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/coordinates/Sgp4Propagator.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/health/HealthTable.h>
#include <fsfw/parameters/ParameterHelper.h>
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
#include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h>
#include <fsfw_hal/devicehandlers/MgmRM3100Handler.h>
#include <mission/acs/defs.h>
#include <mission/acs/imtqHelpers.h>
#include <mission/acs/rwHelpers.h>
#include <mission/acs/susMax1227Helpers.h>
#include <mission/config/torquer.h>
#include <mission/controller/acs/ActuatorCmd.h>
#include <mission/controller/acs/AttitudeEstimation.h>
#include <mission/controller/acs/FusedRotationEstimation.h>
#include <mission/controller/acs/Guidance.h>
#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
@ -23,13 +27,18 @@
#include <mission/controller/acs/control/PtgCtrl.h>
#include <mission/controller/acs/control/SafeCtrl.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <mission/memory/SdCardMountedIF.h>
#include <mission/utility/trace.h>
#include <filesystem>
#include <fstream>
#include <optional>
class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
public:
static constexpr dur_millis_t INIT_DELAY = 500;
AcsController(object_id_t objectId, bool enableHkSets);
AcsController(object_id_t objectId, bool enableHkSets, SdCardMountedIF& sdcMan);
MessageQueueId_t getCommandQueue() const;
ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
@ -37,6 +46,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
uint16_t startAtIndex) override;
protected:
void performAttitudeControl();
void performSafe();
void performDetumble();
void performPointingCtrl();
@ -49,8 +59,16 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
bool enableHkSets = false;
SdCardMountedIF& sdcMan;
timeval timeAbsolute;
timeval timeRelative;
double timeDelta = 0.0;
timeval oldTimeRelative;
AcsParameters acsParameters;
SensorProcessing sensorProcessing;
AttitudeEstimation attitudeEstimation;
FusedRotationEstimation fusedRotationEstimation;
Navigation navigation;
ActuatorCmd actuatorCmd;
@ -66,7 +84,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
uint8_t detumbleCounter = 0;
uint8_t multipleRwUnavailableCounter = 0;
bool mekfInvalidFlag = false;
uint16_t mekfInvalidCounter = 0;
uint16_t ptgCtrlLostCounter = 0;
bool safeCtrlFailureFlag = false;
uint8_t safeCtrlFailureCounter = 0;
uint8_t resetMekfCount = 0;
@ -87,10 +105,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
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);
ReturnValue_t initialize() override;
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
@ -125,6 +148,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
const double* tgtRotRate);
ReturnValue_t updateTle(const uint8_t* line1, const uint8_t* line2, bool fromFile);
ReturnValue_t writeTleToFs(const uint8_t* tle);
ReturnValue_t readTleFromFs(uint8_t* line1, uint8_t* line2);
const std::string TLE_FILE = "/conf/tle.txt";
/* ACS Sensor Values */
ACS::SensorValues sensorValues;
@ -212,11 +241,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
PoolEntry<uint8_t> gpsSource = PoolEntry<uint8_t>();
// MEKF
acsctrl::MekfData mekfData;
// Attitude Estimation
acsctrl::AttitudeEstimationData attitudeEstimationData;
PoolEntry<double> quatMekf = PoolEntry<double>(4);
PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);
PoolEntry<uint8_t> mekfStatus = PoolEntry<uint8_t>();
PoolEntry<double> quatQuest = PoolEntry<double>(4);
// Ctrl Values
acsctrl::CtrlValData ctrlValData;
@ -237,11 +267,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<double> rotRateOrthogonal = PoolEntry<double>(3);
PoolEntry<double> rotRateParallel = PoolEntry<double>(3);
PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
PoolEntry<uint8_t> rotRateSource = PoolEntry<uint8_t>();
// TLE Dataset
acsctrl::TleData tleData;
PoolEntry<uint8_t> line1 = PoolEntry<uint8_t>(69);
PoolEntry<uint8_t> line2 = PoolEntry<uint8_t>(69);
// Fused Rot Rate Sources
acsctrl::FusedRotRateSourcesData fusedRotRateSourcesData;
PoolEntry<double> rotRateOrthogonalSusMgm = PoolEntry<double>(3);
PoolEntry<double> rotRateParallelSusMgm = PoolEntry<double>(3);
PoolEntry<double> rotRateTotalSusMgm = PoolEntry<double>(3);
PoolEntry<double> rotRateTotalQuest = PoolEntry<double>(3);
PoolEntry<double> rotRateTotalStr = PoolEntry<double>(3);
// Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(INIT_DELAY);

View File

@ -157,7 +157,12 @@ ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode,
void PowerController::calculateStateOfCharge() {
// get time
Clock::getClock_timeval(&now);
Clock::getClockMonotonic(&now);
double timeDelta = 0.0;
if (now.tv_sec != 0 and oldTime.tv_sec != 0) {
timeDelta = timevalOperations::toDouble(now - oldTime);
}
oldTime = now;
// update EPS HK values
ReturnValue_t result = updateEpsData();
@ -173,8 +178,6 @@ void PowerController::calculateStateOfCharge() {
pwrCtrlCoreHk.setValidity(false, true);
}
}
// store time for next run
oldTime = now;
return;
}
@ -195,12 +198,10 @@ void PowerController::calculateStateOfCharge() {
pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
}
}
// store time for next run
oldTime = now;
return;
}
result = calculateCoulombCounterCharge();
result = calculateCoulombCounterCharge(timeDelta);
if (result != returnvalue::OK) {
// notifying events have already been triggered
{
@ -215,8 +216,6 @@ void PowerController::calculateStateOfCharge() {
pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
}
}
// store time for next run
oldTime = now;
return;
}
@ -231,8 +230,6 @@ void PowerController::calculateStateOfCharge() {
pwrCtrlCoreHk.setValidity(true, true);
}
}
// store time for next run
oldTime = now;
}
void PowerController::watchStateOfCharge() {
@ -285,12 +282,14 @@ ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() {
return returnvalue::OK;
}
ReturnValue_t PowerController::calculateCoulombCounterCharge() {
double timeDiff = timevalOperations::toDouble(now - oldTime);
if (timeDiff > maxAllowedTimeDiff) {
ReturnValue_t PowerController::calculateCoulombCounterCharge(double timeDelta) {
if (timeDelta == 0.0) {
return returnvalue::FAILED;
}
if (timeDelta > maxAllowedTimeDiff) {
// should not be a permanent state so no spam protection required
triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDiff * 10));
sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDiff
triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDelta * 10));
sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDelta
<< std::endl;
return returnvalue::FAILED;
}
@ -298,7 +297,7 @@ ReturnValue_t PowerController::calculateCoulombCounterCharge() {
coulombCounterCharge = openCircuitVoltageCharge;
} else {
coulombCounterCharge =
coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDiff * SECONDS_TO_HOURS;
coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDelta * SECONDS_TO_HOURS;
if (coulombCounterCharge >= coulombCounterChargeUpperThreshold) {
coulombCounterCharge = coulombCounterChargeUpperThreshold;
}

View File

@ -45,7 +45,7 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
void calculateStateOfCharge();
void watchStateOfCharge();
ReturnValue_t calculateOpenCircuitVoltageCharge();
ReturnValue_t calculateCoulombCounterCharge();
ReturnValue_t calculateCoulombCounterCharge(double timeDelta);
ReturnValue_t updateEpsData();
float charge2stateOfCharge(float capacity, bool coulombCounter);
ReturnValue_t lookUpTableOcvIdxFinder(float voltage, uint8_t& idx, bool paramCmd);

View File

@ -24,11 +24,20 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(onBoardParams.sampleTime);
break;
case 0x1:
parameterWrapper->set(onBoardParams.mekfViolationTimer);
parameterWrapper->set(onBoardParams.ptgCtrlLostTimer);
break;
case 0x2:
parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse);
break;
case 0x3:
parameterWrapper->set(onBoardParams.fusedRateFromStr);
break;
case 0x4:
parameterWrapper->set(onBoardParams.fusedRateFromQuest);
break;
case 0x5:
parameterWrapper->set(onBoardParams.questFilterWeight);
break;
default:
return INVALID_IDENTIFIER_ID;
}
@ -425,6 +434,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x9:
parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->set(idleModeControllerParameters.useMekf);
break;
default:
return INVALID_IDENTIFIER_ID;
}
@ -462,36 +474,39 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
parameterWrapper->set(targetModeControllerParameters.useMekf);
break;
case 0xB:
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
break;
case 0xC:
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
break;
case 0xD:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
break;
case 0xE:
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
break;
case 0xF:
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
break;
case 0x10:
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
break;
case 0x11:
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
break;
case 0x12:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
break;
case 0x13:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
break;
case 0x14:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
break;
case 0x15:
parameterWrapper->set(targetModeControllerParameters.blindRotRate);
break;
default:
@ -531,18 +546,21 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
parameterWrapper->set(gsTargetModeControllerParameters.useMekf);
break;
case 0xB:
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
break;
case 0xC:
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
break;
case 0xD:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
break;
case 0xE:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
break;
case 0xF:
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
break;
default:
@ -582,15 +600,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
parameterWrapper->set(nadirModeControllerParameters.useMekf);
break;
case 0xB:
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
break;
case 0xC:
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
break;
case 0xD:
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
break;
case 0xE:
parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
break;
default:
@ -630,12 +651,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
parameterWrapper->set(inertialModeControllerParameters.useMekf);
break;
case 0xB:
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
break;
case 0xC:
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
break;
case 0xD:
parameterWrapper->setVector(inertialModeControllerParameters.quatRef);
break;
default:

View File

@ -18,8 +18,11 @@ class AcsParameters : public HasParametersIF {
struct OnBoardParams {
double sampleTime = 0.4; // [s]
uint16_t mekfViolationTimer = 750;
uint16_t ptgCtrlLostTimer = 750;
uint8_t fusedRateSafeDuringEclipse = true;
uint8_t fusedRateFromStr = false;
uint8_t fusedRateFromQuest = false;
double questFilterWeight = 0.0;
} onBoardParams;
struct InertiaEIVE {
@ -75,9 +78,9 @@ class AcsParameters : public HasParametersIF {
{-0.007534, 1.253879, 0.006812},
{-0.037072, 0.006812, 1.313158}};
float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
double mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
double mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
double mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
float mgmVectorFilterWeight = 0.85;
float mgmDerivativeFilterWeight = 0.99;
uint8_t useMgm4 = false;
@ -787,10 +790,10 @@ class AcsParameters : public HasParametersIF {
/* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
* assumed to be equal for the same class of sensors */
float gyr02variance[3] = {pow(4.6e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
pow(4.6e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
pow(6.1e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
double gyr02variance[3] = {pow(4.6e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
pow(4.6e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
pow(6.1e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
double gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
uint8_t preferAdis = false;
float gyrFilterWeight = 0.6;
} gyrHandlingParameters;
@ -861,6 +864,7 @@ class AcsParameters : public HasParametersIF {
double deSatGainFactor = 1000;
uint8_t desatOn = true;
uint8_t enableAntiStiction = true;
uint8_t useMekf = false;
} pointingLawParameters;
struct IdleModeControllerParameters : PointingLawParameters {

View File

@ -0,0 +1,111 @@
#include "AttitudeEstimation.h"
AttitudeEstimation::AttitudeEstimation(AcsParameters *acsParameters_) {
acsParameters = acsParameters_;
}
AttitudeEstimation::~AttitudeEstimation() {}
void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
acsctrl::MgmDataProcessed *mgmData,
acsctrl::AttitudeEstimationData *attitudeEstimation) {
if (not(susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and
mgmData->mgmVecTot.value and mgmData->magIgrfModel.isValid())) {
{
PoolReadGuard pg{attitudeEstimation};
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(attitudeEstimation->quatQuest.value, ZERO_VEC4, 4 * sizeof(double));
attitudeEstimation->quatQuest.setValid(false);
}
}
return;
}
// Normalize Data
double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0},
normSusI[3] = {0, 0, 0};
VectorOperations<double>::normalize(susData->susVecTot.value, normMgmB, 3);
VectorOperations<double>::normalize(susData->sunIjkModel.value, normMgmI, 3);
VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normSusB, 3);
VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normSusI, 3);
// Create Helper Vectors
double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
helperSum[3] = {0, 0, 0};
VectorOperations<double>::cross(normSusB, normMgmB, normHelperB);
VectorOperations<double>::cross(normSusI, normMgmI, normHelperI);
VectorOperations<double>::normalize(normHelperB, normHelperB, 3);
VectorOperations<double>::normalize(normHelperI, normHelperI, 3);
VectorOperations<double>::cross(normHelperB, normHelperI, helperCross);
VectorOperations<double>::add(normHelperB, normHelperI, helperSum, 3);
// Sensor Weights
double kSus = 0, kMgm = 0;
kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2);
kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2);
// Weighted Vectors
double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},
kMgmVec[3] = {0, 0, 0}, kSumVec[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(normSusB, kSus, weightedSusB, 3);
VectorOperations<double>::mulScalar(normMgmB, kMgm, weightedMgmB, 3);
VectorOperations<double>::cross(weightedSusB, normSusI, kSusVec);
VectorOperations<double>::cross(weightedMgmB, normMgmI, kMgmVec);
VectorOperations<double>::add(kSusVec, kMgmVec, kSumVec, 3);
// Some weird Angles
double alpha = (1 + VectorOperations<double>::dot(normHelperB, normHelperI)) *
(VectorOperations<double>::dot(weightedSusB, normSusI) +
VectorOperations<double>::dot(weightedMgmB, normMgmI)) +
VectorOperations<double>::dot(helperCross, kSumVec);
double beta = VectorOperations<double>::dot(helperSum, kSumVec);
double gamma = std::sqrt(std::pow(alpha, 2) + std::pow(beta, 2));
// I don't even know what this is supposed to be
double constPlus =
1. / (2 * std::sqrt(gamma * (gamma + alpha) *
(1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
double constMinus =
1. / (2 * std::sqrt(gamma * (gamma - alpha) *
(1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
// Calculate Quaternion
double qBI[4] = {0, 0, 0, 0}, qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0},
qRotVecPt1[3] = {0, 0, 0};
if (alpha >= 0) {
// Scalar Part
qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
// Rotational Vector Part
VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3);
VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3);
QuaternionOperations::normalize(qBI, qBI);
} else {
// Scalar Part
qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
// Rotational Vector Part
VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3);
VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
QuaternionOperations::normalize(qBI, qBI);
}
// Low Pass
if (VectorOperations<double>::norm(qOld, 4) != 0.0) {
QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI);
}
{
PoolReadGuard pg{attitudeEstimation};
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(attitudeEstimation->quatQuest.value, qBI, 4 * sizeof(double));
attitudeEstimation->quatQuest.setValid(true);
}
}
}

View File

@ -0,0 +1,31 @@
#ifndef MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_
#define MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <cmath>
#include <iostream>
class AttitudeEstimation {
public:
AttitudeEstimation(AcsParameters *acsParameters_);
virtual ~AttitudeEstimation();
;
void quest(acsctrl::SusDataProcessed *susData, acsctrl::MgmDataProcessed *mgmData,
acsctrl::AttitudeEstimationData *attitudeEstimation);
protected:
private:
AcsParameters *acsParameters;
double qOld[4] = {0, 0, 0, 0};
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
};
#endif /* MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ */

View File

@ -2,6 +2,7 @@ target_sources(
${LIB_EIVE_MISSION}
PRIVATE AcsParameters.cpp
ActuatorCmd.cpp
AttitudeEstimation.cpp
FusedRotationEstimation.cpp
Guidance.cpp
Igrf13Model.cpp

View File

@ -4,19 +4,220 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_)
acsParameters = acsParameters_;
}
void FusedRotationEstimation::estimateFusedRotationRateSafe(
void FusedRotationEstimation::estimateFusedRotationRate(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
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) {
PoolReadGuard pg(fusedRotRateData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value,
fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
fusedRotRateData->rotRateSource.setValid(true);
}
} else if (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));
fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value,
fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST;
fusedRotRateData->rotRateSource.setValid(true);
}
} else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(
fusedRotRateSourcesData->rotRateOrthogonalSusMgm.isValid());
std::memcpy(fusedRotRateData->rotRateParallel.value,
fusedRotRateSourcesData->rotRateParallelSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(
fusedRotRateSourcesData->rotRateParallelSusMgm.isValid());
std::memcpy(fusedRotRateData->rotRateTotal.value,
fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM;
fusedRotRateData->rotRateSource.setValid(true);
} else {
PoolReadGuard pg(fusedRotRateData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE;
fusedRotRateData->rotRateSource.setValid(true);
}
}
}
void FusedRotationEstimation::estimateFusedRotationRateStr(
ACS::SensorValues *sensorValues, const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if (not(sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
}
}
std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr));
return;
}
double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
if (VectorOperations<double>::norm(quatOldStr, 4) != 0 and timeDelta != 0) {
double quatOldInv[4] = {0, 0, 0, 0};
double quatDelta[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatOldStr, quatOldInv);
QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
QuaternionOperations::normalize(quatDelta);
}
double rotVec[3] = {0, 0, 0};
double angle = QuaternionOperations::getAngle(quatDelta);
if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
}
}
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
return;
}
VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
}
}
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
return;
}
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
}
}
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
return;
}
void FusedRotationEstimation::estimateFusedRotationRateQuest(
acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if (not attitudeEstimationData->quatQuest.isValid()) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
}
}
std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest));
}
if (VectorOperations<double>::norm(quatOldQuest, 4) != 0 and timeDelta != 0) {
double quatOldInv[4] = {0, 0, 0, 0};
double quatDelta[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatOldQuest, quatOldInv);
QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, quatDelta);
if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
QuaternionOperations::normalize(quatDelta);
}
double rotVec[3] = {0, 0, 0};
double angle = QuaternionOperations::getAngle(quatDelta);
if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
}
}
std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
return;
}
VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
}
}
std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
return;
}
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
}
}
std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
return;
}
void FusedRotationEstimation::estimateFusedRotationRateSusMgm(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and
not fusedRotRateData->rotRateTotal.isValid()) or
not fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) or
(not susDataProcessed->susVecTotDerivative.isValid() and
not mgmDataProcessed->mgmVecTotDerivative.isValid())) {
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false);
}
}
// store for calculation of angular acceleration
if (gyrDataProcessed->gyrVecTot.isValid()) {
@ -25,7 +226,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
return;
}
if (not susDataProcessed->susVecTot.isValid()) {
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData);
// store for calculation of angular acceleration
if (gyrDataProcessed->gyrVecTot.isValid()) {
std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
@ -49,7 +250,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
VectorOperations<double>::mulScalar(susDataProcessed->susVecTot.value, omegaParallel,
fusedRotRateParallel, 3);
} else {
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData);
// store for calculation of angular acceleration
if (gyrDataProcessed->gyrVecTot.isValid()) {
std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
@ -71,12 +272,18 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
VectorOperations<double>::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal);
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal,
3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, fusedRotRateParallel, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
fusedRotRateData->setValidity(true, true);
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, fusedRotRateOrthogonal,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(true);
std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, fusedRotRateParallel,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(true);
std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true);
}
}
// store for calculation of angular acceleration
@ -86,31 +293,44 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
}
void FusedRotationEstimation::estimateFusedRotationRateEclipse(
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if (not acsParameters->onBoardParams.fusedRateSafeDuringEclipse or
not gyrDataProcessed->gyrVecTot.isValid() or
VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) {
VectorOperations<double>::norm(fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3) == 0) {
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false);
}
}
return;
}
double angAccelB[3] = {0, 0, 0};
VectorOperations<double>::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3);
double fusedRotRateTotal[3] = {0, 0, 0};
VectorOperations<double>::add(fusedRotRateData->rotRateTotal.value, angAccelB, fusedRotRateTotal,
3);
VectorOperations<double>::add(fusedRotRateSourcesData->rotRateTotalSusMgm.value, angAccelB,
fusedRotRateTotal, 3);
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true);
}
}
}

View File

@ -2,28 +2,46 @@
#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
#include <fsfw/datapool/PoolReadGuard.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 FusedRotationEstimation {
public:
FusedRotationEstimation(AcsParameters *acsParameters_);
void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateData *fusedRotRateData);
void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
ACS::SensorValues *sensorValues,
acsctrl::AttitudeEstimationData *attitudeEstimationData,
const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
acsctrl::FusedRotRateData *fusedRotRateData);
protected:
private:
static constexpr double ZERO_VEC[3] = {0, 0, 0};
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
AcsParameters *acsParameters;
double quatOldQuest[4] = {0, 0, 0, 0};
double quatOldStr[4] = {0, 0, 0, 0};
double rotRateOldB[3] = {0, 0, 0};
void estimateFusedRotationRateSusMgm(acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateData *fusedRotRateData);
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
void estimateFusedRotationRateQuest(acsctrl::AttitudeEstimationData *attitudeEstimationData,
const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
void estimateFusedRotationRateStr(ACS::SensorValues *sensorValues, const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
};
#endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */

View File

@ -4,21 +4,21 @@
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
#include <mission/controller/acs/util/MathOperations.h>
#include <cmath>
#include <filesystem>
#include "string.h"
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
#include <string>
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
Guidance::~Guidance() {}
void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3],
double sunDirI[3], double refDirB[3], double quatBI[4],
double targetQuat[4], double targetSatRotRate[3]) {
[[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
//-------------------------------------------------------------------------------------
@ -38,7 +38,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
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(now, *dcmEI, *dcmEIDot);
MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -136,8 +136,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
}
void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double targetQuat[4], double targetSatRotRate[3]) {
void Guidance::targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta,
double posSatE[3], double velSatE[3], double targetQuat[4],
double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing
//-------------------------------------------------------------------------------------
@ -154,7 +155,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
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(now, *dcmEI, *dcmEIDot);
MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -199,11 +200,12 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
QuaternionOperations::fromDcm(dcmIX, targetQuat);
int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
}
void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3],
double targetQuat[4], double targetSatRotRate[3]) {
void Guidance::targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta,
double posSatE[3], double sunDirI[3], double targetQuat[4],
double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for ground station pointing
//-------------------------------------------------------------------------------------
@ -221,7 +223,7 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
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(now, *dcmEI, *dcmEIDot);
MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -263,10 +265,10 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
}
void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
void Guidance::targetQuatPtgSun(double timeDelta, double sunDirI[3], double targetQuat[4],
double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion to sun
@ -298,12 +300,13 @@ void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQua
// Calculation of reference rotation rate
//----------------------------------------------------------------------------
int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
}
void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
double targetQuat[4], double refDirB[3],
double refSatRate[3]) {
[[deprecated]] void Guidance::targetQuatPtgNadirSingleAxis(const timeval timeAbsolute,
double posSatE[3], double quatBI[4],
double targetQuat[4], double refDirB[3],
double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
@ -314,7 +317,7 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub
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(now, *dcmEI, *dcmEIDot);
MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -362,7 +365,8 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub
QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
}
void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
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
@ -371,7 +375,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl
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(now, *dcmEI, *dcmEIDot);
MathOperations<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -407,7 +411,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate);
targetRotationRate(timeElapsedMax, timeDelta, targetQuat, refSatRate);
}
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
@ -448,23 +452,21 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
}
void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double *refSatRate) {
void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
double quatInertialTarget[4], double *refSatRate) {
//-------------------------------------------------------------------------------------
// Calculation of target rotation rate
//-------------------------------------------------------------------------------------
double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 -
(timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6);
if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
}
if (timeElapsed < timeElapsedMax) {
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 / timeElapsed, 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]};
@ -488,33 +490,29 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua
refSatRate[2] = 0;
}
timeSavedQuaternion = now;
savedQuaternion[0] = quatInertialTarget[0];
savedQuaternion[1] = quatInertialTarget[1];
savedQuaternion[2] = quatInertialTarget[2];
savedQuaternion[3] = quatInertialTarget[3];
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
}
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
double *rwPseudoInv) {
bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid());
bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid());
bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
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());
if (rw1valid && rw2valid && rw3valid && rw4valid) {
if (rw1valid and rw2valid and rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
return returnvalue::OK;
} else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
} 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 && !rw2valid && rw3valid && rw4valid) {
} 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 && rw2valid && !rw3valid && rw4valid) {
} 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 && rw2valid && rw3valid && !rw4valid) {
} else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
return returnvalue::OK;
} else {

View File

@ -17,25 +17,26 @@ class Guidance {
// Function to get the target quaternion and reference rotation rate from gps position and
// position of the ground station
void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3],
double refDirB[3], double quatBI[4], double targetQuat[4],
double targetSatRotRate[3]);
void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4],
double targetSatRotRate[3]);
void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
double targetSatRotRate[3]);
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]);
// Function to get the target quaternion and reference rotation rate for sun pointing after ground
// station
void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
void targetQuatPtgSun(const double timeDelta, double sunDirI[3], double targetQuat[4],
double targetSatRotRate[3]);
// Function to get the target quaternion and refence rotation rate from gps position for Nadir
// pointing
void targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
void targetQuatPtgNadirSingleAxis(const timeval timeAbsolute, double posSatE[3], double quatBI[4],
double targetQuat[4], double refDirB[3], double refSatRate[3]);
void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double targetQuat[4], double refSatRate[3]);
void targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta,
double posSatE[3], double velSatE[3], double targetQuat[4],
double refSatRate[3]);
// @note: Calculates the error quaternion between the current orientation and the target
// quaternion, considering a reference quaternion. Additionally the difference between the actual
@ -48,8 +49,8 @@ class Guidance {
double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
double &errorAngle);
void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double *targetSatRotRate);
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"
@ -59,7 +60,6 @@ class Guidance {
const AcsParameters *acsParameters;
bool strBlindAvoidFlag = false;
timeval timeSavedQuaternion;
double savedQuaternion[4] = {0, 0, 0, 0};
double omegaRefSaved[3] = {0, 0, 0};

View File

@ -19,7 +19,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
ReturnValue_t MultiplicativeKalmanFilter::init(
const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, acsctrl::MekfData *mekfData,
const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters) { // valids for "model measurements"?
// check for valid mag/sun
if (validMagField_ && validSS && validSSModel && validMagModel) {
@ -191,7 +191,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
const bool validGYRs_, const double *magneticField_, const bool validMagField_,
const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
const double *magFieldJ, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters) {
// Check for GYR Measurements
int MDF = 0; // Matrix Dimension Factor
@ -1090,7 +1090,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
return MEKF_RUNNING;
}
ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::AttitudeEstimationData *mekfData) {
double resetQuaternion[4] = {0, 0, 0, 1};
double resetCovarianceMatrix[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}};
@ -1100,7 +1100,7 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
return MEKF_UNINITIALIZED;
}
void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,
void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData,
MekfStatus mekfStatus) {
{
PoolReadGuard pg(mekfData);
@ -1115,8 +1115,9 @@ void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mek
}
}
void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus,
double quat[4], double satRotRate[3]) {
void MultiplicativeKalmanFilter::updateDataSet(acsctrl::AttitudeEstimationData *mekfData,
MekfStatus mekfStatus, double quat[4],
double satRotRate[3]) {
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {

View File

@ -21,7 +21,7 @@ class MultiplicativeKalmanFilter {
MultiplicativeKalmanFilter();
virtual ~MultiplicativeKalmanFilter();
ReturnValue_t reset(acsctrl::MekfData *mekfData);
ReturnValue_t reset(acsctrl::AttitudeEstimationData *mekfData);
/* @brief: init() - This function initializes the Kalman Filter and will provide the first
* quaternion through the QUEST algorithm
@ -32,8 +32,8 @@ class MultiplicativeKalmanFilter {
*/
ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
AcsParameters *acsParameters);
const double *magFieldJ, const bool validMagModel,
acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters);
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
* for the current step after the initalization
@ -53,7 +53,7 @@ class MultiplicativeKalmanFilter {
const bool validGYRs_, const double *magneticField_,
const bool validMagField_, const double *sunDir_, const bool validSS,
const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, acsctrl::MekfData *mekfData,
const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters);
enum MekfStatus : uint8_t {
@ -99,9 +99,9 @@ class MultiplicativeKalmanFilter {
double biasGYR[3]; /*Between measured and estimated sat Rate*/
/*Parameter INIT*/
/*Functions*/
void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
double satRotRate[3]);
void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus);
void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus,
double quat[4], double satRotRate[3]);
};
#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */

View File

@ -16,7 +16,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MekfData *mekfData, AcsParameters *acsParameters) {
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;
@ -41,7 +42,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
}
}
void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
void Navigation::resetMekf(acsctrl::AttitudeEstimationData *mekfData) {
mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
}
@ -54,7 +55,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
{
PoolReadGuard pg(gpsDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->source = acs::GpsSource::SPG4;
gpsDataProcessed->source = acs::gps::Source::SPG4;
gpsDataProcessed->source.setValid(true);
std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
gpsDataProcessed->gpsPosition.setValid(true);
@ -66,7 +67,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
{
PoolReadGuard pg(gpsDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->source = acs::GpsSource::NONE;
gpsDataProcessed->source = acs::gps::Source::NONE;
gpsDataProcessed->source.setValid(true);
std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
gpsDataProcessed->gpsPosition.setValid(false);

View File

@ -17,9 +17,9 @@ class Navigation {
ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
AcsParameters *acsParameters);
void resetMekf(acsctrl::MekfData *mekfData);
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters);
void resetMekf(acsctrl::AttitudeEstimationData *mekfData);
ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);

View File

@ -7,7 +7,7 @@ SensorProcessing::~SensorProcessing() {}
void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value,
bool mgm1valid, const float *mgm2Value, bool mgm2valid,
const float *mgm3Value, bool mgm3valid, const float *mgm4Value,
bool mgm4valid, timeval timeOfMgmMeasurement,
bool mgm4valid, timeval timeAbsolute, double timeDelta,
const AcsParameters::MgmHandlingParameters *mgmParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed) {
@ -15,14 +15,14 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
// ------------------------------------------------
double magIgrfModel[3] = {0.0, 0.0, 0.0};
bool gpsValid = false;
if (gpsDataProcessed->source.value != acs::GpsSource::NONE) {
if (gpsDataProcessed->source.value != acs::gps::Source::NONE) {
// There seems to be a bug here, which causes the model vector to drift until infinity, if the
// model class is not initialized new every time. Works for now, but should be investigated.
Igrf13Model igrf13;
igrf13.schmidtNormalization();
igrf13.updateCoeffGH(timeOfMgmMeasurement);
// maybe put a condition here, to only update after a full day, this
// class function has around 700 steps to perform
igrf13.updateCoeffGH(timeAbsolute);
igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
gpsDataProcessed->altitude.value, timeOfMgmMeasurement, magIgrfModel);
gpsDataProcessed->altitude.value, timeAbsolute, magIgrfModel);
gpsValid = true;
}
if (not mgm0valid and not mgm1valid and not mgm2valid and not mgm3valid and
@ -129,14 +129,12 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
//-----------------------Mgm Rate Computation ---------------------------------------------------
double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool mgmVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and
VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
if (timeDelta > 0 and VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
VectorOperations<double>::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3);
VectorOperations<double>::mulScalar(mgmVecTotDerivative, 1. / timeDiff, mgmVecTotDerivative, 3);
VectorOperations<double>::mulScalar(mgmVecTotDerivative, 1. / timeDelta, mgmVecTotDerivative,
3);
mgmVecTotDerivativeValid = true;
}
timeOfSavedMagFieldEst = timeOfMgmMeasurement;
std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot));
if (VectorOperations<double>::norm(mgmVecTotDerivative, 3) != 0 and
@ -177,11 +175,12 @@ void SensorProcessing::processSus(
const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, bool sus7valid,
const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid,
const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
timeval timeAbsolute, double timeDelta,
const AcsParameters::SusHandlingParameters *susParameters,
const AcsParameters::SunModelParameters *sunModelParameters,
acsctrl::SusDataProcessed *susDataProcessed) {
/* -------- Sun Model Direction (IJK frame) ------- */
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeAbsolute);
// Julean Centuries
double sunIjkModel[3] = {0.0, 0.0, 0.0};
@ -354,11 +353,10 @@ void SensorProcessing::processSus(
double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool susVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0 and
VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
if (timeDelta > 0 and VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
VectorOperations<double>::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3);
VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDiff, susVecTotDerivative, 3);
VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDelta, susVecTotDerivative,
3);
susVecTotDerivativeValid = true;
}
std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot));
@ -367,7 +365,6 @@ void SensorProcessing::processSus(
lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,
susParameters->susRateFilterWeight);
}
timeOfSavedSusDirEst = timeOfSusMeasurement;
{
PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
@ -414,7 +411,7 @@ void SensorProcessing::processGyr(
const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, bool gyr2axYvalid,
const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid,
const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters,
const AcsParameters::GyrHandlingParameters *gyrParameters,
acsctrl::GyrDataProcessed *gyrDataProcessed) {
bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid);
bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid);
@ -521,16 +518,16 @@ void SensorProcessing::processGyr(
}
void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude,
const double gpsAltitude, const double gpsUnixSeconds,
const double gpsAltitude, const double timeDelta,
const bool validGps,
const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed) {
// init variables
double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
gpsVelocityE[3] = {0, 0, 0};
uint8_t gpsSource = acs::GpsSource::NONE;
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::GpsSource::SPG4 and gpsParameters->useSpg4) {
if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) {
MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude,
gdLongitude, altitude);
double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
@ -563,21 +560,17 @@ 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);
if (validSavedPosSatE and
(gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax) and
(gpsUnixSeconds - timeOfSavedPosSatE) > 0) {
if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE;
VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3);
VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3);
}
savedPosSatE[0] = posSatE[0];
savedPosSatE[1] = posSatE[1];
savedPosSatE[2] = posSatE[2];
timeOfSavedPosSatE = gpsUnixSeconds;
validSavedPosSatE = true;
gpsSource = acs::GpsSource::GPS;
gpsSource = acs::gps::Source::GPS;
}
{
PoolReadGuard pg(gpsDataProcessed);
@ -594,13 +587,15 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
}
}
void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
void SensorProcessing::process(timeval timeAbsolute, double timeDelta,
ACS::SensorValues *sensorValues,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed,
const AcsParameters *acsParameters) {
sensorValues->update();
processGps(
sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
@ -617,7 +612,8 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->mgm3Rm3100Set.fieldStrengths.value,
sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(),
sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(),
now, &acsParameters->mgmHandlingParameters, gpsDataProcessed, mgmDataProcessed);
timeAbsolute, timeDelta, &acsParameters->mgmHandlingParameters, gpsDataProcessed,
mgmDataProcessed);
processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),
@ -631,8 +627,8 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->susSets[9].channels.value, sensorValues->susSets[9].channels.isValid(),
sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(),
sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(),
now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters,
susDataProcessed);
timeAbsolute, timeDelta, &acsParameters->susHandlingParameters,
&acsParameters->sunModelParameters, susDataProcessed);
processGyr(
sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(),
@ -646,7 +642,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->gyr2AdisSet.angVelocZ.value, sensorValues->gyr2AdisSet.angVelocZ.isValid(),
sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(),
sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(),
sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now,
sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(),
&acsParameters->gyrHandlingParameters, gyrDataProcessed);
}

View File

@ -24,22 +24,21 @@ class SensorProcessing {
SensorProcessing();
virtual ~SensorProcessing();
void process(timeval now, ACS::SensorValues *sensorValues,
void process(timeval timeAbsolute, double timeDelta, ACS::SensorValues *sensorValues,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed,
const AcsParameters *acsParameters); // Will call protected functions
acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters);
private:
static constexpr float ZERO_VEC_F[3] = {0, 0, 0};
static constexpr double ZERO_VEC_D[3] = {0, 0, 0};
static constexpr double ECCENTRICITY_WGS84 = 0.0818195;
protected:
// short description needed for every function
void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid,
const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement,
const float *mgm4Value, bool mgm4valid, timeval timeAbsolute, double timeDelta,
const AcsParameters::MgmHandlingParameters *mgmParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed);
@ -52,7 +51,7 @@ class SensorProcessing {
bool sus7valid, const uint16_t *sus8Value, bool sus8valid,
const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus10Value,
bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
timeval timeOfSusMeasurement,
timeval timeAbsolute, double timeDelta,
const AcsParameters::SusHandlingParameters *susParameters,
const AcsParameters::SunModelParameters *sunModelParameters,
acsctrl::SusDataProcessed *susDataProcessed);
@ -65,7 +64,6 @@ class SensorProcessing {
bool gyr2axYvalid, const double gyr2axZvalue, bool gyr2axZvalid,
const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue,
bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
timeval timeOfGyrMeasurement,
const AcsParameters::GyrHandlingParameters *gyrParameters,
acsctrl::GyrDataProcessed *gyrDataProcessed);
@ -77,13 +75,9 @@ class SensorProcessing {
void lowPassFilter(double *newValue, double *oldValue, const double weight);
double savedMgmVecTot[3] = {0.0, 0.0, 0.0};
timeval timeOfSavedMagFieldEst;
double savedSusVecTot[3] = {0.0, 0.0, 0.0};
timeval timeOfSavedSusDirEst;
bool validMagField = false;
double savedPosSatE[3] = {0.0, 0.0, 0.0};
double timeOfSavedPosSatE = 0.0;
bool validSavedPosSatE = false;
SusConverter susConverter;

View File

@ -7,18 +7,18 @@ Detumble::Detumble() {}
Detumble::~Detumble() {}
acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
const bool satRotRateValid,
const bool magFieldRateValid,
const bool useFullDetumbleLaw) {
acs::ControlModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
const bool satRotRateValid,
const bool magFieldRateValid,
const bool useFullDetumbleLaw) {
if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (satRotRateValid and useFullDetumbleLaw) {
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL;
return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL;
} else if (magFieldRateValid) {
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
}

View File

@ -11,9 +11,9 @@ class Detumble {
Detumble();
virtual ~Detumble();
acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
const bool magFieldRateValid,
const bool useFullDetumbleLaw);
acs::ControlModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
const bool magFieldRateValid,
const bool useFullDetumbleLaw);
void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
double gain);

View File

@ -10,6 +10,22 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_
PtgCtrl::~PtgCtrl() {}
acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
const bool magFieldValid, const bool mekfValid, const bool strValid, const bool questValid,
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 (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
return acs::ControlModeStrategy::PTGCTRL_QUEST;
} else {
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
}
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
double *torqueRws) {

View File

@ -2,6 +2,7 @@
#define PTGCTRL_H_
#include <math.h>
#include <mission/acs/defs.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <stdio.h>
@ -9,7 +10,7 @@
class PtgCtrl {
/*
* @brief: This class handles the pointing control mechanism. Calculation of an commanded
* torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation
* torque for the reaction wheels, and magnetic Field strength for magnetorquer for desaturation
*
* @note: A description of the used algorithms can be found in
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110
@ -21,6 +22,12 @@ class PtgCtrl {
PtgCtrl(AcsParameters *acsParameters_);
virtual ~PtgCtrl();
acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool strValid, const bool questValid,
const bool fusedRateValid,
const uint8_t rotRateSource,
const uint8_t mekfEnabled);
/* @brief: Calculates the needed torque for the pointing control mechanism
*/
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
@ -36,7 +43,7 @@ class PtgCtrl {
const int32_t speedRw3, double *mgtDpDes);
/* @brief: Commands the stiction torque in case wheel speed is to low
* torqueCommand modified torque after antistiction
* torqueCommand modified torque after anti-stiction
*/
void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed);

View File

@ -9,40 +9,38 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
SafeCtrl::~SafeCtrl() {}
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const bool fusedRateTotalValid,
const uint8_t mekfEnabled,
const uint8_t gyrEnabled,
const uint8_t dampingEnabled) {
acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(
const bool magFieldValid, const bool mekfValid, const bool satRotRateValid,
const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled,
const uint8_t gyrEnabled, const uint8_t dampingEnabled) {
if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) {
return acs::SafeModeStrategy::SAFECTRL_MEKF;
return acs::ControlModeStrategy::SAFECTRL_MEKF;
} else if (sunDirValid) {
if (gyrEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_GYR;
return acs::ControlModeStrategy::SAFECTRL_GYR;
} else if (not gyrEnabled and fusedRateTotalValid) {
return acs::SafeModeStrategy::SAFECTRL_SUSMGM;
return acs::ControlModeStrategy::SAFECTRL_SUSMGM;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
} else if (not sunDirValid) {
if (dampingEnabled) {
if (gyrEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
} else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
} else if (not dampingEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING;
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
}

View File

@ -12,10 +12,11 @@ class SafeCtrl {
SafeCtrl(AcsParameters *acsParameters_);
virtual ~SafeCtrl();
acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const bool fusedRateTotalValid, const uint8_t mekfEnabled,
const uint8_t gyrEnabled, const uint8_t dampingEnabled);
acs::ControlModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const bool fusedRateTotalValid,
const uint8_t mekfEnabled, const uint8_t gyrEnabled,
const uint8_t dampingEnabled);
void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
const double *quatBI, const double *sunDirRefB, double *magMomB,

View File

@ -20,6 +20,7 @@ enum SetIds : uint32_t {
CTRL_VAL_DATA,
ACTUATOR_CMD_DATA,
FUSED_ROTATION_RATE_DATA,
FUSED_ROTATION_RATE_SOURCES_DATA,
TLE_SET,
};
@ -96,6 +97,7 @@ enum PoolIds : lp_id_t {
SAT_ROT_RATE_MEKF,
QUAT_MEKF,
MEKF_STATUS,
QUAT_QUEST,
// Ctrl Values
SAFE_STRAT,
TGT_QUAT,
@ -110,9 +112,13 @@ enum PoolIds : lp_id_t {
ROT_RATE_ORTHOGONAL,
ROT_RATE_PARALLEL,
ROT_RATE_TOTAL,
// TLE
TLE_LINE_1,
TLE_LINE_2,
ROT_RATE_SOURCE,
// Fused Rotation Rate Sources
ROT_RATE_ORTHOGONAL_SUSMGM,
ROT_RATE_PARALLEL_SUSMGM,
ROT_RATE_TOTAL_SUSMGM,
ROT_RATE_TOTAL_QUEST,
ROT_RATE_TOTAL_STR,
};
static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6;
@ -122,11 +128,11 @@ static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6;
static constexpr uint8_t MEKF_SET_ENTRIES = 3;
static constexpr uint8_t ATTITUDE_ESTIMATION_SET_ENTRIES = 4;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3;
static constexpr uint8_t TLE_SET_ENTRIES = 2;
static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 4;
static constexpr uint8_t FUSED_ROT_RATE_SOURCES_SET_ENTRIES = 5;
/**
* @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status.
@ -250,13 +256,14 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
private:
};
class MekfData : public StaticLocalDataSet<MEKF_SET_ENTRIES> {
class AttitudeEstimationData : public StaticLocalDataSet<ATTITUDE_ESTIMATION_SET_ENTRIES> {
public:
MekfData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {}
AttitudeEstimationData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {}
lp_vec_t<double, 4> quatMekf = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this);
lp_vec_t<double, 3> satRotRateMekf = lp_vec_t<double, 3>(sid.objectId, SAT_ROT_RATE_MEKF, this);
lp_var_t<uint8_t> mekfStatus = lp_var_t<uint8_t>(sid.objectId, MEKF_STATUS, this);
lp_vec_t<double, 4> quatQuest = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this);
private:
};
@ -295,16 +302,25 @@ class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL, this);
lp_vec_t<double, 3> rotRateParallel = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL, this);
lp_vec_t<double, 3> rotRateTotal = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL, this);
lp_var_t<uint8_t> rotRateSource = lp_var_t<uint8_t>(sid.objectId, ROT_RATE_SOURCE, this);
private:
};
class TleData : public StaticLocalDataSet<TLE_SET_ENTRIES> {
class FusedRotRateSourcesData : public StaticLocalDataSet<FUSED_ROT_RATE_SOURCES_SET_ENTRIES> {
public:
TleData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, TLE_SET) {}
FusedRotRateSourcesData(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_SOURCES_DATA) {}
lp_vec_t<uint8_t, 69> line1 = lp_vec_t<uint8_t, 69>(sid.objectId, TLE_LINE_1, this);
lp_vec_t<uint8_t, 69> line2 = lp_vec_t<uint8_t, 69>(sid.objectId, TLE_LINE_1, this);
lp_vec_t<double, 3> rotRateOrthogonalSusMgm =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL_SUSMGM, this);
lp_vec_t<double, 3> rotRateParallelSusMgm =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL_SUSMGM, this);
lp_vec_t<double, 3> rotRateTotalSusMgm =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_SUSMGM, this);
lp_vec_t<double, 3> rotRateTotalQuest =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_QUEST, this);
lp_vec_t<double, 3> rotRateTotalStr = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_STR, this);
private:
};

View File

@ -6,7 +6,7 @@ StrFdir::StrFdir(object_id_t strObject)
: DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {}
ReturnValue_t StrFdir::eventReceived(EventMessage* event) {
if (event->getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) {
if (event->getEvent() == acs::PTG_CTRL_NO_ATTITUDE_INFORMATION) {
setFaulty(event->getEvent());
return returnvalue::OK;
}

View File

@ -106,7 +106,7 @@ Subsystem& satsystem::acs::init() {
// Build TARGET PT transition 0
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TRANS_0.second, true);
iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TRANS_0.second, true);
iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TRANS_0.second, true);
iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
check(ACS_SUBSYSTEM.addTable(
@ -161,6 +161,7 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) {
iht(objects::IMTQ_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
iht(objects::STR_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
iht(objects::ACS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
iht(objects::SUS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
check(ss.addTable(TableEntry(ACS_TABLE_OFF_TRANS_1.first, &ACS_TABLE_OFF_TRANS_1.second)), ctxc);
@ -199,13 +200,13 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
iht(objects::ACS_CONTROLLER, acs::AcsMode::SAFE, acs::SafeSubmode::DEFAULT,
ACS_TABLE_SAFE_TGT.second, true);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_SAFE_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second, true);
iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_SAFE_TGT.second, true);
check(ss.addTable(&ACS_TABLE_SAFE_TGT.second, ACS_TABLE_SAFE_TGT.first, false, true), ctxc);
// Build SAFE transition 0
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_SAFE_TRANS_0.second);
iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true);
iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true);
iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true);
iht(objects::STR_ASSY, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second);
iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second);
@ -256,13 +257,13 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::STR_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second, true);
iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_IDLE_TGT.second, true);
ss.addTable(&ACS_TABLE_IDLE_TGT.second, ACS_TABLE_IDLE_TGT.first, false, true);
// Build IDLE transition 0
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true);
iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true);
iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true);
iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
iht(objects::STR_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
@ -306,8 +307,8 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {
// Build TARGET PT table
iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second, true);
iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_TGT.second, true);
iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
check(ss.addTable(&ACS_TABLE_PTG_TARGET_TGT.second, ACS_TABLE_PTG_TARGET_TGT.first, false, true),
@ -355,8 +356,8 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) {
// Build TARGET PT table
iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_NADIR, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true);
iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true);
iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TGT.first,
@ -408,8 +409,8 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) {
// Build TARGET PT table
iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET_GS, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second, true);
iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_GS_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_GS_TGT.second, true);
iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
check(ss.addTable(
@ -461,8 +462,10 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {
iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_INERTIAL, 0,
ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second, true);
iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second,
true);
iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second,
true);
iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TGT.first,

2
tmtc

@ -1 +1 @@
Subproject commit 74e55b16dcca73023254493d911be3debc36adb2
Subproject commit f63a834d9acd4506f0dbd7f8e63007f17c063c1d