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

53 lines
1.9 KiB
C
Raw Normal View History

2022-09-20 13:46:42 +02:00
#ifndef SAFECTRL_H_
#define SAFECTRL_H_
2023-03-24 11:35:46 +01:00
#include <eive/resultClassIds.h>
2023-04-06 10:37:59 +02:00
#include <mission/acs/defs.h>
2023-03-24 11:35:46 +01:00
#include <mission/controller/acs/AcsParameters.h>
2022-09-20 13:46:42 +02:00
#include <stdio.h>
2022-10-20 11:07:45 +02:00
#include <string.h>
2022-09-20 13:46:42 +02:00
2022-10-20 11:07:45 +02:00
class SafeCtrl {
public:
SafeCtrl(AcsParameters *acsParameters_);
virtual ~SafeCtrl();
2022-09-20 13:46:42 +02:00
2023-04-14 18:57:06 +02:00
uint8_t safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const uint8_t mekfEnabled, const uint8_t dampingEnabled);
2022-09-20 13:46:42 +02:00
2023-03-24 14:51:33 +01:00
void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
2023-04-14 11:37:23 +02:00
const double *quatBI, const double *sunDirRefB, double *magMomB,
double &errorAngle);
2023-03-24 14:51:33 +01:00
void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
2023-04-14 11:37:23 +02:00
const double *sunDirRefB, double *magMomB, double &errorAngle);
2023-03-24 14:51:33 +01:00
2023-04-13 15:02:07 +02:00
void safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB,
2023-04-14 11:37:23 +02:00
double *magMomB, double &errorAngle);
2023-04-05 16:11:28 +02:00
void splitRotationalRate(const double *satRotRateB, const double *sunDirB);
2023-04-13 15:02:07 +02:00
void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB,
double &errorAngle, const double gainParallel,
2023-04-14 11:37:23 +02:00
const double gainOrtho);
2023-04-05 16:11:28 +02:00
void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
2023-04-13 17:06:11 +02:00
const double gainAlign);
2023-04-05 16:11:28 +02:00
void calculateMagneticMoment(double *magMomB);
2022-09-20 13:46:42 +02:00
2022-10-20 11:07:45 +02:00
protected:
private:
2023-02-28 09:18:44 +01:00
AcsParameters *acsParameters;
2023-04-05 16:11:28 +02:00
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};
2022-09-20 13:46:42 +02:00
};
#endif /* ACS_CONTROL_SAFECTRL_H_ */