eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

Refactor TM handling #450

Merged
muellerr merged 47 commits from refactor_tm_handling into develop 2023-03-11 15:05:22 +01:00
Conversation 0 Commits 47 Files Changed 71 +474 -485
23 changed files with 474 additions and 485 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 8c450596f6 - Show all commits

28
CHANGELOG.md
View File

@ -16,10 +16,35 @@ will consitute of a breaking change warranting a new major release:
# [unreleased]
## Added
- `SensorProcessing` now includes an FDIR for GPS altitude. If the measured GPS altitude is out
of bounds of the range defined in the `AcsParameters`, the altitude defaults to an altitude
set in the `AcsParameters`.
- `AcsController` will now never command a RW speed larger than the maximum allowed speed.
## Fixed
- `PAPB_EMPTY_SIGNAL_VC1` GPIO was not set up properly.
- Fix for heater names: HPA heater (index 7) is now the Syrlinks heater.
- `AcsParameters` setter were previously all for scalar parameters. Now vector and matrix
parameters use their respective setters.
- Several `AcsController` components had their own implementation of `AcsParameters`. This resulted
in those parameters not being updated, while the actual ones were updated. All instances of
`AcsParameters` not belonging to `AcsController` are eiter removed or replaced by pointer
instances.
- Instead of updating the `gsTargetModeControllerParameters`, the `targetModeControllerParameters`
were updated.
- Instead of updating the `idleModeControllerParameters`, the `targetModeControllerParameters`
were updated.
- Fixed Idle Mode Controller never calling `ptgLaw` and therefore never calculating control
values.
- Fixed wrong check on wether file used for persistant boolean flag on successful still existed.
- Scaling of MTQ Cmds now scales the current values to command with the current values and not
the values of the last step, which would result in undefined behaviour.
- Solved naming collision between file used for solar array deployment and confirmation for
ACS for solar array deployment.
- Fixed that scaling of RW torque would result in a zero vector unless the maximum value was exceeded.
## Changed
@ -31,6 +56,9 @@ will consitute of a breaking change warranting a new major release:
- Service 5 now handles 40 events per cycle instead of 15
- Remove periodic SD card check. The file system is not mounted read-only anymore when using an
ext4 filesystem
- The `detumbleCounter` now does not get hard reset anymore, if the critical rate does not get
violated anymore. Instead it is incrementally reset.
- The RW antistiction now only takes the RW speeds in account.
- ACS CTRL transition to DETUBMLE is now done in CTRL internally. No
system level handling necessary anymore.
- More fixes and improvements for SD card handling. Extend SD card setup in core controller to

77
mission/controller/AcsController.cpp
View File

@ -6,12 +6,8 @@
AcsController::AcsController(object_id_t objectId)
: ExtendedControllerBase(objectId),
sensorProcessing(&acsParameters),
navigation(&acsParameters),
actuatorCmd(&acsParameters),
guidance(&acsParameters),
safeCtrl(&acsParameters),
detumble(&acsParameters),
ptgCtrl(&acsParameters),
parameterHelper(this),
mgmDataRaw(this),
@ -146,7 +142,7 @@ void AcsController::performSafe() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
@ -180,7 +176,9 @@ void AcsController::performSafe() {
// ToDo: this should never ever happen or we are dead. prob add an event at least
}
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipolMax);
// detumble check and switch
if (mekfData.satRotRateMekf.isValid() &&
@ -191,8 +189,8 @@ void AcsController::performSafe() {
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
} else {
detumbleCounter = 0;
} else if (detumbleCounter > 0) {
detumbleCounter -= 1;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
detumbleCounter = 0;
@ -203,7 +201,7 @@ void AcsController::performSafe() {
updateCtrlValData(errAng);
updateActuatorCmdData(cmdDipolMtqs);
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2]/*500, 500, 500*/,
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime);
}
@ -215,7 +213,7 @@ void AcsController::performDetumble() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
@ -228,8 +226,11 @@ void AcsController::performDetumble() {
double magMomMtq[3] = {0, 0, 0};
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq,
acsParameters.detumbleParameter.gainD);
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipolMax);
if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@ -239,8 +240,8 @@ void AcsController::performDetumble() {
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
} else {
detumbleCounter = 0;
} else if (detumbleCounter > 0) {
detumbleCounter -= 1;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
detumbleCounter = 0;
@ -263,7 +264,7 @@ void AcsController::performPointingCtrl() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
@ -296,24 +297,26 @@ void AcsController::performPointingCtrl() {
} else {
multipleRwUnavailableCounter = 0;
}
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0};
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
double mgtDpDes[3] = {0, 0, 0};
// Variables required for guidance
double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
errorAngle = 0, errorSatRotRate[3] = {0, 0, 0};
// Variables required for setting actuators
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0},
mgtDpDes[3] = {0, 0, 0};
switch (submode) {
case acs::PTG_IDLE:
guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
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);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -337,7 +340,7 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -351,20 +354,20 @@ void AcsController::performPointingCtrl() {
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&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, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
&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);
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
break;
case acs::PTG_NADIR:
@ -382,7 +385,7 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -405,7 +408,7 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -415,18 +418,21 @@ void AcsController::performPointingCtrl() {
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);
if (enableAntiStiction) {
ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
}
actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs,
*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipolMax);
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs);
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs);
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
// cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
@ -584,6 +590,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
// GPS Processed
localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0});

1
mission/controller/AcsController.h
View File

@ -190,6 +190,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
acsctrl::GpsDataProcessed gpsDataProcessed;
PoolEntry<double> gcLatitude = PoolEntry<double>();
PoolEntry<double> gdLongitude = PoolEntry<double>();
PoolEntry<double> altitude = PoolEntry<double>();
PoolEntry<double> gpsPosition = PoolEntry<double>(3);
PoolEntry<double> gpsVelocity = PoolEntry<double>(3);

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

