eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests Releases 118 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
mission/controller/acs/AcsParameters.cpp
View File

@ -315,7 +315,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->setMatrix(rwMatrices.without4); parameterWrapper->setMatrix(rwMatrices.without4);
break; break;
case 0x6: case 0x6:
parameterWrapper->setVector(rwMatrices.nullspace); parameterWrapper->setVector(rwMatrices.nullspaceVector);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
@ -375,15 +375,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(idleModeControllerParameters.gainNullspace); parameterWrapper->set(idleModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef); parameterWrapper->set(idleModeControllerParameters.nullspaceSpeed);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(idleModeControllerParameters.deSatGainFactor); parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(idleModeControllerParameters.desatOn); parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(idleModeControllerParameters.desatOn);
break;
case 0x9:
parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
break; break;
default: default:
@ -408,48 +411,51 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(targetModeControllerParameters.gainNullspace); parameterWrapper->set(targetModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); parameterWrapper->set(targetModeControllerParameters.nullspaceSpeed);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(targetModeControllerParameters.desatOn); parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); parameterWrapper->set(targetModeControllerParameters.desatOn);
break; break;
case 0x9: case 0x9:
parameterWrapper->setVector(targetModeControllerParameters.refDirection); parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
break; break;
case 0xA: case 0xA:
parameterWrapper->setVector(targetModeControllerParameters.refRotRate); parameterWrapper->setVector(targetModeControllerParameters.refDirection);
break; break;
case 0xB: case 0xB:
parameterWrapper->setVector(targetModeControllerParameters.quatRef); parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
break; break;
case 0xC: case 0xC:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); parameterWrapper->setVector(targetModeControllerParameters.quatRef);
break; break;
case 0xD: case 0xD:
parameterWrapper->set(targetModeControllerParameters.latitudeTgt); parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
break; break;
case 0xE: case 0xE:
parameterWrapper->set(targetModeControllerParameters.longitudeTgt); parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
break; break;
case 0xF: case 0xF:
parameterWrapper->set(targetModeControllerParameters.altitudeTgt); parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
break; break;
case 0x10: case 0x10:
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
break; break;
case 0x11: case 0x11:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
break; break;
case 0x12: case 0x12:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
break; break;
case 0x13: case 0x13:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
break;
case 0x14:
parameterWrapper->set(targetModeControllerParameters.blindRotRate); parameterWrapper->set(targetModeControllerParameters.blindRotRate);
break; break;
default: default:
@ -474,30 +480,33 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace); parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef); parameterWrapper->set(gsTargetModeControllerParameters.nullspaceSpeed);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor); parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(gsTargetModeControllerParameters.desatOn); parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
break; break;
case 0x9: case 0x9:
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
break; break;
case 0xA: case 0xA:
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
break; break;
case 0xB: case 0xB:
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
break; break;
case 0xC: case 0xC:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
break; break;
case 0xD: case 0xD:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
break;
case 0xE:
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
break; break;
default: default:
@ -522,21 +531,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(nadirModeControllerParameters.gainNullspace); parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef); parameterWrapper->set(nadirModeControllerParameters.nullspaceSpeed);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(nadirModeControllerParameters.desatOn); parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); parameterWrapper->set(nadirModeControllerParameters.desatOn);
break; break;
case 0x9: case 0x9:
parameterWrapper->setVector(nadirModeControllerParameters.refDirection); parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
break; break;
case 0xA: case 0xA:
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
break;
case 0xB:
parameterWrapper->setVector(nadirModeControllerParameters.quatRef); parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
break; break;
case 0xC: case 0xC:
@ -564,21 +576,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(inertialModeControllerParameters.gainNullspace); parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef); parameterWrapper->set(inertialModeControllerParameters.nullspaceSpeed);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(inertialModeControllerParameters.desatOn); parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); parameterWrapper->set(inertialModeControllerParameters.desatOn);
break; break;
case 0x9: case 0x9:
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
break; break;
case 0xA: case 0xA:
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
break;
case 0xB:
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
break; break;
case 0xC: case 0xC:

4
mission/controller/acs/AcsParameters.h
View File

@ -814,7 +814,7 @@ class AcsParameters : public HasParametersIF {
{1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}}; {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
double without4[4][3] = { double without4[4][3] = {
{0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}}; {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000}; double nullspaceVector[4] = {-1, 1, -1, 1};
} rwMatrices; } rwMatrices;
struct SafeModeControllerParameters { struct SafeModeControllerParameters {
@ -838,7 +838,9 @@ class AcsParameters : public HasParametersIF {
double om = 0.3; double om = 0.3;
double omMax = 1 * M_PI / 180; double omMax = 1 * M_PI / 180;
double qiMin = 0.1; double qiMin = 0.1;
double gainNullspace = 0.01; double gainNullspace = 0.01;
double nullspaceSpeed = 32500; // 0.1 RPM
double desatMomentumRef[3] = {0, 0, 0}; double desatMomentumRef[3] = {0, 0, 0};
double deSatGainFactor = 1000; double deSatGainFactor = 1000;

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

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

@ -42,7 +42,7 @@ class PtgCtrl {
private: private:
const AcsParameters *acsParameters; const AcsParameters *acsParameters;
static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI); static constexpr double RPM_TO_RAD_PER_SEC = (2 * M_PI) / 60;
}; };
#endif /* ACS_CONTROL_PTGCTRL_H_ */ #endif /* ACS_CONTROL_PTGCTRL_H_ */