152 lines
7.0 KiB
C++
152 lines
7.0 KiB
C++
#include "SafeCtrl.h"
|
|
|
|
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
|
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
#include <math.h>
|
|
|
|
SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
|
|
|
|
SafeCtrl::~SafeCtrl() {}
|
|
|
|
uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t mekfValid,
|
|
const bool satRotRateValid, const bool sunDirValid) {
|
|
if (not magFieldValid) {
|
|
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
|
|
} else if (mekfValid) {
|
|
return acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF;
|
|
} else if (satRotRateValid and sunDirValid) {
|
|
return acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF;
|
|
} else if (satRotRateValid and not sunDirValid) {
|
|
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING;
|
|
} else {
|
|
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
|
|
}
|
|
}
|
|
|
|
void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
|
|
const double *sunDirModelI, const double *quatBI, const double *sunDirRefB,
|
|
const double satRotRateRef, const double inertiaMatrix[3][3],
|
|
double *magMomB, double &errorAngle) {
|
|
// convert magFieldB from uT to T
|
|
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
|
|
|
|
// convert sunDirModel to body rf
|
|
double sunDirB[3] = {0, 0, 0};
|
|
QuaternionOperations::multiplyVector(quatBI, sunDirModelI, sunDirB);
|
|
|
|
// calculate angle alpha between sunDirRef and sunDir
|
|
double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
|
|
errorAngle = acos(dotSun);
|
|
|
|
splitRotationalRate(satRotRateB, sunDirB);
|
|
calculateRotationalRateTorque(satRotRateRef, sunDirB, sunDirRefB, errorAngle,
|
|
acsParameters->safeModeControllerParameters.k_parallelMekf,
|
|
acsParameters->safeModeControllerParameters.k_orthoMekf);
|
|
calculateAngleErrorTorque(sunDirB, sunDirRefB,
|
|
acsParameters->safeModeControllerParameters.k_alignMekf, inertiaMatrix);
|
|
|
|
// sum of all torques
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
|
|
}
|
|
|
|
calculateMagneticMoment(magMomB);
|
|
}
|
|
|
|
void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
|
|
const double *sunDirB, const double *sunDirRefB,
|
|
const double satRotRateRef, const double inertiaMatrix[3][3],
|
|
double *magMomB, double &errorAngle) {
|
|
// convert magFieldB from uT to T
|
|
double magFieldBT[3] = {0, 0, 0};
|
|
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
|
|
|
|
// calculate error angle between sunDirRef and sunDir
|
|
double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
|
|
errorAngle = acos(dotSun);
|
|
|
|
splitRotationalRate(satRotRateB, sunDirB);
|
|
calculateRotationalRateTorque(satRotRateRef, sunDirB, sunDirRefB, errorAngle,
|
|
acsParameters->safeModeControllerParameters.k_parallelNonMekf,
|
|
acsParameters->safeModeControllerParameters.k_orthoNonMekf);
|
|
calculateAngleErrorTorque(sunDirB, sunDirRefB,
|
|
acsParameters->safeModeControllerParameters.k_alignNonMekf,
|
|
inertiaMatrix);
|
|
|
|
// sum of all torques
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
|
|
}
|
|
|
|
calculateMagneticMoment(magMomB);
|
|
}
|
|
|
|
void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB,
|
|
const double satRotRateRef, const double *sunDirRefB,
|
|
double *magMomB, double &errorAngle) {
|
|
// convert magFieldB from uT to T
|
|
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
|
|
|
|
// no error angle available for eclipse
|
|
errorAngle = NAN;
|
|
|
|
splitRotationalRate(satRotRateB, sunDirRefB);
|
|
calculateRotationalRateTorque(satRotRateRef, sunDirRefB, sunDirRefB, errorAngle,
|
|
acsParameters->safeModeControllerParameters.k_parallelNonMekf,
|
|
acsParameters->safeModeControllerParameters.k_orthoNonMekf);
|
|
|
|
// sum of all torques
|
|
double cmdTorque[3] = {0, 0, 0};
|
|
VectorOperations<double>::add(cmdParallel, cmdOrtho, cmdTorque, 3);
|
|
|
|
// calculate magnetic moment to command
|
|
calculateMagneticMoment(magMomB);
|
|
}
|
|
|
|
void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunDirB) {
|
|
// split rotational rate into parallel and orthogonal parts
|
|
double parallelLength = VectorOperations<double>::dot(satRotRateB, sunDirB) *
|
|
pow(VectorOperations<double>::norm(sunDirB, 3), -2);
|
|
VectorOperations<double>::mulScalar(sunDirB, parallelLength, satRotRateParallelB, 3);
|
|
VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3);
|
|
}
|
|
|
|
void SafeCtrl::calculateRotationalRateTorque(const double satRotRateRef, const double *sunDirB,
|
|
const double *sunDirRefB, double &errorAngle,
|
|
const double gainParallel, const double gainOrtho) {
|
|
// calculate torque for parallel rotational rate
|
|
if ((isfinite(errorAngle)) and
|
|
(errorAngle < (double)acsParameters->safeModeControllerParameters.angleStartSpin)) {
|
|
double satRotRateNorm = VectorOperations<double>::norm(satRotRateParallelB, 3);
|
|
double satRotRateUnitVec[3] = {0, 0, 0};
|
|
VectorOperations<double>::normalize(satRotRateParallelB, satRotRateUnitVec, 3);
|
|
VectorOperations<double>::mulScalar(satRotRateUnitVec, satRotRateRef - satRotRateNorm,
|
|
cmdParallel, 3);
|
|
} else {
|
|
VectorOperations<double>::mulScalar(cmdParallel, -gainParallel, cmdParallel, 3);
|
|
}
|
|
|
|
// calculate torque for orthogonal rotational rate
|
|
VectorOperations<double>::mulScalar(satRotRateOrthogonalB, -gainOrtho, cmdOrtho, 3);
|
|
if (cos(VectorOperations<double>::dot(sunDirB, sunDirRefB)) < 0) {
|
|
VectorOperations<double>::mulScalar(cmdOrtho, -1, cmdOrtho, 3);
|
|
}
|
|
}
|
|
|
|
void SafeCtrl::calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
|
|
const double gainAlign, const double inertiaMatrix[3][3]) {
|
|
// calculate torque for alignment
|
|
double crossAlign[3] = {0, 0, 0}, alignFactor[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
MatrixOperations<double>::multiplyScalar(*inertiaMatrix, gainAlign, *alignFactor, 3, 3);
|
|
VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign);
|
|
MatrixOperations<double>::multiply(*alignFactor, crossAlign, cmdAlign, 3, 3, 1);
|
|
}
|
|
|
|
void SafeCtrl::calculateMagneticMoment(double *magMomB) {
|
|
double torqueMgt[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
|
|
double normMag = VectorOperations<double>::norm(magFieldBT, 3);
|
|
VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
|
|
}
|