From 7885205a0740d4866ee9a48b0a48d5a2d9fcb882 Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Tue, 20 Sep 2022 14:15:55 +0200
Subject: [PATCH] added Navitation

---
 mission/controller/acs/Navigation.h   | 35 +++++++++++++++++++
 mission/controller/acs/Navitation.cpp | 49 +++++++++++++++++++++++++++
 2 files changed, 84 insertions(+)
 create mode 100644 mission/controller/acs/Navigation.h
 create mode 100644 mission/controller/acs/Navitation.cpp

diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h
new file mode 100644
index 00000000..7d71df99
--- /dev/null
+++ b/mission/controller/acs/Navigation.h
@@ -0,0 +1,35 @@
+/*
+ * Navigation.h
+ *
+ *  Created on: 19 Apr 2022
+ *      Author: Robin Marquardt
+ */
+
+#ifndef NAVIGATION_H_
+#define NAVIGATION_H_
+
+
+#include "AcsParameters.h"
+#include "SensorProcessing.h"
+#include <acs/MultiplicativeKalmanFilter.h>
+#include <acs/SensorValues.h>
+#include <acs/OutputValues.h>
+
+class Navigation{
+public:
+	Navigation(AcsParameters *acsParameters_); //Input mode ?
+	virtual ~Navigation();
+
+	void useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid);
+	void processSensorData();
+
+protected:
+
+private:
+	MultiplicativeKalmanFilter multiplicativeKalmanFilter;
+	AcsParameters acsParameters;
+	bool kalmanInit = false;
+};
+
+#endif /* ACS_NAVIGATION_H_ */
+
diff --git a/mission/controller/acs/Navitation.cpp b/mission/controller/acs/Navitation.cpp
new file mode 100644
index 00000000..50dbb049
--- /dev/null
+++ b/mission/controller/acs/Navitation.cpp
@@ -0,0 +1,49 @@
+/*
+ * Navigation.cpp
+ *
+ *  Created on: 23 May 2022
+ *      Author: Robin Marquardt
+ */
+
+#include "Navigation.h"
+#include <math.h>
+#include <acs/math/MathOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <acs/math/CholeskyDecomposition.h>
+
+Navigation::Navigation(AcsParameters *acsParameters_): multiplicativeKalmanFilter(acsParameters_){
+	acsParameters = *acsParameters_;
+}
+
+Navigation::~Navigation(){
+
+}
+
+void Navigation::useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid){
+
+	if (kalmanInit) {
+		*mekfValid = multiplicativeKalmanFilter.mekfEst(sensorValues->quatJB, &sensorValues->quatJBValid,
+				outputValues->satRateEst, &outputValues->satRateEstValid,
+				outputValues->magFieldEst, &outputValues->magFieldEstValid,
+				outputValues->sunDirEst, &outputValues->sunDirEstValid,
+				outputValues->sunDirModel, &outputValues->sunDirModelValid,
+				outputValues->magFieldModel, &outputValues->magFieldModelValid,
+				outputValues->quatMekfBJ, outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ??
+
+	}
+	else {
+		multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid,
+				outputValues->sunDirEst, &outputValues->sunDirEstValid,
+				outputValues->sunDirModel, &outputValues->sunDirModelValid,
+				outputValues->magFieldModel, &outputValues->magFieldModelValid);
+		kalmanInit = true;
+		*mekfValid = 0;
+
+//		Maybe we need feedback from kalmanfilter to identify if there was a problem with the init of kalman filter
+//		where does this class know from that kalman filter was not initialized ?
+	}
+}
+
+