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(
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));
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_
#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_ */