eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 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 0 Commits 35 Files Changed 15 +315 -280
15 changed files with 315 additions and 280 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
CHANGELOG.md
View File

@ -62,6 +62,7 @@ will consitute of a breaking change warranting a new major release:
- ACU dummy HK sets - ACU dummy HK sets
- IMTQ HK sets - IMTQ HK sets
- IMTQ dummy now handles power switch - IMTQ dummy now handles power switch
- Added some new ACS parameters
## Fixed ## Fixed
@ -96,6 +97,8 @@ will consitute of a breaking change warranting a new major release:
- STR datasets were not set to invalid on shutdown. - STR datasets were not set to invalid on shutdown.
- Fixed usage of quaternion valid flag, which does not actually represent the validity of the - Fixed usage of quaternion valid flag, which does not actually represent the validity of the
quaternion. quaternion.
- Various fixes for the pointing modes of the `ACS Controller`. All modes should work now as
intended.
# [v2.0.5] 2023-05-11 # [v2.0.5] 2023-05-11

112
mission/controller/AcsController.cpp
View File

@ -169,7 +169,7 @@ void AcsController::performSafe() {
guidance.getTargetParamsSafe(sunTargetDir); guidance.getTargetParamsSafe(sunTargetDir);
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy( acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.useMekf,
@ -205,11 +205,13 @@ void AcsController::performSafe() {
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
safeCtrlFailure(0, 1); safeCtrlFailure(0, 1);
break; break;
default:
sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl;
break;
} }
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
acsParameters.magnetorquerParameter.dipolMax);
// detumble check and switch // detumble check and switch
if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf && if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf &&
@ -231,8 +233,8 @@ void AcsController::performSafe() {
} }
updateCtrlValData(errAng, safeCtrlStrat); updateCtrlValData(errAng, safeCtrlStrat);
updateActuatorCmdData(cmdDipolMtqs); updateActuatorCmdData(cmdDipoleMtqs);
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
acsParameters.magnetorquerParameter.torqueDuration); acsParameters.magnetorquerParameter.torqueDuration);
} }
@ -259,7 +261,7 @@ void AcsController::performDetumble() {
triggerEvent(acs::MEKF_RECOVERY); triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false; mekfInvalidFlag = false;
} }
uint8_t safeCtrlStrat = detumble.detumbleStrategy( acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(),
acsParameters.detumbleParameter.useFullDetumbleLaw); acsParameters.detumbleParameter.useFullDetumbleLaw);
@ -279,11 +281,13 @@ void AcsController::performDetumble() {
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
safeCtrlFailure(0, 1); safeCtrlFailure(0, 1);
break; break;
default:
sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl;
break;
} }
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
acsParameters.magnetorquerParameter.dipolMax);
if (mekfData.satRotRateMekf.isValid() && if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) < VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@ -304,8 +308,8 @@ void AcsController::performDetumble() {
} }
disableCtrlValData(); disableCtrlValData();
updateActuatorCmdData(cmdDipolMtqs); updateActuatorCmdData(cmdDipoleMtqs);
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
acsParameters.magnetorquerParameter.torqueDuration); acsParameters.magnetorquerParameter.torqueDuration);
} }
@ -366,24 +370,26 @@ void AcsController::performPointingCtrl() {
// 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},
mgtDpDes[3] = {0, 0, 0}; mgtDpDes[3] = {0, 0, 0};
switch (mode) { switch (mode) {
case acs::PTG_IDLE: case acs::PTG_IDLE:
guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
&acsParameters.idleModeControllerParameters, &(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,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
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(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
break; break;
@ -397,17 +403,17 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
&acsParameters.targetModeControllerParameters, &(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,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
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(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break; break;
@ -418,17 +424,17 @@ void AcsController::performPointingCtrl() {
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
&acsParameters.gsTargetModeControllerParameters, &(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,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
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(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
break; break;
@ -442,63 +448,61 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
&acsParameters.nadirModeControllerParameters, &(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,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
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(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
break; break;
case acs::PTG_INERTIAL: case acs::PTG_INERTIAL:
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
4 * sizeof(double)); sizeof(targetQuat));
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef, targetSatRotRate, 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,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
&acsParameters.inertialModeControllerParameters, &(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,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
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(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
break; break;
default: default:
sif::error << "AcsController: Invalid mode for performPointingCtrl"; sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
break; break;
} }
actuatorCmd.cmdSpeedToRws( actuatorCmd.cmdSpeedToRws(
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
cmdSpeedRws, acsParameters.onBoardParams.sampleTime, acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
acsParameters.rwHandlingParameters.maxRwSpeed, acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
acsParameters.rwHandlingParameters.inertiaWheel);
if (enableAntiStiction) { if (enableAntiStiction) {
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
} }
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
acsParameters.magnetorquerParameter.dipolMax);
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs); updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime); acsParameters.rwHandlingParameters.rampTime);

2
mission/controller/AcsController.h
View File

@ -69,7 +69,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
bool mekfLost = false; bool mekfLost = false;
int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
int16_t cmdDipolMtqs[3] = {0, 0, 0}; int16_t cmdDipoleMtqs[3] = {0, 0, 0};
#if OBSW_THREAD_TRACING == 1 #if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0; uint32_t opCounter = 0;

93
mission/controller/acs/AcsParameters.cpp
View File

@ -318,7 +318,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;
@ -378,15 +378,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);
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 0x6: case 0x6:
parameterWrapper->set(idleModeControllerParameters.deSatGainFactor); parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
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:
@ -411,48 +414,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:
@ -477,30 +483,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:
@ -525,27 +534,30 @@ 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.quatRef); parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
break; break;
case 0xB: case 0xB:
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
break; break;
case 0xC: case 0xC:
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
break;
case 0xD:
parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
break; break;
default: default:
@ -570,21 +582,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:
@ -696,7 +711,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment); parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(magnetorquerParameter.dipolMax); parameterWrapper->set(magnetorquerParameter.dipoleMax);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(magnetorquerParameter.torqueDuration); parameterWrapper->set(magnetorquerParameter.torqueDuration);

6
mission/controller/acs/AcsParameters.h
View File

@ -816,7 +816,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 {
@ -840,7 +840,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;
@ -933,7 +935,7 @@ class AcsParameters : public HasParametersIF {
double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}}; double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}};
double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}};
double dipolMax = 0.2; // [Am^2] double dipoleMax = 0.2; // [Am^2]
uint16_t torqueDuration = 300; // [ms] uint16_t torqueDuration = 300; // [ms]
} magnetorquerParameter; } magnetorquerParameter;

