diff --git a/CHANGELOG.md b/CHANGELOG.md index d403c0b5..dc4499c4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,11 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- SUS total vector was not reset to being a zero vector during eclipse due to a wrong memcpy + length. + # [v4.0.0] to be released # [v3.1.1] 2023-06-14 diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index b106baaa..b726ca53 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -30,10 +30,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const // ------------------------------------------------ double magIgrfModel[3] = {0.0, 0.0, 0.0}; if (gpsValid) { - // Should be existing class object which will be called and modified here. Igrf13Model igrf13; - // So the line above should not be done here. Update: Can be done here as long updated coffs - // stored in acsParameters ? igrf13.schmidtNormalization(); igrf13.updateCoeffGH(timeOfMgmMeasurement); // maybe put a condition here, to only update after a full day, this @@ -45,14 +42,13 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const { PoolReadGuard pg(mgmDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - float zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm0vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm1vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm2vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm3vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm4vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTot.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(double)); mgmDataProcessed->setValidity(false, true); std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double)); mgmDataProcessed->magIgrfModel.setValid(gpsValid); @@ -252,21 +248,20 @@ void SensorProcessing::processSus( { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - float zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus3vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus4vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus5vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus6vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus7vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus8vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus0vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus1vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus2vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus3vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus4vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus5vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus6vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus7vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus8vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus9vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus10vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus11vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTot.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(susDataProcessed->susVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(double)); susDataProcessed->setValidity(false, true); std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); susDataProcessed->sunIjkModel.setValid(true); @@ -274,7 +269,6 @@ void SensorProcessing::processSus( } return; } - // WARNING: NOT TRANSFORMED IN BODY FRAME YET // Transformation into Geomtry Frame float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0}, sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0}, @@ -459,12 +453,11 @@ void SensorProcessing::processGyr( { PoolReadGuard pg(gyrDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - double zeroVector[3] = {0.0, 0.0, 0.0}; - std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyr3vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyrVecTot.value, zeroVector, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr0vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr1vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr2vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr3vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyrVecTot.value, ZERO_VEC_D, 3 * sizeof(double)); gyrDataProcessed->setValidity(false, true); } } diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 47449a75..6dbc5d58 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -23,6 +23,9 @@ class SensorProcessing { acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters); // Will call protected functions private: + static constexpr float ZERO_VEC_F[3] = {0, 0, 0}; + static constexpr double ZERO_VEC_D[3] = {0, 0, 0}; + protected: // short description needed for every function void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,