diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 9199e1d4..d2bd0510 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -8,6 +8,7 @@ AcsController::AcsController(object_id_t objectId) navigation(&acsParameters), actuatorCmd(&acsParameters), guidance(&acsParameters), + safeCtrl(&acsParameters), detumble(&acsParameters), ptgCtrl(&acsParameters), detumbleCounter{0}, @@ -35,7 +36,7 @@ void AcsController::performControlOperation() { if (mode != MODE_OFF) { switch (submode) { case SUBMODE_SAFE: - // performSafe(); + performSafe(); break; case SUBMODE_DETUMBLE: @@ -45,9 +46,14 @@ void AcsController::performControlOperation() { case SUBMODE_PTG_GS: performPointingCtrl(); break; + case SUBMODE_PTG_SUN: - performPointingCtrlSun(); - break; + performPointingCtrl(); + break; + + case SUBMODE_PTG_NADIR: + performPointingCtrl(); + break; } } break; @@ -75,7 +81,40 @@ void AcsController::performControlOperation() { // DEBUG END } -void AcsController::performSafe() {} +void AcsController::performSafe() { + ACS::SensorValues sensorValues; + ACS::OutputValues outputValues; + + timeval now; + Clock::getClock_timeval(&now); + + sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &outputValues, &validMekf); + // Give desired satellite rate and sun direction to align + double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; + guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); +// IF MEKF is working + double magMomMtq[3] = {0, 0, 0}; + bool magMomMtqValid = false; + if ( !validMekf ) { // Valid = 0, Failed = 1 + safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid), + (outputValues.magFieldModel), &(outputValues.magFieldModelValid), + (outputValues.sunDirModel), &(outputValues.sunDirModelValid), + (outputValues.satRateMekf), &(outputValues.satRateMekfValid), + sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid); + } + else { + + safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid, + outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid), + outputValues.magFieldEst, &(outputValues.magFieldEstValid), + outputValues.magneticFieldVectorDerivative, &(outputValues.magneticFieldVectorDerivativeValid), + sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid); + + } + +} void AcsController::performDetumble() { ACS::SensorValues sensorValues; @@ -121,7 +160,18 @@ void AcsController::performPointingCtrl() { 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); + + switch (submode) { + case SUBMODE_PTG_GS: + guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + break; + case SUBMODE_PTG_SUN: + guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate); + break; + case SUBMODE_PTG_NADIR: + guidance.targetQuatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + break; + } 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}}; @@ -132,45 +182,21 @@ void AcsController::performPointingCtrl() { 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 (acsParameters.pointingModeControllerParameters.enableAntiStiction) { + bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? + double rwSpeed[4] = {&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value)}; + ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); + } + double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input 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.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); - actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); -} - -void AcsController::performPointingCtrlSun() { - 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.sunQuatPtg(&sensorValues, &outputValues, 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.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 cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input - actuatorCmd.cmdSpeedToRws( - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws, - rwTrqNs, cmdSpeedRws); + &(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( outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf, diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index fda3f0ec..21a0c86d 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -9,6 +9,7 @@ #include "acs/Guidance.h" #include "acs/Navigation.h" #include "acs/SensorProcessing.h" +#include "acs/control/SafeCtrl.h" #include "acs/control/Detumble.h" #include "acs/control/PtgCtrl.h" #include "controllerdefinitions/AcsCtrlDefinitions.h" @@ -34,7 +35,6 @@ class AcsController : public ExtendedControllerBase { void performSafe(); void performDetumble(); void performPointingCtrl(); - void performPointingCtrlSun(); private: AcsParameters acsParameters; @@ -43,6 +43,7 @@ class AcsController : public ExtendedControllerBase { ActuatorCmd actuatorCmd; Guidance guidance; + SafeCtrl safeCtrl; Detumble detumble; PtgCtrl ptgCtrl; diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index fe8654f5..ec535b75 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -776,6 +776,9 @@ class AcsParameters /*: public HasParametersIF*/ { double rw3orientationMatrix[3][3]; double inertiaWheel = 0.000028198; double maxTrq = 0.0032; // 3.2 [mNm] + double stictionSpeed = 80; //RPM + double stictionReleaseSpeed = 120; //RPM + double stictionTorque = 0.0006; } rwHandlingParameters; struct RwMatrices { @@ -842,10 +845,12 @@ class AcsParameters /*: public HasParametersIF*/ { double desatMomentumRef[3] = {0, 0, 0}; double deSatGainFactor = 1000; bool desatOn = true; + bool enableAntiStiction = true; double omegaEarth = 0.000072921158553; - } inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; + double nadirRefDirection[3] = {-1, 0, 0}; //Camera + } pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; struct StrParameters { double exclusionAngle = 20 * M_PI / 180; diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 920989ed..00f36d24 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -23,26 +23,27 @@ ActuatorCmd::~ActuatorCmd(){ } -void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, - const int32_t *speedRw2, const int32_t *speedRw3, - const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed) { - using namespace Math; +void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) { // Scaling the commanded torque to a maximum value - double torque[4] = {0, 0, 0, 0}; double maxTrq = acsParameters.rwHandlingParameters.maxTrq; - VectorOperations::add(rwTrqIn, rwTrqNS, torque, 4); double maxValue = 0; for (int i = 0; i < 4; i++) { // size of torque, always 4 ? - if (abs(torque[i]) > maxValue) { - maxValue = abs(torque[i]); + if (abs(rwTrq[i]) > maxValue) { + maxValue = abs(rwTrq[i]); } } if (maxValue > maxTrq) { - double scalingFactor = maxTrq / maxValue; - VectorOperations::mulScalar(torque, scalingFactor, torque, 4); + double scalingFactor = maxTrq / maxValue; + VectorOperations::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4); } +} + +void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, + const int32_t *speedRw2, const int32_t *speedRw3, + const double *rwTorque, double *rwCmdSpeed) { + using namespace Math; // Calculating the commanded speed in RPM for every reaction wheel double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; @@ -52,7 +53,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1 double radToRpm = 60 / (2 * PI); // factor for conversion to RPM // W_RW = Torque_RW / I_RW * delta t [rad/s] double factor = commandTime / inertiaWheel * radToRpm; - VectorOperations::mulScalar(torque, factor, deltaSpeed, 4); + VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); VectorOperations::add(speedRws, deltaSpeed, rwCmdSpeed, 4); } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 1da2c89a..95d492c1 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -20,6 +20,14 @@ public: ActuatorCmd(AcsParameters *acsParameters_); //Input mode ? virtual ~ActuatorCmd(); + /* + * @brief: scalingTorqueRws() scales the torque via maximum part in case this part is higher + * then the maximum torque + * @param: rwTrq given torque for reaction wheels + * rwTrqScaled possible scaled torque + */ + void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled); + /* * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction wheels, also * will calculate the needed revolutions per minute for the RWs, which will be given @@ -29,8 +37,8 @@ public: * rwCmdSpeed output revolutions per minute for every reaction wheel */ void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, - const int32_t *speedRw2, const int32_t *speedRw3, const double *rwTrqIn, - const double *rwTrqNS, double *rwCmdSpeed); + const int32_t *speedRw2, const int32_t *speedRw3, const double *rwTorque, + double *rwCmdSpeed); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index ce6abf48..bd7ccf6c 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -234,6 +234,73 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou refSatRate[2] = 0; } +void Guidance::targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]) { + //------------------------------------------------------------------------------------- + // Calculation of target quaternion for Nadir pointing + //------------------------------------------------------------------------------------- + // Position of the satellite in the earth/fixed frame via GPS + double posSatE[3] = {0, 0, 0}; + double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; + double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; + MathOperations::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, + sensorValues->gpsSet.altitude.value, posSatE); + double targetDirE[3] = {0, 0, 0}; + VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); + + // Transformation between ECEF and IJK frame + double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); + MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + + double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + + // Transformation between ECEF and Body frame + double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double quatBJ[4] = {0, 0, 0, 0}; + quatBJ[0] = outputValues->quatMekfBJ[0]; + quatBJ[1] = outputValues->quatMekfBJ[1]; + quatBJ[2] = outputValues->quatMekfBJ[2]; + quatBJ[3] = outputValues->quatMekfBJ[3]; + QuaternionOperations::toDcm(quatBJ, dcmBJ); + MatrixOperations::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); + + // Target Direction in the body frame + double targetDirB[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); + + // rotation quaternion from two vectors + double refDir[3] = {0, 0, 0}; + refDir[0] = acsParameters.targetModeControllerParameters.nadirRefDirection[0]; + refDir[1] = acsParameters.targetModeControllerParameters.nadirRefDirection[1]; + refDir[2] = acsParameters.targetModeControllerParameters.nadirRefDirection[2]; + double noramlizedTargetDirB[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); + VectorOperations::normalize(refDir, refDir, 3); + double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); + double normRefDir = VectorOperations::norm(refDir, 3); + double crossDir[3] = {0, 0, 0}; + double dotDirections = VectorOperations::dot(noramlizedTargetDirB, refDir); + VectorOperations::cross(noramlizedTargetDirB, refDir, crossDir); + targetQuat[0] = crossDir[0]; + targetQuat[1] = crossDir[1]; + targetQuat[2] = crossDir[2]; + targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections); + VectorOperations::normalize(targetQuat, targetQuat, 4); + + //------------------------------------------------------------------------------------- + // Calculation of reference rotation rate + //------------------------------------------------------------------------------------- + refSatRate[0] = 0; + refSatRate[1] = 0; + refSatRate[2] = 0; + +} + void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3]) { double quatRef[4] = {0, 0, 0, 0}; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 7b05a251..eb58daf8 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -30,6 +30,10 @@ public: void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, double targetQuat[4], double refSatRate[3]); + // Function to get the target quaternion and refence rotation rate from gps position for Nadir pointing + void targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]); + // @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] ); diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index a412982d..337a8d8f 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -16,7 +16,7 @@ #include #include -PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){ +PtgCtrl::PtgCtrl(AcsParameters *acsParameters_): torqueMemory {0, 0, 0, 0}, omegaMemory {0, 0, 0, 0} { loadAcsParameters(acsParameters_); } @@ -164,3 +164,36 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); } + +void PtgCtrl::rwAntistiction(const bool* rwAvailable, const double* omegaRw, + double* torqueCommand) { + for (uint8_t i = 0; i < 4; i++) { + if (rwAvailable[i]) { + if (torqueMemory[i] != 0) { + if ((omegaRw[i] * torqueMemory[i]) + > rwHandlingParameters->stictionReleaseSpeed) { + torqueMemory[i] = 0; + } else { + torqueCommand[i] = torqueMemory[i] + * rwHandlingParameters->stictionTorque; + } + } else { + if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) + && (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { + if (omegaRw[i] < omegaMemory[i]) { + torqueMemory[i] = -1; + } else { + torqueMemory[i] = 1; + } + + torqueCommand[i] = torqueMemory[i] + * rwHandlingParameters->stictionTorque; + } + } + } else { + torqueMemory[i] = 0; + } + omegaMemory[i] = omegaRw[i]; + } + } +} diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 2c92a7e3..2a3bde5c 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -52,11 +52,22 @@ class PtgCtrl { void ptgNullspace(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 + * @param: rwAvailable Boolean Flag for all reaction wheels + * omegaRw current wheel speed of reaction wheels + * torqueCommand modified torque after antistiction + */ + void rwAntistiction(const bool* rwAvailable, const double* omegaRw, + double* torqueCommand); + private: AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters; AcsParameters::RwHandlingParameters *rwHandlingParameters; AcsParameters::InertiaEIVE *inertiaEIVE; AcsParameters::RwMatrices *rwMatrices; + + double torqueMemory[4]; + double omegaMemory[4]; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */