eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

naming fixes
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-20 16:14:40 +01:00
parent 2c8cfa3874
commit 48b0ee8662
1 changed files with 31 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
mission/controller/acs/Guidance.cpp
View File

@ -144,7 +144,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta
} }
void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double quatIX[4], double targetSatRotRate[3]) { double targetQuat[4], double targetSatRotRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing // Calculation of target quaternion for target pointing
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
@ -203,14 +203,14 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]}, {xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}}; {xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmIX, quatIX); QuaternionOperations::fromDcm(dcmIX, targetQuat);
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, quatIX, targetSatRotRate); targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
} }
void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3],
double targetSatRotRate[3]) { double targetQuat[4], double targetSatRotRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for ground station pointing // Calculation of target quaternion for ground station pointing
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
@ -266,11 +266,10 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]}, {xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}}; {xAxis[2], yAxis[2], zAxis[2]}};
double quatInertialTarget[4] = {0, 0, 0, 0}; QuaternionOperations::fromDcm(dcmTgt, targetQuat);
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax; int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, quatInertialTarget, targetSatRotRate); targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
} }
void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) { void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) {
@ -289,7 +288,7 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double
VectorOperations<double>::cross(zAxis, helperVec, yAxis); VectorOperations<double>::cross(zAxis, helperVec, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3); VectorOperations<double>::normalize(yAxis, yAxis, 3);
// construct x-axis from y- and z-axes // x-axis completes RHS
double xAxis[3] = {0, 0, 0}; double xAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(yAxis, zAxis, xAxis); VectorOperations<double>::cross(yAxis, zAxis, xAxis);
VectorOperations<double>::normalize(xAxis, xAxis, 3); VectorOperations<double>::normalize(xAxis, xAxis, 3);
@ -365,53 +364,49 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4],
refSatRate[2] = 0; refSatRate[2] = 0;
} }
void Guidance::quatNadirPtgThreeAxes(double posSateE[3], double velSateE[3], timeval now, void Guidance::quatNadirPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double targetQuat[4], double refSatRate[3]) { double targetQuat[4], double refSatRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing // Calculation of target quaternion for Nadir pointing
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
double targetDirE[3] = {0, 0, 0}; // transformation between ECEF and ECI frame
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3); 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<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
// Transformation between ECEF and ECI frame double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
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<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // satellite position in inertial reference frame
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); double posSatI[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
// Target Direction in the body frame // negative x-axis aligned with position vector
double targetDirJ[3] = {0, 0, 0}; // this aligns with the camera, E- and S-band antennas
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
// negative x-Axis aligned with target (Camera position)
double xAxis[3] = {0, 0, 0}; double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirJ, xAxis, 3); VectorOperations<double>::normalize(posSatI, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3); VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// z-Axis parallel to long side of picture resolution // make z-Axis parallel to major part of camera resolution
double zAxis[3] = {0, 0, 0}; double zAxis[3] = {0, 0, 0};
std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3); VectorOperations<double>::cross(xAxis, velocityI, zAxis);
VectorOperations<double>::cross(xAxis, velocityJ, zAxis);
VectorOperations<double>::normalize(zAxis, zAxis, 3); VectorOperations<double>::normalize(zAxis, zAxis, 3);
// y-Axis completes RHS // y-Axis completes RHS
double yAxis[3] = {0, 0, 0}; double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxis, xAxis, yAxis); VectorOperations<double>::cross(zAxis, xAxis, yAxis);
// Complete transformation matrix // join transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]}, {xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}}; {xAxis[2], yAxis[2], zAxis[2]}};
double quatInertialTarget[4] = {0, 0, 0, 0}; QuaternionOperations::fromDcm(dcmTgt, targetQuat);
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax; int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate); refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);