From e87221a8a3efb90e0b840f2fabcf81d622e121cb Mon Sep 17 00:00:00 2001
From: Robin Marquardt <rmarquardt@irs.uni-stuttgart.de>
Date: Fri, 21 Oct 2022 16:46:09 +0200
Subject: [PATCH] added gyro detumble

---
 mission/controller/acs/control/Detumble.cpp | 14 ++++++++++++++
 mission/controller/acs/control/Detumble.h   |  4 ++++
 2 files changed, 18 insertions(+)

diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp
index 67903c7c..f12d5962 100644
--- a/mission/controller/acs/control/Detumble.cpp
+++ b/mission/controller/acs/control/Detumble.cpp
@@ -63,3 +63,17 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool *magRateVa
 	return returnvalue::OK;
 
 }
+
+ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid,
+		const double *magField, const bool *magFieldValid,
+		double *magMom) {
+
+	if (!satRateValid || !magFieldValid) {
+		return DETUMBLE_NO_SENSORDATA;
+	}
+	double gain = detumbleCtrlParameters->gainD;
+	double factor = -gain / pow(VectorOperations<double>::norm(magField,3),2);
+	VectorOperations<double>::mulScalar(satRate, factor, magMom, 3);
+	return returnvalue::OK;
+
+}
diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h
index b85aaf86..e49c88ac 100644
--- a/mission/controller/acs/control/Detumble.h
+++ b/mission/controller/acs/control/Detumble.h
@@ -38,6 +38,10 @@ public:
 
 	ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom);
 
+	ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid,
+			const double *magField, const bool *magFieldValid,
+			double *magMom);
+
 private:
 	AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters;
 	AcsParameters::MagnetorquesParameter* magnetorquesParameter;