From 5d0a1858cce1a4cbe0b72893db8fd54baac2b07b Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Tue, 7 Feb 2023 10:20:36 +0100
Subject: [PATCH] onboard handling of GYR values changed from deg/s to rad/s

---
 mission/controller/acs/AcsParameters.h      | 4 ++--
 mission/controller/acs/SensorProcessing.cpp | 4 ++++
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h
index ae174236..88802a46 100644
--- a/mission/controller/acs/AcsParameters.h
+++ b/mission/controller/acs/AcsParameters.h
@@ -915,8 +915,8 @@ class AcsParameters : public HasParametersIF {
 
   struct DetumbleParameter {
     uint8_t detumblecounter = 75;  // 30 s
-    double omegaDetumbleStart = 2;
-    double omegaDetumbleEnd = 0.4;
+    double omegaDetumbleStart = 2 * M_PI / 180;
+    double omegaDetumbleEnd = 0.4 * M_PI / 180;
     double gainD = pow(10.0, -3.3);
   } detumbleParameter;
 };
diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp
index 9a1a9637..bfb38afc 100644
--- a/mission/controller/acs/SensorProcessing.cpp
+++ b/mission/controller/acs/SensorProcessing.cpp
@@ -477,6 +477,7 @@ void SensorProcessing::processGyr(
     const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
                                        gyr0ValueBody, 3, 3, 1);
+    VectorOperations<double>::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
       sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
@@ -486,6 +487,7 @@ void SensorProcessing::processGyr(
     const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
                                        gyr1ValueBody, 3, 3, 1);
+    VectorOperations<double>::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
       sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
@@ -495,6 +497,7 @@ void SensorProcessing::processGyr(
     const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
                                        gyr2ValueBody, 3, 3, 1);
+    VectorOperations<double>::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
       sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
@@ -504,6 +507,7 @@ void SensorProcessing::processGyr(
     const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
                                        gyr3ValueBody, 3, 3, 1);
+    VectorOperations<double>::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];
       sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];