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

removed inertia matrix for safe mode

Browse Source
This commit is contained in:
Marius Eggert
2023-04-14 11:37:23 +02:00
parent 7f3f99c6aa
commit 53cccc3c13
5 changed files with 23 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -29,7 +29,7 @@ uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t
void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
const double *sunDirModelI, const double *quatBI, const double *sunDirRefB,
const double inertiaMatrix[3][3], double *magMomB, double &errorAngle) {
double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -42,9 +42,9 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(
sunDirB, sunDirRefB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelMekf,
acsParameters->safeModeControllerParameters.k_orthoMekf, inertiaMatrix);
calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelMekf,
acsParameters->safeModeControllerParameters.k_orthoMekf);
calculateAngleErrorTorque(sunDirB, sunDirRefB,
acsParameters->safeModeControllerParameters.k_alignMekf);
@ -57,8 +57,8 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
}
void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
const double *sunDirB, const double *sunDirRefB,
const double inertiaMatrix[3][3], double *magMomB, double &errorAngle) {
const double *sunDirB, const double *sunDirRefB, double *magMomB,
double &errorAngle) {
// convert magFieldB from uT to T
double magFieldBT[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -70,8 +70,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf,
inertiaMatrix);
acsParameters->safeModeControllerParameters.k_orthoNonMekf);
calculateAngleErrorTorque(sunDirB, sunDirRefB,
acsParameters->safeModeControllerParameters.k_alignNonMekf);
@ -84,8 +83,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
}
void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB,
const double *sunDirRefB, const double inertiaMatrix[3][3],
double *magMomB, double &errorAngle) {
const double *sunDirRefB, double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -95,8 +93,7 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate
splitRotationalRate(satRotRateB, sunDirRefB);
calculateRotationalRateTorque(sunDirRefB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf,
inertiaMatrix);
acsParameters->safeModeControllerParameters.k_orthoNonMekf);
// sum of all torques
double cmdTorque[3] = {0, 0, 0};
@ -116,15 +113,12 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD
void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB,
double &errorAngle, const double gainParallel,
const double gainOrtho,
const double inertiaMatrix[3][3]) {
const double gainOrtho) {
// calculate torque for parallel rotational rate
VectorOperations<double>::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3);
// calculate torque for orthogonal rotational rate
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, cmdOrtho, 3, 3, 1);
VectorOperations<double>::mulScalar(satRotRateOrthogonalB, -gainOrtho, cmdOrtho, 3);
}
void SafeCtrl::calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,