diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 392e32ba..9336fa8a 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include "../util/MathOperations.h" @@ -70,89 +71,72 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, return returnvalue::OK; } -// Will be the version in worst case scenario in event of no working MEKF (nor GYRs) -ReturnValue_t 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 *outputAngle, - double *outputMagMomB) { - // Check for invalid Inputs - if (!susDirBValid || !magFieldBValid || !magRateBValid) { +// Will be the version in worst case scenario in event of no working MEKF +ReturnValue_t SafeCtrl::safeNoMekf(const double *magneticFieldVector, + const double *magneticFieldVectorDerivative, + const double *sunVector, const double *sunvectorDerivative, + double omegaRef, double *torqueCommand, double *spinAxis) { + if (0) { return returnvalue::FAILED; } - // change unit from uT to T - double magFieldBT[3] = {0, 0, 0}; - VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); + double magneticFieldVectorT[3] = {0, 0, 0}; + VectorOperations::mulScalar(magneticFieldVector, 1e-6, magneticFieldVectorT, 3); - // normalize sunDir and magDir - double magDirB[3] = {0, 0, 0}; - VectorOperations::normalize(magFieldBT, magDirB, 3); - VectorOperations::normalize(susDirB, susDirB, 3); + double commandParallel[3] = {0, 0, 0}, commandAlign[3] = {0, 0, 0}, commandOrtho[3] = {0, 0, 0}; - // Cosinus angle between sunDir and magDir - double cosAngleSunMag = VectorOperations::dot(magDirB, susDirB); + bool valid; + double omega = estimateRotationAroundSun(magneticFieldVector, magneticFieldVectorDerivative, + sunVector, &valid); - // Rate parallel to sun direction and magnetic field direction - double dotSunRateMag = VectorOperations::dot(sunRateB, magDirB); - double dotmagRateSun = VectorOperations::dot(magRateB, susDirB); - double rateFactor = 1 - pow(cosAngleSunMag, 2); - double rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor; - double 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 angleSunMag = acos(cosAngleSunMag); - if (angleSunMag < acsParameters->safeModeControllerParameters.sunMagAngleMin) { - return returnvalue::FAILED; + if (valid) { + VectorOperations::mulScalar( + sunVector, + acsParameters->safeModeControllerParameters.k_parallel_no_mekf * (omegaRef - omega), + commandParallel, 3); + omega = omega * sign(omega); } - // Rate for Torque Calculation - double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */ - VectorOperations::subtract(estSatRate, satRateRef, diffRate, 3); + VectorOperations::cross(spinAxis, sunVector, commandAlign); - // Torque Align calculation - double kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf; - double kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf; + VectorOperations::mulScalar( + commandAlign, acsParameters->safeModeControllerParameters.k_align_no_mekf, commandAlign, 3); - double cosAngleAlignErr = VectorOperations::dot(sunDirRef, susDirB); - double crossSusSunRef[3] = {0, 0, 0}; - VectorOperations::cross(sunDirRef, susDirB, crossSusSunRef); - double sinAngleAlignErr = VectorOperations::norm(crossSusSunRef, 3); + VectorOperations::cross(sunvectorDerivative, sunVector, commandOrtho); + VectorOperations::mulScalar( + commandOrtho, -acsParameters->safeModeControllerParameters.k_ortho_no_mekf, commandOrtho, 3); - double torqueAlign[3] = {0, 0, 0}; - double angleAlignErr = acos(cosAngleAlignErr); - double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr; - VectorOperations::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3); + // only spin up when the angle to the sun is less than a certain angle. + // note that we check the cosin, thus "<" + if (VectorOperations::dot(spinAxis, sunVector) < + acsParameters->safeModeControllerParameters.cosineStartSpin) { + VectorOperations::mulScalar(commandParallel, 0, commandParallel, 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(*(acsParameters->inertiaEIVE.inertiaMatrix), torqueAlignRate, - torqueB, 3, 3, 1); - - // Magnetic moment - double magMomB[3] = {0, 0, 0}; - double crossMagFieldTorque[3] = {0, 0, 0}; - VectorOperations::cross(magFieldBT, torqueB, crossMagFieldTorque); - double magMomFactor = pow(VectorOperations::norm(magFieldBT, 3), 2); - VectorOperations::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3); - - std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double)); - *outputAngle = angleAlignErr; + for (uint8_t i = 0; i < 3; i++) { + torqueCommand[i] = commandAlign[i] + commandOrtho[i] + commandParallel[i]; + } return returnvalue::OK; } + +double SafeCtrl::estimateRotationAroundSun(const double *magneticFieldVector, + const double *magneticFieldVectorDerivative, + const double *sunVector, bool *updated) { + *updated = true; + double vector[3]; + VectorOperations::cross(magneticFieldVector, sunVector, vector); + + float magLength = VectorOperations::norm(magneticFieldVector, 3); + float length = VectorOperations::norm(vector, 3); + + // only check if angle between B and sun is large enough + if (length > (acsParameters->safeModeControllerParameters.sineCalculateOmegaSun * magLength)) { + float omega = VectorOperations::dot(magneticFieldVectorDerivative, vector); + omega = omega / (length * length); + lastCalculatedOmega = omega; + return omega; + } else { + *updated = false; + return lastCalculatedOmega; + } +} diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index ac5a754a..83cad8e0 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -23,10 +23,14 @@ class SafeCtrl { double *satRatRef, // From Guidance (!) double *outputAngle, double *outputMagMomB); - ReturnValue_t safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, - bool sunRateBValid, double *magFieldB, bool magFieldBValid, - double *magRateB, bool magRateBValid, double *sunDirRef, - double *satRateRef, double *outputAngle, double *outputMagMomB); + ReturnValue_t safeNoMekf(const double *magneticFieldVector, + const double *magneticFieldVectorDerivative, const double *sunVector, + const double *sunvectorDerivative, double omegaRef, + double *torqueCommand, double *spinAxis); + + double estimateRotationAroundSun(const double *magneticFieldVector, + const double *magneticFieldVectorDerivative, + const double *sunvector, bool *updated); protected: private: @@ -35,6 +39,8 @@ class SafeCtrl { double magFieldBState[3]; timeval magFieldBStateTime; + + float lastCalculatedOmega = 0.0; }; #endif /* ACS_CONTROL_SAFECTRL_H_ */