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

201 lines
8.6 KiB
C++
Raw Normal View History

2022-09-20 13:46:42 +02:00
#include "SafeCtrl.h"
2022-10-20 11:07:45 +02:00
2022-09-27 11:06:11 +02:00
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
2022-10-20 11:07:45 +02:00
#include <math.h>
2022-09-27 11:06:11 +02:00
2023-02-28 09:18:44 +01:00
SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
2022-09-20 13:46:42 +02:00
2022-10-20 11:07:45 +02:00
SafeCtrl::~SafeCtrl() {}
2022-09-20 13:46:42 +02:00
2023-11-02 16:59:09 +01:00
acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const bool fusedRateTotalValid,
const uint8_t mekfEnabled,
const uint8_t gyrEnabled,
const uint8_t dampingEnabled) {
2023-03-24 11:35:46 +01:00
if (not magFieldValid) {
2023-11-02 16:59:09 +01:00
return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) {
2023-11-02 16:59:09 +01:00
return acs::ControlModeStrategy::SAFECTRL_MEKF;
2023-07-24 10:10:26 +02:00
} else if (sunDirValid) {
if (gyrEnabled and satRotRateValid) {
2023-11-02 16:59:09 +01:00
return acs::ControlModeStrategy::SAFECTRL_GYR;
} else if (not gyrEnabled and fusedRateTotalValid) {
2023-11-02 16:59:09 +01:00
return acs::ControlModeStrategy::SAFECTRL_SUSMGM;
2023-07-24 10:10:26 +02:00
} else {
2023-11-02 16:59:09 +01:00
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
2023-07-24 10:10:26 +02:00
}
} else if (not sunDirValid) {
2023-07-24 10:14:38 +02:00
if (dampingEnabled) {
if (gyrEnabled and satRotRateValid) {
2023-11-02 16:59:09 +01:00
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
2023-07-24 10:14:38 +02:00
} else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) {
2023-11-02 16:59:09 +01:00
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
2023-07-24 10:14:38 +02:00
} else {
2023-11-02 16:59:09 +01:00
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
2023-07-24 10:14:38 +02:00
}
2023-07-24 10:10:26 +02:00
} else if (not dampingEnabled and satRotRateValid) {
2023-11-02 16:59:09 +01:00
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING;
2023-07-24 10:10:26 +02:00
} else {
2023-11-02 16:59:09 +01:00
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
2023-07-24 10:10:26 +02:00
}
2023-03-24 11:35:46 +01:00
} else {
2023-11-02 16:59:09 +01:00
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
2023-03-24 11:35:46 +01:00
}
}
2023-03-24 14:51:33 +01:00
void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
const double *sunDirModelI, const double *quatBI, const double *sunDirRefB,
2023-04-14 11:37:23 +02:00
double *magMomB, double &errorAngle) {
2023-03-24 14:51:33 +01:00
// convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
2022-10-20 11:07:45 +02:00
2023-03-24 14:51:33 +01:00
// convert sunDirModel to body rf
double sunDirB[3] = {0, 0, 0};
QuaternionOperations::multiplyVector(quatBI, sunDirModelI, sunDirB);
2022-10-20 11:07:45 +02:00
2023-03-24 14:51:33 +01:00
// calculate angle alpha between sunDirRef and sunDir
double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
errorAngle = acos(dotSun);
2022-10-20 11:07:45 +02:00
2023-04-05 16:11:28 +02:00
splitRotationalRate(satRotRateB, sunDirB);
2023-07-21 14:24:52 +02:00
calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelMekf,
2023-04-14 11:37:23 +02:00
acsParameters->safeModeControllerParameters.k_orthoMekf);
2023-04-05 16:11:28 +02:00
calculateAngleErrorTorque(sunDirB, sunDirRefB,
2023-04-13 17:06:11 +02:00
acsParameters->safeModeControllerParameters.k_alignMekf);
2022-10-20 11:07:45 +02:00
2023-03-24 14:51:33 +01:00
// sum of all torques
for (uint8_t i = 0; i < 3; i++) {
cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
}
2022-10-20 11:07:45 +02:00
2023-04-05 16:11:28 +02:00
calculateMagneticMoment(magMomB);
2022-09-20 13:46:42 +02:00
}
2023-07-19 13:24:37 +02:00
void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, double *magMomB, double &errorAngle) {
2023-03-24 11:35:46 +01:00
// convert magFieldB from uT to T
2023-03-22 08:59:29 +01:00
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
2023-03-21 17:19:27 +01:00
2023-04-05 16:11:28 +02:00
// calculate error angle between sunDirRef and sunDir
2023-03-24 11:35:46 +01:00
double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
errorAngle = acos(dotSun);
2023-04-05 16:11:28 +02:00
splitRotationalRate(satRotRateB, sunDirB);
2023-07-21 14:24:52 +02:00
calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr,
2023-07-19 15:30:54 +02:00
acsParameters->safeModeControllerParameters.k_orthoGyr);
2023-04-05 16:11:28 +02:00
calculateAngleErrorTorque(sunDirB, sunDirRefB,
2023-07-19 15:30:54 +02:00
acsParameters->safeModeControllerParameters.k_alignGyr);
2023-04-05 16:11:28 +02:00
// sum of all torques
for (uint8_t i = 0; i < 3; i++) {
cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
}
calculateMagneticMoment(magMomB);
}
void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateTotalB,
const double *rotRateParallelB, const double *rotRateOrthogonalB,
const double *sunDirB, const double *sunDirRefB, double *magMomB,
double &errorAngle) {
2023-07-19 13:24:37 +02:00
// convert magFieldB from uT to T
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);
if (VectorOperations<double>::norm(rotRateParallelB, 3) != 0 and
VectorOperations<double>::norm(rotRateOrthogonalB, 3) != 0) {
std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB));
std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB));
} else {
splitRotationalRate(rotRateTotalB, sunDirB);
}
2023-07-21 14:24:52 +02:00
calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm,
2023-07-19 13:24:37 +02:00
acsParameters->safeModeControllerParameters.k_orthoSusMgm);
calculateAngleErrorTorque(sunDirB, sunDirRefB,
acsParameters->safeModeControllerParameters.k_alignSusMgm);
// sum of all torques
for (uint8_t i = 0; i < 3; i++) {
cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
}
calculateMagneticMoment(magMomB);
}
2023-07-19 15:30:54 +02:00
void SafeCtrl::safeRateDampingGyr(const double *magFieldB, const double *satRotRateB,
const double *sunDirRefB, double *magMomB, double &errorAngle) {
2023-04-05 16:11:28 +02:00
// 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);
2023-07-21 14:24:52 +02:00
calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr,
2023-07-19 15:30:54 +02:00
acsParameters->safeModeControllerParameters.k_orthoGyr);
// sum of all torques
VectorOperations<double>::add(cmdParallel, cmdOrtho, cmdTorque, 3);
// calculate magnetic moment to command
calculateMagneticMoment(magMomB);
}
void SafeCtrl::safeRateDampingSusMgm(const double *magFieldB, const double *satRotRateB,
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);
2023-07-21 14:24:52 +02:00
calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm,
2023-07-19 15:30:54 +02:00
acsParameters->safeModeControllerParameters.k_orthoSusMgm);
2023-04-05 16:11:28 +02:00
// sum of all torques
VectorOperations<double>::add(cmdParallel, cmdOrtho, cmdTorque, 3);
// calculate magnetic moment to command
calculateMagneticMoment(magMomB);
}
void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunDirB) {
2023-03-24 11:35:46 +01:00
// 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);
2023-04-05 16:11:28 +02:00
}
2023-03-24 11:35:46 +01:00
2023-07-21 14:24:52 +02:00
void SafeCtrl::calculateRotationalRateTorque(const double gainParallel, const double gainOrtho) {
2023-03-24 11:35:46 +01:00
// calculate torque for parallel rotational rate
2023-04-13 16:28:54 +02:00
VectorOperations<double>::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3);
2022-10-20 11:07:45 +02:00
2023-03-24 11:35:46 +01:00
// calculate torque for orthogonal rotational rate
2023-04-14 11:37:23 +02:00
VectorOperations<double>::mulScalar(satRotRateOrthogonalB, -gainOrtho, cmdOrtho, 3);
2023-04-05 16:11:28 +02:00
}
2023-03-24 11:35:46 +01:00
2023-04-05 16:11:28 +02:00
void SafeCtrl::calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
2023-04-13 17:06:11 +02:00
const double gainAlign) {
2023-03-24 11:35:46 +01:00
// calculate torque for alignment
2023-04-13 17:06:11 +02:00
double crossAlign[3] = {0, 0, 0};
2023-03-24 11:35:46 +01:00
VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign);
2023-04-13 17:06:11 +02:00
VectorOperations<double>::mulScalar(crossAlign, gainAlign, cmdAlign, 3);
2023-03-21 17:19:27 +01:00
}
2023-03-24 14:51:33 +01:00
2023-04-05 16:11:28 +02:00
void SafeCtrl::calculateMagneticMoment(double *magMomB) {
2023-03-24 14:51:33 +01:00
double torqueMgt[3] = {0, 0, 0};
VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
2023-04-05 16:11:28 +02:00
double normMag = VectorOperations<double>::norm(magFieldBT, 3);
2023-03-24 14:51:33 +01:00
VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
}