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

frmt
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-01-16 15:43:40 +01:00
parent 8fa1d69db3
commit 673c24a34d
1 changed files with 544 additions and 548 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
mission/controller/AcsController.cpp
View File

@ -125,19 +125,17 @@ void AcsController::performControlOperation() {
bool magMomMtqValid = false; bool magMomMtqValid = false;
if (validMekf == returnvalue::OK) { if (validMekf == returnvalue::OK) {
safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
mgmDataProcessed.magIgrfModel.isValid(), susDataProcessed.sunIjkModel.value, susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
susDataProcessed.isValid(), mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
mekfData.satRotRateMekf.isValid(), sunTargetDir, satRateSafe, &errAng, sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
magMomMtq, &magMomMtqValid);
} else { } else {
safeCtrl.safeNoMekf( safeCtrl.safeNoMekf(
now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(), now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
susDataProcessed.susVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
mgmDataProcessed.mgmVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
mgmDataProcessed.mgmVecTotDerivative.isValid(), sunTargetDir, satRateSafe, &errAng, sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
magMomMtq, &magMomMtqValid);
} }
double dipolCmdUnits[3] = {0, 0, 0}; double dipolCmdUnits[3] = {0, 0, 0};
@ -209,9 +207,9 @@ void AcsController::performControlOperation() {
&mekfData, &validMekf); &mekfData, &validMekf);
double magMomMtq[3] = {0, 0, 0}; double magMomMtq[3] = {0, 0, 0};
detumble.bDotLaw( detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
double dipolCmdUnits[3] = {0, 0, 0}; double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
@ -275,12 +273,12 @@ void AcsController::performControlOperation() {
targetQuat, refSatRate); targetQuat, refSatRate);
break; break;
case SUBMODE_PTG_TARGET: case SUBMODE_PTG_TARGET:
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
targetQuat, refSatRate); refSatRate);
break; break;
case SUBMODE_PTG_TARGET_GS: case SUBMODE_PTG_TARGET_GS:
guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
now, targetQuat, refSatRate); targetQuat, refSatRate);
break; break;
case SUBMODE_PTG_NADIR: case SUBMODE_PTG_NADIR:
guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
@ -313,8 +311,7 @@ void AcsController::performControlOperation() {
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
} }
double cmdSpeedRws[4] = {0, 0, 0, double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
@ -528,8 +525,7 @@ void AcsController::performControlOperation() {
3 * sizeof(float)); 3 * sizeof(float));
mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid()); mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid());
mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value; mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value;
mgmDataRaw.actuationCalStatus.setValid( mgmDataRaw.actuationCalStatus.setValid(sensorValues.imtqMgmSet.coilActuationStatus.isValid());
sensorValues.imtqMgmSet.coilActuationStatus.isValid());
} }
} }
} }