64
mission/controller/acs/ActuatorCmd.cpp
View File

@ -5,11 +5,6 @@
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <cmath>
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
ActuatorCmd::ActuatorCmd() {} ActuatorCmd::ActuatorCmd() {}
ActuatorCmd::~ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {}
@ -25,24 +20,30 @@ void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) {
} }
} }
void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed, const int32_t speedRw2, const int32_t speedRw3,
double sampleTime, int32_t maxRwSpeed, double inertiaWheel) { const double sampleTime, const double inertiaWheel,
using namespace Math; const int32_t maxRwSpeed, const double *rwTorque,
int32_t *rwCmdSpeed) {
// Calculating the commanded speed in RPM for every reaction wheel // group RW speed values (in 0.1 [RPM]) in vector
int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3}; int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
// calculate required RW speed as sum of current RW speed and RW speed delta
// delta w_rw = delta t / I_RW * torque_RW [rad/s]
double deltaSpeed[4] = {0, 0, 0, 0}; double deltaSpeed[4] = {0, 0, 0, 0};
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM const double factor = sampleTime / inertiaWheel * RAD_PER_SEC_TO_RPM * 10;
// W_RW = Torque_RW / I_RW * delta t [rad/s]
double factor = sampleTime / inertiaWheel * radToRpm;
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4); VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
// convert double to int32
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
deltaSpeedInt[i] = std::round(deltaSpeed[i]); deltaSpeedInt[i] = std::round(deltaSpeed[i]);
} }
// sum of current RW speed and RW speed delta
VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4); VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
// crop values which would exceed the maximum possible RPM
for (uint8_t i = 0; i < 4; i++) { for (uint8_t i = 0; i < 4; i++) {
if (rwCmdSpeed[i] > maxRwSpeed) { if (rwCmdSpeed[i] > maxRwSpeed) {
rwCmdSpeed[i] = maxRwSpeed; rwCmdSpeed[i] = maxRwSpeed;
@ -52,24 +53,25 @@ void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t spee
} }
} }
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
const double *inverseAlignment, double maxDipol) { const double *dipoleMoment, int16_t *dipoleMomentActuator) {
// Convert to actuator frame // convert to actuator frame
double dipolMomentActuatorDouble[3] = {0, 0, 0}; double dipoleMomentActuatorDouble[3] = {0, 0, 0};
MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3, MatrixOperations<double>::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3,
1); 3, 1);
// Scaling along largest element if dipol exceeds maximum // scaling along largest element if dipole exceeds maximum
uint8_t maxIdx = 0; uint8_t maxIdx = 0;
VectorOperations<double>::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx); VectorOperations<double>::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx);
double maxAbsValue = std::abs(dipolMomentActuatorDouble[maxIdx]); double maxAbsValue = std::abs(dipoleMomentActuatorDouble[maxIdx]);
if (maxAbsValue > maxDipol) { if (maxAbsValue > maxDipole) {
double scalingFactor = maxDipol / maxAbsValue; double scalingFactor = maxDipole / maxAbsValue;
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor, VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, scalingFactor,
dipolMomentActuatorDouble, 3); dipoleMomentActuatorDouble, 3);
} }
// scale dipole from 1 Am^2 to 1e^-4 Am^2 // scale dipole from 1 Am^2 to 1e^-4 Am^2
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3); VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble,
3);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]); dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]);
} }
} }

