eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 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
Download Patch File Download Diff File Expand all files Collapse all files

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);