diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index cfbf85ea..2f5847cc 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -102,6 +102,8 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara // concentrate RW speeds as vector and convert to double double speedRws[4] = {static_cast(speedRw0), static_cast(speedRw1), static_cast(speedRw2), static_cast(speedRw3)}; + VectorOperations::mulScalar(speedRws, 1e-1, speedRws, 4); + VectorOperations::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4); // calculate RPM offset utilizing the nullspace double rpmOffset[4] = {0, 0, 0, 0}; @@ -112,7 +114,6 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara // calculate resulting angular momentum double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0}; VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); - VectorOperations::mulScalar(diffRwSpeed, 1e-1, diffRwSpeed, 4); VectorOperations::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, rwAngMomentum, 4);