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(
bool sunValid, double sunB[3], double sunRateB[3], bool magValid, double magB[3],
double magRateB[3], bool rotRateValid, double rotRateB[3], double fusedRateB[3],
double fusedRateParallelB[3], double fusedRateOrthogonalB[3]) {
if ((not magValid) or (not sunValid and not VectorOperations<double>::norm(fusedRateOldB, 3)) or
((not VectorOperations<double>::norm(sunRateB, 3) and
not VectorOperations<double>::norm(magRateB, 3)))) {
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
if ((not mgmDataProcessed->mgmVecTot.isValid()) or
(not susDataProcessed->susVecTot.isValid() and
VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3)) == 0 or
((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;
}
if (not sunValid) {
estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB,
fusedRateOrthogonalB);
if (not susDataProcessed->susVecTot.isValid()) {
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
return;
}
// calculate rotation around the sun
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 magNorm = VectorOperations<double>::norm(magB, 3);
double magNorm = VectorOperations<double>::norm(mgmDataProcessed->mgmVecTot.value, 3);
double fusedRotRateParallel[3] = {0, 0, 0};
if (magSunCrossNorm >
(acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) {
double omegaParallel =
VectorOperations<double>::dot(magRateB, magSunCross) * pow(magSunCrossNorm, -2);
double omegaParallelVec[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(sunB, omegaParallel, omegaParallelVec, 3);
std::memcpy(fusedRateParallelB, omegaParallelVec, 3 * sizeof(double)); // to dataset
VectorOperations<double>::dot(mgmDataProcessed->mgmVecTotDerivative.value, magSunCross) *
pow(magSunCrossNorm, -2);
VectorOperations<double>::mulScalar(susDataProcessed->susVecTot.value, omegaParallel,
fusedRotRateParallel, 3);
} else {
estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB,
fusedRateOrthogonalB); // ToDo
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
return;
}
// calculate rotation orthogonal to the sun
VectorOperations<double>::cross(sunRateB, sunB, fusedRateOrthogonalB);
VectorOperations<double>::mulScalar(fusedRateOrthogonalB,
pow(VectorOperations<double>::norm(sunB, 2), 2),
fusedRateOrthogonalB, 3);
double fusedRotRateOrthogonal[3] = {0, 0, 0};
VectorOperations<double>::cross(susDataProcessed->susVecTotDerivative.value,
susDataProcessed->susVecTot.value, fusedRotRateOrthogonal);
VectorOperations<double>::mulScalar(
fusedRotRateOrthogonal,
pow(VectorOperations<double>::norm(susDataProcessed->susVecTot.value, 2), 2),
fusedRotRateOrthogonal, 3);
// 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));
if (rotRateValid) {
std::memcpy(rotRateOldB, rotRateB, 3 * sizeof(double));
// store for calculation of angular acceleration
if (gyrDataProcessed->gyrVecTot.isValid()) {
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,
double rotRateB[3],
double fusedRateB[3],
double fusedRateParallelB[3],
double fusedRateOrthogonalB[3]) {
if (not rotRateValid or not VectorOperations<double>::norm(fusedRateOldB, 3)) {
void FusedRotationEstimation::estimateFusedRotationRateEclipse(
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
if (not gyrDataProcessed->gyrVecTot.isValid() or
VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.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;
}
double angAccelB[3] = {0, 0, 0};
VectorOperations<double>::subtract(rotRateB, rotRateOldB, angAccelB, 3);
double omegaTotVec[3] = {0, 0, 0};
VectorOperations<double>::add(fusedRateOldB, angAccelB, fusedRateB, 3);
VectorOperations<double>::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3);
double fusedRotRateTotal[3] = {0, 0, 0};
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_
#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
class FusedRotationEstimation {
public:
FusedRotationEstimation(AcsParameters *acsParameters_);
void estimateFusedRotationRateSafe(bool sunValid, double sunB[3], double sunRateB[3],
bool magValid, double magB[3], double magRateB[3],
bool rotRateValid, double rotRateB[3], double fusedRateB[3],
double fusedRateParallelB[3], double fusedRateOrthogonalB[3]);
void estimateFusedRotationRateEclipse(bool rotRateValid, double rotRateB[3], double fusedRateB[3],
double fusedRateParallelB[3],
double fusedRateOrthogonalB[3]);
void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateData *fusedRotRateData);
protected:
private:
double constexpr ZERO_VEC[3] = {0, 0, 0};
AcsParameters *acsParameters;
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_ */