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

ACS Ptg Ctrl Fixes #643

Merged
muellerr merged 35 commits from acs-ptg-ctrl-fixes-2 into v3.0.0-dev 2023-06-07 11:49:10 +02:00
Conversation 4 Commits 35 Files Changed 15 +74 -53
4 changed files with 74 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 5a9da1a99c - Show all commits

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);
muellerr commented 2023-06-06 11:19:50 +02:00
Review
Copy Link

This looks like something which can break the MIB. Can it? If it does, I'd still be okay with this for v3.0.0 if the current MIB is not able to manipulate the parameter anyway.

This looks like something which can break the MIB. Can it? If it does, I'd still be okay with this for v3.0.0 if the current MIB is not able to manipulate the parameter anyway.
muellerr commented 2023-06-06 11:41:47 +02:00
Review
Copy Link

after discussion: The operator manually specifies the parameter ID, so a release into v3.0.0 is okay.

after discussion: The operator manually specifies the parameter ID, so a release into v3.0.0 is okay.
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_ */