Merge branch 'develop' into stop-the-spam
This commit is contained in:
commit
078299a07c
14
CHANGELOG.md
14
CHANGELOG.md
@ -16,6 +16,14 @@ will consitute of a breaking change warranting a new major release:
|
||||
|
||||
# [unreleased]
|
||||
|
||||
## Added
|
||||
|
||||
- Added NaN and Inf check for the `MEKF`. If these are detected, the `AcsController` will reset
|
||||
the `MEKF` on its own once. This way, there will not be an event spam and operators will have
|
||||
to look into the reason of wrong outputs. To restore the reset ability, an action command has
|
||||
been added.
|
||||
|
||||
|
||||
## Fixed
|
||||
|
||||
- Fixed transition for dual power lane assemblies: When going from dual side submode to single side
|
||||
@ -69,8 +77,7 @@ eive-tmtc: v2.19.1
|
||||
PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/462
|
||||
- Add `PcduHandlerDummy` component.
|
||||
- Added parameter for timeout until `MEKF_INVALID_MODE_VIOLATION` event is triggered.
|
||||
- EIVE system: Add boot mode which is also the initial mode. The fallback mode of the boot mode
|
||||
will be the SAFE mode. The boot mode can also be used to switch as many devices as possible OFF.
|
||||
|
||||
## Fixed
|
||||
|
||||
- Pointing control of the `AcsController` was still expecting submodes instead of modes.
|
||||
@ -96,9 +103,6 @@ eive-tmtc: v2.19.1
|
||||
commanding is done autonomously by the COM subsystem internally or by the operator. This prevents
|
||||
the transmitter from going off during fallbacks to the SAFE mode, which might not always be
|
||||
desired.
|
||||
|
||||
## Changed
|
||||
|
||||
- Initialize switch states to a special `SWITCH_STATE_UNKNOWN` (2) variable. Return
|
||||
`PowerSwitchIF::SWITCH_UNKNOWN` in switch state getter if this is the state.
|
||||
- Wait 1 second before commanding SAFE mode. This ensures or at least increases the chance that
|
||||
|
@ -1,7 +1,7 @@
|
||||
/**
|
||||
* @brief Auto-generated event translation file. Contains 278 translations.
|
||||
* @details
|
||||
* Generated on: 2023-03-15 10:10:04
|
||||
* Generated on: 2023-03-21 12:45:57
|
||||
*/
|
||||
#include "translateEvents.h"
|
||||
|
||||
|
@ -2,7 +2,7 @@
|
||||
* @brief Auto-generated object translation file.
|
||||
* @details
|
||||
* Contains 169 translations.
|
||||
* Generated on: 2023-03-14 17:08:41
|
||||
* Generated on: 2023-03-21 12:45:57
|
||||
*/
|
||||
#include "translateObjects.h"
|
||||
|
||||
|
@ -470,8 +470,9 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
|
||||
0x6a04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6a05;ACSMEKF_MekfNoSusMgmStrData;No description;5;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6a06;ACSMEKF_MekfCovarianceInversionFailed;No description;6;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6a07;ACSMEKF_MekfInitialized;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6a08;ACSMEKF_MekfRunning;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6a07;ACSMEKF_MekfNotFinite;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6a08;ACSMEKF_MekfInitialized;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6a09;ACSMEKF_MekfRunning;No description;9;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h
|
||||
0x6c01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h
|
||||
0x6d01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h
|
||||
|
|
@ -581,8 +581,9 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
|
||||
0x6a04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6a05;ACSMEKF_MekfNoSusMgmStrData;No description;5;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6a06;ACSMEKF_MekfCovarianceInversionFailed;No description;6;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6a07;ACSMEKF_MekfInitialized;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6a08;ACSMEKF_MekfRunning;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6a07;ACSMEKF_MekfNotFinite;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6a08;ACSMEKF_MekfInitialized;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6a09;ACSMEKF_MekfRunning;No description;9;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h
|
||||
0x6c01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h
|
||||
0x6d01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h
|
||||
|
|
@ -1,7 +1,7 @@
|
||||
/**
|
||||
* @brief Auto-generated event translation file. Contains 278 translations.
|
||||
* @details
|
||||
* Generated on: 2023-03-15 10:10:04
|
||||
* Generated on: 2023-03-21 12:45:57
|
||||
*/
|
||||
#include "translateEvents.h"
|
||||
|
||||
|
@ -2,7 +2,7 @@
|
||||
* @brief Auto-generated object translation file.
|
||||
* @details
|
||||
* Contains 173 translations.
|
||||
* Generated on: 2023-03-14 17:08:41
|
||||
* Generated on: 2023-03-21 12:45:57
|
||||
*/
|
||||
#include "translateObjects.h"
|
||||
|
||||
|
@ -1,7 +1,7 @@
|
||||
/**
|
||||
* @brief Auto-generated event translation file. Contains 278 translations.
|
||||
* @details
|
||||
* Generated on: 2023-03-15 10:10:04
|
||||
* Generated on: 2023-03-21 12:45:57
|
||||
*/
|
||||
#include "translateEvents.h"
|
||||
|
||||
|
@ -2,7 +2,7 @@
|
||||
* @brief Auto-generated object translation file.
|
||||
* @details
|
||||
* Contains 173 translations.
|
||||
* Generated on: 2023-03-14 17:08:41
|
||||
* Generated on: 2023-03-21 12:45:57
|
||||
*/
|
||||
#include "translateObjects.h"
|
||||
|
||||
|
@ -55,6 +55,10 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
|
||||
navigation.resetMekf(&mekfData);
|
||||
return HasActionsIF::EXECUTION_FINISHED;
|
||||
}
|
||||
case RESTORE_MEKF_NONFINITE_RECOVERY: {
|
||||
mekfLost = false;
|
||||
return HasActionsIF::EXECUTION_FINISHED;
|
||||
}
|
||||
default: {
|
||||
return HasActionsIF::INVALID_ACTION_ID;
|
||||
}
|
||||
@ -149,6 +153,10 @@ void AcsController::performSafe() {
|
||||
triggerEvent(acs::MEKF_INVALID_INFO);
|
||||
mekfInvalidFlag = true;
|
||||
}
|
||||
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
|
||||
navigation.resetMekf(&mekfData);
|
||||
mekfLost = true;
|
||||
}
|
||||
} else {
|
||||
mekfInvalidFlag = false;
|
||||
}
|
||||
@ -231,6 +239,9 @@ void AcsController::performDetumble() {
|
||||
triggerEvent(acs::MEKF_INVALID_INFO);
|
||||
mekfInvalidFlag = true;
|
||||
}
|
||||
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
|
||||
navigation.resetMekf(&mekfData);
|
||||
}
|
||||
} else {
|
||||
mekfInvalidFlag = false;
|
||||
}
|
||||
@ -282,6 +293,9 @@ void AcsController::performPointingCtrl() {
|
||||
triggerEvent(acs::MEKF_INVALID_INFO);
|
||||
mekfInvalidFlag = true;
|
||||
}
|
||||
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
|
||||
navigation.resetMekf(&mekfData);
|
||||
}
|
||||
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);
|
||||
|
@ -64,6 +64,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
uint16_t mekfInvalidCounter = 0;
|
||||
bool safeCtrlFailureFlag = false;
|
||||
uint8_t safeCtrlFailureCounter = 0;
|
||||
uint8_t resetMekfCount = 0;
|
||||
bool mekfLost = false;
|
||||
|
||||
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
|
||||
int16_t cmdDipolMtqs[3] = {0, 0, 0};
|
||||
@ -78,6 +80,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
/** Device command IDs */
|
||||
static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
|
||||
static const DeviceCommandId_t RESET_MEKF = 0x1;
|
||||
static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
|
||||
static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0);
|
||||
|
@ -1080,6 +1080,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
||||
|
||||
MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
|
||||
|
||||
if (not(MathOperations<double>::checkVectorIsFinite(propagatedQuaternion, 4)) ||
|
||||
not(MathOperations<double>::checkMatrixIsFinite(initialQuaternion, 6, 6))) {
|
||||
updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE);
|
||||
return MEKF_NOT_FINITE;
|
||||
}
|
||||
|
||||
updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
|
||||
return MEKF_RUNNING;
|
||||
}
|
||||
|
@ -62,6 +62,7 @@ class MultiplicativeKalmanFilter {
|
||||
NO_MODEL_VECTORS = 2,
|
||||
NO_SUS_MGM_STR_DATA = 3,
|
||||
COVARIANCE_INVERSION_FAILED = 4,
|
||||
NOT_FINITE = 5,
|
||||
INITIALIZED = 10,
|
||||
RUNNING = 11,
|
||||
};
|
||||
@ -74,8 +75,9 @@ class MultiplicativeKalmanFilter {
|
||||
static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5);
|
||||
static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED =
|
||||
returnvalue::makeCode(IF_MEKF_ID, 6);
|
||||
static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7);
|
||||
static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8);
|
||||
static constexpr ReturnValue_t MEKF_NOT_FINITE = returnvalue::makeCode(IF_MEKF_ID, 7);
|
||||
static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 8);
|
||||
static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9);
|
||||
|
||||
private:
|
||||
/*Parameters*/
|
||||
|
@ -404,6 +404,26 @@ class MathOperations {
|
||||
std::memcpy(inverse, identity, sizeof(identity));
|
||||
return 0; // successful inversion
|
||||
}
|
||||
|
||||
static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
|
||||
for (uint8_t i = 0; i < size; i++) {
|
||||
if (not isfinite(inputVector[i])) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
|
||||
for (uint8_t col = 0; col < cols; col++) {
|
||||
for (uint8_t row = 0; row < rows; row++) {
|
||||
if (not isfinite(inputMatrix[row * cols + cols])) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* ACS_MATH_MATHOPERATIONS_H_ */
|
||||
|
2
tmtc
2
tmtc
@ -1 +1 @@
|
||||
Subproject commit e5a09e148b45d0380dc6d9a1a88002bab4b0376c
|
||||
Subproject commit b32a9273ae4186a440685ffde7618f017cfdb268
|
Loading…
Reference in New Issue
Block a user