eive-obsw/mission/controller/acs/control/SafeCtrl.h
2023-04-14 18:57:06 +02:00

53 lines
1.9 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();
uint8_t safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const uint8_t mekfEnabled, 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 safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, double *magMomB, double &errorAngle);
void safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB,
double *magMomB, double &errorAngle);
void splitRotationalRate(const double *satRotRateB, const double *sunDirB);
void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB,
double &errorAngle, 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_ */