From 44b384cd17d20be0d1168d26905e5a0ffc9a761c Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Tue, 17 Jan 2023 20:11:45 +0100 Subject: [PATCH] access to different paramset for every pointing mode --- mission/controller/AcsController.cpp | 89 +++++++++++++++++----- mission/controller/acs/control/PtgCtrl.cpp | 8 +- mission/controller/acs/control/PtgCtrl.h | 11 ++- 3 files changed, 76 insertions(+), 32 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index dee2fb73..f829c3e3 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -271,53 +271,106 @@ void AcsController::performControlOperation() { double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; double quatRef[4] = {0, 0, 0, 0}; uint8_t enableAntiStiction = true; + + double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, + deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed + 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}, rwTrqNs[4] = {0, 0, 0, 0}; + double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; + double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol + switch (submode) { case SUBMODE_IDLE: guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, + mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); break; + case SUBMODE_PTG_TARGET: guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; - + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, + mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); break; + case SUBMODE_PTG_TARGET_GS: guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, + mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); break; + case SUBMODE_PTG_NADIR: guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); + ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation(&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, + mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); break; + case SUBMODE_PTG_INERTIAL: guidance.inertialQuatPtg(targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); + ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation(&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, + mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); break; } - double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, - deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, 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 torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; - VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); if ( enableAntiStiction ) { bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? @@ -333,12 +386,6 @@ void AcsController::performControlOperation() { &(sensorValues.rw2Set.currSpeed.value), &(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(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), - mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 53544b8c..6d41e85d 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -21,14 +21,12 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameter PtgCtrl::~PtgCtrl() {} void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { - // TODO: Here correct Parameters have to be loaded according to current submode - pointingLawParameters = &(acsParameters_->targetModeControllerParameters); inertiaEIVE = &(acsParameters_->inertiaEIVE); rwHandlingParameters = &(acsParameters_->rwHandlingParameters); rwMatrices = &(acsParameters_->rwMatrices); } -void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *deltaRate, +void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameters, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws) { //------------------------------------------------------------------------------------------------ // Compute gain matrix K and P matrix @@ -108,7 +106,7 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); } -void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, +void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLawParameters, double *magFieldEst, bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes) { if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) { @@ -139,7 +137,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); } -void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, +void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters * pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double wheelMomentum[4] = {0, 0, 0, 0}; diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 85f13a7a..b036d180 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -41,14 +41,14 @@ class PtgCtrl { /* @brief: Calculates the needed torque for the pointing control mechanism * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ - void ptgLaw(const double mode, const double *qError, const double *deltaRate, + void ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameters, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws); - void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, - int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, - double *mgtDpDes); + void ptgDesaturation(AcsParameters::PointingLawParameters * pointingLawParameters, double *magFieldEst, + bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, + int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes); - void ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, + void ptgNullspace(AcsParameters::PointingLawParameters * pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs); /* @brief: Commands the stiction torque in case wheel speed is to low @@ -59,7 +59,6 @@ class PtgCtrl { void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand); private: - AcsParameters::PointingLawParameters *pointingLawParameters; AcsParameters::RwHandlingParameters *rwHandlingParameters; AcsParameters::InertiaEIVE *inertiaEIVE; AcsParameters::RwMatrices *rwMatrices;