From 989293860140c8d24f780b21d942dfc1a7077fc6 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Sat, 11 Mar 2023 10:21:09 +0100
Subject: [PATCH 1/2] fixed bias substraction

---
 mission/controller/acs/SensorProcessing.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp
index b3423367..b2812f55 100644
--- a/mission/controller/acs/SensorProcessing.cpp
+++ b/mission/controller/acs/SensorProcessing.cpp
@@ -470,10 +470,10 @@ void SensorProcessing::processGyr(
   float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
 
   if (gyr0valid) {
-    const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
+    double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
+    VectorOperations<double>::subtract(gyr0Value, gyrParameters->gyr0bias, gyr0Value, 3);
     MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
                                        gyr0ValueBody, 3, 3, 1);
-    VectorOperations<double>::subtract(gyr0ValueBody, gyrParameters->gyr0bias, gyr0ValueBody, 3);
     VectorOperations<double>::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
@@ -481,10 +481,10 @@ void SensorProcessing::processGyr(
     }
   }
   if (gyr1valid) {
-    const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
+    double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
+    VectorOperations<double>::subtract(gyr1Value, gyrParameters->gyr1bias, gyr1Value, 3);
     MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
                                        gyr1ValueBody, 3, 3, 1);
-    VectorOperations<double>::subtract(gyr1ValueBody, gyrParameters->gyr1bias, gyr1ValueBody, 3);
     VectorOperations<double>::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
@@ -492,10 +492,10 @@ void SensorProcessing::processGyr(
     }
   }
   if (gyr2valid) {
-    const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
+    double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
+    VectorOperations<double>::subtract(gyr2Value, gyrParameters->gyr2bias, gyr2Value, 3);
     MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
                                        gyr2ValueBody, 3, 3, 1);
-    VectorOperations<double>::subtract(gyr2ValueBody, gyrParameters->gyr2bias, gyr2ValueBody, 3);
     VectorOperations<double>::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
@@ -503,10 +503,10 @@ void SensorProcessing::processGyr(
     }
   }
   if (gyr3valid) {
-    const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
+    double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
+    VectorOperations<double>::subtract(gyr3Value, gyrParameters->gyr3bias, gyr3Value, 3);
     MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
                                        gyr3ValueBody, 3, 3, 1);
-    VectorOperations<double>::subtract(gyr3ValueBody, gyrParameters->gyr3bias, gyr3ValueBody, 3);
     VectorOperations<double>::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];

From 0f7fd39d72f509592da5c900216f9f5b5d442cf2 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Sat, 11 Mar 2023 10:22:21 +0100
Subject: [PATCH 2/2] changelog

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 79939ff0..35a14d84 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -43,6 +43,7 @@ will consitute of a breaking change warranting a new major release:
 - Solved naming collision between file used for solar array deployment and confirmation for
   ACS for solar array deployment.
 - Fixed that scaling of RW torque would result in a zero vector unless the maximum value was exceeded.
+- Bias for the GYR data was substracted within the wrong rf (sensor rf vs body rf).
 
 ## Changed