eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

changed mekf events again
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-22 14:50:38 +01:00
parent 2c3a7791e1
commit 83b81e1ba8
3 changed files with 22 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
mission/acsDefs.h
View File

@ -25,10 +25,10 @@ static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
//!< Multiple RWs are invalid, not commandable and therefore higher ACS modes cannot be maintained.
static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH);
//!< MEKF was not able to compute a solution during safe or detumble mode.
static constexpr Event MEKF_INVALID_SAFE_MODE = MAKE_EVENT(3, severity::INFO);
//!< MEKF was not able to compute a solution during any pointing ACS mode.
static constexpr Event MEKF_INVALID_PTG_MODE = MAKE_EVENT(4, severity::HIGH);
//!< MEKF was not able to compute a solution.
static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO);
//!< 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(4, severity::HIGH);
extern const char* getModeStr(AcsMode mode);

26
mission/controller/AcsController.cpp
View File

@ -14,8 +14,6 @@ AcsController::AcsController(object_id_t objectId)
safeCtrl(&acsParameters),
detumble(&acsParameters),
ptgCtrl(&acsParameters),
detumbleCounter{0},
multipleRwUnavailableCounter{0},
parameterHelper(this),
mgmDataRaw(this),
mgmDataProcessed(this),
@ -129,10 +127,9 @@ void AcsController::performSafe() {
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_SAFE_MODE);
triggerEvent(acs::MEKF_INVALID_INFO);
mekfInvalidFlag = true;
}
return;
} else {
mekfInvalidFlag = false;
}
@ -157,10 +154,9 @@ void AcsController::performSafe() {
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
}
int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
// Detumble check and switch
// detumble check and switch
if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
@ -196,10 +192,9 @@ void AcsController::performDetumble() {
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_SAFE_MODE);
triggerEvent(acs::MEKF_INVALID_INFO);
mekfInvalidFlag = true;
}
return;
} else {
mekfInvalidFlag = false;
}
@ -207,7 +202,6 @@ void AcsController::performDetumble() {
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
if (mekfData.satRotRateMekf.isValid() &&
@ -245,21 +239,29 @@ void AcsController::performPointingCtrl() {
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_PTG_MODE);
triggerEvent(acs::MEKF_INVALID_INFO);
mekfInvalidFlag = true;
}
if (mekfInvalidCounter > 4) {
triggerEvent(acs::MEKF_INVALID_MODE_VIOLATION);
}
mekfInvalidCounter++;
commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
return;
} else {
mekfInvalidFlag = false;
mekfInvalidCounter = 0;
}
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);
if (result == returnvalue::FAILED) {
multipleRwUnavailableCounter++;
if (multipleRwUnavailableCounter > 4) {
triggerEvent(acs::MULTIPLE_RW_INVALID);
}
multipleRwUnavailableCounter++;
return;
} else {
multipleRwUnavailableCounter = 0;
@ -387,12 +389,10 @@ void AcsController::performPointingCtrl() {
ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
}
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
updateCtrlValData(targetQuat, errorQuat, errorAngle);

8
mission/controller/AcsController.h
View File

@ -50,12 +50,14 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
Detumble detumble;
PtgCtrl ptgCtrl;
uint8_t detumbleCounter;
uint8_t multipleRwUnavailableCounter;
ParameterHelper parameterHelper;
uint8_t detumbleCounter = 0;
uint8_t multipleRwUnavailableCounter = 0;
bool mekfInvalidFlag = true;
uint8_t mekfInvalidCounter = 0;
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
int16_t cmdDipolMtqs[3] = {0, 0, 0};
#if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0;