@ -30,19 +30,19 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x2: // InertiaEIVE
switch (parameterId) {
case 0x0:
parameterWrapper->set(inertiaEIVE.inertiaMatrix);
parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrix);
break;
case 0x1:
parameterWrapper->set(inertiaEIVE.inertiaMatrixDeployed);
parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixDeployed);
break;
case 0x2:
parameterWrapper->set(inertiaEIVE.inertiaMatrixUndeployed);
parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixUndeployed);
break;
case 0x3:
parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel1);
parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel1);
break;
case 0x4:
parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel3);
parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel3);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -51,58 +51,58 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x3: // MgmHandlingParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm0orientationMatrix);
break;
case 0x1:
parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm1orientationMatrix);
break;
case 0x2:
parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm2orientationMatrix);
break;
case 0x3:
parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm3orientationMatrix);
break;
case 0x4:
parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm4orientationMatrix);
break;
case 0x5:
parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset);
parameterWrapper->setVector(mgmHandlingParameters.mgm0hardIronOffset);
break;
case 0x6:
parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset);
parameterWrapper->setVector(mgmHandlingParameters.mgm1hardIronOffset);
break;
case 0x7:
parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset);
parameterWrapper->setVector(mgmHandlingParameters.mgm2hardIronOffset);
break;
case 0x8:
parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset);
parameterWrapper->setVector(mgmHandlingParameters.mgm3hardIronOffset);
break;
case 0x9:
parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset);
parameterWrapper->setVector(mgmHandlingParameters.mgm4hardIronOffset);
break;
case 0xA:
parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm0softIronInverse);
break;
case 0xB:
parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm1softIronInverse);
break;
case 0xC:
parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm2softIronInverse);
break;
case 0xD:
parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm3softIronInverse);
break;
case 0xE:
parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm4softIronInverse);
break;
case 0xF:
parameterWrapper->set(mgmHandlingParameters.mgm02variance);
parameterWrapper->setVector(mgmHandlingParameters.mgm02variance);
break;
case 0x10:
parameterWrapper->set(mgmHandlingParameters.mgm13variance);
parameterWrapper->setVector(mgmHandlingParameters.mgm13variance);
break;
case 0x11:
parameterWrapper->set(mgmHandlingParameters.mgm4variance);
parameterWrapper->setVector(mgmHandlingParameters.mgm4variance);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -111,112 +111,112 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x4: // SusHandlingParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(susHandlingParameters.sus0orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus0orientationMatrix);
break;
case 0x1:
parameterWrapper->set(susHandlingParameters.sus1orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus1orientationMatrix);
break;
case 0x2:
parameterWrapper->set(susHandlingParameters.sus2orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus2orientationMatrix);
break;
case 0x3:
parameterWrapper->set(susHandlingParameters.sus3orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus3orientationMatrix);
break;
case 0x4:
parameterWrapper->set(susHandlingParameters.sus4orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus4orientationMatrix);
break;
case 0x5:
parameterWrapper->set(susHandlingParameters.sus5orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus5orientationMatrix);
break;
case 0x6:
parameterWrapper->set(susHandlingParameters.sus6orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus6orientationMatrix);
break;
case 0x7:
parameterWrapper->set(susHandlingParameters.sus7orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus7orientationMatrix);
break;
case 0x8:
parameterWrapper->set(susHandlingParameters.sus8orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus8orientationMatrix);
break;
case 0x9:
parameterWrapper->set(susHandlingParameters.sus9orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus9orientationMatrix);
break;
case 0xA:
parameterWrapper->set(susHandlingParameters.sus10orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus10orientationMatrix);
break;
case 0xB:
parameterWrapper->set(susHandlingParameters.sus11orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus11orientationMatrix);
break;
case 0xC:
parameterWrapper->set(susHandlingParameters.sus0coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus0coeffAlpha);
break;
case 0xD:
parameterWrapper->set(susHandlingParameters.sus0coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus0coeffBeta);
break;
case 0xE:
parameterWrapper->set(susHandlingParameters.sus1coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus1coeffAlpha);
break;
case 0xF:
parameterWrapper->set(susHandlingParameters.sus1coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus1coeffBeta);
break;
case 0x10:
parameterWrapper->set(susHandlingParameters.sus2coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus2coeffAlpha);
break;
case 0x11:
parameterWrapper->set(susHandlingParameters.sus2coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus2coeffBeta);
break;
case 0x12:
parameterWrapper->set(susHandlingParameters.sus3coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus3coeffAlpha);
break;
case 0x13:
parameterWrapper->set(susHandlingParameters.sus3coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus3coeffBeta);
break;
case 0x14:
parameterWrapper->set(susHandlingParameters.sus4coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus4coeffAlpha);
break;
case 0x15:
parameterWrapper->set(susHandlingParameters.sus4coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus4coeffBeta);
break;
case 0x16:
parameterWrapper->set(susHandlingParameters.sus5coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus5coeffAlpha);
break;
case 0x17:
parameterWrapper->set(susHandlingParameters.sus5coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus5coeffBeta);
break;
case 0x18:
parameterWrapper->set(susHandlingParameters.sus6coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus6coeffAlpha);
break;
case 0x19:
parameterWrapper->set(susHandlingParameters.sus6coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus6coeffBeta);
break;
case 0x1A:
parameterWrapper->set(susHandlingParameters.sus7coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus7coeffAlpha);
break;
case 0x1B:
parameterWrapper->set(susHandlingParameters.sus7coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus7coeffBeta);
break;
case 0x1C:
parameterWrapper->set(susHandlingParameters.sus8coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus8coeffAlpha);
break;
case 0x1D:
parameterWrapper->set(susHandlingParameters.sus8coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus8coeffBeta);
break;
case 0x1E:
parameterWrapper->set(susHandlingParameters.sus9coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus9coeffAlpha);
break;
case 0x1F:
parameterWrapper->set(susHandlingParameters.sus9coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus9coeffBeta);
break;
case 0x20:
parameterWrapper->set(susHandlingParameters.sus10coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus10coeffAlpha);
break;
case 0x21:
parameterWrapper->set(susHandlingParameters.sus10coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus10coeffBeta);
break;
case 0x22:
parameterWrapper->set(susHandlingParameters.sus11coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus11coeffAlpha);
break;
case 0x23:
parameterWrapper->set(susHandlingParameters.sus11coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -225,34 +225,34 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0x5): // GyrHandlingParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix);
parameterWrapper->setMatrix(gyrHandlingParameters.gyr0orientationMatrix);
break;
case 0x1:
parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix);
parameterWrapper->setMatrix(gyrHandlingParameters.gyr1orientationMatrix);
break;
case 0x2:
parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix);
parameterWrapper->setMatrix(gyrHandlingParameters.gyr2orientationMatrix);
break;
case 0x3:
parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix);
parameterWrapper->setMatrix(gyrHandlingParameters.gyr3orientationMatrix);
break;
case 0x4:
parameterWrapper->set(gyrHandlingParameters.gyr0bias);
parameterWrapper->setVector(gyrHandlingParameters.gyr0bias);
break;
case 0x5:
parameterWrapper->set(gyrHandlingParameters.gyr1bias);
parameterWrapper->setVector(gyrHandlingParameters.gyr1bias);
break;
case 0x6:
parameterWrapper->set(gyrHandlingParameters.gyr2bias);
parameterWrapper->setVector(gyrHandlingParameters.gyr2bias);
break;
case 0x7:
parameterWrapper->set(gyrHandlingParameters.gyr3bias);
parameterWrapper->setVector(gyrHandlingParameters.gyr3bias);
break;
case 0x8:
parameterWrapper->set(gyrHandlingParameters.gyr02variance);
parameterWrapper->setVector(gyrHandlingParameters.gyr02variance);
break;
case 0x9:
parameterWrapper->set(gyrHandlingParameters.gyr13variance);
parameterWrapper->setVector(gyrHandlingParameters.gyr13variance);
break;
case 0xA:
parameterWrapper->set(gyrHandlingParameters.preferAdis);
@ -270,15 +270,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(rwHandlingParameters.maxTrq);
break;
case 0x2:
parameterWrapper->set(rwHandlingParameters.stictionSpeed);
parameterWrapper->set(rwHandlingParameters.maxRwSpeed);
break;
case 0x3:
parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed);
parameterWrapper->set(rwHandlingParameters.stictionSpeed);
break;
case 0x4:
parameterWrapper->set(rwHandlingParameters.stictionTorque);
parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed);
break;
case 0x5:
parameterWrapper->set(rwHandlingParameters.stictionTorque);
break;
case 0x6:
parameterWrapper->set(rwHandlingParameters.rampTime);
break;
default:
@ -288,25 +291,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0x7): // RwMatrices
switch (parameterId) {
case 0x0:
parameterWrapper->set(rwMatrices.alignmentMatrix);
parameterWrapper->setMatrix(rwMatrices.alignmentMatrix);
break;
case 0x1:
parameterWrapper->set(rwMatrices.pseudoInverse);
parameterWrapper->setMatrix(rwMatrices.pseudoInverse);
break;
case 0x2:
parameterWrapper->set(rwMatrices.without1);
parameterWrapper->setMatrix(rwMatrices.without1);
break;
case 0x3:
parameterWrapper->set(rwMatrices.without2);
parameterWrapper->setMatrix(rwMatrices.without2);
break;
case 0x4:
parameterWrapper->set(rwMatrices.without3);
parameterWrapper->setMatrix(rwMatrices.without3);
break;
case 0x5:
parameterWrapper->set(rwMatrices.without4);
parameterWrapper->setMatrix(rwMatrices.without4);
break;
case 0x6:
parameterWrapper->set(rwMatrices.nullspace);
parameterWrapper->setVector(rwMatrices.nullspace);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -330,13 +333,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin);
break;
case 0x5:
parameterWrapper->set(safeModeControllerParameters.sunTargetDirLeop);
parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop);
break;
case 0x6:
parameterWrapper->set(safeModeControllerParameters.sunTargetDir);
parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir);
break;
case 0x7:
parameterWrapper->set(safeModeControllerParameters.satRateRef);
parameterWrapper->setVector(safeModeControllerParameters.satRateRef);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -345,31 +348,31 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0x9): // IdleModeControllerParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(targetModeControllerParameters.zeta);
parameterWrapper->set(idleModeControllerParameters.zeta);
break;
case 0x1:
parameterWrapper->set(targetModeControllerParameters.om);
parameterWrapper->set(idleModeControllerParameters.om);
break;
case 0x2:
parameterWrapper->set(targetModeControllerParameters.omMax);
parameterWrapper->set(idleModeControllerParameters.omMax);
break;
case 0x3:
parameterWrapper->set(targetModeControllerParameters.qiMin);
parameterWrapper->set(idleModeControllerParameters.qiMin);
break;
case 0x4:
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
parameterWrapper->set(idleModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
break;
case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
break;
case 0x7:
parameterWrapper->set(targetModeControllerParameters.desatOn);
parameterWrapper->set(idleModeControllerParameters.desatOn);
break;
case 0x8:
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
break;
default:
@ -394,7 +397,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
break;
case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
@ -406,13 +409,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
break;
case 0x9:
parameterWrapper->set(targetModeControllerParameters.refDirection);
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
break;
case 0xA:
parameterWrapper->set(targetModeControllerParameters.refRotRate);
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
break;
case 0xB:
parameterWrapper->set(targetModeControllerParameters.quatRef);
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
break;
case 0xC:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
@ -445,52 +448,46 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0xB): // GsTargetModeControllerParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(targetModeControllerParameters.zeta);
parameterWrapper->set(gsTargetModeControllerParameters.zeta);
break;
case 0x1:
parameterWrapper->set(targetModeControllerParameters.om);
parameterWrapper->set(gsTargetModeControllerParameters.om);
break;
case 0x2:
parameterWrapper->set(targetModeControllerParameters.omMax);
parameterWrapper->set(gsTargetModeControllerParameters.omMax);
break;
case 0x3:
parameterWrapper->set(targetModeControllerParameters.qiMin);
parameterWrapper->set(gsTargetModeControllerParameters.qiMin);
break;
case 0x4:
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
break;
case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
break;
case 0x7:
parameterWrapper->set(targetModeControllerParameters.desatOn);
parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
break;
case 0x8:
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
break;
case 0x9:
parameterWrapper->set(targetModeControllerParameters.refDirection);
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
break;
case 0xA:
parameterWrapper->set(targetModeControllerParameters.refRotRate);
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
break;
case 0xB:
parameterWrapper->set(targetModeControllerParameters.quatRef);
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
break;
case 0xC:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
break;
case 0xD:
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
break;
case 0xE:
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
break;
case 0xF:
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -514,7 +511,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->set(nadirModeControllerParameters.desatMomentumRef);
parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
break;
case 0x6:
parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
@ -526,10 +523,10 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
break;
case 0x9:
parameterWrapper->set(nadirModeControllerParameters.refDirection);
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
break;
case 0xA:
parameterWrapper->set(nadirModeControllerParameters.quatRef);
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
break;
case 0xC:
parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
@ -556,7 +553,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->set(inertialModeControllerParameters.desatMomentumRef);
parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
break;
case 0x6:
parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
@ -568,13 +565,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
break;
case 0x9:
parameterWrapper->set(inertialModeControllerParameters.tgtQuat);
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
break;
case 0xA:
parameterWrapper->set(inertialModeControllerParameters.refRotRate);
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
break;
case 0xC:
parameterWrapper->set(inertialModeControllerParameters.quatRef);
parameterWrapper->setVector(inertialModeControllerParameters.quatRef);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -586,7 +583,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(strParameters.exclusionAngle);
break;
case 0x1:
parameterWrapper->set(strParameters.boresightAxis);
parameterWrapper->setVector(strParameters.boresightAxis);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -597,6 +594,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x0:
parameterWrapper->set(gpsParameters.timeDiffVelocityMax);
break;
case 0x1:
parameterWrapper->set(gpsParameters.minimumFdirAltitude);
break;
case 0x2:
parameterWrapper->set(gpsParameters.maximumFdirAltitude);
break;
case 0x3:
parameterWrapper->set(gpsParameters.fdirAltitude);
break;
default:
return INVALID_IDENTIFIER_ID;
}
@ -658,25 +664,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0x12): // MagnetorquesParameter
switch (parameterId) {
case 0x0:
parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix);
parameterWrapper->setMatrix(magnetorquerParameter.mtq0orientationMatrix);
break;
case 0x1:
parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix);
parameterWrapper->setMatrix(magnetorquerParameter.mtq1orientationMatrix);
break;
case 0x2:
parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix);
parameterWrapper->setMatrix(magnetorquerParameter.mtq2orientationMatrix);
break;
case 0x3:
parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq);
parameterWrapper->setMatrix(magnetorquerParameter.alignmentMatrixMtq);
break;
case 0x4:
parameterWrapper->set(magnetorquesParameter.inverseAlignment);
parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment);
break;
case 0x5:
parameterWrapper->set(magnetorquesParameter.DipolMax);
parameterWrapper->set(magnetorquerParameter.dipolMax);
break;
case 0x6:
parameterWrapper->set(magnetorquesParameter.torqueDuration);
parameterWrapper->set(magnetorquerParameter.torqueDuration);
break;
default:
return INVALID_IDENTIFIER_ID;

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

@ -772,7 +772,7 @@ class AcsParameters : public HasParametersIF {
double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594};
double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845};
/* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
/* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
* assumed to be equal for the same class of sensors */
float gyr02variance[3] = {pow(3.0e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
pow(3.0e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
@ -783,9 +783,10 @@ class AcsParameters : public HasParametersIF {
struct RwHandlingParameters {
double inertiaWheel = 0.000028198;
double maxTrq = 0.0032; // 3.2 [mNm]
int32_t stictionSpeed = 100; // RPM
int32_t stictionReleaseSpeed = 120; // RPM
double maxTrq = 0.0032; // 3.2 [mNm]
int32_t maxRwSpeed = 65000; // 0.1 RPM
int32_t stictionSpeed = 1000; // 0.1 RPM
int32_t stictionReleaseSpeed = 1000; // 0.1 RPM
double stictionTorque = 0.0006;
uint16_t rampTime = 10;
@ -817,7 +818,7 @@ class AcsParameters : public HasParametersIF {
double sunMagAngleMin = 5 * M_PI / 180;
double sunTargetDirLeop[3] = {0, .5, .5};
double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)};
double sunTargetDir[3] = {0, 0, 1};
double satRateRef[3] = {0, 0, 0};
@ -843,7 +844,7 @@ class AcsParameters : public HasParametersIF {
double refDirection[3] = {-1, 0, 0}; // Antenna
double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1};
int8_t timeElapsedMax = 10; // rot rate calculations
uint8_t timeElapsedMax = 10; // rot rate calculations
// Default is Stuttgart GS
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
@ -859,7 +860,7 @@ class AcsParameters : public HasParametersIF {
struct GsTargetModeControllerParameters : PointingLawParameters {
double refDirection[3] = {-1, 0, 0}; // Antenna
int8_t timeElapsedMax = 10; // rot rate calculations
uint8_t timeElapsedMax = 10; // rot rate calculations
// Default is Stuttgart GS
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
@ -871,7 +872,7 @@ class AcsParameters : public HasParametersIF {
double refDirection[3] = {-1, 0, 0}; // Antenna
double quatRef[4] = {0, 0, 0, 1};
double refRotRate[3] = {0, 0, 0};
int8_t timeElapsedMax = 10; // rot rate calculations
uint8_t timeElapsedMax = 10; // rot rate calculations
} nadirModeControllerParameters;
struct InertialModeControllerParameters : PointingLawParameters {
@ -886,7 +887,10 @@ class AcsParameters : public HasParametersIF {
} strParameters;
struct GpsParameters {
double timeDiffVelocityMax = 30; //[s]
double timeDiffVelocityMax = 30; // [s]
double minimumFdirAltitude = 475 * 1e3; // [m]
double maximumFdirAltitude = 575 * 1e3; // [m]
double fdirAltitude = 525 * 1e3; // [m]
} gpsParameters;
struct SunModelParameters {
@ -912,16 +916,16 @@ class AcsParameters : public HasParametersIF {
double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability
} kalmanFilterParameters;
struct MagnetorquesParameter {
struct MagnetorquerParameter {
double mtq0orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}};
double mtq1orientationMatrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
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 dipolMax = 0.2; // [Am^2]
uint16_t torqueDuration = 300; // [ms]
} magnetorquesParameter;
} magnetorquerParameter;
struct DetumbleParameter {
uint8_t detumblecounter = 75; // 30 s

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

@ -10,64 +10,60 @@
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; }
ActuatorCmd::ActuatorCmd() {}
ActuatorCmd::~ActuatorCmd() {}
void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
// Scaling the commanded torque to a maximum value
double maxTrq = acsParameters.rwHandlingParameters.maxTrq;
void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) {
uint8_t maxIdx = 0;
VectorOperations<double>::maxAbsValue(rwTrq, 4, &maxIdx);
double maxValue = rwTrq[maxIdx];
double maxValue = 0;
for (int i = 0; i < 4; i++) { // size of torque, always 4 ?
if (abs(rwTrq[i]) > maxValue) {
maxValue = abs(rwTrq[i]);
}
}
if (maxValue > maxTrq) {
double scalingFactor = maxTrq / maxValue;
VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4);
if (maxValue > maxTorque) {
double scalingFactor = maxTorque / maxValue;
VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrq, 4);
}
}
void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
const int32_t speedRw2, const int32_t speedRw3,
const double *rwTorque, int32_t *rwCmdSpeed) {
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
int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
double deltaSpeed[4] = {0, 0, 0, 0};
double commandTime = acsParameters.onBoardParams.sampleTime,
inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel;
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
// W_RW = Torque_RW / I_RW * delta t [rad/s]
double factor = commandTime / inertiaWheel * radToRpm;
double factor = sampleTime / inertiaWheel * radToRpm;
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
for (int i = 0; i < 4; i++) {
deltaSpeedInt[i] = std::round(deltaSpeed[i]);
}
VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
for (uint8_t i = 0; i < 4; i++) {
if (rwCmdSpeed[i] > maxRwSpeed) {
rwCmdSpeed[i] = maxRwSpeed;
} else if (rwCmdSpeed[i] < -maxRwSpeed) {
rwCmdSpeed[i] = -maxRwSpeed;
}
}
VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
}
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) {
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(*acsParameters.magnetorquesParameter.inverseAlignment,
dipolMoment, dipolMomentActuatorDouble, 3, 3, 1);
MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3,
1);
// Scaling along largest element if dipol exceeds maximum
double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
double maxValue = 0;
for (int i = 0; i < 3; i++) {
if (abs(dipolMomentActuator[i]) > maxDipol) {
maxValue = abs(dipolMomentActuator[i]);
}
}
if (maxValue > maxDipol) {
double scalingFactor = maxDipol / maxValue;
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);
}

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

@ -1,23 +1,22 @@
#ifndef ACTUATORCMD_H_
#define ACTUATORCMD_H_
#include "AcsParameters.h"
#include "MultiplicativeKalmanFilter.h"
#include "SensorProcessing.h"
#include "SensorValues.h"
class ActuatorCmd {
public:
ActuatorCmd(AcsParameters *acsParameters_); // Input mode ?
ActuatorCmd();
virtual ~ActuatorCmd();
/*
* @brief: scalingTorqueRws() scales the torque via maximum part in case this part is
* higher then the maximum torque
* @param: rwTrq given torque for reaction wheels
* rwTrqScaled possible scaled torque
* @param: rwTrq given torque for reaction wheels which will be
* scaled if needed to be
*/
void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled);
void scalingTorqueRws(double *rwTrq, double maxTorque);
/*
* @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction
@ -28,8 +27,9 @@ class ActuatorCmd {
* rwCmdSpeed output revolutions per minute for every
* reaction wheel
*/
void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed);
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);
/*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques
@ -37,11 +37,11 @@ class ActuatorCmd {
* @param: dipolMoment given dipol moment in spacecraft frame
* dipolMomentActuator resulting dipol moment in actuator reference frame
*/
void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator);
void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
const double *inverseAlignment, double maxDipol);
protected:
private:
AcsParameters acsParameters;
};
#endif /* ACTUATORCMD_H_ */

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

@ -12,7 +12,7 @@
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {}
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
Guidance::~Guidance() {}
@ -26,9 +26,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
double targetE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
acsParameters->targetModeControllerParameters.latitudeTgt,
acsParameters->targetModeControllerParameters.longitudeTgt,
acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
// target direction in the ECEF frame
double targetDirE[3] = {0, 0, 0};
@ -57,9 +57,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
// rotation quaternion from two vectors
double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1];
refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2];
refDir[0] = acsParameters->targetModeControllerParameters.refDirection[0];
refDir[1] = acsParameters->targetModeControllerParameters.refDirection[1];
refDir[2] = acsParameters->targetModeControllerParameters.refDirection[2];
double noramlizedTargetDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 3);
@ -96,15 +96,15 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate in case of star tracker blinding
//-------------------------------------------------------------------------------------
if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
if (acsParameters->targetModeControllerParameters.avoidBlindStr) {
double sunDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1);
double exclAngle = acsParameters.strParameters.exclusionAngle,
blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop;
double exclAngle = acsParameters->strParameters.exclusionAngle,
blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart,
blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop;
double sightAngleSun =
VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);
VectorOperations<double>::dot(acsParameters->strParameters.boresightAxis, sunDirB);
if (!(strBlindAvoidFlag)) {
double critSightAngle = blindStart * exclAngle;
@ -113,7 +113,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
}
} else {
if (sightAngleSun < blindEnd * exclAngle) {
double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
double normBlindRefRate = acsParameters->targetModeControllerParameters.blindRotRate;
double blindRefRate[3] = {0, 0, 0};
if (sunDirB[1] < 0) {
blindRefRate[0] = normBlindRefRate;
@ -144,9 +144,9 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
double targetE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
acsParameters->targetModeControllerParameters.latitudeTgt,
acsParameters->targetModeControllerParameters.longitudeTgt,
acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
@ -198,7 +198,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
{xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmIX, targetQuat);
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
}
@ -211,9 +211,9 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
double groundStationE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.gsTargetModeControllerParameters.latitudeTgt,
acsParameters.gsTargetModeControllerParameters.longitudeTgt,
acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE);
acsParameters->gsTargetModeControllerParameters.latitudeTgt,
acsParameters->gsTargetModeControllerParameters.longitudeTgt,
acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationE, posSatE, targetDirE, 3);
@ -262,7 +262,7 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
{xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax;
int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
}
@ -332,9 +332,9 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub
// rotation quaternion from two vectors
double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
refDir[0] = acsParameters->nadirModeControllerParameters.refDirection[0];
refDir[1] = acsParameters->nadirModeControllerParameters.refDirection[1];
refDir[2] = acsParameters->nadirModeControllerParameters.refDirection[2];
double noramlizedTargetDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 3);
@ -406,7 +406,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl
{xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate);
}
@ -516,19 +516,19 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
if (rw1valid && rw2valid && rw3valid && rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double));
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
return returnvalue::OK;
} else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without1, 12 * sizeof(double));
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without2, 12 * sizeof(double));
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && rw2valid && !rw3valid && rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without3, 12 * sizeof(double));
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without4, 12 * sizeof(double));
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
return returnvalue::OK;
} else {
// @note: This one takes the normal pseudoInverse of all four raction wheels valid.
@ -542,15 +542,14 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
std::error_code e;
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE,
e)) { // ToDo: if file does not exist anymore
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir,
3 * sizeof(double));
} else {
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop,
3 * sizeof(double));
}
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
std::memcpy(satRateSafe, acsParameters->safeModeControllerParameters.satRateRef,
3 * sizeof(double));
}

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

