63 lines
2.8 KiB
C++
63 lines
2.8 KiB
C++
#ifndef PTGCTRL_H_
|
|
#define PTGCTRL_H_
|
|
|
|
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
|
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
#include <fsfw/globalfunctions/sign.h>
|
|
#include <mission/acs/defs.h>
|
|
#include <mission/controller/acs/AcsParameters.h>
|
|
#include <mission/controller/acs/SensorValues.h>
|
|
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
|
|
|
#include <cmath>
|
|
|
|
class PtgCtrl {
|
|
/*
|
|
* @brief: This class handles the pointing control mechanism. Calculation of an commanded
|
|
* torque for the reaction wheels, and magnetic Field strength for magnetorquer for desaturation
|
|
*
|
|
* @note: A description of the used algorithms can be found in
|
|
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110
|
|
*/
|
|
public:
|
|
/* @brief: Constructor
|
|
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
|
|
*/
|
|
PtgCtrl(AcsParameters *acsParameters_);
|
|
virtual ~PtgCtrl();
|
|
|
|
acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
|
const bool strValid, const bool questValid,
|
|
const bool fusedRateValid,
|
|
const uint8_t rotRateSource,
|
|
const uint8_t mekfEnabled);
|
|
|
|
/* @brief: Calculates the needed torque for the pointing control mechanism
|
|
*/
|
|
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
|
|
const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
|
|
|
|
void ptgNullspace(const bool allRwAvabilable,
|
|
AcsParameters::PointingLawParameters *pointingLawParameters,
|
|
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
|
|
const int32_t speedRw3, double *rwTrqNs);
|
|
|
|
void ptgDesaturation(const bool allRwAvabilable, const acsctrl::RwAvail *rwAvail,
|
|
AcsParameters::PointingLawParameters *pointingLawParameters,
|
|
const double *magFieldB, const bool magFieldBValid, const int32_t speedRw0,
|
|
const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3,
|
|
double *mgtDpDes);
|
|
|
|
/* @brief: Commands the stiction torque in case wheel speed is to low
|
|
* torqueCommand modified torque after anti-stiction
|
|
*/
|
|
void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed);
|
|
|
|
private:
|
|
const AcsParameters *acsParameters;
|
|
static constexpr double RPM_TO_RAD_PER_SEC = (2 * M_PI) / 60;
|
|
};
|
|
|
|
#endif /* ACS_CONTROL_PTGCTRL_H_ */
|