eive-obsw/mission/controller/acs/FusedRotationEstimation.h

#ifndef MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_

#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>

class FusedRotationEstimation {
 public:
  FusedRotationEstimation(AcsParameters *acsParameters_);

  void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed,
                                     acsctrl::MgmDataProcessed *mgmDataProcessed,
                                     acsctrl::GyrDataProcessed *gyrDataProcessed,
                                     acsctrl::FusedRotRateData *fusedRotRateData);

 protected:
 private:
  double constexpr ZERO_VEC[3] = {0, 0, 0};

  AcsParameters *acsParameters;
  double rotRateOldB[3] = {0, 0, 0};
  // double fusedRateOldB[3] = {0, 0, 0};

  void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed,
                                        acsctrl::FusedRotRateData *fusedRotRateData);
};

#endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */