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

fixed calculation of rotational rate for ptgCtrl which also is used for idle now
Some checks failed
EIVE/eive-obsw/pipeline/pr-v3.0.0-dev There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-06-02 13:28:59 +02:00
parent f8d9925785
commit bd15d7b0e2
2 changed files with 21 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -266,7 +266,8 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
} }
void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) { void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
double targetSatRotRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion to sun // Calculation of target quaternion to sun
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
@ -296,9 +297,8 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double
//---------------------------------------------------------------------------- //----------------------------------------------------------------------------
// Calculation of reference rotation rate // Calculation of reference rotation rate
//---------------------------------------------------------------------------- //----------------------------------------------------------------------------
refSatRate[0] = 0; int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
refSatRate[1] = 0; targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
refSatRate[2] = 0;
} }
void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
@ -453,20 +453,25 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target rotation rate // Calculation of target rotation rate
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 -
(timeSavedQuaternion.tv_sec + (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6);
timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
}
if (timeElapsed < timeElapsedMax) { if (timeElapsed < timeElapsedMax) {
double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatInertialTarget, q);
QuaternionOperations::inverse(savedQuaternion, qS);
double qDiff[4] = {0, 0, 0, 0}; double qDiff[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4); VectorOperations<double>::subtract(q, qS, qDiff, 4);
VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, double tgtQuatVec[3] = {q[0], q[1], q[2]};
qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1); VectorOperations<double>::cross(tgtQuatVec, qDiffVec, sum1);
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); VectorOperations<double>::mulScalar(qDiffVec, q[3], sum3, 3);
VectorOperations<double>::add(sum1, sum2, sum, 3); VectorOperations<double>::add(sum1, sum2, sum, 3);
VectorOperations<double>::subtract(sum, sum3, sum, 3); VectorOperations<double>::subtract(sum, sum3, sum, 3);
double omegaRefNew[3] = {0, 0, 0}; double omegaRefNew[3] = {0, 0, 0};

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

@ -15,7 +15,7 @@ class Guidance {
void getTargetParamsSafe(double sunTargetSafe[3]); void getTargetParamsSafe(double sunTargetSafe[3]);
ReturnValue_t solarArrayDeploymentComplete(); ReturnValue_t solarArrayDeploymentComplete();
// Function to get the target quaternion and refence rotation rate from gps position and // Function to get the target quaternion and reference rotation rate from gps position and
// position of the ground station // position of the ground station
void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3], void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3],
double refDirB[3], double quatBI[4], double targetQuat[4], double refDirB[3], double quatBI[4], double targetQuat[4],
@ -25,9 +25,10 @@ class Guidance {
void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
double targetSatRotRate[3]); double targetSatRotRate[3]);
// Function to get the target quaternion and refence rotation rate for sun pointing after ground // Function to get the target quaternion and reference rotation rate for sun pointing after ground
// station // station
void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]); void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
double targetSatRotRate[3]);
// Function to get the target quaternion and refence rotation rate from gps position for Nadir // Function to get the target quaternion and refence rotation rate from gps position for Nadir
// pointing // pointing