159 lines
7.2 KiB
C++
159 lines
7.2 KiB
C++
#include "SafeCtrl.h"
|
|
|
|
#include <fsfw/globalfunctions/constants.h>
|
|
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
|
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
#include <math.h>
|
|
|
|
#include "../util/MathOperations.h"
|
|
|
|
SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
|
|
|
|
SafeCtrl::~SafeCtrl() {}
|
|
|
|
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 *outputAngle, double *outputMagMomB) {
|
|
if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
|
|
return SAFECTRL_MEKF_INPUT_INVALID;
|
|
}
|
|
|
|
double kRate = acsParameters->safeModeControllerParameters.k_rate_mekf;
|
|
double kAlign = acsParameters->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<double>::dcmFromQuat(quatBJ, *dcmBJ);
|
|
double sunDirB[3] = {0, 0, 0}, magFieldB[3] = {0, 0, 0};
|
|
MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
|
|
MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
|
|
|
|
// change unit from uT to T
|
|
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);
|
|
|
|
double crossSun[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
|
|
double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
|
|
|
|
// calc angle alpha between sunDirRef and sunDIr
|
|
double dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
|
|
double 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 = kAlign * alpha / normCrossSun;
|
|
VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
|
|
|
|
double rateSafeMode[3] = {0, 0, 0};
|
|
VectorOperations<double>::subtract(satRateMekf, satRatRef, rateSafeMode, 3);
|
|
VectorOperations<double>::mulScalar(rateSafeMode, -kRate, torqueRate, 3);
|
|
|
|
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
|
|
// Adding factor of inertia for axes
|
|
MatrixOperations<double>::multiplyScalar(*(acsParameters->inertiaEIVE.inertiaMatrix), 10,
|
|
*gainMatrixInertia, 3,
|
|
3); // why only for mekf one and not for no mekf
|
|
MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
|
|
|
|
// MagMom B (orthogonal torque)
|
|
double torqueMgt[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
|
|
double normMag = VectorOperations<double>::norm(magFieldB, 3);
|
|
VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
|
|
|
|
*outputAngle = alpha;
|
|
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) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
// change unit from uT to T
|
|
double magFieldBT[3] = {0, 0, 0};
|
|
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
|
|
|
|
// normalize sunDir and magDir
|
|
double magDirB[3] = {0, 0, 0};
|
|
VectorOperations<double>::normalize(magFieldBT, magDirB, 3);
|
|
VectorOperations<double>::normalize(susDirB, susDirB, 3);
|
|
|
|
// Cosinus angle between sunDir and magDir
|
|
double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
|
|
|
|
// Rate parallel to sun direction and magnetic field direction
|
|
double dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
|
|
double dotmagRateSun = VectorOperations<double>::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<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
|
|
VectorOperations<double>::add(sunRateB, estSatRateSun, estSatRateSun, 3);
|
|
VectorOperations<double>::mulScalar(magDirB, rateParaMag, estSatRateMag, 3);
|
|
VectorOperations<double>::add(magRateB, estSatRateMag, estSatRateMag, 3);
|
|
VectorOperations<double>::add(estSatRateSun, estSatRateMag, estSatRate, 3);
|
|
VectorOperations<double>::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;
|
|
}
|
|
|
|
// Rate for Torque Calculation
|
|
double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */
|
|
VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
|
|
|
|
// Torque Align calculation
|
|
double kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf;
|
|
double kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf;
|
|
|
|
double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
|
|
double crossSusSunRef[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(sunDirRef, susDirB, crossSusSunRef);
|
|
double sinAngleAlignErr = VectorOperations<double>::norm(crossSusSunRef, 3);
|
|
|
|
double torqueAlign[3] = {0, 0, 0};
|
|
double angleAlignErr = acos(cosAngleAlignErr);
|
|
double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr;
|
|
VectorOperations<double>::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3);
|
|
|
|
// Torque Rate Calculations
|
|
double torqueRate[3] = {0, 0, 0};
|
|
VectorOperations<double>::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3);
|
|
|
|
// Final torque
|
|
double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
|
|
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
|
|
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), torqueAlignRate,
|
|
torqueB, 3, 3, 1);
|
|
|
|
// Magnetic moment
|
|
double magMomB[3] = {0, 0, 0};
|
|
double crossMagFieldTorque[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(magFieldBT, torqueB, crossMagFieldTorque);
|
|
double magMomFactor = pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
|
|
VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
|
|
|
|
std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
|
|
*outputAngle = angleAlignErr;
|
|
return returnvalue::OK;
|
|
}
|