eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

Fix for GYR Bias Removal #457

Merged
muellerr merged 2 commits from gyr-bias-fix into develop 2023-03-11 14:44:50 +01:00
Conversation 1 Commits 2 Files Changed 2 +9 -8
2 changed files with 9 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CHANGELOG.md
View File

@ -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 - Solved naming collision between file used for solar array deployment and confirmation for
ACS for solar array deployment. ACS for solar array deployment.
- Fixed that scaling of RW torque would result in a zero vector unless the maximum value was exceeded. - 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 ## Changed

16
mission/controller/acs/SensorProcessing.cpp
View File

@ -470,10 +470,10 @@ void SensorProcessing::processGyr(
float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0}; float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
if (gyr0valid) { 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, MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
gyr0ValueBody, 3, 3, 1); gyr0ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr0ValueBody, gyrParameters->gyr0bias, gyr0ValueBody, 3);
VectorOperations<double>::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3); VectorOperations<double>::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i]; sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
@ -481,10 +481,10 @@ void SensorProcessing::processGyr(
} }
} }
if (gyr1valid) { 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, MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
gyr1ValueBody, 3, 3, 1); gyr1ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr1ValueBody, gyrParameters->gyr1bias, gyr1ValueBody, 3);
VectorOperations<double>::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3); VectorOperations<double>::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i]; sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
@ -492,10 +492,10 @@ void SensorProcessing::processGyr(
} }
} }
if (gyr2valid) { 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, MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
gyr2ValueBody, 3, 3, 1); gyr2ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr2ValueBody, gyrParameters->gyr2bias, gyr2ValueBody, 3);
VectorOperations<double>::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3); VectorOperations<double>::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i]; sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
@ -503,10 +503,10 @@ void SensorProcessing::processGyr(
} }
} }
if (gyr3valid) { 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, MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
gyr3ValueBody, 3, 3, 1); gyr3ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr3ValueBody, gyrParameters->gyr3bias, gyr3ValueBody, 3);
VectorOperations<double>::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3); VectorOperations<double>::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i]; sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];