2023-07-19 16:25:03 +02:00
|
|
|
#ifndef MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
|
|
|
|
#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
|
|
|
|
|
2023-07-20 11:09:34 +02:00
|
|
|
#include <fsfw/datapool/PoolReadGuard.h>
|
2023-11-14 13:22:35 +01:00
|
|
|
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
2023-07-19 16:25:03 +02:00
|
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
|
|
#include <mission/controller/acs/AcsParameters.h>
|
2023-11-14 13:22:35 +01:00
|
|
|
#include <mission/controller/acs/SensorValues.h>
|
2023-07-20 11:09:34 +02:00
|
|
|
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
2023-07-19 16:25:03 +02:00
|
|
|
|
|
|
|
class FusedRotationEstimation {
|
|
|
|
public:
|
|
|
|
FusedRotationEstimation(AcsParameters *acsParameters_);
|
|
|
|
|
2024-02-26 10:14:15 +01:00
|
|
|
void estimateFusedRotationRate(const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed,
|
2023-11-14 13:22:35 +01:00
|
|
|
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
|
|
|
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
2023-11-23 16:56:36 +01:00
|
|
|
ACS::SensorValues *sensorValues,
|
|
|
|
acsctrl::AttitudeEstimationData *attitudeEstimationData,
|
|
|
|
const double timeDelta,
|
|
|
|
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
|
2023-11-14 13:22:35 +01:00
|
|
|
acsctrl::FusedRotRateData *fusedRotRateData);
|
|
|
|
|
2023-07-19 16:25:03 +02:00
|
|
|
protected:
|
|
|
|
private:
|
2023-11-14 13:22:35 +01:00
|
|
|
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
|
|
|
|
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
|
2023-07-20 11:09:34 +02:00
|
|
|
|
2023-07-19 16:25:03 +02:00
|
|
|
AcsParameters *acsParameters;
|
2023-11-23 16:56:36 +01:00
|
|
|
double quatOldQuest[4] = {0, 0, 0, 0};
|
|
|
|
double quatOldStr[4] = {0, 0, 0, 0};
|
2023-07-19 16:25:03 +02:00
|
|
|
double rotRateOldB[3] = {0, 0, 0};
|
2023-07-20 11:09:34 +02:00
|
|
|
|
2023-11-23 16:56:36 +01:00
|
|
|
void estimateFusedRotationRateSusMgm(acsctrl::SusDataProcessed *susDataProcessed,
|
|
|
|
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
|
|
|
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
|
|
|
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
|
2023-07-20 11:09:34 +02:00
|
|
|
void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed,
|
2023-11-24 10:52:36 +01:00
|
|
|
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
|
2023-11-23 16:56:36 +01:00
|
|
|
void estimateFusedRotationRateQuest(acsctrl::AttitudeEstimationData *attitudeEstimationData,
|
|
|
|
const double timeDelta,
|
|
|
|
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
|
|
|
|
void estimateFusedRotationRateStr(ACS::SensorValues *sensorValues, const double timeDelta,
|
|
|
|
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
|
2023-07-19 16:25:03 +02:00
|
|
|
};
|
|
|
|
|
|
|
|
#endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */
|