@ -55,14 +55,15 @@ class Guidance {
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
private:
AcsParameters acsParameters;
const AcsParameters *acsParameters;
bool strBlindAvoidFlag = false;
timeval timeSavedQuaternion;
double savedQuaternion[4] = {0, 0, 0, 0};
double omegaRefSaved[3] = {0, 0, 0};
static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/deployment";
static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/deployment";
static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";
};
#endif /* ACS_GUIDANCE_H_ */

96
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

@ -12,33 +12,22 @@
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
/*Initialisation of values for parameters in constructor*/
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_)
: initialQuaternion{0, 0, 0, 1},
initialCovarianceMatrix{{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}} {
loadAcsParameters(acsParameters_);
}
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {}
MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE);
kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
}
ReturnValue_t MultiplicativeKalmanFilter::init(
const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, acsctrl::MekfData *mekfData) { // valids for "model measurements"?
const bool validMagModel, acsctrl::MekfData *mekfData,
AcsParameters *acsParameters) { // valids for "model measurements"?
// check for valid mag/sun
if (validMagField_ && validSS && validSSModel && validMagModel) {
validInit = true;
// QUEST ALGO -----------------------------------------------------------------------
double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
sigmaSun = kalmanFilterParameters->sensorNoiseSS;
sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
sigmaGyro = kalmanFilterParameters->sensorNoiseGYR;
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS;
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG;
sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGYR;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0};
@ -192,21 +181,18 @@ ReturnValue_t MultiplicativeKalmanFilter::init(
return MEKF_INITIALIZED;
} else {
// no initialisation possible, no valid measurements
validInit = false;
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
return MEKF_UNINITIALIZED;
}
}
// --------------- MEKF DISCRETE TIME STEP -------------------------------
ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, const bool validSTR_,
const double *rateGYRs_, const bool validGYRs_,
const double *magneticField_,
const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ,
const bool validSSModel, const double *magFieldJ,
const bool validMagModel, double sampleTime,
acsctrl::MekfData *mekfData) {
ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
const bool validGYRs_, const double *magneticField_, const bool validMagField_,
const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
AcsParameters *acsParameters) {
// Check for GYR Measurements
int MDF = 0; // Matrix Dimension Factor
if (!validGYRs_) {
@ -248,9 +234,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
// If we are here, MEKF will perform
double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0;
sigmaSun = kalmanFilterParameters->sensorNoiseSS;
sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
sigmaStr = kalmanFilterParameters->sensorNoiseSTR;
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS;
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG;
sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseSTR;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0};
@ -912,8 +898,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
biasGYR[2] = updatedGyroBias[2];
/* ----------- PROPAGATION ----------*/
double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseBsGYR;
double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseArwGYR;
double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
@ -931,27 +917,31 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ12trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
if (normRotEst * sampleTime < M_PI / 10) {
double fact11 = pow(sigmaV, 2) * sampleTime + 1. / 3. * pow(sigmaU, 2) * pow(sampleTime, 3);
if (normRotEst * acsParameters->onBoardParams.sampleTime < M_PI / 10) {
double fact11 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime +
1. / 3. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11, *covQ11, 3, 3);
double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(sampleTime, 2));
double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 2));
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3);
std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double));
double fact22 = pow(sigmaU, 2) * sampleTime;
double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
} else {
double fact22 = pow(sigmaU, 2) * sampleTime;
double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3];
double fact12_0 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
double fact12_0 =
(normRotEst * acsParameters->onBoardParams.sampleTime -
sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3);
double fact12_1 = 1. / 2. * pow(sampleTime, 2);
double fact12_1 = 1. / 2. * pow(acsParameters->onBoardParams.sampleTime, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3);
double fact12_2 =
(1. / 2. * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) /
(1. / 2. * pow(normRotEst, 2) * pow(acsParameters->onBoardParams.sampleTime, 2) +
cos(normRotEst * acsParameters->onBoardParams.sampleTime) - 1) /
pow(normRotEst, 4);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3);
@ -961,13 +951,15 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
MatrixOperations<double>::transpose(*covQ12, *covQ12trans, 3);
double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3];
double fact11_0 = pow(sigmaV, 2) * sampleTime;
double fact11_0 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3);
double fact11_1 = 1. / 3. * pow(sampleTime, 3);
double fact11_1 = 1. / 3. * pow(acsParameters->onBoardParams.sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3);
double fact11_2 = (2 * normRotEst * sampleTime - 2 * sin(normRotEst * sampleTime) -
1. / 3. * pow(normRotEst, 3) * pow(sampleTime, 3)) /
pow(normRotEst, 5);
double fact11_2 =
(2 * normRotEst * acsParameters->onBoardParams.sampleTime -
2 * sin(normRotEst * acsParameters->onBoardParams.sampleTime) -
1. / 3. * pow(normRotEst, 3) * pow(acsParameters->onBoardParams.sampleTime, 3)) /
pow(normRotEst, 5);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3);
MatrixOperations<double>::subtract(*covQ11_1, *covQ11_2, *covQ11_12, 3, 3);
@ -1017,9 +1009,10 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3];
double fact11_1 = sin(normRotEst * sampleTime) / normRotEst;
double fact11_1 = sin(normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst;
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3);
double fact11_2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2);
double fact11_2 =
(1 - cos(normRotEst * acsParameters->onBoardParams.sampleTime)) / pow(normRotEst, 2);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3);
MatrixOperations<double>::subtract(*identityMatrix3, *phi11_1, *phi11_01, 3, 3);
@ -1028,8 +1021,11 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
double phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3];
double fact12_0 = fact11_2;
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi12_1, 3, 3);
double fact12_2 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiplyScalar(*identityMatrix3,
acsParameters->onBoardParams.sampleTime, *phi12_1, 3, 3);
double fact12_2 =
(normRotEst * acsParameters->onBoardParams.sampleTime -
sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3);
MatrixOperations<double>::subtract(*phi12_0, *phi12_1, *phi12_01, 3, 3);
@ -1056,8 +1052,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
// Propagated Quaternion
double rotSin[3] = {0, 0, 0}, rotCosMat[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double rotCos = cos(0.5 * normRotEst * sampleTime);
double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst;
double rotCos = cos(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime);
double sinFac = sin(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst;
VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};

22
mission/controller/acs/MultiplicativeKalmanFilter.h
View File

@ -17,9 +17,8 @@ class MultiplicativeKalmanFilter {
*/
public:
/* @brief: Constructor
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
MultiplicativeKalmanFilter();
virtual ~MultiplicativeKalmanFilter();
ReturnValue_t reset(acsctrl::MekfData *mekfData);
@ -33,8 +32,8 @@ class MultiplicativeKalmanFilter {
*/
ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel,
acsctrl::MekfData *mekfData);
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
AcsParameters *acsParameters);
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
* for the current step after the initalization
@ -54,7 +53,8 @@ class MultiplicativeKalmanFilter {
const bool validGYRs_, const double *magneticField_,
const bool validMagField_, const double *sunDir_, const bool validSS,
const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData);
const bool validMagModel, acsctrl::MekfData *mekfData,
AcsParameters *acsParameters);
enum MekfStatus : uint8_t {
UNINITIALIZED = 0,
@ -79,23 +79,21 @@ class MultiplicativeKalmanFilter {
private:
/*Parameters*/
AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
double quaternion_STR_SB[4];
bool validInit;
/*States*/
double initialQuaternion[4]; /*after reset?QUEST*/
double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/
double initialQuaternion[4] = {0, 0, 0, 1}; /*after reset?QUEST*/
double initialCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
uint8_t sensorsAvail;
uint8_t sensorsAvail = 0;
/*Outputs*/
double quatBJ[4]; /* Output Quaternion */
double biasGYR[3]; /*Between measured and estimated sat Rate*/
/*Parameter INIT*/
/*Functions*/
void loadAcsParameters(AcsParameters *acsParameters_);
void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
double satRotRate[3]);

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

@ -8,9 +8,7 @@
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilter(acsParameters_) {
acsParameters = *acsParameters_;
}
Navigation::Navigation() {}
Navigation::~Navigation() {}
@ -18,7 +16,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MekfData *mekfData) {
acsctrl::MekfData *mekfData, AcsParameters *acsParameters) {
double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
bool quatIBValid = sensorValues->strSet.caliQx.isValid() &&
@ -30,7 +28,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData,
acsParameters);
return mekfStatus;
} else {
mekfStatus = multiplicativeKalmanFilter.mekfEst(
@ -39,7 +38,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData);
mgmDataProcessed->magIgrfModel.isValid(), mekfData, acsParameters);
return mekfStatus;
}
}

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

