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

Revert ACS Safe Mode Controller to FLP Design #466

Merged
muellerr merged 64 commits from acs-flp-safe into develop 2023-04-14 20:26:20 +02:00
Conversation 1 Commits 64 Files Changed 31 +1138 -1938
108 changed files with 1138 additions and 1938 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit d4080fe5cb - Show all commits

2
mission/controller/AcsController.cpp
View File

@@ -191,7 +191,7 @@ void AcsController::performSafe() {
break; break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING): case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING):
safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
sunTargetDir, magMomMtq, errAng); sunTargetDir, inertiaEive, magMomMtq, errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;

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

@@ -42,11 +42,11 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun); errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB); splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, calculateRotationalRateTorque(
acsParameters->safeModeControllerParameters.k_parallelMekf, sunDirB, sunDirRefB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelMekf,
acsParameters->safeModeControllerParameters.k_orthoMekf); acsParameters->safeModeControllerParameters.k_orthoMekf, inertiaMatrix);
calculateAngleErrorTorque(sunDirB, sunDirRefB, calculateAngleErrorTorque(sunDirB, sunDirRefB,
acsParameters->safeModeControllerParameters.k_alignMekf, inertiaMatrix); acsParameters->safeModeControllerParameters.k_alignMekf);
// sum of all torques // sum of all torques
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
@@ -70,10 +70,10 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
splitRotationalRate(satRotRateB, sunDirB); splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf); acsParameters->safeModeControllerParameters.k_orthoNonMekf,
inertiaMatrix);
calculateAngleErrorTorque(sunDirB, sunDirRefB, calculateAngleErrorTorque(sunDirB, sunDirRefB,
acsParameters->safeModeControllerParameters.k_alignNonMekf, acsParameters->safeModeControllerParameters.k_alignNonMekf);
inertiaMatrix);
// sum of all torques // sum of all torques
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
@@ -84,7 +84,8 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
} }
void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB, void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB,
const double *sunDirRefB, double *magMomB, double &errorAngle) { const double *sunDirRefB, const double inertiaMatrix[3][3],
double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T // convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3); VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@@ -94,7 +95,8 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate
splitRotationalRate(satRotRateB, sunDirRefB); splitRotationalRate(satRotRateB, sunDirRefB);
calculateRotationalRateTorque(sunDirRefB, sunDirRefB, errorAngle, calculateRotationalRateTorque(sunDirRefB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf); acsParameters->safeModeControllerParameters.k_orthoNonMekf,
inertiaMatrix);
// sum of all torques // sum of all torques
double cmdTorque[3] = {0, 0, 0}; double cmdTorque[3] = {0, 0, 0};
@@ -114,21 +116,23 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD
void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB,
double &errorAngle, const double gainParallel, double &errorAngle, const double gainParallel,
const double gainOrtho) { const double gainOrtho,
const double inertiaMatrix[3][3]) {
// 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);
// calculate torque for orthogonal rotational rate // calculate torque for orthogonal rotational rate
VectorOperations<double>::mulScalar(satRotRateOrthogonalB, -gainOrtho, cmdOrtho, 3); double orthoFactor[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiplyScalar(*inertiaMatrix, gainOrtho, *orthoFactor, 3, 3);
MatrixOperations<double>::multiply(*orthoFactor, satRotRateOrthogonalB, cmdAlign, 3, 3, 1);
} }
void SafeCtrl::calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, void SafeCtrl::calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
const double gainAlign, const double inertiaMatrix[3][3]) { const double gainAlign) {
// calculate torque for alignment // calculate torque for alignment
double crossAlign[3] = {0, 0, 0}, alignFactor[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double crossAlign[3] = {0, 0, 0};
MatrixOperations<double>::multiplyScalar(*inertiaMatrix, gainAlign, *alignFactor, 3, 3);
VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign); VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign);
MatrixOperations<double>::multiply(*alignFactor, crossAlign, cmdAlign, 3, 3, 1); VectorOperations<double>::mulScalar(crossAlign, gainAlign, cmdAlign, 3);
} }
void SafeCtrl::calculateMagneticMoment(double *magMomB) { void SafeCtrl::calculateMagneticMoment(double *magMomB) {

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

@@ -25,16 +25,16 @@ class SafeCtrl {
double &errorAngle); double &errorAngle);
void safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB, void safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB,
double *magMomB, double &errorAngle); const double inertiaMatrix[3][3], double *magMomB, double &errorAngle);
void splitRotationalRate(const double *satRotRateB, const double *sunDirB); void splitRotationalRate(const double *satRotRateB, const double *sunDirB);
void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB,
double &errorAngle, const double gainParallel, double &errorAngle, const double gainParallel,
const double gainOrtho); const double gainOrtho, const double inertiaMatrix[3][3]);
void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
const double gainAlign, const double inertiaMatrix[3][3]); const double gainAlign);
void calculateMagneticMoment(double *magMomB); void calculateMagneticMoment(double *magMomB);