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

cleaned up ptgCtrl
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-28 10:47:37 +01:00
parent 84643020da
commit 737d5ecb53
3 changed files with 23 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
mission/controller/acs/Guidance.cpp
View File

@ -204,7 +204,6 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3],
double targetQuat[4], double targetSatRotRate[3]) { double targetQuat[4], double targetSatRotRate[3]) {
sif::debug << acsParameters->gsTargetModeControllerParameters.altitudeTgt << std::endl;
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for ground station pointing // Calculation of target quaternion for ground station pointing
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------

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

@ -1,10 +1,3 @@
/*
* PtgCtrl.cpp
*
* Created on: 17 Jul 2022
* Author: Robin Marquardt
*/
#include "PtgCtrl.h" #include "PtgCtrl.h"
#include <fsfw/globalfunctions/constants.h> #include <fsfw/globalfunctions/constants.h>
@ -16,16 +9,10 @@
#include "../util/MathOperations.h" #include "../util/MathOperations.h"
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
PtgCtrl::~PtgCtrl() {} PtgCtrl::~PtgCtrl() {}
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices = &(acsParameters_->rwMatrices);
}
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *errorQuat, const double *deltaRate, const double *rwPseudoInv, const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
double *torqueRws) { double *torqueRws) {
@ -62,8 +49,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
gainMatrixDiagonal[0][0] = gainVector[0]; gainMatrixDiagonal[0][0] = gainVector[0];
gainMatrixDiagonal[1][1] = gainVector[1]; gainMatrixDiagonal[1][1] = gainVector[1];
gainMatrixDiagonal[2][2] = gainVector[2]; gainMatrixDiagonal[2][2] = gainVector[2];
MatrixOperations<double>::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), MatrixOperations<double>::multiply(
*gainMatrix, 3, 3, 3); *gainMatrixDiagonal, *(acsParameters->inertiaEIVE.inertiaMatrix), *gainMatrix, 3, 3, 3);
// Inverse of gainMatrix // Inverse of gainMatrix
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -72,8 +59,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
gainMatrixInverse[2][2] = 1 / gainMatrix[2][2]; gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, MatrixOperations<double>::multiply(
3, 3); *gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrix), *pMatrix, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3); MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
//------------------------------------------------------------------------------------------------ //------------------------------------------------------------------------------------------------
@ -98,7 +85,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
// Torque for rate error // Torque for rate error
double torqueRate[3] = {0, 0, 0}; double torqueRate[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1); MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), deltaRate,
torqueRate, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3); VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3); VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
@ -123,11 +111,13 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
// calculating momentum of satellite and momentum of reaction wheels // calculating momentum of satellite and momentum of reaction wheels
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, momentumRwU, 4);
1); MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU,
momentumRw, 3, 4, 1);
double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1); MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), satRate,
momentumSat, 3, 3, 1);
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3); VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
// calculating momentum error // calculating momentum error
double deltaMomentum[3] = {0, 0, 0}; double deltaMomentum[3] = {0, 0, 0};
@ -152,11 +142,12 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara
VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
double diffRwSpeed[4] = {0, 0, 0, 0}; double diffRwSpeed[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4); VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
wheelMomentum, 4); wheelMomentum, 4);
double gainNs = pointingLawParameters->gainNullspace; double gainNs = pointingLawParameters->gainNullspace;
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, MathOperations<double>::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace,
acsParameters->rwMatrices.nullspace,
*nullSpaceMatrix, 4); *nullSpaceMatrix, 4);
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
@ -174,21 +165,22 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, double *torqueComm
for (uint8_t i = 0; i < 4; i++) { for (uint8_t i = 0; i < 4; i++) {
if (rwAvailable[i]) { if (rwAvailable[i]) {
if (torqueMemory[i] != 0) { if (torqueMemory[i] != 0) {
if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) { if ((omegaRw[i] * torqueMemory[i]) >
acsParameters->rwHandlingParameters.stictionReleaseSpeed) {
torqueMemory[i] = 0; torqueMemory[i] = 0;
} else { } else {
torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; torqueCommand[i] = torqueMemory[i] * acsParameters->rwHandlingParameters.stictionTorque;
} }
} else { } else {
if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) && if ((omegaRw[i] < acsParameters->rwHandlingParameters.stictionSpeed) &&
(omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { (omegaRw[i] > -acsParameters->rwHandlingParameters.stictionSpeed)) {
if (omegaRw[i] < omegaMemory[i]) { if (omegaRw[i] < omegaMemory[i]) {
torqueMemory[i] = -1; torqueMemory[i] = -1;
} else { } else {
torqueMemory[i] = 1; torqueMemory[i] = 1;
} }
torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; torqueCommand[i] = torqueMemory[i] * acsParameters->rwHandlingParameters.stictionTorque;
} }
} }
} else { } else {

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

@ -33,13 +33,7 @@ class PtgCtrl {
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_PTG; static const uint8_t INTERFACE_ID = CLASS_ID::ACS_PTG;
static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
/* @brief: Load AcsParameters for 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 /* @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, void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
const double *deltaRate, const double *rwPseudoInv, double *torqueRws); const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
@ -54,15 +48,12 @@ class PtgCtrl {
const int32_t *speedRw3, double *rwTrqNs); const int32_t *speedRw3, double *rwTrqNs);
/* @brief: Commands the stiction torque in case wheel speed is to low /* @brief: Commands the stiction torque in case wheel speed is to low
* @param: sensorValues class containing all RW related values
* torqueCommand modified torque after antistiction * torqueCommand modified torque after antistiction
*/ */
void rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand); void rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand);
private: private:
AcsParameters::RwHandlingParameters *rwHandlingParameters; const AcsParameters *acsParameters;
AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::RwMatrices *rwMatrices;
double torqueMemory[4] = {0, 0, 0, 0}; double torqueMemory[4] = {0, 0, 0, 0};
double omegaMemory[4] = {0, 0, 0, 0}; double omegaMemory[4] = {0, 0, 0, 0};