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

amended gyr sensor fusion
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2022-11-04 12:41:35 +01:00
parent c7bfe4002d
commit a4735defaa
2 changed files with 23 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
mission/controller/acs/AcsParameters.h
View File

@ -762,6 +762,8 @@ class AcsParameters /*: public HasParametersIF*/ {
pow(3.0e-3 * sqrt(2), 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
pow(4.3e-3 * sqrt(2), 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
enum PreferAdis { NO = 0, YES = 1 };
uint8_t preferAdis = PreferAdis::YES;
} gyrHandlingParameters;
struct RwHandlingParameters {

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

@ -480,80 +480,57 @@ void SensorProcessing::processGyr(
// Transforming Values to the Body Frame (actually it is the geometry frame atm)
double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0},
gyr3ValueBody[3] = {0, 0, 0};
float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
if (gyr0valid) {
const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
gyr0ValueBody, 3, 3, 1);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
}
}
if (gyr1valid) {
const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
gyr1ValueBody, 3, 3, 1);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
}
}
if (gyr2valid) {
const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
gyr2ValueBody, 3, 3, 1);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
}
}
if (gyr3valid) {
const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
gyr3ValueBody, 3, 3, 1);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
}
}
/* -------- SatRateEst: Middle Value ------- */
// take ADIS measurements, if both avail
// if just one ADIS measurement avail, perform sensor fusion
double gyrVecTot[3] = {0.0, 0.0, 0.0};
if (gyr0valid && gyr2valid) {
if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == gyrParameters->PreferAdis::YES) {
double gyr02ValuesSum[3];
VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3);
} else if ((gyr0valid || gyr2valid) && !(gyr1valid || gyr3valid)) {
if (gyr0valid) {
std::memcpy(gyrVecTot, gyr0ValueBody, 3 * sizeof(double));
} else if (gyr2valid) {
std::memcpy(gyrVecTot, gyr2ValueBody, 3 * sizeof(double));
} else {
for (uint8_t i = 0; i < 3; i++) {
gyrVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
}
} else if (gyr1valid && gyr3valid) {
double gyr13ValuesSum[3];
double gyr13ValuesMean[3];
VectorOperations<double>::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3);
VectorOperations<double>::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3);
if (gyr0valid) {
gyrVecTot[0] =
((gyr0ValueBody[0] / gyrParameters->gyr02variance[0]) +
(gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) /
((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0]));
gyrVecTot[1] =
((gyr0ValueBody[1] / gyrParameters->gyr02variance[1]) +
(gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) /
((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1]));
gyrVecTot[2] =
((gyr0ValueBody[2] / gyrParameters->gyr02variance[2]) +
(gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) /
((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2]));
} else if (gyr2valid) {
gyrVecTot[0] =
((gyr2ValueBody[0] / gyrParameters->gyr02variance[0]) +
(gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) /
((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0]));
gyrVecTot[1] =
((gyr2ValueBody[1] / gyrParameters->gyr02variance[1]) +
(gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) /
((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1]));
gyrVecTot[2] =
((gyr2ValueBody[2] / gyrParameters->gyr02variance[2]) +
(gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) /
((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2]));
} else {
std::memcpy(gyrVecTot, gyr13ValuesMean, 3 * sizeof(double));
}
} else if (gyr1valid) {
std::memcpy(gyrVecTot, gyr1ValueBody, 3 * sizeof(double));
} else if (gyr3valid) {
std::memcpy(gyrVecTot, gyr3ValueBody, 3 * sizeof(double));
}
{
PoolReadGuard pg(gyrDataProcessed);