diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1a47076f..d75a7b96 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -409,10 +409,30 @@ void AcsController::performPointingCtrl() { commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], acsParameters.rwHandlingParameters.rampTime); + return; } else { ptgCtrlLostCounter = 0; } + double quatBI[4] = {0, 0, 0, 0}, rotRateB[3] = {0, 0, 0}; + switch (ptgCtrlStrat) { + case acs::ControlModeStrategy::PTGCTRL_MEKF: + std::memcpy(quatBI, attitudeEstimationData.quatMekf.value, sizeof(quatBI)); + std::memcpy(rotRateB, attitudeEstimationData.satRotRateMekf.value, sizeof(rotRateB)); + break; + case acs::ControlModeStrategy::PTGCTRL_STR: + quatBI[0] = sensorValues.strSet.caliQx.value; + quatBI[1] = sensorValues.strSet.caliQy.value; + quatBI[2] = sensorValues.strSet.caliQz.value; + quatBI[3] = sensorValues.strSet.caliQw.value; + std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB)); + break; + case acs::ControlModeStrategy::PTGCTRL_QUEST: + std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI)); + std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB)); + break; + } + uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); @@ -439,9 +459,8 @@ void AcsController::performPointingCtrl() { case acs::PTG_IDLE: guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); - guidance.comparePtg(attitudeEstimationData.quatMekf.value, - attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, - errorQuat, errorSatRotRate, errorAngle); + guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat, + errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters, @@ -452,9 +471,9 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; break; @@ -462,8 +481,7 @@ void AcsController::performPointingCtrl() { guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); - guidance.comparePtg(attitudeEstimationData.quatMekf.value, - attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef, acsParameters.targetModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); @@ -477,18 +495,17 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case acs::PTG_TARGET_GS: guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); - guidance.comparePtg(attitudeEstimationData.quatMekf.value, - attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, - errorQuat, errorSatRotRate, errorAngle); + guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat, + errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters, @@ -499,9 +516,9 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; break; @@ -509,8 +526,7 @@ void AcsController::performPointingCtrl() { guidance.targetQuatPtgNadirThreeAxes( timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); - guidance.comparePtg(attitudeEstimationData.quatMekf.value, - attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef, acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); @@ -524,17 +540,16 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; break; case acs::PTG_INERTIAL: std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, sizeof(targetQuat)); - guidance.comparePtg(attitudeEstimationData.quatMekf.value, - attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef, acsParameters.inertialModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); @@ -548,9 +563,9 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; default: