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>
|
|
|
|
|
#include <mission/controller/acs/AcsParameters.h>
|
|
|
|
|
#include <mission/controller/acs/SensorValues.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
|
|
|
#include <time.h>
|
|
|
|
|
|
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-03-24 11:35:46 +01:00
|
|
|
static constexpr uint8_t IF_SAFE_ID = CLASS_ID::ACS_SAFE;
|
|
|
|
|
static constexpr ReturnValue_t SAFECTRL_NO_MAG_FIELD_FOR_CONTROL =
|
|
|
|
|
returnvalue::makeCode(IF_SAFE_ID, 2);
|
|
|
|
|
static constexpr ReturnValue_t SAFECTRL_USE_MEKF = returnvalue::makeCode(IF_SAFE_ID, 3);
|
|
|
|
|
static constexpr ReturnValue_t SAFECTRL_USE_NONMEKF = returnvalue::makeCode(IF_SAFE_ID, 4);
|
|
|
|
|
static constexpr ReturnValue_t SAFECTRL_USE_DAMPING = returnvalue::makeCode(IF_SAFE_ID, 5);
|
|
|
|
|
static constexpr ReturnValue_t SAFECTRL_NO_SENSORS_FOR_CONTROL =
|
|
|
|
|
returnvalue::makeCode(IF_SAFE_ID, 6);
|
|
|
|
|
|
|
|
|
|
ReturnValue_t safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t mekfValid,
|
|
|
|
|
const bool satRotRateValid, const bool sunDirValid);
|
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,
|
|
|
|
|
const double *quatBI, const double *sunDirRefB, const double *satRotRateRefB,
|
|
|
|
|
double *magMomB, double &errorAngle);
|
|
|
|
|
|
|
|
|
|
void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
|
|
|
|
|
const double *sunDirRefB, const double *satRotRateRefB, double *magMomB,
|
|
|
|
|
double &errorAngle);
|
|
|
|
|
|
|
|
|
|
void safeRateDamping(const double *magFieldB, const double *satRotRateB,
|
|
|
|
|
const double *satRotRateRefB, double *magMomB, double &errorAngle);
|
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;
|
2022-09-20 13:46:42 +02:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
#endif /* ACS_CONTROL_SAFECTRL_H_ */
|
