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

added calibration of raw MGM values
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-12 17:14:33 +02:00
parent 8edf7fc0de
commit cc35db91cd
2 changed files with 49 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -36,6 +36,25 @@ class AcsParameters /*: public HasParametersIF*/ {
float mgm2orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; float mgm2orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; 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 mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
float mgm0hardIronOffset[3] = {19.89364, -29.94111, -31.07508};
float mgm1hardIronOffset[3] = {10.95500, -8.053403, -33.36383};
float mgm2hardIronOffset[3] = {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 mgm0softIronInverse[3][3] = {{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] = {{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] = {{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}};
} mgmHandlingParameters; } mgmHandlingParameters;
struct SusHandlingParameters { struct SusHandlingParameters {

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

@ -42,38 +42,62 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
validMagField = false; validMagField = false;
return false; return false;
} }
// Transforming Values to the Body Frame (actually it is the geometry frame atm) 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}, 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}; mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0};
bool validUnit[5] = {false, false, false, false, false}; bool validUnit[5] = {false, false, false, false, false};
uint8_t validCount = 0; uint8_t validCount = 0;
if (mgm0valid) { if (mgm0valid) {
MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value, VectorOperations<float>::subtract(mgm0Value, 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); mgm0ValueBody, 3, 3, 1);
validCount += 1; validCount += 1;
validUnit[0] = true; validUnit[0] = true;
} }
if (mgm1valid) { if (mgm1valid) {
MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value, VectorOperations<float>::subtract(mgm1Value, 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); mgm1ValueBody, 3, 3, 1);
validCount += 1; validCount += 1;
validUnit[1] = true; validUnit[1] = true;
} }
if (mgm2valid) { if (mgm2valid) {
MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value, VectorOperations<float>::subtract(mgm2Value, 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); mgm2ValueBody, 3, 3, 1);
validCount += 1; validCount += 1;
validUnit[2] = true; validUnit[2] = true;
} }
if (mgm3valid) { if (mgm3valid) {
MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value, VectorOperations<float>::subtract(mgm3Value, 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); mgm3ValueBody, 3, 3, 1);
validCount += 1; validCount += 1;
validUnit[3] = true; validUnit[3] = true;
} }
if (mgm4valid) { if (mgm4valid) {
MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4Value, VectorOperations<float>::subtract(mgm4Value, 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); mgm4ValueBody, 3, 3, 1);
validCount += 1; validCount += 1;
validUnit[4] = true; validUnit[4] = true;