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

WIP: Fixes for Pointing Controller #631

Closed
muellerr wants to merge 16 commits from acs-ptg-ctrl-fixes-2 into v2.1.0-dev
pull from: acs-ptg-ctrl-fixes-2
merge into: eive:v2.1.0-dev
Conversation 0 Commits 16 Files Changed 12 +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);
break;
case 0x6:
parameterWrapper->setVector(rwMatrices.nullspace);
parameterWrapper->setVector(rwMatrices.nullspaceVector);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -375,15 +375,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(idleModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
parameterWrapper->set(idleModeControllerParameters.nullspaceSpeed);
break;
case 0x6:
parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
break;
case 0x7:
parameterWrapper->set(idleModeControllerParameters.desatOn);
parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
break;
case 0x8:
parameterWrapper->set(idleModeControllerParameters.desatOn);
break;
case 0x9:
parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
break;
default:
@ -408,48 +411,51 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
parameterWrapper->set(targetModeControllerParameters.nullspaceSpeed);
break;
case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
break;
case 0x7:
parameterWrapper->set(targetModeControllerParameters.desatOn);
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
break;
case 0x8:
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
parameterWrapper->set(targetModeControllerParameters.desatOn);
break;
case 0x9:
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
break;
case 0xB:
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
break;
case 0xC:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
break;
case 0xD:
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
break;
case 0xE:
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
break;
case 0xF:
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
break;
case 0x10:
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
break;
case 0x11:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
break;
case 0x12:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
break;
case 0x13:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
break;
case 0x14:
parameterWrapper->set(targetModeControllerParameters.blindRotRate);
break;
default:
@ -474,30 +480,33 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
parameterWrapper->set(gsTargetModeControllerParameters.nullspaceSpeed);
break;
case 0x6:
parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
break;
case 0x7:
parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
break;
case 0x8:
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
break;
case 0x9:
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
break;
case 0xB:
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
break;
case 0xC:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
break;
case 0xD:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
break;
case 0xE:
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
break;
default:
@ -522,21 +531,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
parameterWrapper->set(nadirModeControllerParameters.nullspaceSpeed);
break;
case 0x6:
parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
break;
case 0x7:
parameterWrapper->set(nadirModeControllerParameters.desatOn);
parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
break;
case 0x8:
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
parameterWrapper->set(nadirModeControllerParameters.desatOn);
break;
case 0x9:
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
break;
case 0xB:
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
break;
case 0xC:
@ -564,21 +576,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
parameterWrapper->set(inertialModeControllerParameters.nullspaceSpeed);
break;
case 0x6:
parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
break;
case 0x7:
parameterWrapper->set(inertialModeControllerParameters.desatOn);
parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
break;
case 0x8:
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
parameterWrapper->set(inertialModeControllerParameters.desatOn);
break;
case 0x9:
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
break;
case 0xB:
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
break;
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}};
double without4[4][3] = {
{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;
struct SafeModeControllerParameters {
@ -838,7 +838,9 @@ class AcsParameters : public HasParametersIF {
double om = 0.3;
double omMax = 1 * M_PI / 180;
double qiMin = 0.1;
double gainNullspace = 0.01;
double nullspaceSpeed = 32500; // 0.1 RPM
double desatMomentumRef[3] = {0, 0, 0};
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
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) {

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

@ -42,7 +42,7 @@ class PtgCtrl {
private:
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_ */