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

in case we need this later
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 17:39:03 +01:00
parent 7275454f8a
commit 3ad6c8a56c
1 changed files with 26 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -16,7 +16,8 @@ Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters
Guidance::~Guidance() {}
void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double targetQuat[4],
void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double refDirB[3],
double quatIB[4], double targetQuat[4],
double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion to groundstation or given latitude, longitude and altitude
@ -39,25 +40,26 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
// target direction in the ECI frame
double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI = {0, 0, 0};
// target direction in the ECI frame
double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 3, 3, 1);
VectorOperations<double>::subtract(targetI, posSatI, &targetDirI, 3);
VectorOperations<double>::subtract(targetI, posSatI, targetDirI, 3);
// reference direction in ECI frame
double refDirI[3] = {0, 0, 0};
QuaternionOperations::multiplyVector(quatIB, refDirB, refDirI);
// rotation quaternion from two vectors
double refDirB[3] = {0, 0, 0};
std::memcpy(refDirB, acsParameters.targetModeControllerParameters.refDirection,
3 * sizeof(double));
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
double normRefDirB = VectorOperations<double>::norm(refDir, 3);
double crossDir[3] = {0, 0, 0};
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
VectorOperations<double>::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);
double crossDirI[3] = {0, 0, 0};
double dotDirections = VectorOperations<double>::dot(targetDirI, refDirI);
VectorOperations<double>::cross(targetDirI, refDirI, crossDirI);
targetQuat[0] = crossDirI[0];
targetQuat[1] = crossDirI[1];
targetQuat[2] = crossDirI[2];
targetQuat[3] = sqrt(pow(VectorOperations<double>::norm(targetDirI, 3), 2) *
pow(VectorOperations<double>::norm(refDirI, 3), 2) +
dotDirections);
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
//-------------------------------------------------------------------------------------
@ -229,10 +231,11 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
MatrixOperations<double>::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1);
VectorOperations<double>::subtract(groundStationI, posSatI, groundStationDirI, 3);
// x-axis aligned with target direction
// negative x-axis aligned with target direction
// this aligns with the camera, E- and S-band antennas
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(groundStationDirI, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// get sun vector model in ECI
VectorOperations<double>::normalize(sunDirI, sunDirI, 3);
@ -296,15 +299,15 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double
refSatRate[2] = 0;
}
void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4],
double refSatRate[3]) { // old version of Nadir Pointing
void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double refDirB[3],
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
// 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}};
@ -314,7 +317,7 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4],
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Transformation between ECEF and Body frame
// 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};
@ -322,11 +325,11 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4],
QuaternionOperations::toDcm(quatBJ, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
// Target Direction in the body frame
// target Direction in the body frame
double targetDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
// rotation quaternion from two vectors
// rotation quaternion from two vectors
double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];