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

added PtgCtrl code. matched inputs to actual SensorValues inputs
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2022-10-12 10:28:44 +02:00
parent 7be6bbc948
commit 4aecd07970
3 changed files with 75 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
mission/controller/AcsController.cpp
View File

@@ -1,14 +1,15 @@
#include "AcsController.h"
#include <fsfw/datapool/PoolReadGuard.h>
//#include <fsfw/timemanager/Clock.h>
AcsController::AcsController(object_id_t objectId)
: ExtendedControllerBase(objectId, objects::NO_OBJECT),
sensorProcessing(&acsParameters),
navigation(&acsParameters),
actuatorCmd(&acsParameters),
guidance(&acsParameters),
detumble(&acsParameters),
ptgCtrl(&acsParameters),
detumbleCounter{0},
mgmData(this),
susData(this) {}
@@ -42,7 +43,7 @@ void AcsController::performControlOperation() {
break;
case SUBMODE_PTG_GS:
// performPointingCtrl();
performPointingCtrl();
break;
}
}
@@ -65,7 +66,6 @@ void AcsController::performControlOperation() {
}
}
// DEBUG : REMOVE AFTER COMPLETION
mode = MODE_ON;
submode = SUBMODE_DETUMBLE;
@@ -81,7 +81,7 @@ void AcsController::performDetumble() {
timeval now;
Clock::getClock_timeval(&now);
sensorProcessing.process(&susData, now, &sensorValues, &outputValues, &acsParameters);
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
double magMomMtq[3] = {0, 0, 0};
@@ -108,7 +108,37 @@ void AcsController::performDetumble() {
}
}
void AcsController::performPointingCtrl() {}
void AcsController::performPointingCtrl() {
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.targetQuatPtg(&sensorValues, &outputValues, now, 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.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
double rwTrqNs[4] = {0, 0, 0, 0};
ptgCtrl.ptgNullspace(&(sensorValues.speedRw0), &(sensorValues.speedRw1), &(sensorValues.speedRw2),
&(sensorValues.speedRw3), rwTrqNs);
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws(&(sensorValues.speedRw0), &(sensorValues.speedRw1),
&(sensorValues.speedRw2), &(sensorValues.speedRw3), 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.speedRw0),
&(sensorValues.speedRw1), &(sensorValues.speedRw2),
&(sensorValues.speedRw3), mgtDpDes);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
}
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {