This commit is contained in:
parent
3d123a09f3
commit
497149a628
165
mission/controller/acs/control/PtgCtrl.cpp
Normal file
165
mission/controller/acs/control/PtgCtrl.cpp
Normal file
@ -0,0 +1,165 @@
|
||||
/*
|
||||
* PtgCtrl.cpp
|
||||
*
|
||||
* Created on: 17 Jul 2022
|
||||
* Author: Robin Marquardt
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "PtgCtrl.h"
|
||||
#include <fsfw/src/fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <acs/util/MathOperations.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/sign.h>
|
||||
#include <math.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);
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
// 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);
|
||||
|
||||
//------------------------------------------------------------------------------------------------
|
||||
// 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);
|
||||
|
||||
}
|
||||
|
||||
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, double *speedRw0,
|
||||
double *speedRw1, double *speedRw2, double *speedRw3, double *mgtDpDes) {
|
||||
if ( !(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {
|
||||
|
||||
mgtDpDes[0] = 0;
|
||||
mgtDpDes[1] = 0;
|
||||
mgtDpDes[2] = 0;
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
// calculating momentum of satellite and momentum of reaction wheels
|
||||
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *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, 1);
|
||||
double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1);
|
||||
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
|
||||
// calculating momentum error
|
||||
double deltaMomentum[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3);
|
||||
// resulting magnetic dipole command
|
||||
double crossMomentumMagField[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
|
||||
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
|
||||
factor = (pointingModeControllerParameters->deSatGainFactor) / normMag;
|
||||
VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
|
||||
|
||||
}
|
||||
|
||||
void PtgCtrl::ptgNullspace(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3, double *rwTrqNs) {
|
||||
|
||||
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *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
|
||||
VectorOperations<double>::mulScalar(rpmOffset, factor, rpmOffset, 4);
|
||||
VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
|
||||
double diffRwSpeed[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
|
||||
VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, wheelMomentum, 4);
|
||||
double gainNs = pointingModeControllerParameters->gainNullspace;
|
||||
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, *nullSpaceMatrix, 4);
|
||||
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
|
||||
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
|
||||
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
|
||||
|
||||
|
||||
}
|
||||
60
mission/controller/acs/control/PtgCtrl.h
Normal file
60
mission/controller/acs/control/PtgCtrl.h
Normal file
@ -0,0 +1,60 @@
|
||||
/*
|
||||
* PtgCtrl.h
|
||||
*
|
||||
* Created on: 17 Jul 2022
|
||||
* Author: Robin Marquardt
|
||||
*
|
||||
* @brief: This class handles the pointing control mechanism. Calculation of an commanded torque
|
||||
* for the reaction wheels, and magnetic Field strength for magnetorques 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
|
||||
*/
|
||||
|
||||
#ifndef PTGCTRL_H_
|
||||
#define PTGCTRL_H_
|
||||
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <acs/SensorValues.h>
|
||||
#include <acs/OutputValues.h>
|
||||
#include <acs/AcsParameters.h>
|
||||
#include "acs/config/classIds.h"
|
||||
#include <fsfw/src/fsfw/returnvalues/HasReturnvaluesIF.h>
|
||||
#include <time.h>
|
||||
|
||||
class PtgCtrl : public HasReturnvaluesIF {
|
||||
|
||||
public:
|
||||
/* @brief: Constructor
|
||||
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
|
||||
*/
|
||||
PtgCtrl(AcsParameters *acsParameters_);
|
||||
virtual ~PtgCtrl();
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::PTG;
|
||||
static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
|
||||
|
||||
/* @brief: Load AcsParameters für this class
|
||||
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
|
||||
*/
|
||||
void loadAcsParameters(AcsParameters *acsParameters_);
|
||||
|
||||
/* @brief: Calculates the needed torque for the pointing control mechanism
|
||||
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
|
||||
*/
|
||||
void ptgGroundstation(const double mode,const double *qError,const double *deltaRate,const double *rwPseudoInv, double *torqueRws);
|
||||
|
||||
void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, double *speedRw0,
|
||||
double *speedRw1, double *speedRw2, double *speedRw3, double *mgtDpDes);
|
||||
|
||||
void ptgNullspace(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3, double *rwTrqNs);
|
||||
|
||||
private:
|
||||
AcsParameters::PointingModeControllerParameters* pointingModeControllerParameters;
|
||||
AcsParameters::RwHandlingParameters* rwHandlingParameters;
|
||||
AcsParameters::InertiaEIVE* inertiaEIVE;
|
||||
AcsParameters::RWMatrices* rwMatrices;
|
||||
};
|
||||
|
||||
#endif /* ACS_CONTROL_PTGCTRL_H_ */
|
||||
Loading…
Reference in New Issue
Block a user