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

use datasets

Browse Source
This commit is contained in:
Marius Eggert 2023-07-20 11:09:34 +02:00
parent d90fd07ba3
commit 300a6c5ff2
2 changed files with 82 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
mission/controller/acs/FusedRotationEstimation.cpp
View File

@ -5,64 +5,100 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_)
} }
void FusedRotationEstimation::estimateFusedRotationRateSafe( void FusedRotationEstimation::estimateFusedRotationRateSafe(
bool sunValid, double sunB[3], double sunRateB[3], bool magValid, double magB[3], acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
double magRateB[3], bool rotRateValid, double rotRateB[3], double fusedRateB[3], acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
double fusedRateParallelB[3], double fusedRateOrthogonalB[3]) { if ((not mgmDataProcessed->mgmVecTot.isValid()) or
if ((not magValid) or (not sunValid and not VectorOperations<double>::norm(fusedRateOldB, 3)) or (not susDataProcessed->susVecTot.isValid() and
((not VectorOperations<double>::norm(sunRateB, 3) and VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3)) == 0 or
not VectorOperations<double>::norm(magRateB, 3)))) { ((VectorOperations<double>::norm(susDataProcessed->susVecTotDerivative.value, 3) == 0 and
VectorOperations<double>::norm(mgmDataProcessed->mgmVecTotDerivative.value, 3) == 0))) {
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
}
return; return;
} }
if (not sunValid) { if (not susDataProcessed->susVecTot.isValid()) {
estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB, estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
fusedRateOrthogonalB);
return; return;
} }
// calculate rotation around the sun // calculate rotation around the sun
double magSunCross[3] = {0, 0, 0}; double magSunCross[3] = {0, 0, 0};
VectorOperations<double>::cross(magB, sunB, magSunCross); VectorOperations<double>::cross(mgmDataProcessed->mgmVecTot.value,
susDataProcessed->susVecTot.value, magSunCross);
double magSunCrossNorm = VectorOperations<double>::norm(magSunCross, 3); double magSunCrossNorm = VectorOperations<double>::norm(magSunCross, 3);
double magNorm = VectorOperations<double>::norm(magB, 3); double magNorm = VectorOperations<double>::norm(mgmDataProcessed->mgmVecTot.value, 3);
double fusedRotRateParallel[3] = {0, 0, 0};
if (magSunCrossNorm > if (magSunCrossNorm >
(acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) { (acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) {
double omegaParallel = double omegaParallel =
VectorOperations<double>::dot(magRateB, magSunCross) * pow(magSunCrossNorm, -2); VectorOperations<double>::dot(mgmDataProcessed->mgmVecTotDerivative.value, magSunCross) *
double omegaParallelVec[3] = {0, 0, 0}; pow(magSunCrossNorm, -2);
VectorOperations<double>::mulScalar(sunB, omegaParallel, omegaParallelVec, 3); VectorOperations<double>::mulScalar(susDataProcessed->susVecTot.value, omegaParallel,
std::memcpy(fusedRateParallelB, omegaParallelVec, 3 * sizeof(double)); // to dataset fusedRotRateParallel, 3);
} else { } else {
estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB, estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
fusedRateOrthogonalB); // ToDo
return; return;
} }
// calculate rotation orthogonal to the sun // calculate rotation orthogonal to the sun
VectorOperations<double>::cross(sunRateB, sunB, fusedRateOrthogonalB); double fusedRotRateOrthogonal[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(fusedRateOrthogonalB, VectorOperations<double>::cross(susDataProcessed->susVecTotDerivative.value,
pow(VectorOperations<double>::norm(sunB, 2), 2), susDataProcessed->susVecTot.value, fusedRotRateOrthogonal);
fusedRateOrthogonalB, 3); VectorOperations<double>::mulScalar(
fusedRotRateOrthogonal,
pow(VectorOperations<double>::norm(susDataProcessed->susVecTot.value, 2), 2),
fusedRotRateOrthogonal, 3);
// calculate total rotation rate // calculate total rotation rate
VectorOperations<double>::add(fusedRateParallelB, fusedRateOrthogonalB, fusedRateB); double fusedRotRateTotal[3] = {0, 0, 0};
VectorOperations<double>::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal);
std::memcpy(fusedRateOldB, fusedRateB, 3 * sizeof(double)); // store for calculation of angular acceleration
if (rotRateValid) { if (gyrDataProcessed->gyrVecTot.isValid()) {
std::memcpy(rotRateOldB, rotRateB, 3 * sizeof(double)); std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
}
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal,
3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, fusedRotRateParallel, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
fusedRotRateData->setValidity(true, true);
} }
} }
void FusedRotationEstimation::estimateFusedRotationRateEclipse(bool rotRateValid, void FusedRotationEstimation::estimateFusedRotationRateEclipse(
double rotRateB[3], acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
double fusedRateB[3], if (not gyrDataProcessed->gyrVecTot.isValid() or
double fusedRateParallelB[3], VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) {
double fusedRateOrthogonalB[3]) { {
if (not rotRateValid or not VectorOperations<double>::norm(fusedRateOldB, 3)) { PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
}
return; return;
} }
double angAccelB[3] = {0, 0, 0}; double angAccelB[3] = {0, 0, 0};
VectorOperations<double>::subtract(rotRateB, rotRateOldB, angAccelB, 3); VectorOperations<double>::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3);
double omegaTotVec[3] = {0, 0, 0}; double fusedRotRateTotal[3] = {0, 0, 0};
VectorOperations<double>::add(fusedRateOldB, angAccelB, fusedRateB, 3); VectorOperations<double>::add(fusedRotRateData->rotRateTotal.value, angAccelB, fusedRotRateTotal,
3);
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
}
} }

20
mission/controller/acs/FusedRotationEstimation.h
View File

@ -1,26 +1,30 @@
#ifndef MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ #ifndef MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ #define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h> #include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
class FusedRotationEstimation { class FusedRotationEstimation {
public: public:
FusedRotationEstimation(AcsParameters *acsParameters_); FusedRotationEstimation(AcsParameters *acsParameters_);
void estimateFusedRotationRateSafe(bool sunValid, double sunB[3], double sunRateB[3], void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed,
bool magValid, double magB[3], double magRateB[3], acsctrl::MgmDataProcessed *mgmDataProcessed,
bool rotRateValid, double rotRateB[3], double fusedRateB[3], acsctrl::GyrDataProcessed *gyrDataProcessed,
double fusedRateParallelB[3], double fusedRateOrthogonalB[3]); acsctrl::FusedRotRateData *fusedRotRateData);
void estimateFusedRotationRateEclipse(bool rotRateValid, double rotRateB[3], double fusedRateB[3],
double fusedRateParallelB[3],
double fusedRateOrthogonalB[3]);
protected: protected:
private: private:
double constexpr ZERO_VEC[3] = {0, 0, 0};
AcsParameters *acsParameters; AcsParameters *acsParameters;
double rotRateOldB[3] = {0, 0, 0}; double rotRateOldB[3] = {0, 0, 0};
double fusedRateOldB[3] = {0, 0, 0}; // double fusedRateOldB[3] = {0, 0, 0};
void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateData *fusedRotRateData);
}; };
#endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */ #endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */