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

fixed units used in controler calculations

Browse Source
This commit is contained in:
Marius Eggert
2023-03-02 17:52:36 +01:00
parent b5049fa8d0
commit 3e2338f3a4
4 changed files with 46 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -32,15 +32,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 = safeModeControllerParameters->k_rate_mekf;
kAlign = safeModeControllerParameters->k_align_mekf;
double kRate = safeModeControllerParameters->k_rate_mekf;
double kAlign = 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}};
@ -49,22 +47,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};
@ -82,23 +80,22 @@ 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,
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,
bool *outputValid) {
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
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);
// normalize sunDir and magDir
double magDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldB, magDirB, 3);
@ -108,13 +105,11 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
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};
@ -130,7 +125,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
* is sufficiently large */
double angleSunMag = acos(cosAngleSunMag);
if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) {
return;
return returnvalue::FAILED;
}
// Rate for Torque Calculation
@ -138,9 +133,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 = safeModeControllerParameters->k_rate_no_mekf;
kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
double kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
double kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
double crossSusSunRef[3] = {0, 0, 0};
@ -171,5 +165,5 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
*outputAngle = angleAlignErr;
*outputValid = true;
return returnvalue::OK;
}