diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp new file mode 100644 index 00000000..55772ae4 --- /dev/null +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -0,0 +1,186 @@ +/* + * SafeCtrl.cpp + * + * Created on: 19 Apr 2022 + * Author: Robin Marquardt + */ + +#include "SafeCtrl.h" +#include +#include +#include +#include +#include +#include + +SafeCtrl::SafeCtrl(AcsParameters *acsParameters_){ + loadAcsParameters(acsParameters_); + MatrixOperations::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3, 3); +} + +SafeCtrl::~SafeCtrl(){ + +} + +void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_){ + safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters); + inertiaEIVE = &(acsParameters_->inertiaEIVE); +} + +ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid, + double *magFieldModel, bool *magFieldModelValid, + double *sunDirModel, bool *sunDirModelValid, + double *satRateMekf, bool *rateMekfValid, + double *sunDirRef, double *satRatRef, + double *outputMagMomB, bool *outputValid){ + + if ( !(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) || + !(*rateMekfValid)) { + *outputValid = false; + return SAFECTRL_MEKF_INPUT_INVALID; + } + + double kRate = 0, kAlign = 0; + kRate = safeModeControllerParameters->k_rate_mekf; + kAlign = safeModeControllerParameters->k_align_mekf; + +// Calc sunDirB ,magFieldB with mekf output and model + double dcmBJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; + MathOperations::dcmFromQuat(quatBJ, *dcmBJ); + double sunDirB[3] = {0,0,0}, magFieldB[3] = {0,0,0}; + MatrixOperations::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1); + MatrixOperations::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1); + + double crossSun[3] = {0, 0, 0}; + + VectorOperations::cross(sunDirRef, sunDirB, crossSun); + double normCrossSun = VectorOperations::norm(crossSun, 3); + +// calc angle alpha between sunDirRef and sunDIr + double alpha = 0, dotSun = 0; + dotSun = VectorOperations::dot(sunDirRef, sunDirB); + alpha = acos(dotSun); + +// Law Torque calculations + double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, + torqueRate[3] = {0, 0, 0}, torqueAll[3] = {0, 0, 0}; + + double scalarFac = 0; + scalarFac = kAlign * alpha / normCrossSun; + VectorOperations::mulScalar(crossSun, scalarFac, torqueAlign, 3); + + double rateSafeMode[3] = {0,0,0}; + VectorOperations::subtract(satRateMekf, satRatRef, rateSafeMode, 3); + VectorOperations::mulScalar(rateSafeMode, -kRate, torqueRate, 3); + + VectorOperations::add(torqueRate, torqueAlign, torqueAll, 3); +// Adding factor of inertia for axes + MatrixOperations::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1); + +// MagMom B (orthogonal torque) + double torqueMgt[3] = {0,0,0}; + VectorOperations::cross(magFieldB, torqueCmd, torqueMgt); + double normMag = VectorOperations::norm(magFieldB, 3); + VectorOperations::mulScalar(torqueMgt, 1/pow(normMag,2), outputMagMomB, 3); + *outputValid = true; + + return RETURN_OK; + +} + +// Will be the version in worst case scenario in event of no working MEKF (nor RMUs) +void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, + double *sunRateB, bool *sunRateBValid, + double *magFieldB, bool *magFieldBValid, + double *magRateB, bool *magRateBValid, + double *sunDirRef, double *satRateRef, + double *outputMagMomB, bool *outputValid){ + +// Check for invalid Inputs + if ( !susDirBValid || !magFieldBValid || !magRateBValid) { + *outputValid = false; + return; + } + +// normalize sunDir and magDir + double magDirB[3] = {0, 0, 0}; + VectorOperations::normalize(magFieldB, magDirB, 3); + VectorOperations::normalize(susDirB, susDirB, 3); + +// Cosinus angle between sunDir and magDir + double cosAngleSunMag = VectorOperations::dot(magDirB, susDirB); + +// Rate parallel to sun direction and magnetic field direction + double rateParaSun = 0, rateParaMag = 0; + double dotSunRateMag = 0, dotmagRateSun = 0, + rateFactor = 0; + dotSunRateMag = VectorOperations::dot(sunRateB, magDirB); + dotmagRateSun = VectorOperations::dot(magRateB, susDirB); + rateFactor = 1 - pow(cosAngleSunMag,2); + rateParaSun = ( dotmagRateSun + cosAngleSunMag * dotSunRateMag ) / rateFactor; + rateParaMag = ( dotSunRateMag + cosAngleSunMag * dotmagRateSun ) / rateFactor; + +// Full rate or estimate + double estSatRate[3] = {0, 0, 0}; + double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0}; + VectorOperations::mulScalar(susDirB, rateParaSun, estSatRateSun, 3); + VectorOperations::add(sunRateB, estSatRateSun, estSatRateSun, 3); + VectorOperations::mulScalar(magDirB, rateParaMag, estSatRateMag, 3); + VectorOperations::add(magRateB, estSatRateMag, estSatRateMag, 3); + VectorOperations::add(estSatRateSun, estSatRateMag, estSatRate, 3); + VectorOperations::mulScalar(estSatRate, 0.5, estSatRate, 3); + +/* Only valid if angle between sun direction and magnetic field direction + is sufficiently large */ + + double sinAngle = 0; + sinAngle = sin(acos(cos(cosAngleSunMag))); + + if ( !(sinAngle > sin( safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) { + return; + } + +// Rate for Torque Calculation + double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */ + VectorOperations::subtract(estSatRate, satRateRef, diffRate, 3); + +// Torque Align calculation + double kRateNoMekf = 0, kAlignNoMekf = 0; + kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; + kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; + + double cosAngleAlignErr = VectorOperations::dot(sunDirRef, susDirB); + double crossSusSunRef[3] = {0, 0, 0}; + VectorOperations::cross(sunDirRef, susDirB, crossSusSunRef); + double sinAngleAlignErr = VectorOperations::norm(crossSusSunRef, 3); + + double torqueAlign[3] = {0, 0, 0}; + double angleAlignErr = acos(cosAngleAlignErr); + double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr; + VectorOperations::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3); + +//Torque Rate Calculations + double torqueRate[3] = {0, 0, 0}; + VectorOperations::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3); + +//Final torque + double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0}; + VectorOperations::add(torqueRate, torqueAlign, torqueAlignRate, 3); + MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, 1); + +//Magnetic moment + double magMomB[3] = {0, 0, 0}; + double crossMagFieldTorque[3] = {0, 0, 0}; + VectorOperations::cross(magFieldB, torqueB, crossMagFieldTorque); + double magMomFactor = pow( VectorOperations::norm(magFieldB, 3), 2 ); + VectorOperations::mulScalar(crossMagFieldTorque, 1/magMomFactor, magMomB, 3); + + outputMagMomB[0] = magMomB[0]; + outputMagMomB[1] = magMomB[1]; + outputMagMomB[2] = magMomB[2]; + + *outputValid = true; + +} + + diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h new file mode 100644 index 00000000..b0ce6a90 --- /dev/null +++ b/mission/controller/acs/control/SafeCtrl.h @@ -0,0 +1,63 @@ +/* + * safeCtrl.h + * + * Created on: 19 Apr 2022 + * Author: rooob + */ + +#ifndef SAFECTRL_H_ +#define SAFECTRL_H_ + +#include +#include +#include +#include +#include +#include "acs/config/classIds.h" +#include +#include + +class SafeCtrl : public HasReturnvaluesIF { + +public: + + SafeCtrl(AcsParameters *acsParameters_); + virtual ~SafeCtrl(); + + static const uint8_t INTERFACE_ID = CLASS_ID::SAFE; + static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); + + void loadAcsParameters(AcsParameters *acsParameters_); + + ReturnValue_t safeMekf(timeval now, double *quatBJ, bool *quatBJValid, + double *magFieldModel, bool *magFieldModelValid, + double *sunDirModel, bool *sunDirModelValid, + double *satRateMekf, bool *rateMekfValid, + double *sunDirRef, double *satRatRef, // From Guidance (!) + double *outputMagMomB, bool *outputValid); + + void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, + double *sunRateB, bool *sunRateBValid, + double *magFieldB, bool *magFieldBValid, + double *magRateB, bool *magRateBValid, + double *sunDirRef, double *satRateRef, + double *outputMagMomB, bool *outputValid); + + void idleSunPointing(); // with reaction wheels + +protected: + +private: + AcsParameters::SafeModeControllerParameters* safeModeControllerParameters; + AcsParameters::InertiaEIVE* inertiaEIVE; + double gainMatrixInertia[3][3]; + + double magFieldBState[3]; + timeval magFieldBStateTime; + +}; + + + +#endif /* ACS_CONTROL_SAFECTRL_H_ */ +