32
mission/controller/acs/ActuatorCmd.h
View File

@ -1,9 +1,8 @@
#ifndef ACTUATORCMD_H_ #ifndef ACTUATORCMD_H_
#define ACTUATORCMD_H_ #define ACTUATORCMD_H_
#include "MultiplicativeKalmanFilter.h" #include <cmath>
#include "SensorProcessing.h"
#include "SensorValues.h"
class ActuatorCmd { class ActuatorCmd {
public: public:
@ -19,29 +18,30 @@ class ActuatorCmd {
void scalingTorqueRws(double *rwTrq, double maxTorque); void scalingTorqueRws(double *rwTrq, double maxTorque);
/* /*
* @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction * @brief: cmdSpeedToRws() Calculates the RPM for the reaction wheel configuration,
* wheels, also will calculate the needed revolutions per minute for the RWs, which will be given * given the required torque calculated by the controller. Will also scale down the RPM of the
* as Input to the RWs * wheels if they exceed the maximum possible RPM
* @param: rwTrqIn given torque from pointing controller * @param: rwTrq given torque from pointing controller
* rwTrqNS Nullspace torque
* rwCmdSpeed output revolutions per minute for every * rwCmdSpeed output revolutions per minute for every
* reaction wheel * reaction wheel
*/ */
void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3, void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime, const int32_t speedRw3, const double sampleTime, const double inertiaWheel,
int32_t maxRwSpeed, double inertiaWheel); const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed);
/* /*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques * @brief: cmdDipoleMtq() gives the commanded dipole moment for the
* magnetorquer
* *
* @param: dipolMoment given dipol moment in spacecraft frame * @param: dipoleMoment given dipole moment in spacecraft frame
* dipolMomentActuator resulting dipol moment in actuator reference frame * dipoleMomentActuator resulting dipole moment in actuator reference frame
*/ */
void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, void cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
const double *inverseAlignment, double maxDipol); const double *dipoleMoment, int16_t *dipoleMomentActuator);
protected: protected:
private: private:
static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI);
}; };
#endif /* ACTUATORCMD_H_ */ #endif /* ACTUATORCMD_H_ */

71
mission/controller/acs/Guidance.cpp
View File

