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

changed satRotRateRef to scalar
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-04-05 16:46:57 +02:00
parent ce7da9f513
commit 39b2a3420c
7 changed files with 22 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -30,7 +30,7 @@ ReturnValue_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnV
void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
const double *sunDirModelI, const double *quatBI, const double *sunDirRefB,
const double *satRotRateRefB, const double inertiaMatrix[3][3],
const double satRotRateRef, const double inertiaMatrix[3][3],
double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -44,7 +44,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(satRotRateRefB, sunDirB, sunDirRefB, errorAngle,
calculateRotationalRateTorque(satRotRateRef, sunDirB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelMekf,
acsParameters->safeModeControllerParameters.k_orthoMekf);
calculateAngleErrorTorque(sunDirB, sunDirRefB,
@ -60,7 +60,7 @@ 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 *satRotRateRefB, const double inertiaMatrix[3][3],
const double satRotRateRef, const double inertiaMatrix[3][3],
double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T
double magFieldBT[3] = {0, 0, 0};
@ -71,7 +71,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(satRotRateRefB, sunDirB, sunDirRefB, errorAngle,
calculateRotationalRateTorque(satRotRateRef, sunDirB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf);
calculateAngleErrorTorque(sunDirB, sunDirRefB,
@ -87,7 +87,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
}
void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB,
const double *satRotRateRefB, const double *sunDirRefB,
const double satRotRateRef, const double *sunDirRefB,
double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -96,7 +96,7 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate
errorAngle = NAN;
splitRotationalRate(satRotRateB, sunDirRefB);
calculateRotationalRateTorque(satRotRateRefB, sunDirRefB, sunDirRefB, errorAngle,
calculateRotationalRateTorque(satRotRateRef, sunDirRefB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf);
@ -116,14 +116,17 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD
VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3);
}
void SafeCtrl::calculateRotationalRateTorque(const double *satRotRateRefB, const double *sunDirB,
void SafeCtrl::calculateRotationalRateTorque(const double satRotRateRef, const double *sunDirB,
const double *sunDirRefB, double &errorAngle,
const double gainParallel, const double gainOrtho) {
// calculate torque for parallel rotational rate
if ((isfinite(errorAngle)) and
(errorAngle < (double)acsParameters->safeModeControllerParameters.angleStartSpin)) {
VectorOperations<double>::subtract(satRotRateRefB, satRotRateParallelB, cmdParallel, 3);
VectorOperations<double>::mulScalar(cmdParallel, gainParallel, cmdParallel, 3);
double satRotRateNorm = VectorOperations<double>::norm(satRotRateParallelB, 3);
double satRotRateUnitVec[3] = {0, 0, 0};
VectorOperations<double>::normalize(satRotRateParallelB, satRotRateUnitVec, 3);
VectorOperations<double>::mulScalar(satRotRateUnitVec, satRotRateRef - satRotRateNorm,
cmdParallel, 3);
} else {
VectorOperations<double>::mulScalar(cmdParallel, -gainParallel, cmdParallel, 3);
}