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

First Version of ACS Controller #329

Merged
muellerr merged 106 commits from acs-ctrl-v1 into develop 2022-12-02 16:21:58 +01:00
Conversation 0 Commits 106 Files Changed 50 +50 -54
1 changed files with 50 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit c50f8c716f - Show all commits

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

@ -42,9 +42,8 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0},
mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0};
float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
bool validUnit[5] = {false, false, false, false, false};
uint8_t validCount = 0;
if (mgm0valid) {
VectorOperations<float>::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias,
3);
@ -52,8 +51,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm0ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib,
mgm0ValueBody, 3, 3, 1);
validCount += 1;
validUnit[0] = true;
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
}
}
if (mgm1valid) {
VectorOperations<float>::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias,
@ -62,8 +63,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm1ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib,
mgm1ValueBody, 3, 3, 1);
validCount += 1;
validUnit[1] = true;
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
}
}
if (mgm2valid) {
VectorOperations<float>::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias,
@ -72,8 +75,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm2ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib,
mgm2ValueBody, 3, 3, 1);
validCount += 1;
validUnit[2] = true;
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
}
}
if (mgm3valid) {
VectorOperations<float>::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias,
@ -82,8 +87,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm3ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib,
mgm3ValueBody, 3, 3, 1);
validCount += 1;
validUnit[3] = true;
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
}
}
if (mgm4valid) {
VectorOperations<float>::subtract(mgm4Value, mgmParameters->mgm4hardIronOffset, mgm4ValueNoBias,
@ -92,33 +99,17 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm4ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib,
mgm4ValueBody, 3, 3, 1);
validCount += 1;
validUnit[4] = true;
}
/* -------- MagFieldEst: Middle Value ------- */
float mgmValues[3][5] = {
{mgm0ValueBody[0], mgm1ValueBody[0], mgm2ValueBody[0], mgm3ValueBody[0], mgm4ValueBody[0]},
{mgm0ValueBody[1], mgm1ValueBody[1], mgm2ValueBody[1], mgm3ValueBody[1], mgm4ValueBody[1]},
{mgm0ValueBody[2], mgm1ValueBody[2], mgm2ValueBody[2], mgm3ValueBody[2], mgm4ValueBody[2]}};
double mgmValidValues[3][validCount];
uint8_t j = 0;
for (uint8_t i = 0; i < validCount; i++) {
if (validUnit[i]) {
mgmValidValues[0][j] = mgmValues[0][i];
mgmValidValues[1][j] = mgmValues[1][i];
mgmValidValues[2][j] = mgmValues[2][i];
j += 1;
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
}
}
// Selection Sort
double mgmValidValuesSort[3][validCount];
MathOperations<double>::selectionSort(*mgmValidValues, *mgmValidValuesSort, 3, validCount);
uint8_t n = ceil(validCount / 2);
magFieldEst[0] = mgmValidValuesSort[0][n];
magFieldEst[1] = mgmValidValuesSort[1][n];
magFieldEst[2] = mgmValidValuesSort[2][n];
for (uint8_t i = 0; i < 3; i++) {
magFieldEst[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
}
sif::debug << "magFeildEst: " << magFieldEst[0] << " , " << magFieldEst[1] << " , "
<< magFieldEst[2] << std::endl;
;
validMagField = true;
//-----------------------Mag Rate Computation ---------------------------------------------------
@ -434,26 +425,31 @@ void SensorProcessing::processGyr(
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];
satRatEst[0] =
((gyr0ValueBody[0] / gyrParameters->gyr02variance[0]) +
(gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) /
((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0]));
satRatEst[1] =
((gyr0ValueBody[1] / gyrParameters->gyr02variance[1]) +
(gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) /
((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1]));
satRatEst[2] =
((gyr0ValueBody[2] / gyrParameters->gyr02variance[2]) +
(gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) /
((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2]));
} 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];
satRatEst[0] =
((gyr2ValueBody[0] / gyrParameters->gyr02variance[0]) +
(gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) /
((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0]));
satRatEst[1] =
((gyr2ValueBody[1] / gyrParameters->gyr02variance[1]) +
(gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) /
((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1]));
satRatEst[2] =
((gyr2ValueBody[2] / gyrParameters->gyr02variance[2]) +
(gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) /
((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2]));
} else
satRatEst = gyr13ValuesMean;
} else if (validUnit[1]) {