@ -266,7 +266,8 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
} }
void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) { void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
double targetSatRotRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion to sun // Calculation of target quaternion to sun
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
@ -296,9 +297,8 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double
//---------------------------------------------------------------------------- //----------------------------------------------------------------------------
// Calculation of reference rotation rate // Calculation of reference rotation rate
//---------------------------------------------------------------------------- //----------------------------------------------------------------------------
refSatRate[0] = 0; int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
refSatRate[1] = 0; targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
refSatRate[2] = 0;
} }
void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
@ -412,7 +412,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
double errorQuat[4], double errorSatRotRate[3], double errorAngle) { double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
// First calculate error quaternion between current and target orientation // First calculate error quaternion between current and target orientation
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
// Last calculate add rotation from reference quaternion // Last calculate add rotation from reference quaternion
@ -424,26 +424,17 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
// Calculate error angle // Calculate error angle
errorAngle = QuaternionOperations::getAngle(errorQuat, true); errorAngle = QuaternionOperations::getAngle(errorQuat, true);
// Only give back error satellite rotational rate if orientation has already been aquired // Calculate error satellite rotational rate
if (errorAngle < 2. / 180. * M_PI) { // First combine the target and reference satellite rotational rates
// First combine the target and reference satellite rotational rates double combinedRefSatRotRate[3] = {0, 0, 0};
double combinedRefSatRotRate[3] = {0, 0, 0}; VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); // Then subtract the combined required satellite rotational rates from the actual rate
// Then substract the combined required satellite rotational rates from the actual rate VectorOperations<double>::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, 3);
VectorOperations<double>::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate,
3);
} else {
// If orientation has not been aquired yet set satellite rotational rate to zero
errorSatRotRate = 0;
}
// target flag in matlab, importance, does look like it only gives feedback if pointing control is
// under 150 arcsec ??
} }
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
double targetSatRotRate[3], double errorQuat[4], double targetSatRotRate[3], double errorQuat[4],
double errorSatRotRate[3], double errorAngle) { double errorSatRotRate[3], double &errorAngle) {
// First calculate error quaternion between current and target orientation // First calculate error quaternion between current and target orientation
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
// Keep scalar part of quaternion positive // Keep scalar part of quaternion positive
@ -453,17 +444,8 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
// Calculate error angle // Calculate error angle
errorAngle = QuaternionOperations::getAngle(errorQuat, true); errorAngle = QuaternionOperations::getAngle(errorQuat, true);
// Only give back error satellite rotational rate if orientation has already been aquired // Calculate error satellite rotational rate
if (errorAngle < 2. / 180. * M_PI) { VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
// Then substract the combined required satellite rotational rates from the actual rate
VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
} else {
// If orientation has not been aquired yet set satellite rotational rate to zero
errorSatRotRate = 0;
}
// target flag in matlab, importance, does look like it only gives feedback if pointing control is
// under 150 arcsec ??
} }
void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
@ -471,20 +453,25 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target rotation rate // Calculation of target rotation rate
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 -
(timeSavedQuaternion.tv_sec + (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6);
timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
}
if (timeElapsed < timeElapsedMax) { if (timeElapsed < timeElapsedMax) {
double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatInertialTarget, q);
QuaternionOperations::inverse(savedQuaternion, qS);
double qDiff[4] = {0, 0, 0, 0}; double qDiff[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4); VectorOperations<double>::subtract(q, qS, qDiff, 4);
VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, double tgtQuatVec[3] = {q[0], q[1], q[2]};
qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1); VectorOperations<double>::cross(tgtQuatVec, qDiffVec, sum1);
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); VectorOperations<double>::mulScalar(qDiffVec, q[3], sum3, 3);
VectorOperations<double>::add(sum1, sum2, sum, 3); VectorOperations<double>::add(sum1, sum2, sum, 3);
VectorOperations<double>::subtract(sum, sum3, sum, 3); VectorOperations<double>::subtract(sum, sum3, sum, 3);
double omegaRefNew[3] = {0, 0, 0}; double omegaRefNew[3] = {0, 0, 0};
@ -531,10 +518,6 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double)); std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
return returnvalue::OK; return returnvalue::OK;
} else { } else {
// @note: This one takes the normal pseudoInverse of all four raction wheels valid.
// Does not make sense, but is implemented that way in MATLAB ?!
// Thought: It does not really play a role, because in case there are more then one
// reaction wheel invalid the pointing control is destined to fail.
return returnvalue::FAILED; return returnvalue::FAILED;
} }
} }

13
mission/controller/acs/Guidance.h
View File

@ -15,7 +15,7 @@ class Guidance {
void getTargetParamsSafe(double sunTargetSafe[3]); void getTargetParamsSafe(double sunTargetSafe[3]);
ReturnValue_t solarArrayDeploymentComplete(); ReturnValue_t solarArrayDeploymentComplete();
// Function to get the target quaternion and refence rotation rate from gps position and // Function to get the target quaternion and reference rotation rate from gps position and
// position of the ground station // position of the ground station
void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3], void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3],
double refDirB[3], double quatBI[4], double targetQuat[4], double refDirB[3], double quatBI[4], double targetQuat[4],
@ -25,9 +25,10 @@ class Guidance {
void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
double targetSatRotRate[3]); double targetSatRotRate[3]);
// Function to get the target quaternion and refence rotation rate for sun pointing after ground // Function to get the target quaternion and reference rotation rate for sun pointing after ground
// station // station
void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]); void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
double targetSatRotRate[3]);
// Function to get the target quaternion and refence rotation rate from gps position for Nadir // Function to get the target quaternion and refence rotation rate from gps position for Nadir
// pointing // pointing
@ -37,15 +38,15 @@ class Guidance {
double targetQuat[4], double refSatRate[3]); double targetQuat[4], double refSatRate[3]);
// @note: Calculates the error quaternion between the current orientation and the target // @note: Calculates the error quaternion between the current orientation and the target
// quaternion, considering a reference quaternion. Additionally the difference between the actual // quaternion, considering a reference quaternion. Additionally the difference between the actual
// and a desired satellite rotational rate is calculated, again considering a reference rotational // and a desired satellite rotational rate is calculated, again considering a reference rotational
// rate. Lastly gives back the error angle of the error quaternion. // rate. Lastly gives back the error angle of the error quaternion.
void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
double errorQuat[4], double errorSatRotRate[3], double errorAngle); double errorQuat[4], double errorSatRotRate[3], double &errorAngle);
void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
double errorAngle); double &errorAngle);
void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double *targetSatRotRate); double *targetSatRotRate);

