diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index ff8be5ad..3f369ced 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -335,12 +335,13 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_INERTIAL: - guidance.targetQuatPtgInertial(targetQuat, refSatRate); - std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, + std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, 4 * sizeof(double)); - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, - deltaRate); - ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate, + guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, + targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef, + acsParameters.inertialModeControllerParameters.refRotRate, errorQuat, + errorSatRotRate, errorAngle); + ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 4f30e636..1c5dca92 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -412,13 +412,6 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate); } -void Guidance::targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]) { - std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, - 4 * sizeof(double)); - std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate, - 3 * sizeof(double)); -} - void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double errorAngle) { diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 7306d4f6..e5560379 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -32,10 +32,6 @@ class Guidance { void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], double targetQuat[4], double refSatRate[3]); - // Function to get the target quaternion and refence rotation rate from parameters for inertial - // pointing - void targetQuatPtgInertial(double targetQuat[4], double targetSatRotRate[3]); - // @note: Calculates the error quaternion between the current orientation and the target // quaternion, considering a reference quaternion. Additionally the difference between the actual // and a desired satellite rotational rate is calculated, again considering a reference rotational