82 lines
4.0 KiB
C++
82 lines
4.0 KiB
C++
#ifndef 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/AcsParameters.h>
|
|
#include <mission/controller/acs/SensorValues.h>
|
|
#include <mission/controller/acs/util/MathOperations.h>
|
|
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
|
#include <time.h>
|
|
|
|
#include <cmath>
|
|
#include <filesystem>
|
|
#include <string>
|
|
|
|
class Guidance {
|
|
public:
|
|
Guidance(AcsParameters *acsParameters_);
|
|
virtual ~Guidance();
|
|
|
|
void getTargetParamsSafe(double sunTargetSafe[3]);
|
|
ReturnValue_t solarArrayDeploymentComplete();
|
|
void resetValues();
|
|
|
|
// Function to get the target quaternion and reference rotation rate from gps position and
|
|
// position of the ground station
|
|
void targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3], double velSatE[3],
|
|
double sunDirI[3], double refDirB[3], double quatBI[4],
|
|
double targetQuat[4], double targetSatRotRate[3]);
|
|
void targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta, double posSatE[3],
|
|
double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
|
|
void targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta, double posSatE[3],
|
|
double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
|
|
|
|
// Function to get the target quaternion and reference rotation rate for sun pointing after ground
|
|
// station
|
|
void targetQuatPtgSun(const double timeDelta, double sunDirI[3], double targetQuat[4],
|
|
double targetSatRotRate[3]);
|
|
|
|
// Function to get the target quaternion and refence rotation rate from gps position for Nadir
|
|
// pointing
|
|
void targetQuatPtgNadirSingleAxis(const timeval timeAbsolute, double posSatE[3], double quatBI[4],
|
|
double targetQuat[4], double refDirB[3], double refSatRate[3]);
|
|
void targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta,
|
|
double posSatE[3], double velSatE[3], double targetQuat[4],
|
|
double refSatRate[3]);
|
|
|
|
// @note: Calculates the error quaternion between the current orientation and the target
|
|
// quaternion, considering a reference quaternion. Additionally the difference between the actual
|
|
// and a desired satellite rotational rate is calculated, again considering a reference rotational
|
|
// rate. Lastly gives back the error angle of the error quaternion.
|
|
void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
|
|
double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
|
|
double errorQuat[4], double errorSatRotRate[3], double &errorAngle);
|
|
void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
|
|
double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
|
|
double &errorAngle);
|
|
|
|
void targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
|
|
double quatInertialTarget[4], double *targetSatRotRate);
|
|
|
|
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
|
|
// reation wheel maybe can be done in "commanding.h"
|
|
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
|
|
|
|
private:
|
|
const AcsParameters *acsParameters;
|
|
|
|
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
|
|
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
|
|
|
|
bool strBlindAvoidFlag = false;
|
|
double quatIXprev[4] = {0, 0, 0, 0};
|
|
|
|
static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
|
|
static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";
|
|
};
|
|
|
|
#endif /* ACS_GUIDANCE_H_ */
|