@ -9,19 +9,19 @@
class Navigation {
public:
Navigation(AcsParameters *acsParameters_);
Navigation();
virtual ~Navigation();
ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData);
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
AcsParameters *acsParameters);
void resetMekf(acsctrl::MekfData *mekfData);
protected:
private:
MultiplicativeKalmanFilter multiplicativeKalmanFilter;
AcsParameters acsParameters;
ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED;
};

20
mission/controller/acs/SensorProcessing.cpp
View File

@ -14,7 +14,7 @@
using namespace Math;
SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) {}
SensorProcessing::SensorProcessing() {}
SensorProcessing::~SensorProcessing() {}
@ -26,7 +26,8 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
acsctrl::GpsDataProcessed *gpsDataProcessed,
const double gpsAltitude, bool gpsValid,
acsctrl::MgmDataProcessed *mgmDataProcessed) {
// ---------------- IGRF- 13 Implementation here ------------------------------------------------
// ---------------- IGRF- 13 Implementation here
// ------------------------------------------------
double magIgrfModel[3] = {0.0, 0.0, 0.0};
if (gpsValid) {
// Should be existing class object which will be called and modified here.
@ -549,8 +550,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
const bool validGps,
const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed) {
// name to convert not process
double gdLongitude = 0, gcLatitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0};
double gdLongitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
gpsVelocityE[3] = {0, 0, 0};
if (validGps) {
// Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
gdLongitude = gpsLongitude * PI / 180.;
@ -559,9 +560,17 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
double factor = 1 - pow(eccentricityWgs84, 2);
gcLatitude = atan(factor * tan(latitudeRad));
// Altitude FDIR
if (gpsAltitude > gpsParameters->maximumFdirAltitude ||
gpsAltitude < gpsParameters->maximumFdirAltitude) {
altitude = gpsParameters->fdirAltitude;
} else {
altitude = gpsAltitude;
}
// Calculation of the satellite velocity in earth fixed frame
double deltaDistance[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE);
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
if (validSavedPosSatE &&
(gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
@ -580,6 +589,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->gdLongitude.value = gdLongitude;
gpsDataProcessed->gcLatitude.value = gcLatitude;
gpsDataProcessed->altitude.value = altitude;
std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
gpsDataProcessed->setValidity(validGps, true);

3
mission/controller/acs/SensorProcessing.h
View File

@ -15,7 +15,7 @@ class SensorProcessing {
public:
void reset();
SensorProcessing(AcsParameters *acsParameters_);
SensorProcessing();
virtual ~SensorProcessing();
void process(timeval now, ACS::SensorValues *sensorValues,
@ -77,7 +77,6 @@ class SensorProcessing {
bool validSavedPosSatE = false;
SusConverter susConverter;
AcsParameters acsParameters;
};
#endif /*SENSORPROCESSING_H_*/

11
mission/controller/acs/SensorValues.h
View File

@ -1,17 +1,16 @@
#ifndef SENSORVALUES_H_
#define SENSORVALUES_H_
#include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h>
#include <fsfw_hal/devicehandlers/MgmRM3100Handler.h>
#include <linux/devices/devicedefinitions/StarTrackerDefinitions.h>
#include <mission/devices/devicedefinitions/GPSDefinitions.h>
#include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
#include <mission/devices/devicedefinitions/imtqHelpers.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
#include <mission/devices/devicedefinitions/susMax1227Helpers.h>
#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
#include "mission/devices/devicedefinitions/GPSDefinitions.h"
namespace ACS {
class SensorValues {

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

@ -9,39 +9,32 @@
#include "../util/MathOperations.h"
Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
Detumble::Detumble() {}
Detumble::~Detumble() {}
void Detumble::loadAcsParameters(AcsParameters *acsParameters_) {
detumbleParameter = &(acsParameters_->detumbleParameter);
magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
}
ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
const double *magField, const bool magFieldValid, double *magMom) {
const double *magField, const bool magFieldValid, double *magMom,
double gain) {
if (!magRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
// change unit from uT to T
double magFieldT[3] = {0, 0, 0}, magRateT[3] = {0, 0, 0};
// convert uT to T
double magFieldT[3], magRateT[3];
VectorOperations<double>::mulScalar(magField, 1e-6, magFieldT, 3);
VectorOperations<double>::mulScalar(magRate, 1e-6, magRateT, 3);
double gain = detumbleParameter->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
// control law
double factor = -gain / pow(VectorOperations<double>::norm(magFieldT, 3), 2);
VectorOperations<double>::mulScalar(magRateT, factor, magMom, 3);
return returnvalue::OK;
}
ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid,
double *magMom) {
ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, double *magMom,
double dipolMax) {
if (!magRateValid) {
return DETUMBLE_NO_SENSORDATA;
}
double dipolMax = magnetorquesParameter->DipolMax;
for (int i = 0; i < 3; i++) {
magMom[i] = -dipolMax * sign(magRate[i]);
}
@ -49,14 +42,20 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal
return returnvalue::OK;
}
ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid,
ReturnValue_t Detumble::bDotLawFull(const double *satRate, const bool *satRateValid,
const double *magField, const bool *magFieldValid,
double *magMom) {
double *magMom, double gain) {
if (!satRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
double gain = detumbleParameter->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
VectorOperations<double>::mulScalar(satRate, factor, magMom, 3);
// convert uT to T
double magFieldT[3];
VectorOperations<double>::mulScalar(magField, 1e-6, magFieldT, 3);
// control law
double factor = gain / pow(VectorOperations<double>::norm(magField, 3), 2);
double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldT, magFieldNormed, 3);
VectorOperations<double>::cross(satRate, magFieldNormed, crossProduct);
VectorOperations<double>::mulScalar(crossProduct, factor, magMom, 3);
return returnvalue::OK;
}

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

@ -12,30 +12,22 @@
class Detumble {
public:
Detumble(AcsParameters *acsParameters_);
Detumble();
virtual ~Detumble();
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE;
static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
/* @brief: Load AcsParameters for this class
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void loadAcsParameters(AcsParameters *acsParameters_);
ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, const double *magField,
const bool magFieldValid, double *magMom);
const bool magFieldValid, double *magMom, double gain);
ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom,
double dipolMax);
ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom);
ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField,
const bool *magFieldValid, double *magMom);
ReturnValue_t bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField,
const bool *magFieldValid, double *magMom, double gain);
private:
AcsParameters::DetumbleParameter *detumbleParameter;
AcsParameters::MagnetorquesParameter *magnetorquesParameter;
};
#endif /*ACS_CONTROL_DETUMBLE_H_*/

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

@ -1,10 +1,3 @@
/*
* PtgCtrl.cpp
*
* Created on: 17 Jul 2022
* Author: Robin Marquardt
*/
#include "PtgCtrl.h"
#include <fsfw/globalfunctions/constants.h>
@ -16,16 +9,10 @@
#include "../util/MathOperations.h"
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
PtgCtrl::~PtgCtrl() {}
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices = &(acsParameters_->rwMatrices);
}
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
double *torqueRws) {
@ -62,8 +49,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
gainMatrixDiagonal[0][0] = gainVector[0];
gainMatrixDiagonal[1][1] = gainVector[1];
gainMatrixDiagonal[2][2] = gainVector[2];
MatrixOperations<double>::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix),
*gainMatrix, 3, 3, 3);
MatrixOperations<double>::multiply(
*gainMatrixDiagonal, *(acsParameters->inertiaEIVE.inertiaMatrix), *gainMatrix, 3, 3, 3);
// Inverse of gainMatrix
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -72,8 +59,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3,
3, 3);
MatrixOperations<double>::multiply(
*gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrix), *pMatrix, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
//------------------------------------------------------------------------------------------------
@ -91,18 +78,19 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
pErrorSign[i] = pError[i];
}
}
// Torque for quaternion error
// torque for quaternion error
double torqueQuat[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);
// Torque for rate error
// torque for rate error
double torqueRate[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1);
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), deltaRate,
torqueRate, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
// Final commanded Torque for every reaction wheel
// final commanded Torque for every reaction wheel
double torque[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
@ -120,20 +108,22 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
return;
}
// calculating momentum of satellite and momentum of reaction wheels
// 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, rwHandlingParameters->inertiaWheel, momentumRwU, 4);
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4,
1);
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(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1);
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), satRate,
momentumSat, 3, 3, 1);
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
// calculating momentum error
// calculating momentum error
double deltaMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
deltaMomentum, 3);
// resulting magnetic dipole command
// 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;
@ -147,53 +137,50 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara
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
// 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, rwHandlingParameters->inertiaWheel,
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(rwMatrices->nullspace, rwMatrices->nullspace,
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);
}
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand) {
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) {
bool rwAvailable[4] = {
(sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()),
(sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()),
(sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()),
(sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid())};
int32_t omegaRw[4] = {sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value,
sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value};
int32_t currRwSpeed[4] = {
sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value,
sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value};
for (uint8_t i = 0; i < 4; i++) {
if (rwAvailable[i]) {
if (torqueMemory[i] != 0) {
if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) {
torqueMemory[i] = 0;
} else {
torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
}
} else {
if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) &&
(omegaRw[i] > -rwHandlingParameters->stictionSpeed)) {
if (omegaRw[i] < omegaMemory[i]) {
torqueMemory[i] = -1;
} else {
torqueMemory[i] = 1;
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) {
rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
} else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) {
rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
}
torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
}
}
} else {
torqueMemory[i] = 0;
}
omegaMemory[i] = omegaRw[i];
}
}

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

