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

New Safe Mode Controller #748

Merged
muellerr merged 31 commits from acs-safe-flp-v2 into main 2023-07-26 11:08:12 +02:00
Conversation 2 Commits 31 Files Changed 15 +82 -42
2 changed files with 82 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 300a6c5ff2 - Show all commits

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));
muellerr marked this conversation as resolved
muellerr commented 2023-07-25 10:30:29 +02:00
Review
Copy Link

im an AOCS noob, those 2 being invalid is on purpose?

im an AOCS noob, those 2 being invalid is on purpose?
meggert commented 2023-07-25 11:26:51 +02:00
Review
Copy Link

they are. During eclipse we take the last valid rotational rate calculated and use the GYRs to continue from there. Therefore the split into orthogonal and parallel is never calculated here.

they are. During eclipse we take the last valid rotational rate calculated and use the GYRs to continue from there. Therefore the split into orthogonal and parallel is never calculated here.
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_ */