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

Cleaner Calculation of MGM and SUS Rate #775

Merged
muellerr merged 7 commits from mgm-sus-rate-calc into main 2023-08-14 15:28:33 +02:00
Conversation 0 Commits 7 Files Changed 3 +7 -8
1 changed files with 7 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 110c822f41 - Show all commits

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

@ -142,8 +142,6 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool mgmVecTotDerivativeValid = false; bool mgmVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
sif::debug << "timeDiff = " << timeDiff << std::endl;
sif::debug << "tv_sec = " << (double)timeOfSavedMagFieldEst.tv_sec << std::endl;
if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and
VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) { VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
VectorOperations<double>::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3); VectorOperations<double>::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3);
@ -280,6 +278,7 @@ void SensorProcessing::processSus(
susDataProcessed->sunIjkModel.setValid(true); susDataProcessed->sunIjkModel.setValid(true);
} }
} }
std::memcpy(savedSusVecTot, ZERO_VEC_D, sizeof(savedSusVecTot));
return; return;
} }
@ -368,13 +367,13 @@ void SensorProcessing::processSus(
double susVecTotDerivative[3] = {0.0, 0.0, 0.0}; double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool susVecTotDerivativeValid = false; bool susVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0) { if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0 and
for (uint8_t i = 0; i < 3; i++) { VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff; VectorOperations<double>::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3);
savedSusVecTot[i] = susVecTot[i]; VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDiff, susVecTotDerivative, 3);
susVecTotDerivativeValid = true; susVecTotDerivativeValid = true;
} }
} std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot));
if (VectorOperations<double>::norm(susVecTotDerivative, 3) != 0 and if (VectorOperations<double>::norm(susVecTotDerivative, 3) != 0 and
susDataProcessed->susVecTotDerivative.isValid()) { susDataProcessed->susVecTotDerivative.isValid()) {
lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value, lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,