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

Merge branch 'eggert/acs' into marquardt/ptgCtrl
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source 
# Conflicts:
#	mission/controller/AcsController.cpp
#	mission/controller/AcsController.h
#	mission/controller/acs/AcsParameters.h
#	mission/controller/acs/ActuatorCmd.h
#	mission/controller/acs/Guidance.cpp
#	mission/controller/acs/Guidance.h
#	mission/controller/acs/MultiplicativeKalmanFilter.cpp
#	mission/controller/acs/OutputValues.h
#	mission/controller/acs/SensorProcessing.cpp
#	mission/controller/acs/SensorProcessing.h
#	mission/controller/acs/control/Detumble.cpp
#	mission/controller/acs/control/Detumble.h
#	mission/controller/acs/control/PtgCtrl.cpp
#	mission/controller/acs/util/MathOperations.h
This commit is contained in:
Marius Eggert
2022-12-13 11:26:23 +01:00
parent 2b445369fd cf1a1185fc
commit d33ae9ede7
322 changed files with 17249 additions and 9124 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -5,10 +5,8 @@
* Author: Robin Marquardt
*/
#include "PtgCtrl.h"
#include "../util/MathOperations.h"
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
@ -16,77 +14,76 @@
#include <fsfw/globalfunctions/sign.h>
#include <math.h>
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_): torqueMemory {0, 0, 0, 0}, omegaMemory {0, 0, 0, 0} {
#include "../util/MathOperations.h"
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){
loadAcsParameters(acsParameters_);
}
PtgCtrl::~PtgCtrl(){
PtgCtrl::~PtgCtrl() {}
}
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices =&(acsParameters_->rwMatrices);
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices = &(acsParameters_->rwMatrices);
}
void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){
//------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------
double om = pointingModeControllerParameters->om;
double zeta = pointingModeControllerParameters->zeta;
double qErrorMin = pointingModeControllerParameters->qiMin;
double omMax = pointingModeControllerParameters->omMax;
//------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------
double om = pointingModeControllerParameters->om;
double zeta = pointingModeControllerParameters->zeta;
double qErrorMin = pointingModeControllerParameters->qiMin;
double omMax = pointingModeControllerParameters->omMax;
double cInt = 2 * om * zeta;
double kInt = 2 * pow(om, 2);
double cInt = 2 * om * zeta;
double kInt = 2 * pow(om,2);
double qErrorLaw[3] = {0, 0, 0};
double qErrorLaw[3] = {0, 0, 0};
for (int i = 0; i < 3; i++) {
if (abs(qError[i]) < qErrorMin) {
qErrorLaw[i] = qErrorMin;
} else {
qErrorLaw[i] = abs(qError[i]);
}
}
double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);
for (int i = 0; i < 3; i++) {
if (abs(qError[i]) < qErrorMin) {
qErrorLaw[i] = qErrorMin;
}
else {
qErrorLaw[i] = abs(qError[i]);
}
}
double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);
double gain1 = cInt * omMax / qErrorLawNorm;
double gainVector[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(qErrorLaw, gain1, gainVector, 3);
double gain1 = cInt * omMax / qErrorLawNorm;
double gainVector[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(qErrorLaw, gain1, gainVector, 3);
double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
gainMatrixDiagonal[0][0] = gainVector[0];
gainMatrixDiagonal[1][1] = gainVector[1];
gainMatrixDiagonal[2][2] = gainVector[2];
MatrixOperations<double>::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix),
*gainMatrix, 3, 3, 3);
double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
gainMatrixDiagonal[0][0] = gainVector[0];
gainMatrixDiagonal[1][1] = gainVector[1];
gainMatrixDiagonal[2][2] = gainVector[2];
MatrixOperations<double>::multiply( *gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), *gainMatrix, 3, 3, 3);
// Inverse of gainMatrix
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
// Inverse of gainMatrix
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3,
3, 3);
MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
//------------------------------------------------------------------------------------------------
// Torque Calculations for the reaction wheels
//------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------
// Torque Calculations for the reaction wheels
//------------------------------------------------------------------------------------------------
double pError[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*pMatrix, qError, pError, 3, 3, 1);
double pErrorSign[3] = {0, 0, 0};
for (int i = 0; i < 3; i++) {
double pError[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*pMatrix, qError, pError, 3, 3, 1);
double pErrorSign[3] = {0, 0, 0};
for (int i = 0; i < 3; i++) {
if (abs(pError[i]) > 1) {
pErrorSign[i] = sign(pError[i]);
}
@ -113,7 +110,7 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt
}
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
int32_t *speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {
@ -124,7 +121,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl
}
// calculating momentum of satellite and momentum of reaction wheels
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4);
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4,
@ -146,7 +143,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl
void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0};
double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
// Conversion to [rad/s] for further calculations