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

applyed CppStyle Format
All checks were successful
EIVE/eive-obsw/pipeline/pr-eggert/acs This commit looks good
Details

Browse Source
This commit is contained in:
Robin Marquardt
2023-01-23 16:34:52 +01:00
parent 44b384cd17
commit adef468c0b
6 changed files with 714 additions and 683 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -26,8 +26,9 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
rwMatrices = &(acsParameters_->rwMatrices);
}
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameters, const double *qError, const double *deltaRate,
const double *rwPseudoInv, double *torqueRws) {
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *qError, const double *deltaRate, const double *rwPseudoInv,
double *torqueRws) {
//------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------
@ -106,7 +107,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameter
VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
}
void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLawParameters, double *magFieldEst, bool magFieldEstValid, double *satRate,
void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
int32_t *speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
@ -127,8 +129,8 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLaw
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
// calculating momentum error
double deltaMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(
momentumTotal, pointingLawParameters->desatMomentumRef, deltaMomentum, 3);
VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
deltaMomentum, 3);
// resulting magnetic dipole command
double crossMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
@ -137,7 +139,8 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLaw
VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
}
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters * pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1,
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0};

14
mission/controller/acs/control/PtgCtrl.h
View File

@ -41,14 +41,16 @@ class PtgCtrl {
/* @brief: Calculates the needed torque for the pointing control mechanism
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameters, const double *qError, const double *deltaRate,
const double *rwPseudoInv, double *torqueRws);
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
void ptgDesaturation(AcsParameters::PointingLawParameters * pointingLawParameters, double *magFieldEst,
bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1,
int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes);
void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
double *mgtDpDes);
void ptgNullspace(AcsParameters::PointingLawParameters * pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
const int32_t *speedRw3, double *rwTrqNs);
/* @brief: Commands the stiction torque in case wheel speed is to low