fixed nullspace controller
This commit is contained in:
parent
a71be232ac
commit
5a9da1a99c
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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_ */
|
||||
|
|
|
|||
Loading…
Reference in New Issue