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

MGM calibration fix #357

Merged
muellerr merged 13 commits from eggert/mgm-calibration-fix into develop 2023-02-08 13:02:41 +01:00
Conversation 0 Commits 13 Files Changed 5 +65 -54
5 changed files with 65 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
CHANGELOG.md
View File

@ -17,12 +17,19 @@ change warranting a new major release:
# [unreleased]
## Changed
- Readded calibration matrices for MGM calibration
## Fixed
- Build system: Fixed small bug, where the version itself was
stored as the git SHA hash in `commonConfig.h`. This will be
an empty string now for regular versions.
- Bump FSFW for important fix in PUS mode service.
- Bugfixes in 'SensofProcessing' where previously MGM values would be calibrated before being
transformed in body RF. However, the calibration values are in the body RF. Also fixed the
validity flag of 'mgmVecTotDerivative'.
# [v1.25.0] 2023-02-06

2
mission/acsDefs.h
View File

@ -7,7 +7,7 @@
namespace acs {
enum CtrlSubmode {
OFF = HasModesIF::MODE_OFF,
OFF = HasModesIF::SUBMODE_NONE,
SAFE = 10,
DETUMBLE = 11,
IDLE = 12,

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 > 6) || (submode < 2)) {
if ((submode > acs::PTG_TARGET_INERTIAL) || (submode < acs::SAFE)) {
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);