From 053c1a7fe8f7385370eedd894b1ef1d2f399d359 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Sep 2022 15:17:39 +0200 Subject: [PATCH] added Guidance --- mission/controller/acs/Guidance.cpp | 347 ++++++++++++++++++++++++++++ mission/controller/acs/Guidance.h | 42 ++++ 2 files changed, 389 insertions(+) create mode 100644 mission/controller/acs/Guidance.cpp create mode 100644 mission/controller/acs/Guidance.h diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp new file mode 100644 index 00000000..d861769d --- /dev/null +++ b/mission/controller/acs/Guidance.cpp @@ -0,0 +1,347 @@ +/* + * Guidance.cpp + * + * Created on: 6 Jun 2022 + * Author: Robin Marquardt + */ + + +#include "Guidance.h" +#include "string.h" +#include +#include +#include +#include +#include +#include + +Guidance::Guidance(AcsParameters *acsParameters_) { + acsParameters = *acsParameters_; + +} + +Guidance::~Guidance() { + +} + +void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { + + for (int i = 0; i < 3; i++) { + + sunTargetSafe[i] = + acsParameters.safeModeControllerParameters.sunTargetDir[i]; + satRateSafe[i] = + acsParameters.safeModeControllerParameters.satRateRef[i]; + + } + + +// memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); + +} + +void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, + timeval now, double targetQuat[4], double refSatRate[3]){ +//------------------------------------------------------------------------------------- +// Calculation of target quaternion to groundstation +//------------------------------------------------------------------------------------- +// Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth fixed/centered frame) + double groundStationCart[3] = {0, 0, 0}; + + MathOperations::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, + acsParameters.groundStationParameters.longitudeGs, acsParameters.groundStationParameters.altitudeGs, + groundStationCart); + +// Position of the satellite in the earth/fixed frame via GPS + double posSatE[3] = {0, 0, 0}; + MathOperations::cartesianFromLatLongAlt(sensorValues->gps0latitude, sensorValues->gps0longitude, + sensorValues->gps0altitude, posSatE); + +// Target direction in the ECEF frame + 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}}; + MathOperations::dcmEJ(now, *dcmEJ); + MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); +// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION + double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; + double omegaEarth = acsParameters.targetModeControllerParameters.omegaEarth; + +// TEST SECTION ! + double dcmTEST[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(&acsParameters.magnetorquesParameter.mtq0orientationMatrix, dcmTEST, dcmTEST, 3, 3, 3); + + MatrixOperations::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3); + MatrixOperations::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3); + 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}; + quatBJ[0] = outputValues->quatMekfBJ[0]; + quatBJ[1] = outputValues->quatMekfBJ[1]; + quatBJ[2] = outputValues->quatMekfBJ[2]; + quatBJ[3] = outputValues->quatMekfBJ[3]; + 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}; + velSatE[0] = sensorValues->gps0Velocity[0]; + velSatE[1] = sensorValues->gps0Velocity[1]; + velSatE[2] = sensorValues->gps0Velocity[2]; + 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 sunDirJ[3] = {0, 0, 0}; + double sunDirB[3] = {0, 0, 0}; + + if ( outputValues->sunDirModelValid ) { + + sunDirJ[0] = outputValues->sunDirModel[0]; + sunDirJ[1] = outputValues->sunDirModel[1]; + sunDirJ[2] = outputValues->sunDirModel[2]; + MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); + } + + else { + sunDirB[0] = outputValues->sunDirEst[0]; + sunDirB[1] = outputValues->sunDirEst[1]; + sunDirB[2] = outputValues->sunDirEst[2]; + + } + + 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::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], 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}; + satRate[0] = outputValues->satRateMekf[0]; + satRate[1] = outputValues->satRateMekf[1]; + satRate[2] = outputValues->satRateMekf[2]; + 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] }}; + + double quatErrorComplete[4] = {0, 0, 0, 0}; + 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->validRw0 && sensorValues->validRw1 && sensorValues->validRw2 && sensorValues->validRw3) { + + 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->validRw0) && sensorValues->validRw1 && sensorValues->validRw2 && sensorValues->validRw3) { + + 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->validRw0) && !(sensorValues->validRw1) && sensorValues->validRw2 && sensorValues->validRw3) { + + 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->validRw0) && (sensorValues->validRw1) && !(sensorValues->validRw2) && sensorValues->validRw3) { + + 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->validRw0) && (sensorValues->validRw1) && (sensorValues->validRw2) && (sensorValues->validRw3)) { + + 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 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]; + + } +} diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h new file mode 100644 index 00000000..736d8b9b --- /dev/null +++ b/mission/controller/acs/Guidance.h @@ -0,0 +1,42 @@ +/* + * Guidance.h + * + * Created on: 6 Jun 2022 + * Author: Robin Marquardt + */ + +#ifndef GUIDANCE_H_ +#define GUIDANCE_H_ + + +#include +#include +#include +#include + + +class Guidance { +public: + Guidance(AcsParameters *acsParameters_); + virtual ~Guidance(); + + void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]); + + // Function to get the target quaternion and refence rotation rate from gps position and position of the ground station + void targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]); + + // @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired + void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] ); + +// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid reation wheel +// maybe can be done in "commanding.h" + void getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *rwPseudoInv); + + +private: + AcsParameters acsParameters; + bool strBlindAvoidFlag = false; +}; + +#endif /* ACS_GUIDANCE_H_ */