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

added Antistiction, added Nadir Pointing, added performSafe()
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source
This commit is contained in:
Robin Marquardt
2022-11-08 13:48:50 +01:00
parent 75ab11fc35
commit 20936faec6
9 changed files with 213 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
mission/controller/AcsController.cpp
View File

@ -8,6 +8,7 @@ AcsController::AcsController(object_id_t objectId)
navigation(&acsParameters),
actuatorCmd(&acsParameters),
guidance(&acsParameters),
safeCtrl(&acsParameters),
detumble(&acsParameters),
ptgCtrl(&acsParameters),
detumbleCounter{0},
@ -35,7 +36,7 @@ void AcsController::performControlOperation() {
if (mode != MODE_OFF) {
switch (submode) {
case SUBMODE_SAFE:
// performSafe();
performSafe();
break;
case SUBMODE_DETUMBLE:
@ -45,9 +46,14 @@ void AcsController::performControlOperation() {
case SUBMODE_PTG_GS:
performPointingCtrl();
break;
case SUBMODE_PTG_SUN:
performPointingCtrlSun();
break;
performPointingCtrl();
break;
case SUBMODE_PTG_NADIR:
performPointingCtrl();
break;
}
}
break;
@ -75,7 +81,40 @@ void AcsController::performControlOperation() {
// DEBUG END
}
void AcsController::performSafe() {}
void AcsController::performSafe() {
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now;
Clock::getClock_timeval(&now);
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
// Give desired satellite rate and sun direction to align
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// IF MEKF is working
double magMomMtq[3] = {0, 0, 0};
bool magMomMtqValid = false;
if ( !validMekf ) { // Valid = 0, Failed = 1
safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid),
(outputValues.magFieldModel), &(outputValues.magFieldModelValid),
(outputValues.sunDirModel), &(outputValues.sunDirModelValid),
(outputValues.satRateMekf), &(outputValues.satRateMekfValid),
sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid);
}
else {
safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid,
outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid),
outputValues.magFieldEst, &(outputValues.magFieldEstValid),
outputValues.magneticFieldVectorDerivative, &(outputValues.magneticFieldVectorDerivativeValid),
sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid);
}
}
void AcsController::performDetumble() {
ACS::SensorValues sensorValues;
@ -121,7 +160,18 @@ void AcsController::performPointingCtrl() {
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
switch (submode) {
case SUBMODE_PTG_GS:
guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
break;
case SUBMODE_PTG_SUN:
guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate);
break;
case SUBMODE_PTG_NADIR:
guidance.targetQuatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
break;
}
double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate);
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -132,45 +182,21 @@ void AcsController::performPointingCtrl() {
ptgCtrl.ptgNullspace(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
if (acsParameters.pointingModeControllerParameters.enableAntiStiction) {
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
double rwSpeed[4] = {&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value)};
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
}
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
rwTrqNs, cmdSpeedRws);
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
ptgCtrl.ptgDesaturation(
outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
}
void AcsController::performPointingCtrlSun() {
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now; // Übergabe ?
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate);
double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate);
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
double rwTrqNs[4] = {0, 0, 0, 0};
ptgCtrl.ptgNullspace(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
rwTrqNs, cmdSpeedRws);
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
ptgCtrl.ptgDesaturation(
outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,