WIP: Fixes for Pointing Controller #631
@ -169,7 +169,7 @@ void AcsController::performSafe() {
|
||||
guidance.getTargetParamsSafe(sunTargetDir);
|
||||
|
||||
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
|
||||
uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy(
|
||||
acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
|
||||
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
|
||||
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
|
||||
acsParameters.safeModeControllerParameters.useMekf,
|
||||
@ -205,11 +205,13 @@ void AcsController::performSafe() {
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
safeCtrlFailure(0, 1);
|
||||
break;
|
||||
default:
|
||||
sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
|
||||
*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipolMax);
|
||||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
|
||||
|
||||
// detumble check and switch
|
||||
if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf &&
|
||||
@ -231,8 +233,8 @@ void AcsController::performSafe() {
|
||||
}
|
||||
|
||||
updateCtrlValData(errAng, safeCtrlStrat);
|
||||
updateActuatorCmdData(cmdDipolMtqs);
|
||||
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
|
||||
updateActuatorCmdData(cmdDipoleMtqs);
|
||||
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
||||
acsParameters.magnetorquerParameter.torqueDuration);
|
||||
}
|
||||
|
||||
@ -259,7 +261,7 @@ void AcsController::performDetumble() {
|
||||
triggerEvent(acs::MEKF_RECOVERY);
|
||||
mekfInvalidFlag = false;
|
||||
}
|
||||
uint8_t safeCtrlStrat = detumble.detumbleStrategy(
|
||||
acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
|
||||
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
|
||||
mgmDataProcessed.mgmVecTotDerivative.isValid(),
|
||||
acsParameters.detumbleParameter.useFullDetumbleLaw);
|
||||
@ -279,11 +281,13 @@ void AcsController::performDetumble() {
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
safeCtrlFailure(0, 1);
|
||||
break;
|
||||
default:
|
||||
sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
|
||||
*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipolMax);
|
||||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
|
||||
|
||||
if (mekfData.satRotRateMekf.isValid() &&
|
||||
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
|
||||
@ -304,8 +308,8 @@ void AcsController::performDetumble() {
|
||||
}
|
||||
|
||||
disableCtrlValData();
|
||||
updateActuatorCmdData(cmdDipolMtqs);
|
||||
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
|
||||
updateActuatorCmdData(cmdDipoleMtqs);
|
||||
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
||||
acsParameters.magnetorquerParameter.torqueDuration);
|
||||
}
|
||||
|
||||
@ -371,17 +375,17 @@ void AcsController::performPointingCtrl() {
|
||||
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
@ -395,17 +399,17 @@ void AcsController::performPointingCtrl() {
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
@ -416,17 +420,17 @@ void AcsController::performPointingCtrl() {
|
||||
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
@ -440,17 +444,17 @@ void AcsController::performPointingCtrl() {
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
@ -463,40 +467,38 @@ void AcsController::performPointingCtrl() {
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
default:
|
||||
sif::error << "AcsController: Invalid mode for performPointingCtrl";
|
||||
sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
actuatorCmd.cmdSpeedToRws(
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws,
|
||||
cmdSpeedRws, acsParameters.onBoardParams.sampleTime,
|
||||
acsParameters.rwHandlingParameters.maxRwSpeed,
|
||||
acsParameters.rwHandlingParameters.inertiaWheel);
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
|
||||
acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
|
||||
if (enableAntiStiction) {
|
||||
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
|
||||
}
|
||||
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs,
|
||||
*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipolMax);
|
||||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
|
||||
|
||||
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
|
||||
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs);
|
||||
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
|
||||
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
|
||||
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
||||
acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
|
||||
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
|
||||
acsParameters.rwHandlingParameters.rampTime);
|
||||
|
||||
@ -69,7 +69,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
bool mekfLost = false;
|
||||
|
||||
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
|
||||
uint32_t opCounter = 0;
|
||||
|
||||
@ -315,7 +315,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->setMatrix(rwMatrices.without4);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->setVector(rwMatrices.nullspace);
|
||||
parameterWrapper->setVector(rwMatrices.nullspaceVector);
|
||||
break;
|
||||
default:
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
@ -375,15 +375,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->set(idleModeControllerParameters.gainNullspace);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
|
||||
parameterWrapper->set(idleModeControllerParameters.nullspaceSpeed);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
|
||||
parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
|
||||
break;
|
||||
case 0x7:
|
||||
parameterWrapper->set(idleModeControllerParameters.desatOn);
|
||||
parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
|
||||
break;
|
||||
case 0x8:
|
||||
parameterWrapper->set(idleModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
default:
|
||||
@ -408,48 +411,51 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
|
||||
parameterWrapper->set(targetModeControllerParameters.nullspaceSpeed);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
|
||||
parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
|
||||
break;
|
||||
case 0x7:
|
||||
parameterWrapper->set(targetModeControllerParameters.desatOn);
|
||||
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
|
||||
break;
|
||||
case 0x8:
|
||||
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
|
||||
parameterWrapper->set(targetModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
|
||||
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
case 0xA:
|
||||
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
|
||||
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
|
||||
break;
|
||||
case 0xB:
|
||||
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
|
||||
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
|
||||
break;
|
||||
case 0xC:
|
||||
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
|
||||
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
|
||||
break;
|
||||
case 0xD:
|
||||
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
|
||||
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
|
||||
break;
|
||||
case 0xE:
|
||||
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
|
||||
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
|
||||
break;
|
||||
case 0xF:
|
||||
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
|
||||
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
|
||||
break;
|
||||
case 0x10:
|
||||
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
|
||||
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
|
||||
break;
|
||||
case 0x11:
|
||||
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
|
||||
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
|
||||
break;
|
||||
case 0x12:
|
||||
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
|
||||
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
|
||||
break;
|
||||
case 0x13:
|
||||
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
|
||||
break;
|
||||
case 0x14:
|
||||
parameterWrapper->set(targetModeControllerParameters.blindRotRate);
|
||||
break;
|
||||
default:
|
||||
@ -474,30 +480,33 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.nullspaceSpeed);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
|
||||
parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
|
||||
break;
|
||||
case 0x7:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
|
||||
break;
|
||||
case 0x8:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
case 0xA:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
|
||||
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
|
||||
break;
|
||||
case 0xB:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
|
||||
break;
|
||||
case 0xC:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
|
||||
break;
|
||||
case 0xD:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
|
||||
break;
|
||||
case 0xE:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
|
||||
break;
|
||||
default:
|
||||
@ -522,21 +531,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
|
||||
parameterWrapper->set(nadirModeControllerParameters.nullspaceSpeed);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
|
||||
break;
|
||||
case 0x7:
|
||||
parameterWrapper->set(nadirModeControllerParameters.desatOn);
|
||||
parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
|
||||
break;
|
||||
case 0x8:
|
||||
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
|
||||
parameterWrapper->set(nadirModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
|
||||
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
case 0xA:
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
|
||||
break;
|
||||
case 0xB:
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
|
||||
break;
|
||||
case 0xC:
|
||||
@ -564,21 +576,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
|
||||
parameterWrapper->set(inertialModeControllerParameters.nullspaceSpeed);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
|
||||
parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
|
||||
break;
|
||||
case 0x7:
|
||||
parameterWrapper->set(inertialModeControllerParameters.desatOn);
|
||||
parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
|
||||
break;
|
||||
case 0x8:
|
||||
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
|
||||
parameterWrapper->set(inertialModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
|
||||
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
case 0xA:
|
||||
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
|
||||
break;
|
||||
case 0xB:
|
||||
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
|
||||
break;
|
||||
case 0xC:
|
||||
@ -690,7 +705,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->set(magnetorquerParameter.dipolMax);
|
||||
parameterWrapper->set(magnetorquerParameter.dipoleMax);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(magnetorquerParameter.torqueDuration);
|
||||
|
||||
@ -814,7 +814,7 @@ class AcsParameters : public HasParametersIF {
|
||||
{1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
|
||||
double without4[4][3] = {
|
||||
{0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
|
||||
double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000};
|
||||
double nullspaceVector[4] = {-1, 1, -1, 1};
|
||||
} rwMatrices;
|
||||
|
||||
struct SafeModeControllerParameters {
|
||||
@ -838,7 +838,9 @@ class AcsParameters : public HasParametersIF {
|
||||
double om = 0.3;
|
||||
double omMax = 1 * M_PI / 180;
|
||||
double qiMin = 0.1;
|
||||
|
||||
double gainNullspace = 0.01;
|
||||
double nullspaceSpeed = 32500; // 0.1 RPM
|
||||
|
||||
double desatMomentumRef[3] = {0, 0, 0};
|
||||
double deSatGainFactor = 1000;
|
||||
@ -931,7 +933,7 @@ class AcsParameters : public HasParametersIF {
|
||||
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 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]
|
||||
} magnetorquerParameter;
|
||||
|
||||
@ -5,11 +5,6 @@
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "util/CholeskyDecomposition.h"
|
||||
#include "util/MathOperations.h"
|
||||
|
||||
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,
|
||||
int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed,
|
||||
double sampleTime, int32_t maxRwSpeed, double inertiaWheel) {
|
||||
using namespace Math;
|
||||
|
||||
// Calculating the commanded speed in RPM for every reaction wheel
|
||||
void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
|
||||
const int32_t speedRw2, const int32_t speedRw3,
|
||||
const double sampleTime, const double inertiaWheel,
|
||||
const int32_t maxRwSpeed, const double *rwTorque,
|
||||
int32_t *rwCmdSpeed) {
|
||||
// concentrate RW speed values (in 0.1 [RPM]) in vector
|
||||
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 radToRpm = 60 / (2 * PI); // factor for conversion to RPM
|
||||
// W_RW = Torque_RW / I_RW * delta t [rad/s]
|
||||
double factor = sampleTime / inertiaWheel * radToRpm;
|
||||
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
|
||||
const double factor = sampleTime / inertiaWheel * RAD_PER_SEC_TO_RPM * 10;
|
||||
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++) {
|
||||
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>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
|
||||
|
||||
// crop values which would exceed the maximum possible RPM
|
||||
for (uint8_t i = 0; i < 4; i++) {
|
||||
if (rwCmdSpeed[i] > maxRwSpeed) {
|
||||
rwCmdSpeed[i] = maxRwSpeed;
|
||||
@ -52,24 +53,24 @@ void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t spee
|
||||
}
|
||||
}
|
||||
|
||||
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
|
||||
const double *inverseAlignment, double maxDipol) {
|
||||
// Convert to actuator frame
|
||||
double dipolMomentActuatorDouble[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3,
|
||||
void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
|
||||
const double *dipoleMoment, int16_t *dipoleMomentActuator) {
|
||||
// convert to actuator frame
|
||||
double dipoleMomentActuatorDouble[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3, 3,
|
||||
1);
|
||||
// Scaling along largest element if dipol exceeds maximum
|
||||
// scaling along largest element if dipole exceeds maximum
|
||||
uint8_t maxIdx = 0;
|
||||
VectorOperations<double>::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx);
|
||||
double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]);
|
||||
if (maxAbsValue > maxDipol) {
|
||||
double scalingFactor = maxDipol / maxAbsValue;
|
||||
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor,
|
||||
dipolMomentActuatorDouble, 3);
|
||||
VectorOperations<double>::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx);
|
||||
double maxAbsValue = abs(dipoleMomentActuatorDouble[maxIdx]);
|
||||
if (maxAbsValue > maxDipole) {
|
||||
double scalingFactor = maxDipole / maxAbsValue;
|
||||
VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, scalingFactor,
|
||||
dipoleMomentActuatorDouble, 3);
|
||||
}
|
||||
// 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++) {
|
||||
dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]);
|
||||
dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,9 +1,8 @@
|
||||
#ifndef ACTUATORCMD_H_
|
||||
#define ACTUATORCMD_H_
|
||||
|
||||
#include "MultiplicativeKalmanFilter.h"
|
||||
#include "SensorProcessing.h"
|
||||
#include "SensorValues.h"
|
||||
#include <cmath>
|
||||
|
||||
|
||||
class ActuatorCmd {
|
||||
public:
|
||||
@ -19,29 +18,30 @@ class ActuatorCmd {
|
||||
void scalingTorqueRws(double *rwTrq, double maxTorque);
|
||||
|
||||
/*
|
||||
* @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction
|
||||
* wheels, also will calculate the needed revolutions per minute for the RWs, which will be given
|
||||
* as Input to the RWs
|
||||
* @param: rwTrqIn given torque from pointing controller
|
||||
* rwTrqNS Nullspace torque
|
||||
* @brief: cmdSpeedToRws() Calculates the RPM for the reaction wheel configuration,
|
||||
* given the required torque calculated by the controller. Will also scale down the RPM of the
|
||||
* wheels if they exceed the maximum possible RPM
|
||||
* @param: rwTrq given torque from pointing controller
|
||||
* rwCmdSpeed output revolutions per minute for every
|
||||
* reaction wheel
|
||||
*/
|
||||
void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3,
|
||||
const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime,
|
||||
int32_t maxRwSpeed, double inertiaWheel);
|
||||
void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
|
||||
const int32_t speedRw3, const double sampleTime, const 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
|
||||
* dipolMomentActuator resulting dipol moment in actuator reference frame
|
||||
* @param: dipoleMoment given dipole moment in spacecraft frame
|
||||
* dipoleMomentActuator resulting dipole moment in actuator reference frame
|
||||
*/
|
||||
void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
|
||||
const double *inverseAlignment, double maxDipol);
|
||||
void cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
|
||||
const double *dipoleMoment, int16_t *dipoleMomentActuator);
|
||||
|
||||
protected:
|
||||
private:
|
||||
static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI);
|
||||
};
|
||||
|
||||
#endif /* ACTUATORCMD_H_ */
|
||||
|
||||
@ -7,8 +7,10 @@ Detumble::Detumble() {}
|
||||
|
||||
Detumble::~Detumble() {}
|
||||
|
||||
uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
|
||||
const bool magFieldRateValid, const bool useFullDetumbleLaw) {
|
||||
acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
|
||||
const bool satRotRateValid,
|
||||
const bool magFieldRateValid,
|
||||
const bool useFullDetumbleLaw) {
|
||||
if (not magFieldValid) {
|
||||
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
|
||||
} else if (satRotRateValid and useFullDetumbleLaw) {
|
||||
|
||||
@ -11,8 +11,9 @@ class Detumble {
|
||||
Detumble();
|
||||
virtual ~Detumble();
|
||||
|
||||
uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
|
||||
const bool magFieldRateValid, const bool useFullDetumbleLaw);
|
||||
acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
|
||||
const bool magFieldRateValid,
|
||||
const bool useFullDetumbleLaw);
|
||||
|
||||
void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
|
||||
double gain);
|
||||
|
||||
@ -5,9 +5,6 @@
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <fsfw/globalfunctions/sign.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "../util/MathOperations.h"
|
||||
|
||||
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
|
||||
|
||||
@ -98,61 +95,88 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
|
||||
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)};
|
||||
|
||||
// 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,
|
||||
double *magFieldEst, bool magFieldEstValid, double *satRate,
|
||||
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
|
||||
int32_t *speedRw3, double *mgtDpDes) {
|
||||
if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
|
||||
mgtDpDes[0] = 0;
|
||||
mgtDpDes[1] = 0;
|
||||
mgtDpDes[2] = 0;
|
||||
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) {
|
||||
if (not magFieldBValid or not pointingLawParameters->desatOn) {
|
||||
return;
|
||||
}
|
||||
|
||||
// calculating momentum of satellite and momentum of reaction wheels
|
||||
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
|
||||
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
|
||||
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);
|
||||
}
|
||||
// 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)};
|
||||
|
||||
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
const int32_t *speedRw0, const int32_t *speedRw1,
|
||||
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
|
||||
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
|
||||
double wheelMomentum[4] = {0, 0, 0, 0};
|
||||
double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
|
||||
// conversion to [rad/s] for further calculations
|
||||
VectorOperations<double>::mulScalar(rpmOffset, factor, rpmOffset, 4);
|
||||
VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
|
||||
double diffRwSpeed[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
|
||||
VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
|
||||
wheelMomentum, 4);
|
||||
double gainNs = pointingLawParameters->gainNullspace;
|
||||
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace,
|
||||
acsParameters->rwMatrices.nullspace,
|
||||
*nullSpaceMatrix, 4);
|
||||
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
|
||||
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
|
||||
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
|
||||
// convert magFieldB from uT to T
|
||||
double magFieldBT[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
|
||||
|
||||
// calculate angular momentum of the satellite
|
||||
double angMomentumSat[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
|
||||
angMomentumSat, 3, 3, 1);
|
||||
|
||||
// calculate angular momentum of the reaction wheels with respect to the nullspace RW speed
|
||||
// relocate RW speed zero to nullspace RW speed
|
||||
double refSpeedRws[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
|
||||
pointingLawParameters->nullspaceSpeed, refSpeedRws, 4);
|
||||
VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
|
||||
// convert to rad/s
|
||||
VectorOperations<double>::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4);
|
||||
// calculate angular momentum of each RW
|
||||
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) {
|
||||
@ -169,15 +193,9 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee
|
||||
if (rwCmdSpeeds[i] != 0) {
|
||||
if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed &&
|
||||
rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) {
|
||||
if (currRwSpeed[i] == 0) {
|
||||
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) {
|
||||
if (rwCmdSpeeds[i] > currRwSpeed[i]) {
|
||||
rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
|
||||
} else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) {
|
||||
} else if (rwCmdSpeeds[i] < currRwSpeed[i]) {
|
||||
rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,13 +1,10 @@
|
||||
#ifndef PTGCTRL_H_
|
||||
#define PTGCTRL_H_
|
||||
|
||||
#include <math.h>
|
||||
#include <mission/controller/acs/AcsParameters.h>
|
||||
#include <mission/controller/acs/SensorValues.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
#include "../AcsParameters.h"
|
||||
#include "../SensorValues.h"
|
||||
#include "eive/resultClassIds.h"
|
||||
|
||||
class PtgCtrl {
|
||||
/*
|
||||
@ -29,14 +26,14 @@ class PtgCtrl {
|
||||
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
|
||||
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,
|
||||
const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
|
||||
const int32_t *speedRw3, double *rwTrqNs);
|
||||
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
|
||||
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
|
||||
* torqueCommand modified torque after antistiction
|
||||
@ -45,6 +42,7 @@ class PtgCtrl {
|
||||
|
||||
private:
|
||||
const AcsParameters *acsParameters;
|
||||
static constexpr double RPM_TO_RAD_PER_SEC = (2 * M_PI) / 60;
|
||||
};
|
||||
|
||||
#endif /* ACS_CONTROL_PTGCTRL_H_ */
|
||||
|
||||
@ -9,9 +9,10 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
|
||||
|
||||
SafeCtrl::~SafeCtrl() {}
|
||||
|
||||
uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
const bool satRotRateValid, const bool sunDirValid,
|
||||
const uint8_t mekfEnabled, const uint8_t dampingEnabled) {
|
||||
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
const bool satRotRateValid, const bool sunDirValid,
|
||||
const uint8_t mekfEnabled,
|
||||
const uint8_t dampingEnabled) {
|
||||
if (not magFieldValid) {
|
||||
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
|
||||
} else if (mekfEnabled and mekfValid) {
|
||||
|
||||
@ -12,9 +12,9 @@ class SafeCtrl {
|
||||
SafeCtrl(AcsParameters *acsParameters_);
|
||||
virtual ~SafeCtrl();
|
||||
|
||||
uint8_t safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
const bool satRotRateValid, const bool sunDirValid,
|
||||
const uint8_t mekfEnabled, const uint8_t dampingEnabled);
|
||||
acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
const bool satRotRateValid, const bool sunDirValid,
|
||||
const uint8_t mekfEnabled, const uint8_t dampingEnabled);
|
||||
|
||||
void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
|
||||
const double *quatBI, const double *sunDirRefB, double *magMomB,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user