diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 73730649..943e2c84 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -228,8 +228,6 @@ void AcsController::performPointingCtrl() { &mekfData, &validMekf); uint8_t enableAntiStiction = true; - double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, - deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); if (result == returnvalue::FAILED) { @@ -245,8 +243,10 @@ void AcsController::performPointingCtrl() { double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; double mgtDpDes[3] = {0, 0, 0}; - double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1}, - errorAngle = 0; + // Variables required for guidance + double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, + errorQuatInterim[4] = {0, 0, 0, 1}, errorQuat[4] = {0, 0, 0, 1}, errorAngle = 0, + satRotRate[3] = {0, 0, 0}, satRotRateError[3] = {0, 0, 0}; switch (submode) { case acs::PTG_IDLE: guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, @@ -279,10 +279,17 @@ void AcsController::performPointingCtrl() { guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); - guidance.calculateErrorQuat(targetQuat, mekfData.quatMekf.value, errorQuat, errorAngle); - guidance.comparePtg(errorQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, - deltaRate); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, + guidance.calculateErrorQuat(targetQuat, mekfData.quatMekf.value, errorQuatInterim, + errorAngle); + if (mekfData.satRotRateMekf.isValid()) { + std::memcpy(satRotRate, mekfData.satRotRateMekf.value, 3 * sizeof(double)); + } else { + std::memcpy(satRotRate, gyrDataProcessed.gyrVecTot.value, 3 * sizeof(double)); + } + guidance.comparePtg(errorQuatInterim, acsParameters.targetModeControllerParameters.quatRef, + errorQuat, satRotRate, targetSatRotRate, + acsParameters.targetModeControllerParameters.refRotRate, satRotRateError); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, satRotRateError, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index cdb3ffe7..86d70840 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -470,27 +470,23 @@ void Guidance::targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]) 3 * sizeof(double)); } -void Guidance::comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3], - double quatErrorComplete[4], double quatError[3], double deltaRate[3]) { - double satRate[3] = {0, 0, 0}; - std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double)); - VectorOperations::subtract(satRate, refSatRate, deltaRate, 3); - // valid checks ? +void Guidance::comparePtg(double oldErrorQuat[4], double quatRef[4], double newErrorQuatComplete[4], + double satRotRate[3], double satRotRateGuidance[3], + double satRotRateRef[3], double satRotRateError[3]) { + double combinedRefSatRate[3] = {0, 0, 0}; + VectorOperations::add(satRotRateGuidance, satRotRateRef, combinedRefSatRate, 3); + VectorOperations::subtract(satRotRate, combinedRefSatRate, satRotRateError, 3); + double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]}, {-quatRef[2], quatRef[3], quatRef[0], -quatRef[1]}, {quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]}, {quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}}; - MatrixOperations::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1); + MatrixOperations::multiply(*quatErrorMtx, oldErrorQuat, newErrorQuatComplete, 4, 4, 1); - if (quatErrorComplete[3] < 0) { - quatErrorComplete[3] *= -1; + if (newErrorQuatComplete[3] < 0) { + VectorOperations::mulScalar(newErrorQuatComplete, -1, newErrorQuatComplete, 4); } - - quatError[0] = quatErrorComplete[0]; - quatError[1] = quatErrorComplete[1]; - quatError[2] = quatErrorComplete[2]; - // target flag in matlab, importance, does look like it only gives feedback if pointing control is // under 150 arcsec ?? } @@ -537,6 +533,12 @@ void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatIn savedQuaternion[3] = quatInertialTarget[3]; } +void Guidance::calculateErrorQuat(double targetQuat[4], double currentQuat[4], double errorQuat[4], + double errorAngle) { + QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); + errorAngle = 2 * acos(errorQuat[3]); +} + ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()); diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index d68936bd..985c858d 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -34,15 +34,21 @@ class Guidance { // pointing void targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]); + // @note: compares the target Quaternion in the ECI to the current orientation in ECI to compute + // the error quaternion and error angle + void calculateErrorQuat(double targetQuat[4], double currentQuat[4], double errorQuat[4], + double errorAngle); + // @note: compares target Quaternion and reference quaternion, also actual and desired satellite // rate - void comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3], - double quatErrorComplete[4], double quatError[3], double deltaRate[3]); + void comparePtg(double oldErrorQuat[4], double quatRef[4], double newErrorQuatComplete[4], + double satRotRate[3], double satRotRateGuidance[3], double satRotRateRef[3], + double satRotRateError[3]); void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *refSatRate); - // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid + // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid // reation wheel maybe can be done in "commanding.h" ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 60394421..74a32a4a 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -27,7 +27,7 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { } void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, - const double *qError, const double *deltaRate, const double *rwPseudoInv, + const double *errorQuat, const double *deltaRate, const double *rwPseudoInv, double *torqueRws) { //------------------------------------------------------------------------------------------------ // Compute gain matrix K and P matrix @@ -37,6 +37,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters double qErrorMin = pointingLawParameters->qiMin; double omMax = pointingLawParameters->omMax; + double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]}; + double cInt = 2 * om * zeta; double kInt = 2 * pow(om, 2);