combined SensorProcessing and SusConverter

2022-10-06 15:38:23 +02:00
parent 84e960a9ef
commit 3079dabc20
4 changed files with 582 additions and 668 deletions
--- a/mission/controller/acs/SensorProcessing.h
+++ b/mission/controller/acs/SensorProcessing.h
@@ -5,14 +5,16 @@
 #ifndef SENSORPROCESSING_H_
 #define SENSORPROCESSING_H_

-#include "AcsParameters.h"
-#include "SensorValues.h"
-#include "OutputValues.h"
-#include "config/classIds.h"
-#include <stdint.h> //uint8_t
-#include <time.h> /*purpose, timeval ?*/
 #include <fsfw/returnvalues/returnvalue.h>
+#include <stdint.h>  //uint8_t
+#include <time.h>    /*purpose, timeval ?*/

+#include "SusConverter.h"
+#include "../controllerdefinitions/AcsCtrlDefinitions.h"
+#include "AcsParameters.h"
+#include "OutputValues.h"
+#include "SensorValues.h"
+#include "config/classIds.h"

 /*Planned:
 * - Fusion of Sensor Measurements -
@@ -26,65 +28,53 @@
 * magField
 * SunDirEst*/

-class SensorProcessing{
-public:
+class SensorProcessing {
+ public:
+  void reset();

-	void reset();
+  SensorProcessing(AcsParameters *acsParameters_);
+  virtual ~SensorProcessing();

-	SensorProcessing(AcsParameters *acsParameters_);
-	virtual ~SensorProcessing();
+  void process(timeval now, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
+               const AcsParameters *acsParameters);  // Will call protected functions
+ private:
+ protected:
+  // short description needed for every function
+  bool 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 timeOfMgmMeasurement,
+                  const AcsParameters::MgmHandlingParameters *mgmParameters,
+                  const double gpsLatitude, const double gpsLongitude, const double gpsAltitude,
+                  bool gpsValid, double *magFieldEst, bool *outputValid, double *magFieldModel,
+                  bool *magFieldModelValid, double *magneticFieldVectorDerivative,
+                  bool *magneticFieldVectorDerivativeValid);  // Output

-	void process(timeval now, ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues,
-			const AcsParameters *acsParameters); // Will call protected functions
-private:
+  void processSus(acsctrl::SusDataRaw susData, timeval timeOfSusMeasurement,
+                  const AcsParameters::SusHandlingParameters *susParameters,
+                  const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst,
+                  bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid,
+                  double *sunVectorDerivative, bool *sunVectorDerivativeValid);

-protected:
-	// short description needed for every function
-	bool 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 timeOfMgmMeasurement,
-			const AcsParameters::MgmHandlingParameters *mgmParameters,
-			const double gpsLatitude, const double gpsLongitude,
-			const double gpsAltitude, bool gpsValid,
-			double *magFieldEst, bool *outputValid,
-			double *magFieldModel, bool*magFieldModelValid,
-			double *magneticFieldVectorDerivative, bool *magneticFieldVectorDerivativeValid); //Output
+  void processRmu(const double rmu0Value[], bool rmu0valid,  // processRmu
+                  const double rmu1Value[], bool rmu1valid, const double rmu2Value[],
+                  bool rmu2valid, timeval timeOfrmuMeasurement,
+                  const AcsParameters::RmuHandlingParameters *rmuParameters, double *satRatEst,
+                  bool *satRateEstValid);

-	void processSus(const float sus0Value[], bool sus0valid, const float sus1Value[], bool sus1valid,
-			const float sus2Value[], bool sus2valid, const float sus3Value[], bool sus3valid,
-			const float sus4Value[], bool sus4valid, const float sus5Value[], bool sus5valid,
-			const float sus6Value[], bool sus6valid, const float sus7Value[], bool sus7valid,
-			const float sus8Value[], bool sus8valid, const float sus9Value[], bool sus9valid,
-			const float sus10Value[], bool sus10valid, const float sus11Value[], bool sus11valid,
-			timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
-			const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, bool *sunDirEstValid,
-			double *sunVectorModel, bool *sunVectorModelValid,
-			double *sunVectorDerivative, bool *sunVectorDerivativeValid);
+  void processStr();

-	void processRmu(const double rmu0Value[], bool rmu0valid, // processRmu
-			const double rmu1Value[], bool rmu1valid,
-			const double rmu2Value[], bool rmu2valid,
-			timeval timeOfrmuMeasurement, const AcsParameters::RmuHandlingParameters *rmuParameters,
-			double *satRatEst, bool *satRateEstValid);
-
-	void processStr();
-
-	void processGps(const double gps0latitude, const double gps0longitude,
-			const bool validGps, double *gcLatitude, double *gdLongitude);
-
-
-	double savedMagFieldEst[3];
-	timeval timeOfSavedMagFieldEst;
-	double savedSunVector[3];
-	timeval timeOfSavedSusDirEst;
-	bool validMagField;
-	bool validGcLatitude;
+  void processGps(const double gps0latitude, const double gps0longitude, const bool validGps,
+                  double *gcLatitude, double *gdLongitude);

+  double savedMagFieldEst[3];
+  timeval timeOfSavedMagFieldEst;
+  double savedSunVector[3];
+  timeval timeOfSavedSusDirEst;
+  bool validMagField;
+  bool validGcLatitude;

+  SusConverter susConverter;
+  AcsParameters acsParameters;
 };

 #endif /*SENSORPROCESSING_H_*/
-