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

fixed GPS and STR 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 15:06:24 +02:00
parent 2a9dc518a0
commit 84fc44fd5f
9 changed files with 434 additions and 438 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
mission/controller/AcsController.cpp
View File

@@ -126,17 +126,19 @@ void AcsController::performPointingCtrl() {
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);
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.speedRw0), &(sensorValues.speedRw1),
&(sensorValues.speedRw2), &(sensorValues.speedRw3), torquePtgRws,
rwTrqNs, cmdSpeedRws);
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.speedRw0),
&(sensorValues.speedRw1), &(sensorValues.speedRw2),
&(sensorValues.speedRw3), mgtDpDes);
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);
}