Robin Mueller
acf693636a
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
65 lines
2.6 KiB
C++
65 lines
2.6 KiB
C++
#ifndef SAFECTRL_H_
|
|
#define SAFECTRL_H_
|
|
|
|
#include <eive/resultClassIds.h>
|
|
#include <mission/acs/defs.h>
|
|
#include <mission/controller/acs/AcsParameters.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
class SafeCtrl {
|
|
public:
|
|
SafeCtrl(AcsParameters *acsParameters_);
|
|
virtual ~SafeCtrl();
|
|
|
|
acs::ControlModeStrategy 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);
|
|
|
|
void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
|
|
const double *quatBI, const double *sunDirRefB, double *magMomB,
|
|
double &errorAngle);
|
|
|
|
void safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
|
|
const double *sunDirRefB, double *magMomB, double &errorAngle);
|
|
|
|
void safeSusMgm(const double *magFieldB, const double *rotRateTotalB,
|
|
const double *rotRateParallelB, const double *rotRateOrthogonalB,
|
|
const double *sunDirB, const double *sunDirRefB, double *magMomB,
|
|
double &errorAngle);
|
|
|
|
void safeRateDampingGyr(const double *magFieldB, const double *satRotRateB,
|
|
const double *sunDirRefB, double *magMomB, double &errorAngle);
|
|
|
|
void safeRateDampingSusMgm(const double *magFieldB, const double *satRotRateB,
|
|
const double *sunDirRefB, double *magMomB, double &errorAngle);
|
|
|
|
void splitRotationalRate(const double *satRotRateB, const double *sunDirB);
|
|
|
|
void calculateRotationalRates(const double *magFieldB, const double *magRateB,
|
|
const double *sunDirB, const double *sunRateB,
|
|
double *fusedRotRate);
|
|
|
|
void calculateRotationalRateTorque(const double gainParallel, const double gainOrtho);
|
|
|
|
void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
|
|
const double gainAlign);
|
|
|
|
void calculateMagneticMoment(double *magMomB);
|
|
|
|
protected:
|
|
private:
|
|
AcsParameters *acsParameters;
|
|
double magFieldBT[3] = {0, 0, 0};
|
|
double satRotRateParallelB[3] = {0, 0, 0};
|
|
double satRotRateOrthogonalB[3] = {0, 0, 0};
|
|
double cmdParallel[3] = {0, 0, 0};
|
|
double cmdOrtho[3] = {0, 0, 0};
|
|
double cmdAlign[3] = {0, 0, 0};
|
|
double cmdTorque[3] = {0, 0, 0};
|
|
};
|
|
|
|
#endif /* ACS_CONTROL_SAFECTRL_H_ */
|