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

fixed bias substraction

Browse Source
This commit is contained in:
Marius Eggert 2023-03-11 10:21:09 +01:00
parent b4129950cf
commit 9892938601
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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};
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];