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

removed OutputValues

Browse Source 
amended sumbode list
inserted writes to output DataPools
This commit is contained in:
Marius Eggert
2022-11-03 10:43:27 +01:00
parent 44dda9455d
commit a13ccb43d2
24 changed files with 590 additions and 522 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -28,12 +28,12 @@ void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE);
}
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 *outputMagMomB, bool *outputValid) {
if (!(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) || !(*rateMekfValid)) {
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) {
if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
*outputValid = false;
return SAFECTRL_MEKF_INPUT_INVALID;
}
@ -80,31 +80,34 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
double normMag = VectorOperations<double>::norm(magFieldB, 3);
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 RMUs)
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 *outputMagMomB, bool *outputValid) {
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) {
// Check for invalid Inputs
if (!susDirBValid || !magFieldBValid || !magRateBValid) {
*outputValid = false;
return;
}
// normalize sunDir and magDir
// normalize sunDir and magDir
double magDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldB, magDirB, 3);
VectorOperations<double>::normalize(susDirB, susDirB, 3);
// Cosinus angle between sunDir and magDir
// Cosinus angle between sunDir and magDir
double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
// Rate parallel to sun direction and magnetic field direction
// 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);
@ -113,7 +116,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, doub
rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
// Full rate or estimate
// Full rate or estimate
double estSatRate[3] = {0, 0, 0};
double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
@ -123,8 +126,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, doub
VectorOperations<double>::add(estSatRateSun, estSatRateMag, estSatRate, 3);
VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);
/* Only valid if angle between sun direction and magnetic field direction
is sufficiently large */
/* Only valid if angle between sun direction and magnetic field direction
is sufficiently large */
double sinAngle = 0;
sinAngle = sin(acos(cos(cosAngleSunMag)));
@ -169,9 +172,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, doub
double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2);
VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
outputMagMomB[0] = magMomB[0];
outputMagMomB[1] = magMomB[1];
outputMagMomB[2] = magMomB[2];
std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
*outputAngle = angleAlignErr;
*outputValid = true;
}