eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 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],
double quatIX[4], double targetSatRotRate[3]) {
double targetQuat[4], double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// 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]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmIX, quatIX);
QuaternionOperations::fromDcm(dcmIX, targetQuat);
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],
double targetSatRotRate[3]) {
void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3],
double targetQuat[4], double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// 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]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
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]) {
@ -289,7 +288,7 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double
VectorOperations<double>::cross(zAxis, helperVec, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// construct x-axis from y- and z-axes
// x-axis completes RHS
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(yAxis, zAxis, xAxis);
VectorOperations<double>::normalize(xAxis, xAxis, 3);
@ -365,53 +364,49 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4],
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]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::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<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
// Transformation between ECEF and ECI 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<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// satellite position in inertial reference frame
double posSatI[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
// Target Direction in the body frame
double targetDirJ[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
// negative x-Axis aligned with target (Camera position)
// negative x-axis aligned with position vector
// this aligns with the camera, E- and S-band antennas
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);
// 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};
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<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
VectorOperations<double>::cross(xAxis, velocityJ, zAxis);
double velocityI[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(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
VectorOperations<double>::cross(xAxis, velocityI, zAxis);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// y-Axis completes RHS
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
// Complete transformation matrix
// 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]}};
double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);