6
mission/controller/acs/control/Detumble.cpp
View File

@ -7,8 +7,10 @@ Detumble::Detumble() {}
Detumble::~Detumble() {} Detumble::~Detumble() {}
uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
const bool magFieldRateValid, const bool useFullDetumbleLaw) { const bool satRotRateValid,
const bool magFieldRateValid,
const bool useFullDetumbleLaw) {
if (not magFieldValid) { if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (satRotRateValid and useFullDetumbleLaw) { } else if (satRotRateValid and useFullDetumbleLaw) {

5
mission/controller/acs/control/Detumble.h
View File

@ -11,8 +11,9 @@ class Detumble {
Detumble(); Detumble();
virtual ~Detumble(); virtual ~Detumble();
uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
const bool magFieldRateValid, const bool useFullDetumbleLaw); const bool magFieldRateValid,
const bool useFullDetumbleLaw);
void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB, void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
double gain); double gain);

151
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -5,9 +5,6 @@
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/sign.h> #include <fsfw/globalfunctions/sign.h>
#include <math.h>
#include "../util/MathOperations.h"
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
@ -32,12 +29,13 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
double qErrorLaw[3] = {0, 0, 0}; double qErrorLaw[3] = {0, 0, 0};
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
if (abs(qError[i]) < qErrorMin) { if (std::abs(qError[i]) < qErrorMin) {
qErrorLaw[i] = qErrorMin; qErrorLaw[i] = qErrorMin;
} else { } else {
qErrorLaw[i] = abs(qError[i]); qErrorLaw[i] = std::abs(qError[i]);
} }
} }
double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3); double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);
double gain1 = cInt * omMax / qErrorLawNorm; double gain1 = cInt * omMax / qErrorLawNorm;
@ -73,7 +71,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
double pErrorSign[3] = {0, 0, 0}; double pErrorSign[3] = {0, 0, 0};
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
if (abs(pError[i]) > 1) { if (std::abs(pError[i]) > 1) {
pErrorSign[i] = sign(pError[i]); pErrorSign[i] = sign(pError[i]);
} else { } else {
pErrorSign[i] = pError[i]; pErrorSign[i] = pError[i];
@ -98,61 +96,92 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4); VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
} }
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, double *rwTrqNs) {
// 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)};
VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
VectorOperations<double>::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4);
// 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,
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::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
double *magFieldEst, bool magFieldEstValid, double *satRate, const double *magFieldB, const bool magFieldBValid,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, const double *satRate, const int32_t speedRw0, const int32_t speedRw1,
int32_t *speedRw3, double *mgtDpDes) { const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) { if (not magFieldBValid or not pointingLawParameters->desatOn) {
mgtDpDes[0] = 0;
mgtDpDes[1] = 0;
mgtDpDes[2] = 0;
return; return;
} }
// calculating momentum of satellite and momentum of reaction wheels // concentrate RW speeds as vector and convert to double
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
momentumRwU, 4);
MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU,
momentumRw, 3, 4, 1);
double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
momentumSat, 3, 3, 1);
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
// calculating momentum error
double deltaMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
deltaMomentum, 3);
// resulting magnetic dipole command
double crossMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
factor = (pointingLawParameters->deSatGainFactor) / normMag;
VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
}
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, // convert magFieldB from uT to T
const int32_t *speedRw0, const int32_t *speedRw1, double magFieldBT[3] = {0, 0, 0};
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0}; // calculate angular momentum of the satellite
double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; double angMomentumSat[3] = {0, 0, 0};
// conversion to [rad/s] for further calculations MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
VectorOperations<double>::mulScalar(rpmOffset, factor, rpmOffset, 4); angMomentumSat, 3, 3, 1);
VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
double diffRwSpeed[4] = {0, 0, 0, 0}; // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4); // relocate RW speed zero to nullspace RW speed
VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, double refSpeedRws[4] = {0, 0, 0, 0};
wheelMomentum, 4); VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
double gainNs = pointingLawParameters->gainNullspace; pointingLawParameters->nullspaceSpeed, refSpeedRws, 4);
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
MathOperations<double>::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace, // convert speed from 10 RPM to 1 RPM
acsParameters->rwMatrices.nullspace, VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
*nullSpaceMatrix, 4); // convert to rad/s
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); VectorOperations<double>::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4);
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); // calculate angular momentum of each RW
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4); double angMomentumRwU[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
angMomentumRwU, 4);
// convert RW angular momentum to body RF
double angMomentumRw[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), angMomentumRwU,
angMomentumRw, 3, 4, 1);
// calculate total angular momentum
double angMomentumTotal[3] = {0, 0, 0};
VectorOperations<double>::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3);
// calculating momentum error
double deltaAngMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef,
deltaAngMomentum, 3);
// resulting magnetic dipole command
double crossAngMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField);
double factor =
pointingLawParameters->deSatGainFactor / VectorOperations<double>::norm(magFieldBT, 3);
VectorOperations<double>::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3);
} }
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) { void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) {
@ -169,15 +198,9 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee
if (rwCmdSpeeds[i] != 0) { if (rwCmdSpeeds[i] != 0) {
if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed && if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed &&
rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) { rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) {
if (currRwSpeed[i] == 0) { if (rwCmdSpeeds[i] > currRwSpeed[i]) {
if (rwCmdSpeeds[i] > 0) {
rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
} else if (rwCmdSpeeds[i] < 0) {
rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
}
} else if (currRwSpeed[i] < -acsParameters->rwHandlingParameters.stictionSpeed) {
rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
} else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) { } else if (rwCmdSpeeds[i] < currRwSpeed[i]) {
rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
} }
} }

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

