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

Final Version of the ACS Controller #367

Merged
muellerr merged 78 commits from eggert/acs into develop 2023-02-08 13:50:11 +01:00
Conversation 1 Commits 78 Files Changed 27 +15 -9
1 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 25867e76b1 - Show all commits

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

@ -337,21 +337,25 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// get Sun Vector Model in ECI as helper vector (element of x-z plane)
// get Sun Vector Model in ECI
double sunJ[3];
std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
VectorOperations<double>::normalize(sunJ, sunJ, 3);
// calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
// z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x
double xDotS = VectorOperations<double>::dot(xAxis, sunJ);
xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
double sunParallel[3], zAxis[3];
VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
VectorOperations<double>::subtract(sunJ, sunParallel, zAxis, 3);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// calculate y-axis
double yAxis[3];
VectorOperations<double>::cross(sunJ, xAxis, yAxis);
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// calculate z-axis
double zAxis[3];
VectorOperations<double>::cross(xAxis, yAxis, zAxis);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// Complete transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
@ -383,8 +387,10 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me
if (susDataProcessed->sunIjkModel.isValid()) {
std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
} else {
} else if (susDataProcessed->susVecTot.isValid()) {
std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
} else {
return;
}
// Transformation between ECEF and IJK frame
@ -416,7 +422,7 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me
double velocityB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBJ, velocityJ, velocityB, 3, 3, 1);
// Normale to velocity and sunDir
// Normal to velocity and sunDir
double crossVelSun[3] = {0, 0, 0};
VectorOperations<double>::cross(velocityB, sunDirB, crossVelSun);