@ -1,16 +1,3 @@
/*
* PtgCtrl.h
*
* Created on: 17 Jul 2022
* Author: Robin Marquardt
*
* @brief: This class handles the pointing control mechanism. Calculation of an commanded
* torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation
*
* @note: A description of the used algorithms can be found in
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110
*/
#ifndef PTGCTRL_H_
#define PTGCTRL_H_
@ -23,6 +10,13 @@
#include "eive/resultClassIds.h"
class PtgCtrl {
/*
* @brief: This class handles the pointing control mechanism. Calculation of an commanded
* torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation
*
* @note: A description of the used algorithms can be found in
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110
*/
public:
/* @brief: Constructor
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
@ -33,13 +27,7 @@ class PtgCtrl {
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_PTG;
static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
/* @brief: Load AcsParameters for this class
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void loadAcsParameters(AcsParameters *acsParameters_);
/* @brief: Calculates the needed torque for the pointing control mechanism
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
@ -54,18 +42,12 @@ class PtgCtrl {
const int32_t *speedRw3, double *rwTrqNs);
/* @brief: Commands the stiction torque in case wheel speed is to low
* @param: sensorValues class containing all RW related values
* torqueCommand modified torque after antistiction
*/
void rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand);
void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed);
private:
AcsParameters::RwHandlingParameters *rwHandlingParameters;
AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::RwMatrices *rwMatrices;
double torqueMemory[4] = {0, 0, 0, 0};
double omegaMemory[4] = {0, 0, 0, 0};
const AcsParameters *acsParameters;
};
#endif /* ACS_CONTROL_PTGCTRL_H_ */

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

