diff --git a/CHANGELOG.md b/CHANGELOG.md index b3ca4e51..0ec0f64f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,8 +16,14 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Changed + +- Guidance now uses the coordinate functions from the FSFW. +- Idle should now point the STR away from the earth + ## Fixed +- Fixed bugs in `Guidance::comparePtg` and corrected overloading - Detumbling State Machine is now robust to commanded mode changes. # [v7.6.0] 2024-01-30 diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 2036e6ab..8d35a8fd 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -420,8 +420,8 @@ void AcsController::performPointingCtrl() { switch (mode) { case acs::PTG_IDLE: - guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat, - targetSatRotRate); + guidance.targetQuatPtgIdle(timeAbsolute, timeDelta, susDataProcessed.sunIjkModel.value, + gpsDataProcessed.gpsPosition.value, targetQuat, targetSatRotRate); guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, @@ -440,9 +440,9 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_TARGET: - guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, - gpsDataProcessed.gpsVelocity.value, targetQuat, - targetSatRotRate); + guidance.targetQuatPtgTarget(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, + gpsDataProcessed.gpsVelocity.value, targetQuat, + targetSatRotRate); guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef, acsParameters.targetModeControllerParameters.refRotRate, errorQuat, @@ -483,9 +483,8 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_NADIR: - guidance.targetQuatPtgNadirThreeAxes( - timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, - gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); + guidance.targetQuatPtgNadir(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, + gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef, acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 12957ab2..3641e41e 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -4,415 +4,200 @@ Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameter Guidance::~Guidance() {} -[[deprecated]] void Guidance::targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3], - double velSatE[3], double sunDirI[3], - double refDirB[3], double quatBI[4], - double targetQuat[4], - double targetSatRotRate[3]) { - //------------------------------------------------------------------------------------- - // Calculation of target quaternion to groundstation or given latitude, longitude and altitude - //------------------------------------------------------------------------------------- - // transform longitude, latitude and altitude to ECEF - double targetE[3] = {0, 0, 0}; +void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, + const double sunDirI[3], const double posSatF[4], + double targetQuat[4], double targetSatRotRate[3]) { + // positive z-Axis of EIVE in direction of sun + double zAxisXI[3] = {0, 0, 0}; + VectorOperations::normalize(sunDirI, zAxisXI, 3); - MathOperations::cartesianFromLatLongAlt( - acsParameters->targetModeControllerParameters.latitudeTgt, - acsParameters->targetModeControllerParameters.longitudeTgt, - acsParameters->targetModeControllerParameters.altitudeTgt, targetE); + // determine helper vector to point x-Axis and therefore the STR away from Earth + double helperXI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0}; + CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); + VectorOperations::normalize(posSatI, helperXI, 3); - // target direction in the ECEF frame - double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(targetE, posSatE, targetDirE, 3); + // construct y-axis from helper vector and z-axis + double yAxisXI[3] = {0, 0, 0}; + VectorOperations::cross(zAxisXI, helperXI, yAxisXI); + VectorOperations::normalize(yAxisXI, yAxisXI, 3); - // transformation between ECEF and ECI frame - double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); - MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); + // x-axis completes RHS + double xAxisXI[3] = {0, 0, 0}; + VectorOperations::cross(yAxisXI, zAxisXI, xAxisXI); + VectorOperations::normalize(xAxisXI, xAxisXI, 3); - double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); + // join transformation matrix + double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, + {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, + {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; + QuaternionOperations::fromDcm(dcmXI, targetQuat); - // transformation between ECEF and Body frame - double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - - QuaternionOperations::toDcm(quatBI, dcmBI); - MatrixOperations::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3); - - // target Direction in the body frame - double targetDirB[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); - - // rotation quaternion from two vectors - double refDir[3] = {0, 0, 0}; - refDir[0] = acsParameters->targetModeControllerParameters.refDirection[0]; - refDir[1] = acsParameters->targetModeControllerParameters.refDirection[1]; - refDir[2] = acsParameters->targetModeControllerParameters.refDirection[2]; - double noramlizedTargetDirB[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); - VectorOperations::normalize(refDir, refDir, 3); - double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); - double normRefDir = VectorOperations::norm(refDir, 3); - double crossDir[3] = {0, 0, 0}; - double dotDirections = VectorOperations::dot(noramlizedTargetDirB, refDir); - VectorOperations::cross(noramlizedTargetDirB, refDir, crossDir); - targetQuat[0] = crossDir[0]; - targetQuat[1] = crossDir[1]; - targetQuat[2] = crossDir[2]; - targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections); - VectorOperations::normalize(targetQuat, targetQuat, 4); - - //------------------------------------------------------------------------------------- - // calculation of reference rotation rate - //------------------------------------------------------------------------------------- - double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0}; - // velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E - MatrixOperations::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1); - double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(*dcmBI, *dcmIEDot, *dcmBEDot, 3, 3, 3); - MatrixOperations::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); - VectorOperations::add(velSatBPart1, velSatBPart2, velSatB, 3); - - double normVelSatB = VectorOperations::norm(velSatB, 3); - double normRefSatRate = normVelSatB / normTargetDirB; - - double satRateDir[3] = {0, 0, 0}; - VectorOperations::cross(velSatB, targetDirB, satRateDir); - VectorOperations::normalize(satRateDir, satRateDir, 3); - VectorOperations::mulScalar(satRateDir, normRefSatRate, targetSatRotRate, 3); - - //------------------------------------------------------------------------------------- - // Calculation of reference rotation rate in case of star tracker blinding - //------------------------------------------------------------------------------------- - if (acsParameters->targetModeControllerParameters.avoidBlindStr) { - double sunDirB[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1); - - double exclAngle = acsParameters->strParameters.exclusionAngle, - blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart, - blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop; - double sightAngleSun = - VectorOperations::dot(acsParameters->strParameters.boresightAxis, sunDirB); - - if (!(strBlindAvoidFlag)) { - double critSightAngle = blindStart * exclAngle; - if (sightAngleSun < critSightAngle) { - strBlindAvoidFlag = true; - } - } else { - if (sightAngleSun < blindEnd * exclAngle) { - double normBlindRefRate = acsParameters->targetModeControllerParameters.blindRotRate; - double blindRefRate[3] = {0, 0, 0}; - if (sunDirB[1] < 0) { - blindRefRate[0] = normBlindRefRate; - blindRefRate[1] = 0; - blindRefRate[2] = 0; - } else { - blindRefRate[0] = -normBlindRefRate; - blindRefRate[1] = 0; - blindRefRate[2] = 0; - } - VectorOperations::add(blindRefRate, targetSatRotRate, targetSatRotRate, 3); - } else { - strBlindAvoidFlag = false; - } - } - } - // revert calculated quaternion from qBX to qIX - double quatIB[4] = {0, 0, 0, 1}; - QuaternionOperations::inverse(quatBI, quatIB); - QuaternionOperations::multiply(quatIB, targetQuat, targetQuat); + // calculate of reference rotation rate + targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta, - double posSatE[3], double velSatE[3], double targetQuat[4], - double targetSatRotRate[3]) { +void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], + double velSatF[3], double targetQuat[4], + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) - double targetE[3] = {0, 0, 0}; + double targetF[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters->targetModeControllerParameters.latitudeTgt, acsParameters->targetModeControllerParameters.longitudeTgt, - acsParameters->targetModeControllerParameters.altitudeTgt, targetE); - double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(targetE, posSatE, targetDirE, 3); - - // transformation between ECEF and ECI frame - double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); - MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - - double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); + acsParameters->targetModeControllerParameters.altitudeTgt, targetF); + double targetDirF[3] = {0, 0, 0}; + VectorOperations::subtract(targetF, posSatF, targetDirF, 3); // target direction in the ECI frame double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); - MatrixOperations::multiply(*dcmIE, targetE, targetI, 3, 3, 1); + CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); + CoordinateTransformations::positionEcfToEci(targetF, targetI, &timeAbsolute); VectorOperations::subtract(targetI, posSatI, targetDirI, 3); // x-axis aligned with target direction // this aligns with the camera, E- and S-band antennas - double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirI, xAxis, 3); + double xAxisXI[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirI, xAxisXI, 3); // transform velocity into inertial frame - double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1); - MatrixOperations::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1); - VectorOperations::add(velPart1, velPart2, velocityI, 3); + double velSatI[3] = {0, 0, 0}; + CoordinateTransformations::velocityEcfToEci(velSatF, posSatF, velSatI, &timeAbsolute); // orbital normal vector of target and velocity vector double orbitalNormalI[3] = {0, 0, 0}; - VectorOperations::cross(posSatI, velocityI, orbitalNormalI); + VectorOperations::cross(posSatI, velSatI, orbitalNormalI); VectorOperations::normalize(orbitalNormalI, orbitalNormalI, 3); // y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture // resolution - double yAxis[3] = {0, 0, 0}; - VectorOperations::cross(orbitalNormalI, xAxis, yAxis); - VectorOperations::normalize(yAxis, yAxis, 3); + double yAxisXI[3] = {0, 0, 0}; + VectorOperations::cross(orbitalNormalI, xAxisXI, yAxisXI); + VectorOperations::normalize(yAxisXI, yAxisXI, 3); // z-axis completes RHS - double zAxis[3] = {0, 0, 0}; - VectorOperations::cross(xAxis, yAxis, zAxis); + double zAxisXI[3] = {0, 0, 0}; + VectorOperations::cross(xAxisXI, yAxisXI, zAxisXI); // join transformation matrix - double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, - {xAxis[1], yAxis[1], zAxis[1]}, - {xAxis[2], yAxis[2], zAxis[2]}}; + double dcmIX[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, + {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, + {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; QuaternionOperations::fromDcm(dcmIX, targetQuat); - int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate); + targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta, - double posSatE[3], double sunDirI[3], double targetQuat[4], +void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3], + double sunDirI[3], double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for ground station pointing //------------------------------------------------------------------------------------- // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) - double groundStationE[3] = {0, 0, 0}; - + double posGroundStationF[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters->gsTargetModeControllerParameters.latitudeTgt, acsParameters->gsTargetModeControllerParameters.longitudeTgt, - acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE); - double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(groundStationE, posSatE, targetDirE, 3); - - // transformation between ECEF and ECI frame - double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); - MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - - double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); + acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF); // target direction in the ECI frame - double posSatI[3] = {0, 0, 0}, groundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); - MatrixOperations::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1); - VectorOperations::subtract(groundStationI, posSatI, groundStationDirI, 3); + double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0}; + CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); + CoordinateTransformations::positionEcfToEci(posGroundStationI, posGroundStationI, &timeAbsolute); + VectorOperations::subtract(posGroundStationI, posSatI, groundStationDirI, 3); // negative x-axis aligned with target direction // this aligns with the camera, E- and S-band antennas - double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(groundStationDirI, xAxis, 3); - VectorOperations::mulScalar(xAxis, -1, xAxis, 3); + double xAxisXI[3] = {0, 0, 0}; + VectorOperations::normalize(groundStationDirI, xAxisXI, 3); + VectorOperations::mulScalar(xAxisXI, -1, xAxisXI, 3); // get sun vector model in ECI VectorOperations::normalize(sunDirI, sunDirI, 3); // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector // z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x - double xDotS = VectorOperations::dot(xAxis, sunDirI); - xDotS /= pow(VectorOperations::norm(xAxis, 3), 2); - double sunParallel[3], zAxis[3]; - VectorOperations::mulScalar(xAxis, xDotS, sunParallel, 3); - VectorOperations::subtract(sunDirI, sunParallel, zAxis, 3); - VectorOperations::normalize(zAxis, zAxis, 3); + double xDotS = VectorOperations::dot(xAxisXI, sunDirI); + xDotS /= pow(VectorOperations::norm(xAxisXI, 3), 2); + double sunParallel[3], zAxisXI[3]; + VectorOperations::mulScalar(xAxisXI, xDotS, sunParallel, 3); + VectorOperations::subtract(sunDirI, sunParallel, zAxisXI, 3); + VectorOperations::normalize(zAxisXI, zAxisXI, 3); // y-axis completes RHS - double yAxis[3]; - VectorOperations::cross(zAxis, xAxis, yAxis); - VectorOperations::normalize(yAxis, yAxis, 3); + double yAxisXI[3]; + VectorOperations::cross(zAxisXI, xAxisXI, yAxisXI); + VectorOperations::normalize(yAxisXI, yAxisXI, 3); // join transformation matrix - double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, - {xAxis[1], yAxis[1], zAxis[1]}, - {xAxis[2], yAxis[2], zAxis[2]}}; - QuaternionOperations::fromDcm(dcmTgt, targetQuat); + double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, + {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, + {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; + QuaternionOperations::fromDcm(dcmXI, targetQuat); - int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate); + targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgSun(double timeDelta, double sunDirI[3], double targetQuat[4], - double targetSatRotRate[3]) { - //------------------------------------------------------------------------------------- - // Calculation of target quaternion to sun - //------------------------------------------------------------------------------------- - // positive z-Axis of EIVE in direction of sun - double zAxis[3] = {0, 0, 0}; - VectorOperations::normalize(sunDirI, zAxis, 3); - - // assign helper vector (north pole inertial) - double helperVec[3] = {0, 0, 1}; - - // construct y-axis from helper vector and z-axis - double yAxis[3] = {0, 0, 0}; - VectorOperations::cross(zAxis, helperVec, yAxis); - VectorOperations::normalize(yAxis, yAxis, 3); - - // x-axis completes RHS - double xAxis[3] = {0, 0, 0}; - VectorOperations::cross(yAxis, zAxis, xAxis); - VectorOperations::normalize(xAxis, xAxis, 3); - - // join transformation matrix - double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, - {xAxis[1], yAxis[1], zAxis[1]}, - {xAxis[2], yAxis[2], zAxis[2]}}; - QuaternionOperations::fromDcm(dcmTgt, targetQuat); - - //---------------------------------------------------------------------------- - // Calculation of reference rotation rate - //---------------------------------------------------------------------------- - int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate); -} - -[[deprecated]] void Guidance::targetQuatPtgNadirSingleAxis(const timeval timeAbsolute, - double posSatE[3], double quatBI[4], - double targetQuat[4], double refDirB[3], - double refSatRate[3]) { +void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatE[3], + double velSatE[3], double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- - double targetDirE[3] = {0, 0, 0}; - VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); - - // transformation between ECEF and ECI frame - double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); - MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - - double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - - // transformation between ECEF and Body frame - double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - QuaternionOperations::toDcm(quatBI, dcmBI); - MatrixOperations::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3); - - // target Direction in the body frame - double targetDirB[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); - - // rotation quaternion from two vectors - double refDir[3] = {0, 0, 0}; - refDir[0] = acsParameters->nadirModeControllerParameters.refDirection[0]; - refDir[1] = acsParameters->nadirModeControllerParameters.refDirection[1]; - refDir[2] = acsParameters->nadirModeControllerParameters.refDirection[2]; - double noramlizedTargetDirB[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); - VectorOperations::normalize(refDir, refDir, 3); - double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); - double normRefDir = VectorOperations::norm(refDir, 3); - double crossDir[3] = {0, 0, 0}; - double dotDirections = VectorOperations::dot(noramlizedTargetDirB, refDir); - VectorOperations::cross(noramlizedTargetDirB, refDir, crossDir); - targetQuat[0] = crossDir[0]; - targetQuat[1] = crossDir[1]; - targetQuat[2] = crossDir[2]; - targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections); - VectorOperations::normalize(targetQuat, targetQuat, 4); - - //------------------------------------------------------------------------------------- - // Calculation of reference rotation rate - //------------------------------------------------------------------------------------- - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; - - // revert calculated quaternion from qBX to qIX - double quatIB[4] = {0, 0, 0, 1}; - QuaternionOperations::inverse(quatBI, quatIB); - QuaternionOperations::multiply(quatIB, targetQuat, targetQuat); -} - -void Guidance::targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta, - double posSatE[3], double velSatE[3], - double targetQuat[4], double refSatRate[3]) { - //------------------------------------------------------------------------------------- - // Calculation of target quaternion for Nadir pointing - //------------------------------------------------------------------------------------- - // transformation between ECEF and ECI frame - double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); - MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - - double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); // satellite position in inertial reference frame double posSatI[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); + CoordinateTransformations::positionEcfToEci(posSatE, posSatI, &timeAbsolute); // negative x-axis aligned with position vector // this aligns with the camera, E- and S-band antennas - double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(posSatI, xAxis, 3); - VectorOperations::mulScalar(xAxis, -1, xAxis, 3); + double xAxisXI[3] = {0, 0, 0}; + VectorOperations::normalize(posSatI, xAxisXI, 3); + VectorOperations::mulScalar(xAxisXI, -1, xAxisXI, 3); // make z-Axis parallel to major part of camera resolution - double zAxis[3] = {0, 0, 0}; - double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1); - MatrixOperations::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1); - VectorOperations::add(velPart1, velPart2, velocityI, 3); - VectorOperations::cross(xAxis, velocityI, zAxis); - VectorOperations::normalize(zAxis, zAxis, 3); + double zAxisXI[3] = {0, 0, 0}; + double velSatI[3] = {0, 0, 0}; + CoordinateTransformations::velocityEcfToEci(velSatE, posSatE, velSatI, &timeAbsolute); + VectorOperations::cross(xAxisXI, velSatI, zAxisXI); + VectorOperations::normalize(zAxisXI, zAxisXI, 3); // y-Axis completes RHS - double yAxis[3] = {0, 0, 0}; - VectorOperations::cross(zAxis, xAxis, yAxis); + double yAxisXI[3] = {0, 0, 0}; + VectorOperations::cross(zAxisXI, xAxisXI, yAxisXI); // join transformation matrix - double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, - {xAxis[1], yAxis[1], zAxis[1]}, - {xAxis[2], yAxis[2], zAxis[2]}}; - QuaternionOperations::fromDcm(dcmTgt, targetQuat); + double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, + {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, + {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; + QuaternionOperations::fromDcm(dcmXI, targetQuat); - int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, timeDelta, targetQuat, refSatRate); + targetRotationRate(timeDelta, targetQuat, refSatRate); +} + +void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], double *refSatRate) { + if (VectorOperations::norm(quatIXprev, 4) == 0) { + std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); + } + if (timeDelta != 0.0) { + QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate); + } else { + std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double)); + } + std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); } 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) { - // First calculate error quaternion between current and target orientation - double invTargetQuat[4] = {0, 0, 0, 0}; - QuaternionOperations::inverse(targetQuat, invTargetQuat); - QuaternionOperations::multiply(currentQuat, invTargetQuat, errorQuat); - // Last calculate add rotation from reference quaternion - QuaternionOperations::multiply(refQuat, errorQuat, errorQuat); + // First calculate error quaternion between current and target orientation without reference + // quaternion + double errorQuatWoRef[4] = {0, 0, 0, 0}; + QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef); + // Then add rotation from reference quaternion + QuaternionOperations::multiply(refQuat, errorQuatWoRef, errorQuat); // Keep scalar part of quaternion positive if (errorQuat[3] < 0) { VectorOperations::mulScalar(errorQuat, -1, errorQuat, 4); @@ -435,30 +220,9 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle) { - // First calculate error quaternion between current and target orientation - QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); - // Keep scalar part of quaternion positive - if (errorQuat[3] < 0) { - VectorOperations::mulScalar(errorQuat, -1, errorQuat, 4); - } - // Calculate error angle - errorAngle = QuaternionOperations::getAngle(errorQuat, true); - - // Calculate error satellite rotational rate - VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); -} - -void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta, - double quatIX[4], double *refSatRate) { - if (VectorOperations::norm(quatIXprev, 4) == 0) { - std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); - } - if (timeDelta != 0.0) { - QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate); - } else { - std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double)); - } - std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); + double refQuat[4] = {0, 0, 0, 1}, refSatRotRate[3] = {0, 0, 0}; + comparePtg(currentQuat, currentSatRotRate, targetQuat, targetSatRotRate, refQuat, refSatRotRate, + errorQuat, errorSatRotRate, errorAngle); } ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index bdf6d7fb..9835b762 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -1,6 +1,7 @@ #ifndef GUIDANCE_H_ #define GUIDANCE_H_ +#include #include #include #include @@ -24,33 +25,18 @@ class Guidance { ReturnValue_t solarArrayDeploymentComplete(); void resetValues(); - // Function to get the target quaternion and reference rotation rate from gps position and - // position of the ground station - void targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3], double velSatE[3], - double sunDirI[3], double refDirB[3], double quatBI[4], - double targetQuat[4], double targetSatRotRate[3]); - void targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta, double posSatE[3], - double velSatE[3], double quatIX[4], double targetSatRotRate[3]); - void targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta, double posSatE[3], + void targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, const double sunDirI[3], + const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]); + void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], + double velSatE[3], double quatIX[4], double targetSatRotRate[3]); + void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3], double sunDirI[3], double quatIX[4], double targetSatRotRate[3]); + void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatF[3], + double velSatF[3], double targetQuat[4], double refSatRate[3]); - // Function to get the target quaternion and reference rotation rate for sun pointing after ground - // station - void targetQuatPtgSun(const double timeDelta, double sunDirI[3], double targetQuat[4], - double targetSatRotRate[3]); + void targetRotationRate(const double timeDelta, double quatInertialTarget[4], + double *targetSatRotRate); - // Function to get the target quaternion and refence rotation rate from gps position for Nadir - // pointing - void targetQuatPtgNadirSingleAxis(const timeval timeAbsolute, double posSatE[3], double quatBI[4], - double targetQuat[4], double refDirB[3], double refSatRate[3]); - void targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta, - double posSatE[3], double velSatE[3], double targetQuat[4], - double refSatRate[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 - // rate. Lastly gives back the error angle of the error quaternion. void 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); @@ -58,11 +44,6 @@ class Guidance { double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle); - void targetRotationRate(const int8_t timeElapsedMax, const double timeDelta, - double quatInertialTarget[4], double *targetSatRotRate); - - // @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); private: