diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 9ef9acf5..32a77591 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -99,7 +99,7 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal // normalize sunDir and magDir double magDirB[3] = {0, 0, 0}; - VectorOperations::normalize(magFieldB, magDirB, 3); + VectorOperations::normalize(magFieldBT, magDirB, 3); VectorOperations::normalize(susDirB, susDirB, 3); // Cosinus angle between sunDir and magDir @@ -160,8 +160,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal // Magnetic moment double magMomB[3] = {0, 0, 0}; double crossMagFieldTorque[3] = {0, 0, 0}; - VectorOperations::cross(magFieldB, torqueB, crossMagFieldTorque); - double magMomFactor = pow(VectorOperations::norm(magFieldB, 3), 2); + VectorOperations::cross(magFieldBT, torqueB, crossMagFieldTorque); + double magMomFactor = pow(VectorOperations::norm(magFieldBT, 3), 2); VectorOperations::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3); std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));