88 lines
4.3 KiB
C++
88 lines
4.3 KiB
C++
#ifndef SENSORPROCESSING_H_
|
|
#define SENSORPROCESSING_H_
|
|
|
|
#include <common/config/eive/resultClassIds.h>
|
|
#include <fsfw/coordinates/CoordinateTransformations.h>
|
|
#include <fsfw/datapool/PoolReadGuard.h>
|
|
#include <fsfw/globalfunctions/constants.h>
|
|
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
|
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
#include <fsfw/globalfunctions/timevalOperations.h>
|
|
#include <fsfw/returnvalues/returnvalue.h>
|
|
#include <fsfw/timemanager/Clock.h>
|
|
#include <mission/acs/defs.h>
|
|
#include <mission/controller/acs/AcsParameters.h>
|
|
#include <mission/controller/acs/Igrf13Model.h>
|
|
#include <mission/controller/acs/SensorValues.h>
|
|
#include <mission/controller/acs/SusConverter.h>
|
|
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
|
|
|
#include <cmath>
|
|
|
|
class SensorProcessing {
|
|
public:
|
|
SensorProcessing();
|
|
virtual ~SensorProcessing();
|
|
|
|
void process(timeval timeAbsolute, double timeDelta, ACS::SensorValues *sensorValues,
|
|
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
|
acsctrl::SusDataProcessed *susDataProcessed,
|
|
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
|
acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters);
|
|
|
|
private:
|
|
static constexpr float ZERO_VEC_F[3] = {0, 0, 0};
|
|
static constexpr double ZERO_VEC_D[3] = {0, 0, 0};
|
|
static constexpr double ECCENTRICITY_WGS84 = 0.0818195;
|
|
|
|
protected:
|
|
void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
|
|
const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid,
|
|
const float *mgm4Value, bool mgm4valid, timeval timeAbsolute, double timeDelta,
|
|
const AcsParameters::MgmHandlingParameters *mgmParameters,
|
|
acsctrl::GpsDataProcessed *gpsDataProcessed,
|
|
acsctrl::MgmDataProcessed *mgmDataProcessed);
|
|
|
|
void processSus(const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value,
|
|
bool sus1valid, const uint16_t *sus2Value, bool sus2valid,
|
|
const uint16_t *sus3Value, bool sus3valid, const uint16_t *sus4Value,
|
|
bool sus4valid, const uint16_t *sus5Value, bool sus5valid,
|
|
const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value,
|
|
bool sus7valid, const uint16_t *sus8Value, bool sus8valid,
|
|
const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus10Value,
|
|
bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
|
|
timeval timeAbsolute, double timeDelta,
|
|
const AcsParameters::SusHandlingParameters *susParameters,
|
|
const AcsParameters::SunModelParameters *sunModelParameters,
|
|
acsctrl::SusDataProcessed *susDataProcessed);
|
|
|
|
void processGyr(const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue,
|
|
bool gyr0axYvalid, const double gyr0axZvalue, bool gyr0axZvalid,
|
|
const double gyr1axXvalue, bool gyr1axXvalid, const double gyr1axYvalue,
|
|
bool gyr1axYvalid, const double gyr1axZvalue, bool gyr1axZvalid,
|
|
const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue,
|
|
bool gyr2axYvalid, const double gyr2axZvalue, bool gyr2axZvalid,
|
|
const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue,
|
|
bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
|
|
const AcsParameters::GyrHandlingParameters *gyrParameters,
|
|
acsctrl::GyrDataProcessed *gyrDataProcessed);
|
|
|
|
void processGps(const double gpsLatitude, const double gpsLongitude, const double gpsAltitude,
|
|
const double gpsUnixSeconds, const bool validGps,
|
|
const AcsParameters::GpsParameters *gpsParameters,
|
|
acsctrl::GpsDataProcessed *gpsDataProcessed);
|
|
|
|
void lowPassFilter(double *newValue, double *oldValue, const double weight);
|
|
|
|
double savedMgmVecTot[3] = {0.0, 0.0, 0.0};
|
|
double savedSusVecTot[3] = {0.0, 0.0, 0.0};
|
|
|
|
double savedPosSatE[3] = {0.0, 0.0, 0.0};
|
|
bool validSavedPosSatE = false;
|
|
|
|
SusConverter susConverter;
|
|
};
|
|
|
|
#endif /*SENSORPROCESSING_H_*/
|