@ -1,13 +1,10 @@
#ifndef PTGCTRL_H_ #ifndef PTGCTRL_H_
#define PTGCTRL_H_ #define PTGCTRL_H_
#include <math.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <stdio.h> #include <stdio.h>
#include <string.h>
#include <time.h>
#include "../AcsParameters.h"
#include "../SensorValues.h"
#include "eive/resultClassIds.h"
class PtgCtrl { class PtgCtrl {
/* /*
@ -29,14 +26,14 @@ class PtgCtrl {
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
const double *deltaRate, const double *rwPseudoInv, double *torqueRws); const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
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(AcsParameters::PointingLawParameters *pointingLawParameters, void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t *speedRw3, double *rwTrqNs); const int32_t speedRw3, double *rwTrqNs);
void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *magFieldB, const bool magFieldBValid, const double *satRate,
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, double *mgtDpDes);
/* @brief: Commands the stiction torque in case wheel speed is to low /* @brief: Commands the stiction torque in case wheel speed is to low
* torqueCommand modified torque after antistiction * torqueCommand modified torque after antistiction
@ -45,6 +42,7 @@ class PtgCtrl {
private: private:
const AcsParameters *acsParameters; const AcsParameters *acsParameters;
static constexpr double RPM_TO_RAD_PER_SEC = (2 * M_PI) / 60;
}; };
#endif /* ACS_CONTROL_PTGCTRL_H_ */ #endif /* ACS_CONTROL_PTGCTRL_H_ */

7
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -9,9 +9,10 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
SafeCtrl::~SafeCtrl() {} SafeCtrl::~SafeCtrl() {}
uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid, const bool satRotRateValid, const bool sunDirValid,
const uint8_t mekfEnabled, const uint8_t dampingEnabled) { const uint8_t mekfEnabled,
const uint8_t dampingEnabled) {
if (not magFieldValid) { if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) { } else if (mekfEnabled and mekfValid) {

6
mission/controller/acs/control/SafeCtrl.h
View File

@ -12,9 +12,9 @@ class SafeCtrl {
SafeCtrl(AcsParameters *acsParameters_); SafeCtrl(AcsParameters *acsParameters_);
virtual ~SafeCtrl(); virtual ~SafeCtrl();
uint8_t safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid, const bool satRotRateValid, const bool sunDirValid,
const uint8_t mekfEnabled, const uint8_t dampingEnabled); const uint8_t mekfEnabled, const uint8_t dampingEnabled);
void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
const double *quatBI, const double *sunDirRefB, double *magMomB, const double *quatBI, const double *sunDirRefB, double *magMomB,