diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index e6b45318..23a86551 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -95,11 +95,12 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal } // change unit from uT to T - VectorOperations::mulScalar(magFieldB, 1e-6, magFieldB, 3); + double magFieldBT[3] = {0,0,0}; + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); // 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 +161,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));