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

Merge remote-tracking branch 'origin/develop' into acs_update_scheduling
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Details

Browse Source
This commit is contained in:
Robin Müller
2023-02-08 13:08:00 +01:00
parent 6fa1084d21 7c53cafbbe
commit 4412690652
7 changed files with 177 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
mission/controller/AcsController.cpp
View File

@ -426,7 +426,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
return INVALID_SUBMODE;
}
} else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
if (submode < acs::AcsModes::SAFE or submode > acs::AcsModes::PTG_TARGET_INERTIAL) {
if ((submode < acs::AcsModes::SAFE) or (submode > acs::AcsModes::PTG_TARGET_INERTIAL)) {
return INVALID_SUBMODE;
} else {
return returnvalue::OK;

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

@ -37,26 +37,28 @@ class AcsParameters /*: public HasParametersIF*/ {
float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}};
float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
float mgm0hardIronOffset[3] = {0.0, 0.0, 0.0}; //{19.89364, -29.94111, -31.07508};
float mgm1hardIronOffset[3] = {0.0, 0.0, 0.0}; //{10.95500, -8.053403, -33.36383};
float mgm2hardIronOffset[3] = {0.0, 0.0, 0.0}; //{15.72181, -26.87090, -62.19010};
float mgm3hardIronOffset[3] = {0.0, 0.0, 0.0};
float mgm4hardIronOffset[3] = {0.0, 0.0, 0.0};
float mgm0hardIronOffset[3] = {6.116487, 6.796264, -19.188060};
float mgm1hardIronOffset[3] = {-1.077152, 2.080583, 1.974483};
float mgm2hardIronOffset[3] = {-19.285857, 5.401821, -16.096297};
float mgm3hardIronOffset[3] = {-0.634033, 2.787695, 0.092036};
float mgm4hardIronOffset[3] = {2.702743, 5.236043, 0.726229};
float mgm0softIronInverse[3][3] = {{0.910192, -0.188413, -0.161522},
{-0.188413, 1.642303, -0.033184},
{-0.161522, -0.033184, 0.943904}};
float mgm1softIronInverse[3][3] = {{1.053508, -0.170225, -0.041678},
{-0.170225, 1.274465, -0.040231},
{-0.041678, -0.040231, 1.086352}};
float mgm2softIronInverse[3][3] = {{0.931086, 0.172675, -0.043084},
{0.172675, 1.541296, 0.065489},
{-0.043084, 0.065489, 1.001238}};
float mgm3softIronInverse[3][3] = {{1.073353, 0.177266, -0.058832},
{0.177266, 1.262156, 0.010478},
{-0.058832, 0.010478, 1.068345}};
float mgm4softIronInverse[3][3] = {{1.114887, -0.007534, -0.037072},
{-0.007534, 1.253879, 0.006812},
{-0.037072, 0.006812, 1.313158}};
float mgm0softIronInverse[3][3] = {
{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{1420.727e-3, 9.352825e-3,
-127.1979e-3}, {9.352825e-3, 1031.965e-3, -80.02734e-3},
{-127.1979e-3, -80.02734e-3, 934.8899e-3}};*/
float mgm1softIronInverse[3][3] = {
{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{126.7325e-2, -4.146410e-2, -18.37963e-2},
{-4.146410e-2, 109.3310e-2, -5.246314e-2},
{-18.37963e-2, -5.246314e-2, 105.7300e-2}};*/
float mgm2softIronInverse[3][3] = {
{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{143.0438e-2, 7.095763e-2,
15.67482e-2}, {7.095763e-2, 99.65167e-2, -6.958760e-2},
{15.67482e-2, -6.958760e-2, 94.50124e-2}};*/
float mgm3softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
float mgm4softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
float mgm02variance[3] = {1, 1, 1};
float mgm13variance[3] = {1, 1, 1};
float mgm4variance[3] = {1, 1, 1};

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

@ -47,71 +47,72 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0},
mgm4ValueNoBias[3] = {0, 0, 0};
float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
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 mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
if (mgm0valid) {
VectorOperations<float>::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias,
3);
MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value,
mgm0ValueBody, 3, 3, 1);
VectorOperations<float>::subtract(mgm0ValueBody, mgmParameters->mgm0hardIronOffset,
mgm0ValueNoBias, 3);
MatrixOperations<float>::multiply(mgmParameters->mgm0softIronInverse[0], mgm0ValueNoBias,
mgm0ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib,
mgm0ValueBody, 3, 3, 1);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i];
sensorFusionNumerator[i] += mgm0ValueCalib[i] / mgmParameters->mgm02variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
}
}
if (mgm1valid) {
VectorOperations<float>::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias,
3);
MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value,
mgm1ValueBody, 3, 3, 1);
VectorOperations<float>::subtract(mgm1ValueBody, mgmParameters->mgm1hardIronOffset,
mgm1ValueNoBias, 3);
MatrixOperations<float>::multiply(mgmParameters->mgm1softIronInverse[0], mgm1ValueNoBias,
mgm1ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib,
mgm1ValueBody, 3, 3, 1);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i];
sensorFusionNumerator[i] += mgm1ValueCalib[i] / mgmParameters->mgm13variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
}
}
if (mgm2valid) {
VectorOperations<float>::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias,
3);
MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value,
mgm2ValueBody, 3, 3, 1);
VectorOperations<float>::subtract(mgm2ValueBody, mgmParameters->mgm2hardIronOffset,
mgm2ValueNoBias, 3);
MatrixOperations<float>::multiply(mgmParameters->mgm2softIronInverse[0], mgm2ValueNoBias,
mgm2ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib,
mgm2ValueBody, 3, 3, 1);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i];
sensorFusionNumerator[i] += mgm2ValueCalib[i] / mgmParameters->mgm02variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
}
}
if (mgm3valid) {
VectorOperations<float>::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias,
3);
MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value,
mgm3ValueBody, 3, 3, 1);
VectorOperations<float>::subtract(mgm3ValueBody, mgmParameters->mgm3hardIronOffset,
mgm3ValueNoBias, 3);
MatrixOperations<float>::multiply(mgmParameters->mgm3softIronInverse[0], mgm3ValueNoBias,
mgm3ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib,
mgm3ValueBody, 3, 3, 1);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i];
sensorFusionNumerator[i] += mgm3ValueCalib[i] / mgmParameters->mgm13variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
}
}
if (mgm4valid) {
float mgm4ValueNT[3];
VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueNT, 3); // uT to nT
VectorOperations<float>::subtract(mgm4ValueNT, mgmParameters->mgm4hardIronOffset,
float mgm4ValueUT[3];
VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueUT, 3); // nT to uT
MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueUT,
mgm4ValueBody, 3, 3, 1);
VectorOperations<float>::subtract(mgm4ValueBody, mgmParameters->mgm4hardIronOffset,
mgm4ValueNoBias, 3);
MatrixOperations<float>::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias,
mgm4ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib,
mgm4ValueBody, 3, 3, 1);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i];
sensorFusionNumerator[i] += mgm4ValueCalib[i] / mgmParameters->mgm4variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
}
}
@ -128,6 +129,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
for (uint8_t i = 0; i < 3; i++) {
mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
savedMgmVecTot[i] = mgmVecTot[i];
mgmVecTotDerivativeValid = true;
}
}
timeOfSavedMagFieldEst = timeOfMgmMeasurement;
@ -148,15 +150,15 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
{
PoolReadGuard pg(mgmDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueBody, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueCalib, 3 * sizeof(float));
mgmDataProcessed->mgm0vec.setValid(mgm0valid);
std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueBody, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueCalib, 3 * sizeof(float));
mgmDataProcessed->mgm1vec.setValid(mgm1valid);
std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueBody, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueCalib, 3 * sizeof(float));
mgmDataProcessed->mgm2vec.setValid(mgm2valid);
std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueBody, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueCalib, 3 * sizeof(float));
mgmDataProcessed->mgm3vec.setValid(mgm3valid);
std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueCalib, 3 * sizeof(float));
mgmDataProcessed->mgm4vec.setValid(mgm4valid);
std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(double));
mgmDataProcessed->mgmVecTot.setValid(true);