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

actually calculate the vector from position to target
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:59:05 +01:00
parent 57054c46ab
commit 7275454f8a
1 changed files with 19 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -21,18 +21,13 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion to groundstation or given latitude, longitude and altitude // Calculation of target quaternion to groundstation or given latitude, longitude and altitude
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// transform longitude, latitude and altitude to cartesian coordiantes (earth // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
// fixed/centered frame) double targetE[3] = {0, 0, 0};
double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt( MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
// target direction in the ECEF frame
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
// transformation between ECEF and ECI frame // transformation between ECEF and ECI frame
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -44,17 +39,11 @@ 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);
// Transformation between ECEF and Body frame // target direction in the ECI frame
// double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI = {0, 0, 0};
// double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
// double quatBJ[4] = {0, 0, 0, 0}; MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 3, 3, 1);
VectorOperations<double>::subtract(targetI, posSatI, &targetDirI, 3);
// QuaternionOperations::toDcm(quatBJ, dcmBJ);
// MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
// 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 refDirB[3] = {0, 0, 0}; double refDirB[3] = {0, 0, 0};
@ -167,16 +156,16 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
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 and position vector in the inertial frame // target direction in the ECI frame
double targetDirI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0}; double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmIE, targetDirE, targetDirI, 3, 3, 1);
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);
VectorOperations<double>::subtract(targetI, posSatI, targetDirI, 3);
// negative x-axis aligned with target // 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(targetDirI, xAxis, 3); VectorOperations<double>::normalize(targetDirI, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// transform velocity into inertial frame // transform velocity into inertial frame
double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
@ -234,16 +223,16 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
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 and position vector in the inertial frame // target direction in the ECI frame
double targetDirI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0}; double posSatI[3] = {0, 0, 0}, groundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmIE, targetDirE, targetDirI, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1);
VectorOperations<double>::subtract(groundStationI, posSatI, groundStationDirI, 3);
// negative x-axis aligned with target // 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(targetDirI, 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);