eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests Releases 118 Wiki Activity

WIP: ACS Ptg Modes Strategy #665

Closed
meggert wants to merge 38 commits from acs-ptg-strat into v3.0.0-dev
pull from: acs-ptg-strat
merge into: eive:v3.0.0-dev
Conversation 0 Commits 38 Files Changed 18 +494 -338
18 changed files with 494 additions and 338 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
CHANGELOG.md
View File

@ -60,6 +60,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
@ -91,6 +92,8 @@ will consitute of a breaking change warranting a new major release:
- Prevent spam of TCS controller heater unavailability event if all heaters are in external control. - Prevent spam of TCS controller heater unavailability event if all heaters are in external control.
- TCS heater switch info set contained invalid values because of a faulty `memcpy` in the TCS - TCS heater switch info set contained invalid values because of a faulty `memcpy` in the TCS
controller. There is not crash risk but the heater states were invalid. controller. There is not crash risk but the heater states were invalid.
- 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

8
mission/acs/defs.h
View File

@ -22,16 +22,22 @@ enum AcsMode : Mode_t {
enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 }; enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 };
enum SafeModeStrategy : uint8_t { 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,
SAFECTRL_ECLIPSE_IDELING = 13, SAFECTRL_ECLIPSE_IDELING = 13,
SAFECTRL_DETUMBLE_FULL = 20, SAFECTRL_DETUMBLE_FULL = 20,
SAFECTRL_DETUMBLE_DETERIORATED = 21, SAFECTRL_DETUMBLE_DETERIORATED = 21,
PTGCTRL_ACTIVE_MEKF = 30,
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;

253
mission/controller/AcsController.cpp
View File

@ -169,47 +169,49 @@ 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::ctrlStrategy 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,
acsParameters.safeModeControllerParameters.dampingDuringEclipse); acsParameters.safeModeControllerParameters.dampingDuringEclipse);
switch (safeCtrlStrat) { switch (safeCtrlStrat) {
case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF): case (acs::ctrlStrategy::SAFECTRL_ACTIVE_MEKF):
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value, safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir, susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
magMomMtq, errAng); magMomMtq, errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;
case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF): case (acs::ctrlStrategy::SAFECTRL_WITHOUT_MEKF):
safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING): case (acs::ctrlStrategy::SAFECTRL_ECLIPSE_DAMPING):
safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
sunTargetDir, magMomMtq, errAng); sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING): case (acs::ctrlStrategy::SAFECTRL_ECLIPSE_IDELING):
errAng = NAN; errAng = NAN;
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): case (acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
safeCtrlFailure(1, 0); safeCtrlFailure(1, 0);
break; break;
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): case (acs::ctrlStrategy::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,31 +261,33 @@ void AcsController::performDetumble() {
triggerEvent(acs::MEKF_RECOVERY); triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false; mekfInvalidFlag = false;
} }
uint8_t safeCtrlStrat = detumble.detumbleStrategy( acs::ctrlStrategy 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);
double magMomMtq[3] = {0, 0, 0}; double magMomMtq[3] = {0, 0, 0};
switch (safeCtrlStrat) { switch (safeCtrlStrat) {
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL): case (acs::ctrlStrategy::SAFECTRL_DETUMBLE_FULL):
detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value, detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
magMomMtq, acsParameters.detumbleParameter.gainFull); magMomMtq, acsParameters.detumbleParameter.gainFull);
break; break;
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED): case (acs::ctrlStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value, detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
magMomMtq, acsParameters.detumbleParameter.gainBdot); magMomMtq, acsParameters.detumbleParameter.gainBdot);
break; break;
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): case (acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
safeCtrlFailure(1, 0); safeCtrlFailure(1, 0);
break; break;
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): case (acs::ctrlStrategy::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);
} }
@ -349,159 +353,224 @@ void AcsController::performPointingCtrl() {
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
if (result == returnvalue::FAILED) { if (result == returnvalue::FAILED) {
if (multipleRwUnavailableCounter == 5) { if (multipleRwUnavailableCounter >=
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
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},
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); ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.idleModeControllerParameters, quat,
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, satRotRate);
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); 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,
targetSatRotRate);
guidance.comparePtg(quat, satRotRate, targetQuat, 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(), 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,
*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(), 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( 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(), 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,
*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(), 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,
4 * sizeof(double)); 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,
*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(), 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:
sif::error << "AcsController: Invalid mode for performPointingCtrl"; sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
break; break;
} }
actuatorCmd.cmdSpeedToRws( if (ptgStrat != acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL and
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, ptgStrat != acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws, actuatorCmd.cmdSpeedToRws(
cmdSpeedRws, acsParameters.onBoardParams.sampleTime, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
acsParameters.rwHandlingParameters.maxRwSpeed, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
acsParameters.rwHandlingParameters.inertiaWheel); acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
if (enableAntiStiction) { acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); if (enableAntiStiction) {
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
}
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes,
cmdDipoleMtqs);
} }
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs,
*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipolMax);
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate, ptgStrat);
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);
} }
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);
@ -572,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));
@ -583,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::SafeModeStrategy::SAFECTRL_OFF; ctrlValData.ctrlStrat.value = ptgModeStrat;
ctrlValData.setValidity(true, true); ctrlValData.setValidity(true, true);
} }
} }
@ -690,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);

13
mission/controller/AcsController.h
View File

@ -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;
@ -69,7 +70,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;
@ -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>();

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

