access to different paramset for every pointing mode
This commit is contained in:
parent
ce3841a23d
commit
44b384cd17
@ -271,53 +271,106 @@ void AcsController::performControlOperation() {
|
||||
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
|
||||
double quatRef[4] = {0, 0, 0, 0};
|
||||
uint8_t enableAntiStiction = true;
|
||||
|
||||
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
|
||||
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
|
||||
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
|
||||
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0};
|
||||
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
|
||||
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
|
||||
|
||||
switch (submode) {
|
||||
case SUBMODE_IDLE:
|
||||
guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
|
||||
targetQuat, refSatRate);
|
||||
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double));
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
||||
ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
|
||||
break;
|
||||
|
||||
case SUBMODE_PTG_TARGET:
|
||||
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now,
|
||||
targetQuat, refSatRate);
|
||||
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double));
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
||||
ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
break;
|
||||
|
||||
case SUBMODE_PTG_TARGET_GS:
|
||||
guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed,
|
||||
now, targetQuat, refSatRate);
|
||||
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double));
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
||||
ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
break;
|
||||
|
||||
case SUBMODE_PTG_NADIR:
|
||||
guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
|
||||
refSatRate);
|
||||
std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double));
|
||||
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate);
|
||||
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
||||
ptgCtrl.ptgDesaturation(&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
break;
|
||||
|
||||
case SUBMODE_PTG_INERTIAL:
|
||||
guidance.inertialQuatPtg(targetQuat, refSatRate);
|
||||
std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, 4 * sizeof(double));
|
||||
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate);
|
||||
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
||||
ptgCtrl.ptgDesaturation(&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
break;
|
||||
}
|
||||
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
|
||||
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate);
|
||||
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
|
||||
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
|
||||
ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
|
||||
double rwTrqNs[4] = {0, 0, 0, 0};
|
||||
ptgCtrl.ptgNullspace(
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
||||
|
||||
if ( enableAntiStiction ) {
|
||||
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
|
||||
@ -333,12 +386,6 @@ void AcsController::performControlOperation() {
|
||||
&(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
|
||||
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
|
||||
ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
|
||||
mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
|
||||
|
||||
int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
|
||||
|
@ -21,14 +21,12 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameter
|
||||
PtgCtrl::~PtgCtrl() {}
|
||||
|
||||
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
|
||||
// TODO: Here correct Parameters have to be loaded according to current submode
|
||||
pointingLawParameters = &(acsParameters_->targetModeControllerParameters);
|
||||
inertiaEIVE = &(acsParameters_->inertiaEIVE);
|
||||
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
|
||||
rwMatrices = &(acsParameters_->rwMatrices);
|
||||
}
|
||||
|
||||
void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *deltaRate,
|
||||
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameters, const double *qError, const double *deltaRate,
|
||||
const double *rwPseudoInv, double *torqueRws) {
|
||||
//------------------------------------------------------------------------------------------------
|
||||
// Compute gain matrix K and P matrix
|
||||
@ -108,7 +106,7 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt
|
||||
VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
|
||||
}
|
||||
|
||||
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
|
||||
void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLawParameters, double *magFieldEst, bool magFieldEstValid, double *satRate,
|
||||
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
|
||||
int32_t *speedRw3, double *mgtDpDes) {
|
||||
if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
|
||||
@ -139,7 +137,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double
|
||||
VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
|
||||
}
|
||||
|
||||
void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
|
||||
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters * pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1,
|
||||
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
|
||||
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
|
||||
double wheelMomentum[4] = {0, 0, 0, 0};
|
||||
|
@ -41,14 +41,14 @@ class PtgCtrl {
|
||||
/* @brief: Calculates the needed torque for the pointing control mechanism
|
||||
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
|
||||
*/
|
||||
void ptgLaw(const double mode, const double *qError, const double *deltaRate,
|
||||
void ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameters, const double *qError, const double *deltaRate,
|
||||
const double *rwPseudoInv, double *torqueRws);
|
||||
|
||||
void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
|
||||
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
|
||||
double *mgtDpDes);
|
||||
void ptgDesaturation(AcsParameters::PointingLawParameters * pointingLawParameters, double *magFieldEst,
|
||||
bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1,
|
||||
int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes);
|
||||
|
||||
void ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
|
||||
void ptgNullspace(AcsParameters::PointingLawParameters * pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
|
||||
const int32_t *speedRw3, double *rwTrqNs);
|
||||
|
||||
/* @brief: Commands the stiction torque in case wheel speed is to low
|
||||
@ -59,7 +59,6 @@ class PtgCtrl {
|
||||
void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand);
|
||||
|
||||
private:
|
||||
AcsParameters::PointingLawParameters *pointingLawParameters;
|
||||
AcsParameters::RwHandlingParameters *rwHandlingParameters;
|
||||
AcsParameters::InertiaEIVE *inertiaEIVE;
|
||||
AcsParameters::RwMatrices *rwMatrices;
|
||||
|
Loading…
Reference in New Issue
Block a user