@ -1,10 +1,3 @@
/*
* SafeCtrl.cpp
*
* Created on: 19 Apr 2022
* Author: Robin Marquardt
*/
#include "SafeCtrl.h"
#include <fsfw/globalfunctions/constants.h>
@ -15,19 +8,10 @@
#include "../util/MathOperations.h"
SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) {
loadAcsParameters(acsParameters_);
MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3,
3);
}
SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
SafeCtrl::~SafeCtrl() {}
void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
}
ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
double *magFieldModel, bool magFieldModelValid,
double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
@ -37,8 +21,8 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
return SAFECTRL_MEKF_INPUT_INVALID;
}
double kRate = safeModeControllerParameters->k_rate_mekf;
double kAlign = safeModeControllerParameters->k_align_mekf;
double kRate = acsParameters->safeModeControllerParameters.k_rate_mekf;
double kAlign = acsParameters->safeModeControllerParameters.k_align_mekf;
// Calc sunDirB ,magFieldB with mekf output and model
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -71,6 +55,9 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
// Adding factor of inertia for axes
MatrixOperations<double>::multiplyScalar(*(acsParameters->inertiaEIVE.inertiaMatrix), 10,
*gainMatrixInertia, 3,
3); // why only for mekf one and not for no mekf
MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
// MagMom B (orthogonal torque)
@ -126,7 +113,7 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
/* Only valid if angle between sun direction and magnetic field direction
* is sufficiently large */
double angleSunMag = acos(cosAngleSunMag);
if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) {
if (angleSunMag < acsParameters->safeModeControllerParameters.sunMagAngleMin) {
return returnvalue::FAILED;
}
@ -135,8 +122,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
// Torque Align calculation
double kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
double kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
double kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf;
double kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf;
double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
double crossSusSunRef[3] = {0, 0, 0};
@ -155,8 +142,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
// Final torque
double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3,
1);
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), torqueAlignRate,
torqueB, 3, 3, 1);
// Magnetic moment
double magMomB[3] = {0, 0, 0};

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

@ -17,8 +17,6 @@ class SafeCtrl {
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE;
static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
void loadAcsParameters(AcsParameters *acsParameters_);
ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel,
bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
double *satRateMekf, bool rateMekfValid, double *sunDirRef,
@ -32,8 +30,7 @@ class SafeCtrl {
protected:
private:
AcsParameters::SafeModeControllerParameters *safeModeControllerParameters;
AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters *acsParameters;
double gainMatrixInertia[3][3];
double magFieldBState[3];

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

@ -86,6 +86,7 @@ enum PoolIds : lp_id_t {
// GPS Processed
GC_LATITUDE,
GD_LONGITUDE,
ALTITUDE,
GPS_POSITION,
GPS_VELOCITY,
// MEKF
@ -109,7 +110,7 @@ static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12;
static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t MEKF_SET_ENTRIES = 3;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
@ -228,6 +229,7 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
lp_var_t<double> gcLatitude = lp_var_t<double>(sid.objectId, GC_LATITUDE, this);
lp_var_t<double> gdLongitude = lp_var_t<double>(sid.objectId, GD_LONGITUDE, this);
lp_var_t<double> altitude = lp_var_t<double>(sid.objectId, ALTITUDE, this);
lp_vec_t<double, 3> gpsPosition = lp_vec_t<double, 3>(sid.objectId, GPS_POSITION, this);
lp_vec_t<double, 3> gpsVelocity = lp_vec_t<double, 3>(sid.objectId, GPS_VELOCITY, this);