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

Prevent STR Blinding #859

Merged
meggert merged 35 commits from prevent-str-blinding into main 2024-02-27 13:48:20 +01:00
Conversation 0 Commits 35 Files Changed 15 +21 -23
2 changed files with 21 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit b9e4c51d82 - Show all commits

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

@ -36,9 +36,9 @@ void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta,
targetRotationRate(timeDelta, targetQuat, targetSatRotRate); targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
} }
void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
double velSatF[3], double targetQuat[4], const double posSatF[3], const double velSatF[3],
double targetSatRotRate[3]) { double targetQuat[4], double targetSatRotRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing // Calculation of target quaternion for target pointing
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
@ -115,18 +115,16 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
VectorOperations<double>::normalize(groundStationDirI, xAxisIX, 3); VectorOperations<double>::normalize(groundStationDirI, xAxisIX, 3);
VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3); VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
// normalize sun vector in ECI
double sunDirNormalizedI[3] = {0, 0, 0};
// get earth vector in ECI // get earth vector in ECI
double earthDirNormalizedI[3] = {0, 0, 0}; double earthDirI[3] = {0, 0, 0};
VectorOperations<double>::normalize(posSatI, earthDirNormalizedI, 3); VectorOperations<double>::normalize(posSatI, earthDirI, 3);
VectorOperations<double>::mulScalar(earthDirNormalizedI, -1, earthDirNormalizedI, 3); VectorOperations<double>::mulScalar(earthDirI, -1, earthDirI, 3);
// sun avoidance calculations // sun avoidance calculations
double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0}; double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0};
VectorOperations<double>::mulScalar( VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirI),
xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirNormalizedI), sunPerpendicularX, 3); sunPerpendicularX, 3);
VectorOperations<double>::subtract(sunDirNormalizedI, sunPerpendicularX, sunFloorYZ, 3); VectorOperations<double>::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3);
VectorOperations<double>::normalize(sunFloorYZ, sunFloorYZ, 3); VectorOperations<double>::normalize(sunFloorYZ, sunFloorYZ, 3);
double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0}, double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0},
strVecSunZ[3] = {0, 0, 0}; strVecSunZ[3] = {0, 0, 0};
@ -136,13 +134,13 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
strVecSunZ, 3); strVecSunZ, 3);
VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3); VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3);
VectorOperations<double>::normalize(strVecSun, strVecSun, 3); VectorOperations<double>::normalize(strVecSun, strVecSun, 3);
sunWeight = VectorOperations<double>::dot(strVecSun, sunDirNormalizedI); sunWeight = VectorOperations<double>::dot(strVecSun, sunDirI);
// earth avoidance calculations // earth avoidance calculations
double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}; double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0};
VectorOperations<double>::mulScalar( VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, earthDirI),
xAxisIX, VectorOperations<double>::dot(xAxisIX, earthDirNormalizedI), earthPerpendicularX, 3); earthPerpendicularX, 3);
VectorOperations<double>::subtract(earthDirNormalizedI, earthPerpendicularX, earthFloorYZ, 3); VectorOperations<double>::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3);
VectorOperations<double>::normalize(earthFloorYZ, earthFloorYZ, 3); VectorOperations<double>::normalize(earthFloorYZ, earthFloorYZ, 3);
double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0}, double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0},
strVecEarthZ[3] = {0, 0, 0}; strVecEarthZ[3] = {0, 0, 0};
@ -152,7 +150,7 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
strVecEarthZ, 3); strVecEarthZ, 3);
VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3); VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3);
VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3); VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3);
earthWeight = VectorOperations<double>::dot(strVecEarth, earthDirNormalizedI); earthWeight = VectorOperations<double>::dot(strVecEarth, earthDirI);
if ((sunWeight == 0.0) and (earthWeight == 0.0)) { if ((sunWeight == 0.0) and (earthWeight == 0.0)) {
// if this actually ever happens i will eat a broom // if this actually ever happens i will eat a broom
@ -187,8 +185,9 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
targetRotationRate(timeDelta, targetQuat, targetSatRotRate); targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
} }
void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatE[3], void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta,
double velSatE[3], double targetQuat[4], double refSatRate[3]) { const double posSatE[3], const double velSatE[3],
double targetQuat[4], double refSatRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing // Calculation of target quaternion for Nadir pointing
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------

9
mission/controller/acs/Guidance.h
View File

@ -9,7 +9,6 @@
#include <mission/controller/acs/AcsParameters.h> #include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h> #include <mission/controller/acs/SensorValues.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h> #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <time.h>
#include <cmath> #include <cmath>
#include <filesystem> #include <filesystem>
@ -26,12 +25,12 @@ class Guidance {
void targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, const double sunDirI[3], void targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, const double sunDirI[3],
const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]); const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]);
void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
double velSatE[3], double quatIX[4], double targetSatRotRate[3]); const double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, const double posSatF[3], void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
const double sunDirI[3], double quatIX[4], double targetSatRotRate[3]); const double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatF[3], void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
double velSatF[3], double targetQuat[4], double refSatRate[3]); const double velSatF[3], double targetQuat[4], double refSatRate[3]);
void targetRotationRate(const double timeDelta, double quatInertialTarget[4], void targetRotationRate(const double timeDelta, double quatInertialTarget[4],
double *targetSatRotRate); double *targetSatRotRate);