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 +7 -14
2 changed files with 7 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 7e1e02c2bc - Show all commits

18
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -48,8 +48,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun); errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB); splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(sunDirB, errorAngle, calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelMekf,
acsParameters->safeModeControllerParameters.k_parallelMekf,
acsParameters->safeModeControllerParameters.k_orthoMekf); acsParameters->safeModeControllerParameters.k_orthoMekf);
calculateAngleErrorTorque(sunDirB, sunDirRefB, calculateAngleErrorTorque(sunDirB, sunDirRefB,
acsParameters->safeModeControllerParameters.k_alignMekf); acsParameters->safeModeControllerParameters.k_alignMekf);
@ -72,8 +71,7 @@ void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const
errorAngle = acos(dotSun); errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB); splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(sunDirB, errorAngle, calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr,
acsParameters->safeModeControllerParameters.k_parallelGyr,
acsParameters->safeModeControllerParameters.k_orthoGyr); acsParameters->safeModeControllerParameters.k_orthoGyr);
calculateAngleErrorTorque(sunDirB, sunDirRefB, calculateAngleErrorTorque(sunDirB, sunDirRefB,
acsParameters->safeModeControllerParameters.k_alignGyr); acsParameters->safeModeControllerParameters.k_alignGyr);
@ -98,8 +96,7 @@ void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallel
std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB)); std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB));
std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB)); std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB));
calculateRotationalRateTorque(sunDirB, errorAngle, calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm,
acsParameters->safeModeControllerParameters.k_parallelSusMgm,
acsParameters->safeModeControllerParameters.k_orthoSusMgm); acsParameters->safeModeControllerParameters.k_orthoSusMgm);
calculateAngleErrorTorque(sunDirB, sunDirRefB, calculateAngleErrorTorque(sunDirB, sunDirRefB,
acsParameters->safeModeControllerParameters.k_alignSusMgm); acsParameters->safeModeControllerParameters.k_alignSusMgm);
@ -121,8 +118,7 @@ void SafeCtrl::safeRateDampingGyr(const double *magFieldB, const double *satRotR
errorAngle = NAN; errorAngle = NAN;
splitRotationalRate(satRotRateB, sunDirRefB); splitRotationalRate(satRotRateB, sunDirRefB);
calculateRotationalRateTorque(sunDirRefB, errorAngle, calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr,
acsParameters->safeModeControllerParameters.k_parallelGyr,
acsParameters->safeModeControllerParameters.k_orthoGyr); acsParameters->safeModeControllerParameters.k_orthoGyr);
// sum of all torques // sum of all torques
@ -142,8 +138,7 @@ void SafeCtrl::safeRateDampingSusMgm(const double *magFieldB, const double *satR
errorAngle = NAN; errorAngle = NAN;
splitRotationalRate(satRotRateB, sunDirRefB); splitRotationalRate(satRotRateB, sunDirRefB);
calculateRotationalRateTorque(sunDirRefB, errorAngle, calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm,
acsParameters->safeModeControllerParameters.k_parallelSusMgm,
acsParameters->safeModeControllerParameters.k_orthoSusMgm); acsParameters->safeModeControllerParameters.k_orthoSusMgm);
// sum of all torques // sum of all torques
@ -161,8 +156,7 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD
VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3); VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3);
} }
void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, double &errorAngle, void SafeCtrl::calculateRotationalRateTorque(const double gainParallel, const double gainOrtho) {
const double gainParallel, const double gainOrtho) {
// calculate torque for parallel rotational rate // calculate torque for parallel rotational rate
VectorOperations<double>::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3); VectorOperations<double>::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3);

3
mission/controller/acs/control/SafeCtrl.h
View File

@ -41,8 +41,7 @@ class SafeCtrl {
const double *sunDirB, const double *sunRateB, const double *sunDirB, const double *sunRateB,
double *fusedRotRate); double *fusedRotRate);
void calculateRotationalRateTorque(const double *sunDirB, double &errorAngle, void calculateRotationalRateTorque(const double gainParallel, const double gainOrtho);
const double gainParallel, const double gainOrtho);
void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
const double gainAlign); const double gainAlign);