not sure if i like this
Some checks failed
EIVE/eive-obsw/pipeline/pr-v3.0.0-dev There was a failure building this commit
Some checks failed
EIVE/eive-obsw/pipeline/pr-v3.0.0-dev There was a failure building this commit
This commit is contained in:
parent
69be05e621
commit
24adb844d2
|
@ -26,6 +26,8 @@ enum ctrlStrategy : uint8_t {
|
||||||
SAFECTRL_OFF = 0,
|
SAFECTRL_OFF = 0,
|
||||||
SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
|
SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
|
||||||
SAFECTRL_NO_SENSORS_FOR_CONTROL = 2,
|
SAFECTRL_NO_SENSORS_FOR_CONTROL = 2,
|
||||||
|
PTGCTRL_NO_RW_FOR_CONTROL = 3,
|
||||||
|
PTGCTRL_NO_SENSORS_FOR_CONTROL = 4,
|
||||||
SAFECTRL_ACTIVE_MEKF = 10,
|
SAFECTRL_ACTIVE_MEKF = 10,
|
||||||
SAFECTRL_WITHOUT_MEKF = 11,
|
SAFECTRL_WITHOUT_MEKF = 11,
|
||||||
SAFECTRL_ECLIPSE_DAMPING = 12,
|
SAFECTRL_ECLIPSE_DAMPING = 12,
|
||||||
|
@ -34,6 +36,8 @@ enum ctrlStrategy : uint8_t {
|
||||||
SAFECTRL_DETUMBLE_DETERIORATED = 21,
|
SAFECTRL_DETUMBLE_DETERIORATED = 21,
|
||||||
PTGCTRL_ACTIVE_MEKF = 30,
|
PTGCTRL_ACTIVE_MEKF = 30,
|
||||||
PTGCTRL_WITHOUT_MEKF = 31,
|
PTGCTRL_WITHOUT_MEKF = 31,
|
||||||
|
PTGCTRL_ACTIVE_MEKF_WO_DESAT = 32,
|
||||||
|
PTGCTRL_WITHOUT_MEKF_WO_DESAT = 33,
|
||||||
};
|
};
|
||||||
|
|
||||||
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
|
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
|
||||||
|
|
|
@ -358,13 +358,18 @@ void AcsController::performPointingCtrl() {
|
||||||
triggerEvent(acs::MULTIPLE_RW_INVALID);
|
triggerEvent(acs::MULTIPLE_RW_INVALID);
|
||||||
}
|
}
|
||||||
multipleRwUnavailableCounter++;
|
multipleRwUnavailableCounter++;
|
||||||
|
rwInvalidFlag = true;
|
||||||
return;
|
return;
|
||||||
} else {
|
} else {
|
||||||
multipleRwUnavailableCounter = 0;
|
multipleRwUnavailableCounter = 0;
|
||||||
|
rwInvalidFlag = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Variables required for ptgStrat
|
||||||
|
acs::ctrlStrategy ptgStrat;
|
||||||
|
double quat[4] = {0, 0, 0, 0}, satRotRate[3] = {0, 0, 0};
|
||||||
// Variables required for guidance
|
// Variables required for guidance
|
||||||
double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
|
double targetQuat[4] = {0, 0, 0, 0}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 0},
|
||||||
errorAngle = 0, errorSatRotRate[3] = {0, 0, 0};
|
errorAngle = 0, errorSatRotRate[3] = {0, 0, 0};
|
||||||
// Variables required for setting actuators
|
// Variables required for setting actuators
|
||||||
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0},
|
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0},
|
||||||
|
@ -372,10 +377,16 @@ void AcsController::performPointingCtrl() {
|
||||||
|
|
||||||
switch (mode) {
|
switch (mode) {
|
||||||
case acs::PTG_IDLE:
|
case acs::PTG_IDLE:
|
||||||
|
ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.idleModeControllerParameters, quat,
|
||||||
|
satRotRate);
|
||||||
|
if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
|
||||||
|
ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat,
|
guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat,
|
||||||
targetSatRotRate);
|
targetSatRotRate);
|
||||||
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
|
guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate, errorQuat,
|
||||||
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
|
errorSatRotRate, errorAngle);
|
||||||
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
|
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
|
||||||
*rwPseudoInv, torquePtgRws);
|
*rwPseudoInv, torquePtgRws);
|
||||||
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
|
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
|
||||||
|
@ -386,18 +397,24 @@ void AcsController::performPointingCtrl() {
|
||||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||||
ptgCtrl.ptgDesaturation(
|
ptgCtrl.ptgDesaturation(
|
||||||
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
|
||||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||||
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
|
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case acs::PTG_TARGET:
|
case acs::PTG_TARGET:
|
||||||
|
ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.targetModeControllerParameters, quat,
|
||||||
|
satRotRate);
|
||||||
|
if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
|
||||||
|
ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
|
guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
|
||||||
gpsDataProcessed.gpsVelocity.value, targetQuat,
|
gpsDataProcessed.gpsVelocity.value, targetQuat,
|
||||||
targetSatRotRate);
|
targetSatRotRate);
|
||||||
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
|
guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate,
|
||||||
targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef,
|
acsParameters.targetModeControllerParameters.quatRef,
|
||||||
acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
|
acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
|
||||||
errorSatRotRate, errorAngle);
|
errorSatRotRate, errorAngle);
|
||||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
|
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||||
|
@ -410,17 +427,23 @@ void AcsController::performPointingCtrl() {
|
||||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||||
ptgCtrl.ptgDesaturation(
|
ptgCtrl.ptgDesaturation(
|
||||||
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
|
||||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case acs::PTG_TARGET_GS:
|
case acs::PTG_TARGET_GS:
|
||||||
|
ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.gsTargetModeControllerParameters, quat,
|
||||||
|
satRotRate);
|
||||||
|
if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
|
||||||
|
ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
|
guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
|
||||||
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
|
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
|
||||||
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
|
guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate, errorQuat,
|
||||||
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
|
errorSatRotRate, errorAngle);
|
||||||
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
|
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||||
*rwPseudoInv, torquePtgRws);
|
*rwPseudoInv, torquePtgRws);
|
||||||
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
|
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
|
||||||
|
@ -431,18 +454,24 @@ void AcsController::performPointingCtrl() {
|
||||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||||
ptgCtrl.ptgDesaturation(
|
ptgCtrl.ptgDesaturation(
|
||||||
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
|
||||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||||
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
|
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case acs::PTG_NADIR:
|
case acs::PTG_NADIR:
|
||||||
|
ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.nadirModeControllerParameters, quat,
|
||||||
|
satRotRate);
|
||||||
|
if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
|
||||||
|
ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
|
guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
|
||||||
gpsDataProcessed.gpsVelocity.value, targetQuat,
|
gpsDataProcessed.gpsVelocity.value, targetQuat,
|
||||||
targetSatRotRate);
|
targetSatRotRate);
|
||||||
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
|
guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate,
|
||||||
targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
|
acsParameters.nadirModeControllerParameters.quatRef,
|
||||||
acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
|
acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
|
||||||
errorSatRotRate, errorAngle);
|
errorSatRotRate, errorAngle);
|
||||||
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
|
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
|
||||||
|
@ -455,17 +484,23 @@ void AcsController::performPointingCtrl() {
|
||||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||||
ptgCtrl.ptgDesaturation(
|
ptgCtrl.ptgDesaturation(
|
||||||
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
|
||||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||||
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
|
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case acs::PTG_INERTIAL:
|
case acs::PTG_INERTIAL:
|
||||||
|
ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.inertialModeControllerParameters, quat,
|
||||||
|
satRotRate);
|
||||||
|
if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
|
||||||
|
ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
|
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
|
||||||
sizeof(targetQuat));
|
sizeof(targetQuat));
|
||||||
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
|
guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate,
|
||||||
targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef,
|
acsParameters.inertialModeControllerParameters.quatRef,
|
||||||
acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
|
acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
|
||||||
errorSatRotRate, errorAngle);
|
errorSatRotRate, errorAngle);
|
||||||
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
|
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
|
||||||
|
@ -478,9 +513,9 @@ void AcsController::performPointingCtrl() {
|
||||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||||
ptgCtrl.ptgDesaturation(
|
ptgCtrl.ptgDesaturation(
|
||||||
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
|
||||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||||
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
|
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
|
@ -488,18 +523,22 @@ void AcsController::performPointingCtrl() {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
actuatorCmd.cmdSpeedToRws(
|
if (ptgStrat != acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
|
||||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
ptgStrat != acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
|
||||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
actuatorCmd.cmdSpeedToRws(
|
||||||
acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
|
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||||
acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
|
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||||
if (enableAntiStiction) {
|
acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
|
||||||
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
|
acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
|
||||||
|
if (enableAntiStiction) {
|
||||||
|
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
|
||||||
|
}
|
||||||
|
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||||
|
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes,
|
||||||
|
cmdDipoleMtqs);
|
||||||
}
|
}
|
||||||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
|
||||||
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
|
|
||||||
|
|
||||||
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
|
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate, ptgStrat);
|
||||||
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
|
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
|
||||||
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
||||||
acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
|
acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
|
||||||
|
@ -507,6 +546,31 @@ void AcsController::performPointingCtrl() {
|
||||||
acsParameters.rwHandlingParameters.rampTime);
|
acsParameters.rwHandlingParameters.rampTime);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void AcsController::handlePtgCtrlStrat(acs::ctrlStrategy &ptgStrat,
|
||||||
|
AcsParameters::PointingLawParameters ptgLawParameters,
|
||||||
|
double *quat, double *satRotRate) {
|
||||||
|
ptgStrat = ptgCtrl.ptgCtrlStrategy(
|
||||||
|
not rwInvalidFlag, ptgLawParameters.useMekf, not mekfInvalidFlag,
|
||||||
|
mgmDataProcessed.mgmVecTot.isValid(), ptgLawParameters.desatOn,
|
||||||
|
sensorValues.strSet.isTrustWorthy.value, gyrDataProcessed.gyrVecTot.isValid());
|
||||||
|
switch (ptgStrat) {
|
||||||
|
case acs::ctrlStrategy::PTGCTRL_ACTIVE_MEKF:
|
||||||
|
case acs::ctrlStrategy::PTGCTRL_ACTIVE_MEKF_WO_DESAT:
|
||||||
|
std::memcpy(quat, mekfData.quatMekf.value, 4 * sizeof(double));
|
||||||
|
std::memcpy(satRotRate, mekfData.satRotRateMekf.value, 3 * sizeof(double));
|
||||||
|
break;
|
||||||
|
case acs::ctrlStrategy::PTGCTRL_WITHOUT_MEKF:
|
||||||
|
case acs::ctrlStrategy::PTGCTRL_WITHOUT_MEKF_WO_DESAT:
|
||||||
|
quat[0] = sensorValues.strSet.caliQx.value;
|
||||||
|
quat[1] = sensorValues.strSet.caliQy.value;
|
||||||
|
quat[2] = sensorValues.strSet.caliQz.value;
|
||||||
|
quat[3] = sensorValues.strSet.caliQw.value;
|
||||||
|
std::memcpy(satRotRate, gyrDataProcessed.gyrVecTot.value, 3 * sizeof(double));
|
||||||
|
break;
|
||||||
|
default:;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) {
|
void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) {
|
||||||
if (not safeCtrlFailureFlag) {
|
if (not safeCtrlFailureFlag) {
|
||||||
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure);
|
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure);
|
||||||
|
@ -577,7 +641,7 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
|
void AcsController::updateCtrlValData(double errAng, acs::ctrlStrategy safeModeStrat) {
|
||||||
PoolReadGuard pg(&ctrlValData);
|
PoolReadGuard pg(&ctrlValData);
|
||||||
if (pg.getReadResult() == returnvalue::OK) {
|
if (pg.getReadResult() == returnvalue::OK) {
|
||||||
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
|
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
|
||||||
|
@ -588,21 +652,21 @@ void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
|
||||||
ctrlValData.errAng.setValid(true);
|
ctrlValData.errAng.setValid(true);
|
||||||
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
|
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
|
||||||
ctrlValData.tgtRotRate.setValid(false);
|
ctrlValData.tgtRotRate.setValid(false);
|
||||||
ctrlValData.safeStrat.value = safeModeStrat;
|
ctrlValData.ctrlStrat.value = safeModeStrat;
|
||||||
ctrlValData.safeStrat.setValid(true);
|
ctrlValData.ctrlStrat.setValid(true);
|
||||||
ctrlValData.setValidity(true, false);
|
ctrlValData.setValidity(true, false);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
|
void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
|
||||||
const double *tgtRotRate) {
|
const double *tgtRotRate, acs::ctrlStrategy ptgModeStrat) {
|
||||||
PoolReadGuard pg(&ctrlValData);
|
PoolReadGuard pg(&ctrlValData);
|
||||||
if (pg.getReadResult() == returnvalue::OK) {
|
if (pg.getReadResult() == returnvalue::OK) {
|
||||||
std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
|
std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
|
||||||
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
|
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
|
||||||
ctrlValData.errAng.value = errAng;
|
ctrlValData.errAng.value = errAng;
|
||||||
std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
|
std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
|
||||||
ctrlValData.safeStrat.value = acs::ctrlStrategy::SAFECTRL_OFF;
|
ctrlValData.ctrlStrat.value = ptgModeStrat;
|
||||||
ctrlValData.setValidity(true, true);
|
ctrlValData.setValidity(true, true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -695,7 +759,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
|
||||||
localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
|
localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
|
||||||
poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
|
poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
|
||||||
// Ctrl Values
|
// Ctrl Values
|
||||||
localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
|
localDataPoolMap.emplace(acsctrl::PoolIds::CTRL_STRAT, &ctrlStrat);
|
||||||
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
|
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
|
||||||
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
|
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
|
||||||
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
|
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
|
||||||
|
|
|
@ -60,6 +60,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||||
ParameterHelper parameterHelper;
|
ParameterHelper parameterHelper;
|
||||||
|
|
||||||
uint8_t detumbleCounter = 0;
|
uint8_t detumbleCounter = 0;
|
||||||
|
bool rwInvalidFlag = false;
|
||||||
uint8_t multipleRwUnavailableCounter = 0;
|
uint8_t multipleRwUnavailableCounter = 0;
|
||||||
bool mekfInvalidFlag = false;
|
bool mekfInvalidFlag = false;
|
||||||
uint16_t mekfInvalidCounter = 0;
|
uint16_t mekfInvalidCounter = 0;
|
||||||
|
@ -107,6 +108,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||||
|
|
||||||
void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure);
|
void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure);
|
||||||
|
|
||||||
|
void handlePtgCtrlStrat(acs::ctrlStrategy& ptgStrat,
|
||||||
|
AcsParameters::PointingLawParameters ptgLawParameters, double* quat,
|
||||||
|
double* satRotRate);
|
||||||
|
|
||||||
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
|
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
|
||||||
uint16_t dipoleTorqueDuration);
|
uint16_t dipoleTorqueDuration);
|
||||||
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
|
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
|
||||||
|
@ -115,9 +120,9 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||||
void updateActuatorCmdData(const int16_t* mtqTargetDipole);
|
void updateActuatorCmdData(const int16_t* mtqTargetDipole);
|
||||||
void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
|
void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
|
||||||
const int16_t* mtqTargetDipole);
|
const int16_t* mtqTargetDipole);
|
||||||
void updateCtrlValData(double errAng, uint8_t safeModeStrat);
|
void updateCtrlValData(double errAng, acs::ctrlStrategy safeModeStrat);
|
||||||
void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
|
void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
|
||||||
const double* tgtRotRate);
|
const double* tgtRotRate, acs::ctrlStrategy ptgModeStrat);
|
||||||
void disableCtrlValData();
|
void disableCtrlValData();
|
||||||
|
|
||||||
/* ACS Sensor Values */
|
/* ACS Sensor Values */
|
||||||
|
@ -214,7 +219,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||||
|
|
||||||
// Ctrl Values
|
// Ctrl Values
|
||||||
acsctrl::CtrlValData ctrlValData;
|
acsctrl::CtrlValData ctrlValData;
|
||||||
PoolEntry<uint8_t> safeStrat = PoolEntry<uint8_t>();
|
PoolEntry<uint8_t> ctrlStrat = PoolEntry<uint8_t>();
|
||||||
PoolEntry<double> tgtQuat = PoolEntry<double>(4);
|
PoolEntry<double> tgtQuat = PoolEntry<double>(4);
|
||||||
PoolEntry<double> errQuat = PoolEntry<double>(4);
|
PoolEntry<double> errQuat = PoolEntry<double>(4);
|
||||||
PoolEntry<double> errAng = PoolEntry<double>();
|
PoolEntry<double> errAng = PoolEntry<double>();
|
||||||
|
|
|
@ -846,6 +846,7 @@ class AcsParameters : public HasParametersIF {
|
||||||
|
|
||||||
double desatMomentumRef[3] = {0, 0, 0};
|
double desatMomentumRef[3] = {0, 0, 0};
|
||||||
double deSatGainFactor = 1000;
|
double deSatGainFactor = 1000;
|
||||||
|
uint8_t useMekf = true;
|
||||||
uint8_t desatOn = true;
|
uint8_t desatOn = true;
|
||||||
uint8_t enableAntiStiction = true;
|
uint8_t enableAntiStiction = true;
|
||||||
} pointingLawParameters;
|
} pointingLawParameters;
|
||||||
|
|
|
@ -10,6 +10,27 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_
|
||||||
|
|
||||||
PtgCtrl::~PtgCtrl() {}
|
PtgCtrl::~PtgCtrl() {}
|
||||||
|
|
||||||
|
acs::ctrlStrategy PtgCtrl::ptgCtrlStrategy(const bool rwAvail, const uint8_t mekfEnabled,
|
||||||
|
const bool mekfValid, const bool magFieldValid,
|
||||||
|
const uint8_t desatEnabled, const uint8_t quatValid,
|
||||||
|
const bool satRotRateValid) {
|
||||||
|
if (not rwAvail) {
|
||||||
|
return acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL;
|
||||||
|
} else if ((mekfEnabled and mekfValid) and (desatEnabled and magFieldValid)) {
|
||||||
|
return acs::ctrlStrategy::PTGCTRL_ACTIVE_MEKF;
|
||||||
|
} else if ((mekfEnabled and mekfValid) and not(desatEnabled or magFieldValid)) {
|
||||||
|
return acs::ctrlStrategy::PTGCTRL_ACTIVE_MEKF_WO_DESAT;
|
||||||
|
} else if (not(mekfEnabled or mekfValid) and (quatValid and satRotRateValid) and
|
||||||
|
(desatEnabled and magFieldValid)) {
|
||||||
|
return acs::ctrlStrategy::PTGCTRL_WITHOUT_MEKF;
|
||||||
|
} else if (not(mekfEnabled or mekfValid) and (quatValid and satRotRateValid) and
|
||||||
|
not(desatEnabled and magFieldValid)) {
|
||||||
|
return acs::ctrlStrategy::PTGCTRL_WITHOUT_MEKF_WO_DESAT;
|
||||||
|
} else {
|
||||||
|
return acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
|
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||||
const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
|
const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
|
||||||
double *torqueRws) {
|
double *torqueRws) {
|
||||||
|
|
|
@ -21,6 +21,11 @@ class PtgCtrl {
|
||||||
PtgCtrl(AcsParameters *acsParameters_);
|
PtgCtrl(AcsParameters *acsParameters_);
|
||||||
virtual ~PtgCtrl();
|
virtual ~PtgCtrl();
|
||||||
|
|
||||||
|
acs::ctrlStrategy ptgCtrlStrategy(const bool rwAvail, const uint8_t mekfEnabled,
|
||||||
|
const bool mekfValid, const bool magFieldValid,
|
||||||
|
const uint8_t desatEnabled, const uint8_t quatValid,
|
||||||
|
const bool satRotRateValid);
|
||||||
|
|
||||||
/* @brief: Calculates the needed torque for the pointing control mechanism
|
/* @brief: Calculates the needed torque for the pointing control mechanism
|
||||||
*/
|
*/
|
||||||
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
|
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
|
||||||
|
|
|
@ -94,7 +94,7 @@ enum PoolIds : lp_id_t {
|
||||||
QUAT_MEKF,
|
QUAT_MEKF,
|
||||||
MEKF_STATUS,
|
MEKF_STATUS,
|
||||||
// Ctrl Values
|
// Ctrl Values
|
||||||
SAFE_STRAT,
|
CTRL_STRAT,
|
||||||
TGT_QUAT,
|
TGT_QUAT,
|
||||||
ERROR_QUAT,
|
ERROR_QUAT,
|
||||||
ERROR_ANG,
|
ERROR_ANG,
|
||||||
|
@ -252,7 +252,7 @@ class CtrlValData : public StaticLocalDataSet<CTRL_VAL_SET_ENTRIES> {
|
||||||
public:
|
public:
|
||||||
CtrlValData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CTRL_VAL_DATA) {}
|
CtrlValData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CTRL_VAL_DATA) {}
|
||||||
|
|
||||||
lp_var_t<uint8_t> safeStrat = lp_var_t<uint8_t>(sid.objectId, SAFE_STRAT, this);
|
lp_var_t<uint8_t> ctrlStrat = lp_var_t<uint8_t>(sid.objectId, CTRL_STRAT, this);
|
||||||
lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this);
|
lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this);
|
||||||
lp_vec_t<double, 4> errQuat = lp_vec_t<double, 4>(sid.objectId, ERROR_QUAT, this);
|
lp_vec_t<double, 4> errQuat = lp_vec_t<double, 4>(sid.objectId, ERROR_QUAT, this);
|
||||||
lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this);
|
lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this);
|
||||||
|
|
Loading…
Reference in New Issue
Block a user