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

added sensor fusion logic for GYR
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2022-10-14 14:57:22 +02:00
parent 8f61d14a82
commit 0a5bd6ef73
3 changed files with 47 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
mission/controller/acs/AcsParameters.cpp
View File

@ -226,6 +226,9 @@ AcsParameters::~AcsParameters() {}
case 0x3:
parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix);
break;
case 0x4:
parameterWrapper->set(gyrHandlingParameters.gyrFusionWeight);
break;
default:
return INVALID_IDENTIFIER_ID;
}

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

@ -753,6 +753,7 @@ class AcsParameters /*: public HasParametersIF*/ {
double gyr1orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}};
double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
float gyrFusionWeight = 0.8;
} gyrHandlingParameters;
struct RwHandlingParameters {

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

@ -1,10 +1,3 @@
/*
* SensorProcessing.cpp
*
* Created on: 7 Mar 2022
* Author: Robin Marquardt
*/
#include "SensorProcessing.h"
#include <fsfw/globalfunctions/constants.h>
@ -397,59 +390,77 @@ void SensorProcessing::processGyr(
gyr3ValueBody[3] = {0, 0, 0};
bool validUnit[4] = {false, false, false, false};
uint8_t validCount = 0;
if (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid) {
const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
gyr0ValueBody, 3, 3, 1);
validCount += 1;
validUnit[0] = true;
}
if (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid) {
const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
gyr1ValueBody, 3, 3, 1);
validCount += 1;
validUnit[1] = true;
}
if (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid) {
const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
gyr2ValueBody, 3, 3, 1);
validCount += 1;
validUnit[2] = true;
}
if (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid) {
const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
gyr3ValueBody, 3, 3, 1);
validCount += 1;
validUnit[3] = true;
}
/* -------- SatRateEst: Middle Value ------- */
double gyrValues[3][4] = {
{gyr0ValueBody[0], gyr1ValueBody[0], gyr2ValueBody[0], gyr3ValueBody[0]},
{gyr0ValueBody[1], gyr1ValueBody[1], gyr2ValueBody[1], gyr3ValueBody[1]},
{gyr0ValueBody[2], gyr1ValueBody[2], gyr2ValueBody[2], gyr3ValueBody[2]}};
double gyrValidValues[3][validCount];
uint8_t j = 0;
for (uint8_t i = 0; i < validCount; i++) {
if (validUnit[i]) {
gyrValidValues[0][j] = gyrValues[0][i];
gyrValidValues[1][j] = gyrValues[1][i];
gyrValidValues[2][j] = gyrValues[2][i];
j += 1;
// take ADIS measurements, if both avail
// if just one ADIS measurement avail, perform sensor fusion
if (validUnit[0] && validUnit[2]) {
double gyr02ValuesSum[3];
VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, satRatEst, 3);
} else if ((validUnit[0] || validUnit[2]) && !(validUnit[1] || validUnit[3])) {
if (validUnit[0]) {
satRatEst = gyr0ValueBody;
} else if (validUnit[2]) {
satRatEst = gyr2ValueBody;
}
}
// Selection Sort
double gyrValidValuesSort[3][validCount];
MathOperations<double>::selectionSort(*gyrValidValues, *gyrValidValuesSort, 3, validCount);
} else if ((validUnit[1]) && (validUnit[3])) {
double gyr13ValuesSum[3];
double gyr13ValuesMean[3];
VectorOperations<double>::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3);
VectorOperations<double>::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3);
if (validUnit[0]) {
satRatEst[0] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight *
gyr13ValuesMean[0] +
gyrParameters->gyrFusionWeight * gyr0ValueBody[0];
satRatEst[1] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight *
gyr13ValuesMean[1] +
gyrParameters->gyrFusionWeight * gyr0ValueBody[1];
satRatEst[2] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight *
gyr13ValuesMean[2] +
gyrParameters->gyrFusionWeight * gyr0ValueBody[2];
uint8_t n = ceil(validCount / 2);
satRatEst[0] = gyrValidValuesSort[0][n];
satRatEst[1] = gyrValidValuesSort[1][n];
satRatEst[2] = gyrValidValuesSort[2][n];
} else if (validUnit[2]) {
satRatEst[0] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight *
gyr13ValuesMean[0] +
gyrParameters->gyrFusionWeight * gyr2ValueBody[0];
satRatEst[1] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight *
gyr13ValuesMean[1] +
gyrParameters->gyrFusionWeight * gyr2ValueBody[1];
satRatEst[2] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight *
gyr13ValuesMean[2] +
gyrParameters->gyrFusionWeight * gyr2ValueBody[2];
} else
satRatEst = gyr13ValuesMean;
} else if (validUnit[1]) {
satRatEst = gyr1ValueBody;
} else if (validUnit[3]) {
satRatEst = gyr3ValueBody;
}
*satRateEstValid = true;
}