meggert
d00cfc420c
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
143 lines
6.0 KiB
C++
143 lines
6.0 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 <fsfw/globalfunctions/sign.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
|
|
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;
|
|
}
|
|
|
|
double magneticFieldVectorT[3] = {0, 0, 0};
|
|
VectorOperations<double>::mulScalar(magneticFieldVector, 1e-6, magneticFieldVectorT, 3);
|
|
|
|
double commandParallel[3] = {0, 0, 0}, commandAlign[3] = {0, 0, 0}, commandOrtho[3] = {0, 0, 0};
|
|
|
|
bool valid;
|
|
double omega = estimateRotationAroundSun(magneticFieldVector, magneticFieldVectorDerivative,
|
|
sunVector, &valid);
|
|
|
|
if (valid) {
|
|
VectorOperations<double>::mulScalar(
|
|
sunVector,
|
|
acsParameters->safeModeControllerParameters.k_parallel_no_mekf * (omegaRef - omega),
|
|
commandParallel, 3);
|
|
omega = omega * sign<double>(omega);
|
|
}
|
|
|
|
VectorOperations<double>::cross(spinAxis, sunVector, commandAlign);
|
|
|
|
VectorOperations<double>::mulScalar(
|
|
commandAlign, acsParameters->safeModeControllerParameters.k_align_no_mekf, commandAlign, 3);
|
|
|
|
VectorOperations<double>::cross(sunvectorDerivative, sunVector, commandOrtho);
|
|
VectorOperations<double>::mulScalar(
|
|
commandOrtho, -acsParameters->safeModeControllerParameters.k_ortho_no_mekf, commandOrtho, 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<double>::dot(spinAxis, sunVector) <
|
|
acsParameters->safeModeControllerParameters.cosineStartSpin) {
|
|
VectorOperations<double>::mulScalar(commandParallel, 0, commandParallel, 3);
|
|
}
|
|
|
|
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<double>::cross(magneticFieldVector, sunVector, vector);
|
|
|
|
float magLength = VectorOperations<double>::norm(magneticFieldVector, 3);
|
|
float length = VectorOperations<double>::norm(vector, 3);
|
|
|
|
// only check if angle between B and sun is large enough
|
|
if (length > (acsParameters->safeModeControllerParameters.sineCalculateOmegaSun * magLength)) {
|
|
float omega = VectorOperations<double>::dot(magneticFieldVectorDerivative, vector);
|
|
omega = omega / (length * length);
|
|
lastCalculatedOmega = omega;
|
|
return omega;
|
|
} else {
|
|
*updated = false;
|
|
return lastCalculatedOmega;
|
|
}
|
|
}
|