@ -290,6 +290,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x6: case 0x6:
parameterWrapper->set(rwHandlingParameters.rampTime); parameterWrapper->set(rwHandlingParameters.rampTime);
break; break;
case 0x7:
parameterWrapper->set(rwHandlingParameters.multipleRwInvalidTimeout);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
@ -315,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;
@ -375,17 +378,23 @@ 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);
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;
case 0xA:
parameterWrapper->set(idleModeControllerParameters.useMekf);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
@ -408,48 +417,54 @@ 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->set(targetModeControllerParameters.useMekf);
break; break;
case 0xB: case 0xB:
parameterWrapper->setVector(targetModeControllerParameters.quatRef); parameterWrapper->setVector(targetModeControllerParameters.refDirection);
break; break;
case 0xC: case 0xC:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
break; break;
case 0xD: case 0xD:
parameterWrapper->set(targetModeControllerParameters.latitudeTgt); parameterWrapper->setVector(targetModeControllerParameters.quatRef);
break; break;
case 0xE: case 0xE:
parameterWrapper->set(targetModeControllerParameters.longitudeTgt); parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
break; break;
case 0xF: case 0xF:
parameterWrapper->set(targetModeControllerParameters.altitudeTgt); parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
break; break;
case 0x10: case 0x10:
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
break; break;
case 0x11: case 0x11:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
break; break;
case 0x12: case 0x12:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
break; break;
case 0x13: case 0x13:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
break;
case 0x14:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
break;
case 0x15:
parameterWrapper->set(targetModeControllerParameters.blindRotRate); parameterWrapper->set(targetModeControllerParameters.blindRotRate);
break; break;
default: default:
@ -474,30 +489,36 @@ 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->set(gsTargetModeControllerParameters.useMekf);
break; break;
case 0xB: case 0xB:
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
break; break;
case 0xC: case 0xC:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
break; break;
case 0xD: case 0xD:
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
break;
case 0xE:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
break;
case 0xF:
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
break; break;
default: default:
@ -522,27 +543,33 @@ 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->set(nadirModeControllerParameters.useMekf);
break; break;
case 0xB: case 0xB:
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
break; break;
case 0xC: case 0xC:
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
break;
case 0xD:
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
break;
case 0xE:
parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
break; break;
default: default:
@ -567,24 +594,30 @@ 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.refRotRate); parameterWrapper->set(inertialModeControllerParameters.useMekf);
break;
case 0xB:
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
break; break;
case 0xC: case 0xC:
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
break;
case 0xD:
parameterWrapper->setVector(inertialModeControllerParameters.quatRef); parameterWrapper->setVector(inertialModeControllerParameters.quatRef);
break; break;
default: default:
@ -693,7 +726,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);

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

@ -798,6 +798,8 @@ class AcsParameters : public HasParametersIF {
double stictionTorque = 0.0006; double stictionTorque = 0.0006;
uint16_t rampTime = 10; uint16_t rampTime = 10;
uint32_t multipleRwInvalidTimeout = 25;
} rwHandlingParameters; } rwHandlingParameters;
struct RwMatrices { struct RwMatrices {
@ -814,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 {
@ -838,10 +840,13 @@ 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;
uint8_t useMekf = true;
uint8_t desatOn = true; uint8_t desatOn = true;
uint8_t enableAntiStiction = true; uint8_t enableAntiStiction = true;
} pointingLawParameters; } pointingLawParameters;
@ -931,7 +936,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 // concentrate 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);

4
mission/controller/acs/Navigation.cpp
View File

@ -19,9 +19,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::MekfData *mekfData, AcsParameters *acsParameters) { acsctrl::MekfData *mekfData, AcsParameters *acsParameters) {
double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
bool quatIBValid = sensorValues->strSet.caliQx.isValid() && bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;
sensorValues->strSet.caliQy.isValid() &&
sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) { if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
mekfStatus = multiplicativeKalmanFilter.init( mekfStatus = multiplicativeKalmanFilter.init(

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

@ -7,16 +7,18 @@ Detumble::Detumble() {}
Detumble::~Detumble() {} Detumble::~Detumble() {}
uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, acs::ctrlStrategy 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::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (satRotRateValid and useFullDetumbleLaw) { } else if (satRotRateValid and useFullDetumbleLaw) {
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL; return acs::ctrlStrategy::SAFECTRL_DETUMBLE_FULL;
} else if (magFieldRateValid) { } else if (magFieldRateValid) {
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED; return acs::ctrlStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
} else { } else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; return acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
} }
} }

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::ctrlStrategy 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);

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

@ -5,14 +5,32 @@
#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_; }
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) {
@ -32,12 +50,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 +92,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 +117,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 +219,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;
} }
} }

29
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 {
/* /*
@ -24,19 +21,24 @@ 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,
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 +47,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_ */

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

@ -9,21 +9,22 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
SafeCtrl::~SafeCtrl() {} SafeCtrl::~SafeCtrl() {}
uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, acs::ctrlStrategy 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::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) { } else if (mekfEnabled and mekfValid) {
return acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF; return acs::ctrlStrategy::SAFECTRL_ACTIVE_MEKF;
} else if (satRotRateValid and sunDirValid) { } else if (satRotRateValid and sunDirValid) {
return acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF; return acs::ctrlStrategy::SAFECTRL_WITHOUT_MEKF;
} else if (dampingEnabled and satRotRateValid and not sunDirValid) { } else if (dampingEnabled and satRotRateValid and not sunDirValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING; return acs::ctrlStrategy::SAFECTRL_ECLIPSE_DAMPING;
} else if (not dampingEnabled and satRotRateValid and not sunDirValid) { } else if (not dampingEnabled and satRotRateValid and not sunDirValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; return acs::ctrlStrategy::SAFECTRL_ECLIPSE_IDELING;
} else { } else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; return acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
} }
} }

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::ctrlStrategy 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,

4
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -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);