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

this should work
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-11-24 11:30:27 +01:00
parent 647d5fda7c
commit c7ec9726c4
7 changed files with 94 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
mission/controller/AcsController.cpp
View File

@ -169,7 +169,7 @@ void AcsController::performAttitudeControl() {
attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData);
fusedRotationEstimation.estimateFusedRotationRate(
&susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
&attitudeEstimationData, timeDelta, &fusedRotRateData);
&attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &attitudeEstimationData, &acsParameters);
@ -377,22 +377,39 @@ void AcsController::performDetumble() {
void AcsController::performPointingCtrl() {
bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and
sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid());
acs::ControlModeStrategy ptgCtrlStrat =
ptgCtrl.pointingCtrlStrategy(mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
strValid, fusedRotRateData.rotRateTotal.isValid(), mekfEnabled,
acsParameters.strParameters.useStrForRotRate);
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;
}
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);
// }
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);
}
uint8_t enableAntiStiction = true;
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};