/* * Guidance.cpp * * Created on: 6 Jun 2022 * Author: Robin Marquardt */ #include "Guidance.h" #include #include #include #include #include #include #include "string.h" #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; } Guidance::~Guidance() {} void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 3 * sizeof(double)); } else { std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop, 3 * sizeof(double)); } std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef, 3 * sizeof(double)); } void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- // Transform longitude, latitude and altitude to cartesian coordiantes (earth // fixed/centered frame) double targetCart[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt, acsParameters.ptgTargetParameters.altitudeTgt, targetCart); // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, sensorValues->gpsSet.altitude.value, posSatE); // Target direction in the ECEF frame double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); // Transformation between ECEF and IJK frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); // Transformation between ECEF and Body frame double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double quatBJ[4] = {0, 0, 0, 0}; std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); QuaternionOperations::toDcm(quatBJ, dcmBJ); MatrixOperations::multiply(*dcmBJ, *dcmJE, *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 velSatE[3] = {0, 0, 0}; std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); 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(*dcmBJ, *dcmJEDot, *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, refSatRate, 3); //------------------------------------------------------------------------------------- // Calculation of reference rotation rate in case of star tracker blinding //------------------------------------------------------------------------------------- if (acsParameters.targetModeControllerParameters.avoidBlindStr) { double sunDirB[3] = {0, 0, 0}; if (susDataProcessed->sunIjkModel.isValid()) { double sunDirJ[3] = {0, 0, 0}; std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); } else { std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double)); } 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, refSatRate, refSatRate, 3); } else { strBlindAvoidFlag = false; } } } } void Guidance::refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate) { //------------------------------------------------------------------------------------- // Calculation of reference rotation rate //------------------------------------------------------------------------------------- double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - (timeSavedQuaternionNadir.tv_sec + timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6)); if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { double qDiff[4] = {0, 0, 0, 0}; VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; VectorOperations::cross(quatInertialTarget, qDiff, sum1); VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); VectorOperations::add(sum1, sum2, sum, 3); VectorOperations::subtract(sum, sum3, sum, 3); double omegaRefNew[3] = {0, 0, 0}; VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); VectorOperations::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); omegaRefSavedNadir[0] = omegaRefNew[0]; omegaRefSavedNadir[1] = omegaRefNew[1]; omegaRefSavedNadir[2] = omegaRefNew[2]; } else { refSatRate[0] = 0; refSatRate[1] = 0; refSatRate[2] = 0; } timeSavedQuaternionNadir = now; savedQuaternionNadir[0] = quatInertialTarget[0]; savedQuaternionNadir[1] = quatInertialTarget[1]; savedQuaternionNadir[2] = quatInertialTarget[2]; savedQuaternionNadir[3] = quatInertialTarget[3]; } void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- // Transform longitude, latitude and altitude to cartesian coordiantes (earth // fixed/centered frame) double targetCart[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt, acsParameters.ptgTargetParameters.altitudeTgt, targetCart); // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double)); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); // Transformation between ECEF and IJK frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); // Target Direction and position vector in the inertial frame double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); MatrixOperations::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1); // negative x-Axis aligned with target (Camera/E-band transmitter position) double xAxis[3] = {0, 0, 0}; VectorOperations::normalize(targetDirJ, xAxis, 3); VectorOperations::mulScalar(xAxis, -1, xAxis, 3); // Transform velocity into inertial frame double velocityE[3]; std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); VectorOperations::add(velPart1, velPart2, velocityJ, 3); // orbital normal vector double orbitalNormalJ[3] = {0, 0, 0}; VectorOperations::cross(posSatJ, velocityJ, orbitalNormalJ); VectorOperations::normalize(orbitalNormalJ, orbitalNormalJ, 3); // y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution double yAxis[3] = {0, 0, 0}; VectorOperations::cross(orbitalNormalJ, xAxis, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); // z-Axis completes RHS double zAxis[3] = {0, 0, 0}; VectorOperations::cross(xAxis, yAxis, zAxis); // Complete transformation matrix double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; double quatInertialTarget[4] = {0, 0, 0, 0}; QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); refRotationRate(now, quatInertialTarget, refSatRate); // Transform in system relative to satellite frame double quatBJ[4] = {0, 0, 0, 0}; std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for ground station pointing //------------------------------------------------------------------------------------- // Transform longitude, latitude and altitude to cartesian coordiantes (earth // fixed/centered frame) double groundStationCart[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt, acsParameters.ptgTargetParameters.altitudeTgt, groundStationCart); // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, sensorValues->gpsSet.altitude.value, posSatE); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); // Transformation between ECEF and IJK frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); // Target Direction and position vector in the inertial frame double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); MatrixOperations::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1); // negative x-Axis aligned with target (Camera/E-band transmitter position) double xAxis[3] = {0, 0, 0}; VectorOperations::normalize(targetDirJ, xAxis, 3); VectorOperations::mulScalar(xAxis, -1, xAxis, 3); // get Sun Vector Model in ECI double sunJ[3]; std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); VectorOperations::normalize(sunJ, sunJ, 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, sunJ); xDotS /= pow(VectorOperations::norm(xAxis, 3), 2); double sunParallel[3], zAxis[3]; VectorOperations::mulScalar(xAxis, xDotS, sunParallel, 3); VectorOperations::subtract(sunJ, sunParallel, zAxis, 3); VectorOperations::normalize(zAxis, zAxis, 3); // calculate y-axis double yAxis[3]; VectorOperations::cross(zAxis, xAxis, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); // Complete transformation matrix double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; double quatInertialTarget[4] = {0, 0, 0, 0}; QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); refRotationRate(now, quatInertialTarget, refSatRate); // Transform in system relative to satellite frame double quatBJ[4] = {0, 0, 0, 0}; std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- double quatBJ[4] = {0, 0, 0, 0}; double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); QuaternionOperations::toDcm(quatBJ, dcmBJ); double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0}; if (susDataProcessed->sunIjkModel.isValid()) { std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); } else if (susDataProcessed->susVecTot.isValid()) { std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double)); } else { return; } // Transformation between ECEF and IJK frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); // positive z-Axis of EIVE in direction of sun double zAxis[3] = {0, 0, 0}; VectorOperations::normalize(sunDirB, zAxis, 3); // Position of satellite in ECEF double posSatE[3] = {0, 0, 0}; std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double)); // Assign helper vector (north pole inertial) double helperVec[3] = {0, 0, 1}; // double yAxis[3] = {0, 0, 0}; VectorOperations::cross(zAxis, helperVec, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); // double xAxis[3] = {0, 0, 0}; VectorOperations::cross(yAxis, zAxis, xAxis); VectorOperations::normalize(xAxis, xAxis, 3); // Transformation matrix to Sun, no further transforamtions, reference is already // the EIVE body frame double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; double quatSun[4] = {0, 0, 0, 0}; QuaternionOperations::fromDcm(dcmTgt, quatSun); targetQuat[0] = quatSun[0]; targetQuat[1] = quatSun[1]; targetQuat[2] = quatSun[2]; targetQuat[3] = quatSun[3]; //---------------------------------------------------------------------------- // Calculation of reference rotation rate //---------------------------------------------------------------------------- refSatRate[0] = 0; refSatRate[1] = 0; refSatRate[2] = 0; } void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], double refSatRate[3]) { // old version of Nadir Pointing //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, sensorValues->gpsSet.altitude.value, posSatE); double targetDirE[3] = {0, 0, 0}; VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); // Transformation between ECEF and IJK frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); // Transformation between ECEF and Body frame double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double quatBJ[4] = {0, 0, 0, 0}; std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); QuaternionOperations::toDcm(quatBJ, dcmBJ); MatrixOperations::multiply(*dcmBJ, *dcmJE, *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.nadirRefDirection[0]; refDir[1] = acsParameters.targetModeControllerParameters.nadirRefDirection[1]; refDir[2] = acsParameters.targetModeControllerParameters.nadirRefDirection[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; } void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, sensorValues->gpsSet.altitude.value, posSatE); double targetDirE[3] = {0, 0, 0}; VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); // Transformation between ECEF and IJK frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); // Target Direction in the body frame double targetDirJ[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); // negative x-Axis aligned with target (Camera position) double xAxis[3] = {0, 0, 0}; VectorOperations::normalize(targetDirJ, xAxis, 3); VectorOperations::mulScalar(xAxis, -1, xAxis, 3); // z-Axis parallel to long side of picture resolution double zAxis[3] = {0, 0, 0}, velocityE[3]; std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); VectorOperations::add(velPart1, velPart2, velocityJ, 3); VectorOperations::cross(xAxis, velocityJ, zAxis); VectorOperations::normalize(zAxis, zAxis, 3); // y-Axis completes RHS double yAxis[3] = {0, 0, 0}; VectorOperations::cross(zAxis, xAxis, yAxis); // Complete transformation matrix double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; double quatInertialTarget[4] = {0, 0, 0, 0}; QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); refRotationRate(now, quatInertialTarget, refSatRate); // Transform in system relative to satellite frame double quatBJ[4] = {0, 0, 0, 0}; std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { for (int i = 0; i < 4; i++) { targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuatInertial[i]; } for (int i = 0; i < 3; i++) { refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i]; } } void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3], double quatErrorComplete[4], double quatError[3], double deltaRate[3]) { double quatRef[4] = {0, 0, 0, 0}; quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0]; quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1]; quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2]; quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3]; double satRate[3] = {0, 0, 0}; std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double)); VectorOperations::subtract(satRate, refSatRate, deltaRate, 3); // valid checks ? 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); if (quatErrorComplete[3] < 0) { quatErrorComplete[3] *= -1; } 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 ?? } void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() && sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) { rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0]; rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1]; rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2]; rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0]; rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1]; rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2]; rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0]; rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1]; rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2]; } else if (!(sensorValues->rw1Set.isValid()) && sensorValues->rw2Set.isValid() && sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) { rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0]; rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1]; rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2]; rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0]; rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1]; rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2]; rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0]; rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1]; rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2]; rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0]; rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1]; rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2]; } else if ((sensorValues->rw1Set.isValid()) && !(sensorValues->rw2Set.isValid()) && sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) { rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0]; rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1]; rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2]; rwPseudoInv[3] = acsParameters.rwMatrices.without1[1][0]; rwPseudoInv[4] = acsParameters.rwMatrices.without1[1][1]; rwPseudoInv[5] = acsParameters.rwMatrices.without1[1][2]; rwPseudoInv[6] = acsParameters.rwMatrices.without1[2][0]; rwPseudoInv[7] = acsParameters.rwMatrices.without1[2][1]; rwPseudoInv[8] = acsParameters.rwMatrices.without1[2][2]; rwPseudoInv[9] = acsParameters.rwMatrices.without1[3][0]; rwPseudoInv[10] = acsParameters.rwMatrices.without1[3][1]; rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2]; } else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) && !(sensorValues->rw3Set.isValid()) && sensorValues->rw4Set.isValid()) { rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0]; rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1]; rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2]; rwPseudoInv[3] = acsParameters.rwMatrices.without2[1][0]; rwPseudoInv[4] = acsParameters.rwMatrices.without2[1][1]; rwPseudoInv[5] = acsParameters.rwMatrices.without2[1][2]; rwPseudoInv[6] = acsParameters.rwMatrices.without2[2][0]; rwPseudoInv[7] = acsParameters.rwMatrices.without2[2][1]; rwPseudoInv[8] = acsParameters.rwMatrices.without2[2][2]; rwPseudoInv[9] = acsParameters.rwMatrices.without2[3][0]; rwPseudoInv[10] = acsParameters.rwMatrices.without2[3][1]; rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2]; } else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) && (sensorValues->rw3Set.isValid()) && !(sensorValues->rw4Set.isValid())) { rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0]; rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1]; rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2]; rwPseudoInv[3] = acsParameters.rwMatrices.without3[1][0]; rwPseudoInv[4] = acsParameters.rwMatrices.without3[1][1]; rwPseudoInv[5] = acsParameters.rwMatrices.without3[1][2]; rwPseudoInv[6] = acsParameters.rwMatrices.without3[2][0]; rwPseudoInv[7] = acsParameters.rwMatrices.without3[2][1]; rwPseudoInv[8] = acsParameters.rwMatrices.without3[2][2]; rwPseudoInv[9] = acsParameters.rwMatrices.without3[3][0]; rwPseudoInv[10] = acsParameters.rwMatrices.without3[3][1]; rwPseudoInv[11] = acsParameters.rwMatrices.without3[3][2]; } else { // @note: This one takes the normal pseudoInverse of all four raction wheels valid. // Does not make sense, but is implemented that way in MATLAB ?! // Thought: It does not really play a role, because in case there are more then one // reaction wheel invalid the pointing control is destined to fail. rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0]; rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1]; rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2]; rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0]; rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1]; rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2]; rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0]; rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1]; rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2]; } }