calculate the rotation rate in normal way
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit

This commit is contained in:
Marius Eggert 2024-01-26 11:16:59 +01:00
parent c1e7ac7e9a
commit 291c9ea99b
2 changed files with 44 additions and 58 deletions

View File

@ -1,15 +1,5 @@
#include "Guidance.h" #include "Guidance.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/util/MathOperations.h>
#include <cmath>
#include <filesystem>
#include <string>
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
Guidance::~Guidance() {} Guidance::~Guidance() {}
@ -455,44 +445,16 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
} }
void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta, void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
double quatInertialTarget[4], double *refSatRate) { double quatIX[4], double *refSatRate) {
//------------------------------------------------------------------------------------- if (VectorOperations<double>::norm(quatIXprev, 4) == 0) {
// Calculation of target rotation rate std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
//-------------------------------------------------------------------------------------
if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
} }
if (timeDelta < timeElapsedMax and timeDelta != 0.0) { if (not timeDelta != 0.0) {
double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0}; QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate);
QuaternionOperations::inverse(quatInertialTarget, q);
QuaternionOperations::inverse(savedQuaternion, qS);
double qDiff[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(q, qS, qDiff, 4);
VectorOperations<double>::mulScalar(qDiff, 1. / timeDelta, qDiff, 4);
double tgtQuatVec[3] = {q[0], q[1], q[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};
VectorOperations<double>::cross(tgtQuatVec, qDiffVec, sum1);
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
VectorOperations<double>::mulScalar(qDiffVec, q[3], sum3, 3);
VectorOperations<double>::add(sum1, sum2, sum, 3);
VectorOperations<double>::subtract(sum, sum3, sum, 3);
double omegaRefNew[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
omegaRefSaved[0] = omegaRefNew[0];
omegaRefSaved[1] = omegaRefNew[1];
omegaRefSaved[2] = omegaRefNew[2];
} else { } else {
refSatRate[0] = 0; std::memcpy(refSatRate, ZERO_VEC3, sizeof(refSatRate));
refSatRate[1] = 0;
refSatRate[2] = 0;
} }
std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
} }
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
@ -506,22 +468,27 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
return returnvalue::OK; return returnvalue::OK;
} else if (not rw1valid and rw2valid and rw3valid and rw4valid) { } else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double)); std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1,
return returnvalue::OK; 12 * sizeof(double));
return SINGLE_RW_UNAVAILABLE;
} else if (rw1valid and not rw2valid and rw3valid and rw4valid) { } else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double)); std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2,
return returnvalue::OK; 12 * sizeof(double));
return SINGLE_RW_UNAVAILABLE;
} else if (rw1valid and rw2valid and not rw3valid and rw4valid) { } else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double)); std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3,
return returnvalue::OK; 12 * sizeof(double));
return SINGLE_RW_UNAVAILABLE;
} else if (rw1valid and rw2valid and rw3valid and not rw4valid) { } else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double)); std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4,
return returnvalue::OK; 12 * sizeof(double));
} else { return SINGLE_RW_UNAVAILABLE;
return returnvalue::FAILED;
} }
return MULTIPLE_RW_UNAVAILABLE;
} }
void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); }
void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) { void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) {
std::error_code e; std::error_code e;
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or

View File

@ -1,9 +1,18 @@
#ifndef GUIDANCE_H_ #ifndef GUIDANCE_H_
#define GUIDANCE_H_ #define GUIDANCE_H_
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/util/MathOperations.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <time.h> #include <time.h>
#include "../controllerdefinitions/AcsCtrlDefinitions.h" #include <cmath>
#include <filesystem>
#include <string>
#include "AcsParameters.h" #include "AcsParameters.h"
#include "SensorValues.h" #include "SensorValues.h"
@ -14,6 +23,7 @@ class Guidance {
void getTargetParamsSafe(double sunTargetSafe[3]); void getTargetParamsSafe(double sunTargetSafe[3]);
ReturnValue_t solarArrayDeploymentComplete(); ReturnValue_t solarArrayDeploymentComplete();
void resetValues();
// Function to get the target quaternion and reference 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
@ -56,12 +66,21 @@ class Guidance {
// reation wheel maybe can be done in "commanding.h" // reation wheel maybe can be done in "commanding.h"
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
//! [EXPORT] : [COMMENT] A single RW has failed.
static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE =
returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 3);
//! [EXPORT] : [COMMENT] Multiple RWs have failed.
static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE =
returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 4);
private: private:
const AcsParameters *acsParameters; const AcsParameters *acsParameters;
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
static constexpr double ZERO_VEC4[3] = {0, 0, 0, 0};
bool strBlindAvoidFlag = false; bool strBlindAvoidFlag = false;
double savedQuaternion[4] = {0, 0, 0, 0}; double quatIXprev[4] = {0, 0, 0, 0};
double omegaRefSaved[3] = {0, 0, 0};
static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm"; static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm"; static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";