From 3d123a09f3270ead90dd53cf84292198332289a6 Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Tue, 20 Sep 2022 13:36:17 +0200
Subject: [PATCH] added control subfolder. added Detumble

---
 mission/controller/acs/control/Detumble.cpp | 65 +++++++++++++++++++++
 mission/controller/acs/control/Detumble.h   | 46 +++++++++++++++
 2 files changed, 111 insertions(+)
 create mode 100644 mission/controller/acs/control/Detumble.cpp
 create mode 100644 mission/controller/acs/control/Detumble.h

diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp
new file mode 100644
index 00000000..283d6dc1
--- /dev/null
+++ b/mission/controller/acs/control/Detumble.cpp
@@ -0,0 +1,65 @@
+
+/*
+ * Detumble.cpp
+ *
+ *  Created on: 17 Aug 2022
+ *      Author: Robin Marquardt
+ */
+
+
+#include "Detumble.h"
+#include <fsfw/src/fsfw/globalfunctions/constants.h>
+#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
+#include <acs/util/MathOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
+#include <math.h>
+#include <fsfw/src/fsfw/globalfunctions/sign.h>
+
+
+Detumble::Detumble(AcsParameters *acsParameters_){
+	loadAcsParameters(acsParameters_);
+}
+
+Detumble::~Detumble(){
+
+}
+
+void Detumble::loadAcsParameters(AcsParameters *acsParameters_){
+
+	detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters);
+	magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
+
+}
+
+
+ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool *magRateValid,
+		const double *magField, const bool *magFieldValid,
+		double *magMom) {
+
+	if (!magRateValid || !magFieldValid) {
+		return DETUMBLE_NO_SENSORDATA;
+	}
+	double gain = detumbleCtrlParameters->gainD;
+	double factor = -gain / pow(VectorOperations<double>::norm(magField,3),2);
+	VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
+	return RETURN_OK;
+
+}
+
+ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom) {
+
+	if (!magRateValid) {
+		return DETUMBLE_NO_SENSORDATA;
+	}
+
+	double dipolMax = magnetorquesParameter->DipolMax;
+	for (int i = 0; i<3; i++) {
+
+		magMom[i] = - dipolMax * sign(magRate[i]);
+
+	}
+
+	return RETURN_OK;
+
+}
diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h
new file mode 100644
index 00000000..54d62179
--- /dev/null
+++ b/mission/controller/acs/control/Detumble.h
@@ -0,0 +1,46 @@
+/*
+ * Detumble.h
+ *
+ *  Created on: 17 Aug 2022
+ *      Author: Robin Marquardt
+ */
+
+#ifndef ACS_CONTROL_DETUMBLE_H_
+#define ACS_CONTROL_DETUMBLE_H_
+
+#include <string.h>
+#include <stdio.h>
+#include <acs/SensorValues.h>
+#include <acs/OutputValues.h>
+#include <acs/AcsParameters.h>
+#include "acs/config/classIds.h"
+#include <fsfw/src/fsfw/returnvalues/HasReturnvaluesIF.h>
+#include <time.h>
+
+class Detumble : public HasReturnvaluesIF {
+
+public:
+	Detumble(AcsParameters *acsParameters_);
+	virtual ~Detumble();
+
+	static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE;
+	static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
+
+	/* @brief: Load AcsParameters für this class
+	 * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
+	 */
+	void loadAcsParameters(AcsParameters *acsParameters_);
+
+	ReturnValue_t bDotLaw(const double *magRate, const bool *magRateValid,
+			const double *magField, const bool *magFieldValid,
+			double *magMom);
+
+	ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom);
+
+private:
+	AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters;
+	AcsParameters::MagnetorquesParameter* magnetorquesParameter;
+};
+
+#endif /* ACS_CONTROL_DETUMBLE_H_ */
+