eive-obsw/mission/controller/acs/control/SafeCtrl.h
meggert d82a6ece5a
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
use safeCtrl even if SUS and MGM vectors are too close
2023-08-15 09:11:57 +02:00

64 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::SafeModeStrategy 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_ */