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

55 lines
2.2 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
uint8_t safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t 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-05 16:46:57 +02:00
const double *quatBI, const double *sunDirRefB, const double satRotRateRef,
2023-04-05 16:11:28 +02:00
const double inertiaMatrix[3][3], 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-05 16:46:57 +02:00
const double *sunDirRefB, const double satRotRateRef,
2023-04-05 16:11:28 +02:00
const double inertiaMatrix[3][3], double *magMomB, double &errorAngle);
2023-03-24 14:51:33 +01:00
void safeRateDamping(const double *magFieldB, const double *satRotRateB,
2023-04-05 16:46:57 +02:00
const double satRotRateRef, const double *sunDirRefB, double *magMomB,
2023-04-05 16:11:28 +02:00
double &errorAngle);
void splitRotationalRate(const double *satRotRateB, const double *sunDirB);
2023-04-05 16:46:57 +02:00
void calculateRotationalRateTorque(const double satRotRateRef, const double *sunDirB,
2023-04-05 16:11:28 +02:00
const double *sunDirRefB, double &errorAngle,
const double gainParallel, const double gainOrtho);
void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
const double gainAlign, const double inertiaMatrix[3][3]);
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_ */