eive-obsw/mission/controller/acs/control/SafeCtrl.cpp

178 lines
7.3 KiB
C++

/*
* SafeCtrl.cpp
*
* Created on: 19 Apr 2022
* Author: Robin Marquardt
*/
#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_) {
loadAcsParameters(acsParameters_);
MatrixOperations<double>::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<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);
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 alpha = 0, dotSun = 0;
dotSun = VectorOperations<double>::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<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>::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);
*outputValid = true;
return returnvalue::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<double>::normalize(magFieldB, 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 rateParaSun = 0, rateParaMag = 0;
double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0;
dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
dotmagRateSun = VectorOperations<double>::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<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 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<double>::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<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(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3,
1);
// Magnetic moment
double magMomB[3] = {0, 0, 0};
double crossMagFieldTorque[3] = {0, 0, 0};
VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque);
double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2);
VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
outputMagMomB[0] = magMomB[0];
outputMagMomB[1] = magMomB[1];
outputMagMomB[2] = magMomB[2];
*outputValid = true;
}