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

Merge branch 'develop' into acs-bug-bash
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-03-07 09:54:53 +01:00
parent 2c6ea8dc9f 6aae94823e
commit 37673a2782
141 changed files with 5629 additions and 2549 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -16,15 +16,13 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
double *magFieldModel, bool magFieldModelValid,
double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
bool rateMekfValid, double *sunDirRef, double *satRatRef,
double *outputAngle, double *outputMagMomB, bool *outputValid) {
double *outputAngle, double *outputMagMomB) {
if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
*outputValid = false;
return SAFECTRL_MEKF_INPUT_INVALID;
}
double kRate = 0, kAlign = 0;
kRate = acsParameters->safeModeControllerParameters.k_rate_mekf;
kAlign = acsParameters->safeModeControllerParameters.k_align_mekf;
double kRate = acsParameters->safeModeControllerParameters.k_rate_mekf;
double kAlign = acsParameters->safeModeControllerParameters.k_align_mekf;
// Calc sunDirB ,magFieldB with mekf output and model
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -33,22 +31,22 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
double crossSun[3] = {0, 0, 0};
// change unit from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);
double crossSun[3] = {0, 0, 0};
VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
// calc angle alpha between sunDirRef and sunDIr
double alpha = 0, dotSun = 0;
dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
alpha = acos(dotSun);
double dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
double alpha = acos(dotSun);
// Law Torque calculations
double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0},
torqueAll[3] = {0, 0, 0};
double scalarFac = 0;
scalarFac = kAlign * alpha / normCrossSun;
double scalarFac = kAlign * alpha / normCrossSun;
VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
double rateSafeMode[3] = {0, 0, 0};
@ -69,39 +67,38 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
*outputAngle = alpha;
*outputValid = true;
return returnvalue::OK;
}
// Will be the version in worst case scenario in event of no working MEKF (nor GYRs)
void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
bool sunRateBValid, double *magFieldB, bool magFieldBValid,
double *magRateB, bool magRateBValid, double *sunDirRef,
double *satRateRef, double *outputAngle, double *outputMagMomB,
bool *outputValid) {
ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid,
double *sunRateB, bool sunRateBValid, double *magFieldB,
bool magFieldBValid, double *magRateB, bool magRateBValid,
double *sunDirRef, double *satRateRef, double *outputAngle,
double *outputMagMomB) {
// Check for invalid Inputs
if (!susDirBValid || !magFieldBValid || !magRateBValid) {
*outputValid = false;
return;
return returnvalue::FAILED;
}
// change unit from uT to T
double magFieldBT[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
// normalize sunDir and magDir
double magDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldB, magDirB, 3);
VectorOperations<double>::normalize(magFieldBT, magDirB, 3);
VectorOperations<double>::normalize(susDirB, susDirB, 3);
// Cosinus angle between sunDir and magDir
double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
// Rate parallel to sun direction and magnetic field direction
double rateParaSun = 0, rateParaMag = 0;
double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0;
dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
rateFactor = 1 - pow(cosAngleSunMag, 2);
rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
double dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
double dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
double rateFactor = 1 - pow(cosAngleSunMag, 2);
double rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
double rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
// Full rate or estimate
double estSatRate[3] = {0, 0, 0};
@ -117,7 +114,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
* is sufficiently large */
double angleSunMag = acos(cosAngleSunMag);
if (angleSunMag < acsParameters->safeModeControllerParameters.sunMagAngleMin) {
return;
return returnvalue::FAILED;
}
// Rate for Torque Calculation
@ -125,9 +122,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
// Torque Align calculation
double kRateNoMekf = 0, kAlignNoMekf = 0;
kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf;
kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf;
double kRateNoMekf = acsParameters->safeModeControllerParameters->k_rate_no_mekf;
double kAlignNoMekf = acsParameters->safeModeControllerParameters->k_align_no_mekf;
double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
double crossSusSunRef[3] = {0, 0, 0};
@ -152,11 +148,11 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
// Magnetic moment
double magMomB[3] = {0, 0, 0};
double crossMagFieldTorque[3] = {0, 0, 0};
VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque);
double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2);
VectorOperations<double>::cross(magFieldBT, torqueB, crossMagFieldTorque);
double magMomFactor = pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
*outputAngle = angleAlignErr;
*outputValid = true;
return returnvalue::OK;
}