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

prevent div by zero
Some checks are pending
EIVE/eive-obsw/pipeline/pr-develop Build queued...
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-04-14 18:56:05 +02:00
parent e68ac9cc2d
commit 6934721a42
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -141,7 +141,7 @@ 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);
if (timeOfSavedMagFieldEst.tv_sec != 0) { if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0) {
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff; mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
savedMgmVecTot[i] = mgmVecTot[i]; savedMgmVecTot[i] = mgmVecTot[i];
@ -394,7 +394,7 @@ 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) { if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0) {
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff; susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff;
savedSusVecTot[i] = susVecTot[i]; savedSusVecTot[i] = susVecTot[i];
@ -581,8 +581,9 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
// Calculation of the satellite velocity in earth fixed frame // Calculation of the satellite velocity in earth fixed frame
double deltaDistance[3] = {0, 0, 0}; double deltaDistance[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE); MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
if (validSavedPosSatE && if (validSavedPosSatE and
(gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax) and
(gpsUnixSeconds - timeOfSavedPosSatE) > 0) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3); VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE; double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE;
VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3); VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3);