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

fixed nullspace controller

Browse Source
This commit is contained in:
Marius Eggert
2023-04-27 13:52:41 +02:00
committed by Robin Mueller
parent a71be232ac
commit 5a9da1a99c
4 changed files with 74 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -136,23 +136,27 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara
// concentrate RW speeds as vector and convert to double
double speedRws[4] = {static_cast<double>(*speedRw0), static_cast<double>(*speedRw1),
static_cast<double>(*speedRw2), static_cast<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
VectorOperations<double>::mulScalar(rpmOffset, factor, rpmOffset, 4);
VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
double diffRwSpeed[4] = {0, 0, 0, 0};
// calculate RPM offset utilizing the nullspace
double rpmOffset[4] = {0, 0, 0, 0};
double rpmOffsetSpeed = pointingLawParameters->nullspaceSpeed / 10 * RPM_TO_RAD_PER_SEC;
VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector, rpmOffsetSpeed,
rpmOffset, 4);
// calculate resulting angular momentum
double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
wheelMomentum, 4);
double gainNs = pointingLawParameters->gainNullspace;
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace,
acsParameters->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);
rwAngMomentum, 4);
// calculate resulting torque
double nullspaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(acsParameters->rwMatrices.nullspaceVector,
acsParameters->rwMatrices.nullspaceVector, *nullspaceMatrix, 4,
1, 4);
MatrixOperations<double>::multiply(*nullspaceMatrix, rwAngMomentum, rwTrqNs, 4, 4, 1);
VectorOperations<double>::mulScalar(rwTrqNs, -1 * pointingLawParameters->gainNullspace, rwTrqNs,
4);
}
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) {