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

changed AcsParameters instance in guidance to const pointer
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-02-27 17:08:48 +01:00
parent 309358f447
commit 5c30adb9bb
4 changed files with 51 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
mission/controller/AcsController.cpp
View File

@ -6,9 +6,6 @@
AcsController::AcsController(object_id_t objectId)
: ExtendedControllerBase(objectId),
sensorProcessing(&acsParameters),
navigation(&acsParameters),
actuatorCmd(&acsParameters),
guidance(&acsParameters),
safeCtrl(&acsParameters),
detumble(&acsParameters),
@ -142,7 +139,7 @@ void AcsController::performSafe() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
@ -173,7 +170,7 @@ void AcsController::performSafe() {
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
}
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, &acsParameters.magnetorquerParameter);
// detumble check and switch
if (mekfData.satRotRateMekf.isValid() &&
@ -207,7 +204,7 @@ void AcsController::performDetumble() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
@ -221,7 +218,7 @@ void AcsController::performDetumble() {
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, &acsParameters.magnetorquerParameter);
if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@ -254,7 +251,7 @@ void AcsController::performPointingCtrl() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
@ -303,7 +300,7 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters);
ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -327,7 +324,7 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -348,7 +345,7 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -372,7 +369,7 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters);
ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -395,7 +392,7 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters);
ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -409,11 +406,11 @@ void AcsController::performPointingCtrl() {
ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
}
actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
actuatorCmd.cmdSpeedToRws(
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRwsScaled,
cmdSpeedRws, &acsParameters);
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, &acsParameters.magnetorquerParameter);
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs);