diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index e662948b..84a7682f 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -1,14 +1,15 @@ #include "AcsController.h" #include -//#include 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) { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index fa4ba88e..c23adfc1 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -6,9 +6,11 @@ #include #include "acs/ActuatorCmd.h" +#include "acs/Guidance.h" #include "acs/Navigation.h" #include "acs/SensorProcessing.h" #include "acs/control/Detumble.h" +#include "acs/control/PtgCtrl.h" #include "controllerdefinitions/AcsCtrlDefinitions.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" @@ -37,8 +39,11 @@ class AcsController : public ExtendedControllerBase { SensorProcessing sensorProcessing; Navigation navigation; ActuatorCmd actuatorCmd; + Guidance guidance; Detumble detumble; + PtgCtrl ptgCtrl; + uint8_t detumbleCounter; enum class InternalState { STARTUP, INITIAL_DELAY, READY }; diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index a2e3aa5d..deb50aa1 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -6,45 +6,46 @@ */ #include "Navigation.h" -#include "util/MathOperations.h" -#include "util/CholeskyDecomposition.h" -#include -#include + #include #include +#include +#include +#include "util/CholeskyDecomposition.h" +#include "util/MathOperations.h" -Navigation::Navigation(AcsParameters *acsParameters_): multiplicativeKalmanFilter(acsParameters_){ - acsParameters = *acsParameters_; +Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilter(acsParameters_) { + acsParameters = *acsParameters_; } -Navigation::~Navigation(){ +Navigation::~Navigation() {} +void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + ReturnValue_t *mekfValid) { + double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, + sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; + bool quatJBValid = + (sensorValues->strSet.caliQx.isValid() && sensorValues->strSet.caliQy.isValid() && + sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid()); + + if (kalmanInit) { + *mekfValid = multiplicativeKalmanFilter.mekfEst( + quatJB, &quatJBValid, outputValues->satRateEst, &outputValues->satRateEstValid, + outputValues->magFieldEst, &outputValues->magFieldEstValid, outputValues->sunDirEst, + &outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid, + outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ, + outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ?? + } + else { + multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid, + outputValues->sunDirEst, &outputValues->sunDirEstValid, + outputValues->sunDirModel, &outputValues->sunDirModelValid, + outputValues->magFieldModel, &outputValues->magFieldModelValid); + kalmanInit = true; + *mekfValid = 0; + + // Maybe we need feedback from kalmanfilter to identify if there was a problem with the init + //of kalman filter where does this class know from that kalman filter was not initialized ? + } } - -void Navigation::useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid){ - - if (kalmanInit) { - *mekfValid = multiplicativeKalmanFilter.mekfEst(sensorValues->quatJB, &sensorValues->quatJBValid, - outputValues->satRateEst, &outputValues->satRateEstValid, - outputValues->magFieldEst, &outputValues->magFieldEstValid, - outputValues->sunDirEst, &outputValues->sunDirEstValid, - outputValues->sunDirModel, &outputValues->sunDirModelValid, - outputValues->magFieldModel, &outputValues->magFieldModelValid, - outputValues->quatMekfBJ, outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ?? - - } - else { - multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid, - outputValues->sunDirEst, &outputValues->sunDirEstValid, - outputValues->sunDirModel, &outputValues->sunDirModelValid, - outputValues->magFieldModel, &outputValues->magFieldModelValid); - kalmanInit = true; - *mekfValid = 0; - -// Maybe we need feedback from kalmanfilter to identify if there was a problem with the init of kalman filter -// where does this class know from that kalman filter was not initialized ? - } -} - -