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

afmt
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Details

Browse Source
This commit is contained in:
Robin Müller
2022-12-01 15:56:55 +01:00
parent a3a919437e
commit 828738ba0e
14 changed files with 217 additions and 240 deletions
Download Patch File Download Diff File Expand all files Collapse all files

160
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,100 +14,96 @@
#include <fsfw/globalfunctions/sign.h>
#include <math.h>
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){
loadAcsParameters(acsParameters_);
#include "../util/MathOperations.h"
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
PtgCtrl::~PtgCtrl() {}
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices = &(acsParameters_->rwMatrices);
}
PtgCtrl::~PtgCtrl(){
void PtgCtrl::ptgGroundstation(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;
}
double cInt = 2 * om * zeta;
double kInt = 2 * pow(om, 2);
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices =&(acsParameters_->rwMatrices);
}
double qErrorLaw[3] = {0, 0, 0};
void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){
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);
//------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------
double om = pointingModeControllerParameters->om;
double zeta = pointingModeControllerParameters->zeta;
double qErrorMin = pointingModeControllerParameters->qiMin;
double omMax = pointingModeControllerParameters->omMax;
double gain1 = cInt * omMax / qErrorLawNorm;
double gainVector[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(qErrorLaw, gain1, gainVector, 3);
double cInt = 2 * om * zeta;
double kInt = 2 * pow(om,2);
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 qErrorLaw[3] = {0, 0, 0};
// 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];
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 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 gain1 = cInt * omMax / qErrorLawNorm;
double gainVector[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(qErrorLaw, gain1, gainVector, 3);
//------------------------------------------------------------------------------------------------
// Torque Calculations for the reaction wheels
//------------------------------------------------------------------------------------------------
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 pError[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*pMatrix, qError, pError, 3, 3, 1);
double pErrorSign[3] = {0, 0, 0};
// 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];
for (int i = 0; i < 3; i++) {
if (abs(pError[i]) > 1) {
pErrorSign[i] = sign(pError[i]);
} else {
pErrorSign[i] = pError[i];
}
}
// Torque for quaternion error
double torqueQuat[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 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
//------------------------------------------------------------------------------------------------
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]);
}
else {
pErrorSign[i] = pError[i];
}
}
// Torque for quaternion error
double torqueQuat[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);
// Torque for rate error
double torqueRate[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
// Final commanded Torque for every reaction wheel
double torque[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
// Torque for rate error
double torqueRate[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
// Final commanded Torque for every reaction wheel
double torque[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
}
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,