eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 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() {} 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]) { double targetSatRotRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion to groundstation or given latitude, longitude and altitude // 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}}; double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
// target direction in the ECI frame // target direction in the ECI frame
double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI = {0, 0, 0}; 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, posSatE, posSatI, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 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 // rotation quaternion from two vectors
double refDirB[3] = {0, 0, 0}; double crossDirI[3] = {0, 0, 0};
std::memcpy(refDirB, acsParameters.targetModeControllerParameters.refDirection, double dotDirections = VectorOperations<double>::dot(targetDirI, refDirI);
3 * sizeof(double)); VectorOperations<double>::cross(targetDirI, refDirI, crossDirI);
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3); targetQuat[0] = crossDirI[0];
double normRefDirB = VectorOperations<double>::norm(refDir, 3); targetQuat[1] = crossDirI[1];
double crossDir[3] = {0, 0, 0}; targetQuat[2] = crossDirI[2];
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir); targetQuat[3] = sqrt(pow(VectorOperations<double>::norm(targetDirI, 3), 2) *
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir); pow(VectorOperations<double>::norm(refDirI, 3), 2) +
targetQuat[0] = crossDir[0]; dotDirections);
targetQuat[1] = crossDir[1];
targetQuat[2] = crossDir[2];
targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
VectorOperations<double>::normalize(targetQuat, targetQuat, 4); 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); MatrixOperations<double>::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1);
VectorOperations<double>::subtract(groundStationI, posSatI, groundStationDirI, 3); 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 // this aligns with the camera, E- and S-band antennas
double xAxis[3] = {0, 0, 0}; double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(groundStationDirI, xAxis, 3); VectorOperations<double>::normalize(groundStationDirI, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// get sun vector model in ECI // get sun vector model in ECI
VectorOperations<double>::normalize(sunDirI, sunDirI, 3); VectorOperations<double>::normalize(sunDirI, sunDirI, 3);
@ -296,15 +299,15 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double
refSatRate[2] = 0; refSatRate[2] = 0;
} }
void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double refDirB[3],
double refSatRate[3]) { // old version of Nadir Pointing double refSatRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing // Calculation of target quaternion for Nadir pointing
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
double targetDirE[3] = {0, 0, 0}; double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3); 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 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 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}}; 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}}; double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); 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 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 dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double quatBJ[4] = {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); QuaternionOperations::toDcm(quatBJ, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); 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}; double targetDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); 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}; double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0]; refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1]; refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];