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

Merge branch 'develop' into smaller_lock_tweaks
Some checks are pending
EIVE/eive-obsw/pipeline/pr-develop Build queued...
Details

Browse Source
This commit is contained in:
Robin Müller 2023-03-03 15:21:24 +01:00
commit cc3bcdfaeb
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -95,11 +95,12 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
} }
// change unit from uT to T // change unit from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3); double magFieldBT[3] = {0,0,0};
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
// normalize sunDir and magDir // normalize sunDir and magDir
double magDirB[3] = {0, 0, 0}; double magDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldB, magDirB, 3); VectorOperations<double>::normalize(magFieldBT, magDirB, 3);
VectorOperations<double>::normalize(susDirB, susDirB, 3); VectorOperations<double>::normalize(susDirB, susDirB, 3);
// Cosinus angle between sunDir and magDir // Cosinus angle between sunDir and magDir
@ -160,8 +161,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
// Magnetic moment // Magnetic moment
double magMomB[3] = {0, 0, 0}; double magMomB[3] = {0, 0, 0};
double crossMagFieldTorque[3] = {0, 0, 0}; double crossMagFieldTorque[3] = {0, 0, 0};
VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque); VectorOperations<double>::cross(magFieldBT, torqueB, crossMagFieldTorque);
double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2); double magMomFactor = pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3); VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double)); std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));