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

Merge remote-tracking branch 'origin/develop' into refactor_tm_handling
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller 2023-03-10 19:35:45 +01:00
commit 8c450596f6
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
23 changed files with 474 additions and 485 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
CHANGELOG.md
View File

@ -16,10 +16,35 @@ will consitute of a breaking change warranting a new major release:
# [unreleased] # [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 ## Fixed
- `PAPB_EMPTY_SIGNAL_VC1` GPIO was not set up properly. - `PAPB_EMPTY_SIGNAL_VC1` GPIO was not set up properly.
- Fix for heater names: HPA heater (index 7) is now the Syrlinks heater. - 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 ## 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 - 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 - Remove periodic SD card check. The file system is not mounted read-only anymore when using an
ext4 filesystem 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 - ACS CTRL transition to DETUBMLE is now done in CTRL internally. No
system level handling necessary anymore. system level handling necessary anymore.
- More fixes and improvements for SD card handling. Extend SD card setup in core controller to - 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) AcsController::AcsController(object_id_t objectId)
: ExtendedControllerBase(objectId), : ExtendedControllerBase(objectId),
sensorProcessing(&acsParameters),
navigation(&acsParameters),
actuatorCmd(&acsParameters),
guidance(&acsParameters), guidance(&acsParameters),
safeCtrl(&acsParameters), safeCtrl(&acsParameters),
detumble(&acsParameters),
ptgCtrl(&acsParameters), ptgCtrl(&acsParameters),
parameterHelper(this), parameterHelper(this),
mgmDataRaw(this), mgmDataRaw(this),
@ -146,7 +142,7 @@ void AcsController::performSafe() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters); &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData); &susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) { 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 // 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 // detumble check and switch
if (mekfData.satRotRateMekf.isValid() && if (mekfData.satRotRateMekf.isValid() &&
@ -191,8 +189,8 @@ void AcsController::performSafe() {
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) > VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) { acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++; detumbleCounter++;
} else { } else if (detumbleCounter > 0) {
detumbleCounter = 0; detumbleCounter -= 1;
} }
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
detumbleCounter = 0; detumbleCounter = 0;
@ -203,7 +201,7 @@ void AcsController::performSafe() {
updateCtrlValData(errAng); updateCtrlValData(errAng);
updateActuatorCmdData(cmdDipolMtqs); 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.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime); // acsParameters.rwHandlingParameters.rampTime);
} }
@ -215,7 +213,7 @@ void AcsController::performDetumble() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters); &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData); &susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) { if (not mekfInvalidFlag) {
@ -228,8 +226,11 @@ void AcsController::performDetumble() {
double magMomMtq[3] = {0, 0, 0}; double magMomMtq[3] = {0, 0, 0};
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); mgmDataProcessed.mgmVecTot.isValid(), magMomMtq,
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); acsParameters.detumbleParameter.gainD);
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipolMax);
if (mekfData.satRotRateMekf.isValid() && if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) < VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@ -239,8 +240,8 @@ void AcsController::performDetumble() {
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) < VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) { acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++; detumbleCounter++;
} else { } else if (detumbleCounter > 0) {
detumbleCounter = 0; detumbleCounter -= 1;
} }
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
detumbleCounter = 0; detumbleCounter = 0;
@ -263,7 +264,7 @@ void AcsController::performPointingCtrl() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters); &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData); &susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) { if (not mekfInvalidFlag) {
@ -296,24 +297,26 @@ void AcsController::performPointingCtrl() {
} else { } else {
multipleRwUnavailableCounter = 0; 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 // Variables required for guidance
double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1}, double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
errorAngle = 0, errorSatRotRate[3] = {0, 0, 0}; 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) { switch (submode) {
case acs::PTG_IDLE: case acs::PTG_IDLE:
guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(
&acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -337,7 +340,7 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -351,20 +354,20 @@ void AcsController::performPointingCtrl() {
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
break; break;
case acs::PTG_NADIR: case acs::PTG_NADIR:
@ -382,7 +385,7 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -405,7 +408,7 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -415,18 +418,21 @@ void AcsController::performPointingCtrl() {
break; 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) { if (enableAntiStiction) {
ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled); ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
} }
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs,
actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value, *acsParameters.magnetorquerParameter.inverseAlignment,
sensorValues.rw2Set.currSpeed.value, acsParameters.magnetorquerParameter.dipolMax);
sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs); updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs);
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
// cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
@ -584,6 +590,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
// GPS Processed // GPS Processed
localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); 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_POSITION, &gpsPosition);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); 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; acsctrl::GpsDataProcessed gpsDataProcessed;
PoolEntry<double> gcLatitude = PoolEntry<double>(); PoolEntry<double> gcLatitude = PoolEntry<double>();
PoolEntry<double> gdLongitude = PoolEntry<double>(); PoolEntry<double> gdLongitude = PoolEntry<double>();
PoolEntry<double> altitude = PoolEntry<double>();
PoolEntry<double> gpsPosition = PoolEntry<double>(3); PoolEntry<double> gpsPosition = PoolEntry<double>(3);
PoolEntry<double> gpsVelocity = 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 case 0x2: // InertiaEIVE
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(inertiaEIVE.inertiaMatrix); parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrix);
break; break;
case 0x1: case 0x1:
parameterWrapper->set(inertiaEIVE.inertiaMatrixDeployed); parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixDeployed);
break; break;
case 0x2: case 0x2:
parameterWrapper->set(inertiaEIVE.inertiaMatrixUndeployed); parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixUndeployed);
break; break;
case 0x3: case 0x3:
parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel1); parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel1);
break; break;
case 0x4: case 0x4:
parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel3); parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel3);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
@ -51,58 +51,58 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x3: // MgmHandlingParameters case 0x3: // MgmHandlingParameters
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix); parameterWrapper->setMatrix(mgmHandlingParameters.mgm0orientationMatrix);
break; break;
case 0x1: case 0x1:
parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix); parameterWrapper->setMatrix(mgmHandlingParameters.mgm1orientationMatrix);
break; break;
case 0x2: case 0x2:
parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix); parameterWrapper->setMatrix(mgmHandlingParameters.mgm2orientationMatrix);
break; break;
case 0x3: case 0x3:
parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix); parameterWrapper->setMatrix(mgmHandlingParameters.mgm3orientationMatrix);
break; break;
case 0x4: case 0x4:
parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix); parameterWrapper->setMatrix(mgmHandlingParameters.mgm4orientationMatrix);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset); parameterWrapper->setVector(mgmHandlingParameters.mgm0hardIronOffset);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset); parameterWrapper->setVector(mgmHandlingParameters.mgm1hardIronOffset);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset); parameterWrapper->setVector(mgmHandlingParameters.mgm2hardIronOffset);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset); parameterWrapper->setVector(mgmHandlingParameters.mgm3hardIronOffset);
break; break;
case 0x9: case 0x9:
parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset); parameterWrapper->setVector(mgmHandlingParameters.mgm4hardIronOffset);
break; break;
case 0xA: case 0xA:
parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse); parameterWrapper->setMatrix(mgmHandlingParameters.mgm0softIronInverse);
break; break;
case 0xB: case 0xB:
parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse); parameterWrapper->setMatrix(mgmHandlingParameters.mgm1softIronInverse);
break; break;
case 0xC: case 0xC:
parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse); parameterWrapper->setMatrix(mgmHandlingParameters.mgm2softIronInverse);
break; break;
case 0xD: case 0xD:
parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse); parameterWrapper->setMatrix(mgmHandlingParameters.mgm3softIronInverse);
break; break;
case 0xE: case 0xE:
parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse); parameterWrapper->setMatrix(mgmHandlingParameters.mgm4softIronInverse);
break; break;
case 0xF: case 0xF:
parameterWrapper->set(mgmHandlingParameters.mgm02variance); parameterWrapper->setVector(mgmHandlingParameters.mgm02variance);
break; break;
case 0x10: case 0x10:
parameterWrapper->set(mgmHandlingParameters.mgm13variance); parameterWrapper->setVector(mgmHandlingParameters.mgm13variance);
break; break;
case 0x11: case 0x11:
parameterWrapper->set(mgmHandlingParameters.mgm4variance); parameterWrapper->setVector(mgmHandlingParameters.mgm4variance);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
@ -111,112 +111,112 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x4: // SusHandlingParameters case 0x4: // SusHandlingParameters
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(susHandlingParameters.sus0orientationMatrix); parameterWrapper->setMatrix(susHandlingParameters.sus0orientationMatrix);
break; break;
case 0x1: case 0x1:
parameterWrapper->set(susHandlingParameters.sus1orientationMatrix); parameterWrapper->setMatrix(susHandlingParameters.sus1orientationMatrix);
break; break;
case 0x2: case 0x2:
parameterWrapper->set(susHandlingParameters.sus2orientationMatrix); parameterWrapper->setMatrix(susHandlingParameters.sus2orientationMatrix);
break; break;
case 0x3: case 0x3:
parameterWrapper->set(susHandlingParameters.sus3orientationMatrix); parameterWrapper->setMatrix(susHandlingParameters.sus3orientationMatrix);
break; break;
case 0x4: case 0x4:
parameterWrapper->set(susHandlingParameters.sus4orientationMatrix); parameterWrapper->setMatrix(susHandlingParameters.sus4orientationMatrix);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(susHandlingParameters.sus5orientationMatrix); parameterWrapper->setMatrix(susHandlingParameters.sus5orientationMatrix);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(susHandlingParameters.sus6orientationMatrix); parameterWrapper->setMatrix(susHandlingParameters.sus6orientationMatrix);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(susHandlingParameters.sus7orientationMatrix); parameterWrapper->setMatrix(susHandlingParameters.sus7orientationMatrix);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(susHandlingParameters.sus8orientationMatrix); parameterWrapper->setMatrix(susHandlingParameters.sus8orientationMatrix);
break; break;
case 0x9: case 0x9:
parameterWrapper->set(susHandlingParameters.sus9orientationMatrix); parameterWrapper->setMatrix(susHandlingParameters.sus9orientationMatrix);
break; break;
case 0xA: case 0xA:
parameterWrapper->set(susHandlingParameters.sus10orientationMatrix); parameterWrapper->setMatrix(susHandlingParameters.sus10orientationMatrix);
break; break;
case 0xB: case 0xB:
parameterWrapper->set(susHandlingParameters.sus11orientationMatrix); parameterWrapper->setMatrix(susHandlingParameters.sus11orientationMatrix);
break; break;
case 0xC: case 0xC:
parameterWrapper->set(susHandlingParameters.sus0coeffAlpha); parameterWrapper->setMatrix(susHandlingParameters.sus0coeffAlpha);
break; break;
case 0xD: case 0xD:
parameterWrapper->set(susHandlingParameters.sus0coeffBeta); parameterWrapper->setMatrix(susHandlingParameters.sus0coeffBeta);
break; break;
case 0xE: case 0xE:
parameterWrapper->set(susHandlingParameters.sus1coeffAlpha); parameterWrapper->setMatrix(susHandlingParameters.sus1coeffAlpha);
break; break;
case 0xF: case 0xF:
parameterWrapper->set(susHandlingParameters.sus1coeffBeta); parameterWrapper->setMatrix(susHandlingParameters.sus1coeffBeta);
break; break;
case 0x10: case 0x10:
parameterWrapper->set(susHandlingParameters.sus2coeffAlpha); parameterWrapper->setMatrix(susHandlingParameters.sus2coeffAlpha);
break; break;
case 0x11: case 0x11:
parameterWrapper->set(susHandlingParameters.sus2coeffBeta); parameterWrapper->setMatrix(susHandlingParameters.sus2coeffBeta);
break; break;
case 0x12: case 0x12:
parameterWrapper->set(susHandlingParameters.sus3coeffAlpha); parameterWrapper->setMatrix(susHandlingParameters.sus3coeffAlpha);
break; break;
case 0x13: case 0x13:
parameterWrapper->set(susHandlingParameters.sus3coeffBeta); parameterWrapper->setMatrix(susHandlingParameters.sus3coeffBeta);
break; break;
case 0x14: case 0x14:
parameterWrapper->set(susHandlingParameters.sus4coeffAlpha); parameterWrapper->setMatrix(susHandlingParameters.sus4coeffAlpha);
break; break;
case 0x15: case 0x15:
parameterWrapper->set(susHandlingParameters.sus4coeffBeta); parameterWrapper->setMatrix(susHandlingParameters.sus4coeffBeta);
break; break;
case 0x16: case 0x16:
parameterWrapper->set(susHandlingParameters.sus5coeffAlpha); parameterWrapper->setMatrix(susHandlingParameters.sus5coeffAlpha);
break; break;
case 0x17: case 0x17:
parameterWrapper->set(susHandlingParameters.sus5coeffBeta); parameterWrapper->setMatrix(susHandlingParameters.sus5coeffBeta);
break; break;
case 0x18: case 0x18:
parameterWrapper->set(susHandlingParameters.sus6coeffAlpha); parameterWrapper->setMatrix(susHandlingParameters.sus6coeffAlpha);
break; break;
case 0x19: case 0x19:
parameterWrapper->set(susHandlingParameters.sus6coeffBeta); parameterWrapper->setMatrix(susHandlingParameters.sus6coeffBeta);
break; break;
case 0x1A: case 0x1A:
parameterWrapper->set(susHandlingParameters.sus7coeffAlpha); parameterWrapper->setMatrix(susHandlingParameters.sus7coeffAlpha);
break; break;
case 0x1B: case 0x1B:
parameterWrapper->set(susHandlingParameters.sus7coeffBeta); parameterWrapper->setMatrix(susHandlingParameters.sus7coeffBeta);
break; break;
case 0x1C: case 0x1C:
parameterWrapper->set(susHandlingParameters.sus8coeffAlpha); parameterWrapper->setMatrix(susHandlingParameters.sus8coeffAlpha);
break; break;
case 0x1D: case 0x1D:
parameterWrapper->set(susHandlingParameters.sus8coeffBeta); parameterWrapper->setMatrix(susHandlingParameters.sus8coeffBeta);
break; break;
case 0x1E: case 0x1E:
parameterWrapper->set(susHandlingParameters.sus9coeffAlpha); parameterWrapper->setMatrix(susHandlingParameters.sus9coeffAlpha);
break; break;
case 0x1F: case 0x1F:
parameterWrapper->set(susHandlingParameters.sus9coeffBeta); parameterWrapper->setMatrix(susHandlingParameters.sus9coeffBeta);
break; break;
case 0x20: case 0x20:
parameterWrapper->set(susHandlingParameters.sus10coeffAlpha); parameterWrapper->setMatrix(susHandlingParameters.sus10coeffAlpha);
break; break;
case 0x21: case 0x21:
parameterWrapper->set(susHandlingParameters.sus10coeffBeta); parameterWrapper->setMatrix(susHandlingParameters.sus10coeffBeta);
break; break;
case 0x22: case 0x22:
parameterWrapper->set(susHandlingParameters.sus11coeffAlpha); parameterWrapper->setMatrix(susHandlingParameters.sus11coeffAlpha);
break; break;
case 0x23: case 0x23:
parameterWrapper->set(susHandlingParameters.sus11coeffBeta); parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
@ -225,34 +225,34 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0x5): // GyrHandlingParameters case (0x5): // GyrHandlingParameters
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix); parameterWrapper->setMatrix(gyrHandlingParameters.gyr0orientationMatrix);
break; break;
case 0x1: case 0x1:
parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix); parameterWrapper->setMatrix(gyrHandlingParameters.gyr1orientationMatrix);
break; break;
case 0x2: case 0x2:
parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix); parameterWrapper->setMatrix(gyrHandlingParameters.gyr2orientationMatrix);
break; break;
case 0x3: case 0x3:
parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix); parameterWrapper->setMatrix(gyrHandlingParameters.gyr3orientationMatrix);
break; break;
case 0x4: case 0x4:
parameterWrapper->set(gyrHandlingParameters.gyr0bias); parameterWrapper->setVector(gyrHandlingParameters.gyr0bias);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(gyrHandlingParameters.gyr1bias); parameterWrapper->setVector(gyrHandlingParameters.gyr1bias);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(gyrHandlingParameters.gyr2bias); parameterWrapper->setVector(gyrHandlingParameters.gyr2bias);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(gyrHandlingParameters.gyr3bias); parameterWrapper->setVector(gyrHandlingParameters.gyr3bias);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(gyrHandlingParameters.gyr02variance); parameterWrapper->setVector(gyrHandlingParameters.gyr02variance);
break; break;
case 0x9: case 0x9:
parameterWrapper->set(gyrHandlingParameters.gyr13variance); parameterWrapper->setVector(gyrHandlingParameters.gyr13variance);
break; break;
case 0xA: case 0xA:
parameterWrapper->set(gyrHandlingParameters.preferAdis); parameterWrapper->set(gyrHandlingParameters.preferAdis);
@ -270,15 +270,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(rwHandlingParameters.maxTrq); parameterWrapper->set(rwHandlingParameters.maxTrq);
break; break;
case 0x2: case 0x2:
parameterWrapper->set(rwHandlingParameters.stictionSpeed); parameterWrapper->set(rwHandlingParameters.maxRwSpeed);
break; break;
case 0x3: case 0x3:
parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed); parameterWrapper->set(rwHandlingParameters.stictionSpeed);
break; break;
case 0x4: case 0x4:
parameterWrapper->set(rwHandlingParameters.stictionTorque); parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(rwHandlingParameters.stictionTorque);
break;
case 0x6:
parameterWrapper->set(rwHandlingParameters.rampTime); parameterWrapper->set(rwHandlingParameters.rampTime);
break; break;
default: default:
@ -288,25 +291,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0x7): // RwMatrices case (0x7): // RwMatrices
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(rwMatrices.alignmentMatrix); parameterWrapper->setMatrix(rwMatrices.alignmentMatrix);
break; break;
case 0x1: case 0x1:
parameterWrapper->set(rwMatrices.pseudoInverse); parameterWrapper->setMatrix(rwMatrices.pseudoInverse);
break; break;
case 0x2: case 0x2:
parameterWrapper->set(rwMatrices.without1); parameterWrapper->setMatrix(rwMatrices.without1);
break; break;
case 0x3: case 0x3:
parameterWrapper->set(rwMatrices.without2); parameterWrapper->setMatrix(rwMatrices.without2);
break; break;
case 0x4: case 0x4:
parameterWrapper->set(rwMatrices.without3); parameterWrapper->setMatrix(rwMatrices.without3);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(rwMatrices.without4); parameterWrapper->setMatrix(rwMatrices.without4);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(rwMatrices.nullspace); parameterWrapper->setVector(rwMatrices.nullspace);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
@ -330,13 +333,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin); parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(safeModeControllerParameters.sunTargetDirLeop); parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(safeModeControllerParameters.sunTargetDir); parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(safeModeControllerParameters.satRateRef); parameterWrapper->setVector(safeModeControllerParameters.satRateRef);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
@ -345,31 +348,31 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0x9): // IdleModeControllerParameters case (0x9): // IdleModeControllerParameters
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(targetModeControllerParameters.zeta); parameterWrapper->set(idleModeControllerParameters.zeta);
break; break;
case 0x1: case 0x1:
parameterWrapper->set(targetModeControllerParameters.om); parameterWrapper->set(idleModeControllerParameters.om);
break; break;
case 0x2: case 0x2:
parameterWrapper->set(targetModeControllerParameters.omMax); parameterWrapper->set(idleModeControllerParameters.omMax);
break; break;
case 0x3: case 0x3:
parameterWrapper->set(targetModeControllerParameters.qiMin); parameterWrapper->set(idleModeControllerParameters.qiMin);
break; break;
case 0x4: case 0x4:
parameterWrapper->set(targetModeControllerParameters.gainNullspace); parameterWrapper->set(idleModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(targetModeControllerParameters.desatOn); parameterWrapper->set(idleModeControllerParameters.desatOn);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
break; break;
default: default:
@ -394,7 +397,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(targetModeControllerParameters.gainNullspace); parameterWrapper->set(targetModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
@ -406,13 +409,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
break; break;
case 0x9: case 0x9:
parameterWrapper->set(targetModeControllerParameters.refDirection); parameterWrapper->setVector(targetModeControllerParameters.refDirection);
break; break;
case 0xA: case 0xA:
parameterWrapper->set(targetModeControllerParameters.refRotRate); parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
break; break;
case 0xB: case 0xB:
parameterWrapper->set(targetModeControllerParameters.quatRef); parameterWrapper->setVector(targetModeControllerParameters.quatRef);
break; break;
case 0xC: case 0xC:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
@ -445,52 +448,46 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0xB): // GsTargetModeControllerParameters case (0xB): // GsTargetModeControllerParameters
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(targetModeControllerParameters.zeta); parameterWrapper->set(gsTargetModeControllerParameters.zeta);
break; break;
case 0x1: case 0x1:
parameterWrapper->set(targetModeControllerParameters.om); parameterWrapper->set(gsTargetModeControllerParameters.om);
break; break;
case 0x2: case 0x2:
parameterWrapper->set(targetModeControllerParameters.omMax); parameterWrapper->set(gsTargetModeControllerParameters.omMax);
break; break;
case 0x3: case 0x3:
parameterWrapper->set(targetModeControllerParameters.qiMin); parameterWrapper->set(gsTargetModeControllerParameters.qiMin);
break; break;
case 0x4: case 0x4:
parameterWrapper->set(targetModeControllerParameters.gainNullspace); parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(targetModeControllerParameters.desatOn); parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
break; break;
case 0x9: case 0x9:
parameterWrapper->set(targetModeControllerParameters.refDirection); parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
break; break;
case 0xA: case 0xA:
parameterWrapper->set(targetModeControllerParameters.refRotRate); parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
break; break;
case 0xB: case 0xB:
parameterWrapper->set(targetModeControllerParameters.quatRef); parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
break; break;
case 0xC: case 0xC:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
break; break;
case 0xD: case 0xD:
parameterWrapper->set(targetModeControllerParameters.latitudeTgt); parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
break;
case 0xE:
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
break;
case 0xF:
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
@ -514,7 +511,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(nadirModeControllerParameters.gainNullspace); parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(nadirModeControllerParameters.desatMomentumRef); parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
@ -526,10 +523,10 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
break; break;
case 0x9: case 0x9:
parameterWrapper->set(nadirModeControllerParameters.refDirection); parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
break; break;
case 0xA: case 0xA:
parameterWrapper->set(nadirModeControllerParameters.quatRef); parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
break; break;
case 0xC: case 0xC:
parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
@ -556,7 +553,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(inertialModeControllerParameters.gainNullspace); parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(inertialModeControllerParameters.desatMomentumRef); parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
@ -568,13 +565,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
break; break;
case 0x9: case 0x9:
parameterWrapper->set(inertialModeControllerParameters.tgtQuat); parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
break; break;
case 0xA: case 0xA:
parameterWrapper->set(inertialModeControllerParameters.refRotRate); parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
break; break;
case 0xC: case 0xC:
parameterWrapper->set(inertialModeControllerParameters.quatRef); parameterWrapper->setVector(inertialModeControllerParameters.quatRef);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
@ -586,7 +583,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(strParameters.exclusionAngle); parameterWrapper->set(strParameters.exclusionAngle);
break; break;
case 0x1: case 0x1:
parameterWrapper->set(strParameters.boresightAxis); parameterWrapper->setVector(strParameters.boresightAxis);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
@ -597,6 +594,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x0: case 0x0:
parameterWrapper->set(gpsParameters.timeDiffVelocityMax); parameterWrapper->set(gpsParameters.timeDiffVelocityMax);
break; break;
case 0x1:
parameterWrapper->set(gpsParameters.minimumFdirAltitude);
break;
case 0x2:
parameterWrapper->set(gpsParameters.maximumFdirAltitude);
break;
case 0x3:
parameterWrapper->set(gpsParameters.fdirAltitude);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
@ -658,25 +664,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0x12): // MagnetorquesParameter case (0x12): // MagnetorquesParameter
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); parameterWrapper->setMatrix(magnetorquerParameter.mtq0orientationMatrix);
break; break;
case 0x1: case 0x1:
parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix); parameterWrapper->setMatrix(magnetorquerParameter.mtq1orientationMatrix);
break; break;
case 0x2: case 0x2:
parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix); parameterWrapper->setMatrix(magnetorquerParameter.mtq2orientationMatrix);
break; break;
case 0x3: case 0x3:
parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq); parameterWrapper->setMatrix(magnetorquerParameter.alignmentMatrixMtq);
break; break;
case 0x4: case 0x4:
parameterWrapper->set(magnetorquesParameter.inverseAlignment); parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(magnetorquesParameter.DipolMax); parameterWrapper->set(magnetorquerParameter.dipolMax);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(magnetorquesParameter.torqueDuration); parameterWrapper->set(magnetorquerParameter.torqueDuration);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; 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 gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594};
double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845}; 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 */ * 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 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 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 { struct RwHandlingParameters {
double inertiaWheel = 0.000028198; double inertiaWheel = 0.000028198;
double maxTrq = 0.0032; // 3.2 [mNm] double maxTrq = 0.0032; // 3.2 [mNm]
int32_t stictionSpeed = 100; // RPM int32_t maxRwSpeed = 65000; // 0.1 RPM
int32_t stictionReleaseSpeed = 120; // RPM int32_t stictionSpeed = 1000; // 0.1 RPM
int32_t stictionReleaseSpeed = 1000; // 0.1 RPM
double stictionTorque = 0.0006; double stictionTorque = 0.0006;
uint16_t rampTime = 10; uint16_t rampTime = 10;
@ -817,7 +818,7 @@ class AcsParameters : public HasParametersIF {
double sunMagAngleMin = 5 * M_PI / 180; 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 sunTargetDir[3] = {0, 0, 1};
double satRateRef[3] = {0, 0, 0}; double satRateRef[3] = {0, 0, 0};
@ -843,7 +844,7 @@ class AcsParameters : public HasParametersIF {
double refDirection[3] = {-1, 0, 0}; // Antenna double refDirection[3] = {-1, 0, 0}; // Antenna
double refRotRate[3] = {0, 0, 0}; double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1}; 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 // Default is Stuttgart GS
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
@ -859,7 +860,7 @@ class AcsParameters : public HasParametersIF {
struct GsTargetModeControllerParameters : PointingLawParameters { struct GsTargetModeControllerParameters : PointingLawParameters {
double refDirection[3] = {-1, 0, 0}; // Antenna 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 // Default is Stuttgart GS
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude 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 refDirection[3] = {-1, 0, 0}; // Antenna
double quatRef[4] = {0, 0, 0, 1}; double quatRef[4] = {0, 0, 0, 1};
double refRotRate[3] = {0, 0, 0}; double refRotRate[3] = {0, 0, 0};
int8_t timeElapsedMax = 10; // rot rate calculations uint8_t timeElapsedMax = 10; // rot rate calculations
} nadirModeControllerParameters; } nadirModeControllerParameters;
struct InertialModeControllerParameters : PointingLawParameters { struct InertialModeControllerParameters : PointingLawParameters {
@ -886,7 +887,10 @@ class AcsParameters : public HasParametersIF {
} strParameters; } strParameters;
struct GpsParameters { 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; } gpsParameters;
struct SunModelParameters { struct SunModelParameters {
@ -912,16 +916,16 @@ class AcsParameters : public HasParametersIF {
double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability
} kalmanFilterParameters; } kalmanFilterParameters;
struct MagnetorquesParameter { struct MagnetorquerParameter {
double mtq0orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; 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 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 mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}};
double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}};
double DipolMax = 0.2; // [Am^2] double dipolMax = 0.2; // [Am^2]
uint16_t torqueDuration = 300; // [ms] uint16_t torqueDuration = 300; // [ms]
} magnetorquesParameter; } magnetorquerParameter;
struct DetumbleParameter { struct DetumbleParameter {
uint8_t detumblecounter = 75; // 30 s uint8_t detumblecounter = 75; // 30 s

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

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

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

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

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

@ -12,7 +12,7 @@
#include "util/CholeskyDecomposition.h" #include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h" #include "util/MathOperations.h"
Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {} Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
Guidance::~Guidance() {} Guidance::~Guidance() {}
@ -26,9 +26,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
double targetE[3] = {0, 0, 0}; double targetE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt( MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters->targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters->targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetE); acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
// target direction in the ECEF frame // target direction in the ECEF frame
double targetDirE[3] = {0, 0, 0}; 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 // rotation quaternion from two vectors
double refDir[3] = {0, 0, 0}; double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0]; refDir[0] = acsParameters->targetModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1]; refDir[1] = acsParameters->targetModeControllerParameters.refDirection[1];
refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2]; refDir[2] = acsParameters->targetModeControllerParameters.refDirection[2];
double noramlizedTargetDirB[3] = {0, 0, 0}; double noramlizedTargetDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3); VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 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 // 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}; double sunDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1);
double exclAngle = acsParameters.strParameters.exclusionAngle, double exclAngle = acsParameters->strParameters.exclusionAngle,
blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart,
blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop; blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop;
double sightAngleSun = double sightAngleSun =
VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB); VectorOperations<double>::dot(acsParameters->strParameters.boresightAxis, sunDirB);
if (!(strBlindAvoidFlag)) { if (!(strBlindAvoidFlag)) {
double critSightAngle = blindStart * exclAngle; double critSightAngle = blindStart * exclAngle;
@ -113,7 +113,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
} }
} else { } else {
if (sightAngleSun < blindEnd * exclAngle) { if (sightAngleSun < blindEnd * exclAngle) {
double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate; double normBlindRefRate = acsParameters->targetModeControllerParameters.blindRotRate;
double blindRefRate[3] = {0, 0, 0}; double blindRefRate[3] = {0, 0, 0};
if (sunDirB[1] < 0) { if (sunDirB[1] < 0) {
blindRefRate[0] = normBlindRefRate; 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) // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
double targetE[3] = {0, 0, 0}; double targetE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt( MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters->targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters->targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetE); acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
double targetDirE[3] = {0, 0, 0}; double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3); 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]}}; {xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmIX, targetQuat); QuaternionOperations::fromDcm(dcmIX, targetQuat);
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); 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}; double groundStationE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt( MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.gsTargetModeControllerParameters.latitudeTgt, acsParameters->gsTargetModeControllerParameters.latitudeTgt,
acsParameters.gsTargetModeControllerParameters.longitudeTgt, acsParameters->gsTargetModeControllerParameters.longitudeTgt,
acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE); acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE);
double targetDirE[3] = {0, 0, 0}; double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationE, posSatE, targetDirE, 3); 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]}}; {xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmTgt, targetQuat); QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax; int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
} }
@ -332,9 +332,9 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub
// rotation quaternion from two vectors // rotation quaternion from two vectors
double refDir[3] = {0, 0, 0}; double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0]; refDir[0] = acsParameters->nadirModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1]; refDir[1] = acsParameters->nadirModeControllerParameters.refDirection[1];
refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2]; refDir[2] = acsParameters->nadirModeControllerParameters.refDirection[2];
double noramlizedTargetDirB[3] = {0, 0, 0}; double noramlizedTargetDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3); VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 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]}}; {xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmTgt, targetQuat); QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax; int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate); 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()); bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
if (rw1valid && rw2valid && rw3valid && rw4valid) { 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; return returnvalue::OK;
} else if (!rw1valid && rw2valid && rw3valid && rw4valid) { } 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; return returnvalue::OK;
} else if (rw1valid && !rw2valid && rw3valid && rw4valid) { } 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; return returnvalue::OK;
} else if (rw1valid && rw2valid && !rw3valid && rw4valid) { } 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; return returnvalue::OK;
} else if (rw1valid && rw2valid && rw3valid && !rw4valid) { } 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; return returnvalue::OK;
} else { } else {
// @note: This one takes the normal pseudoInverse of all four raction wheels valid. // @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]) { void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
std::error_code e; std::error_code e;
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
e)) { // ToDo: if file does not exist anymore std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir,
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
3 * sizeof(double)); 3 * sizeof(double));
} else { } else {
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop, std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop,
3 * sizeof(double)); 3 * sizeof(double));
} }
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef, std::memcpy(satRateSafe, acsParameters->safeModeControllerParameters.satRateRef,
3 * sizeof(double)); 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); ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
private: private:
AcsParameters acsParameters; const AcsParameters *acsParameters;
bool strBlindAvoidFlag = false; bool strBlindAvoidFlag = false;
timeval timeSavedQuaternion; timeval timeSavedQuaternion;
double savedQuaternion[4] = {0, 0, 0, 0}; double savedQuaternion[4] = {0, 0, 0, 0};
double omegaRefSaved[3] = {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_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/deployment"; static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";
}; };
#endif /* ACS_GUIDANCE_H_ */ #endif /* ACS_GUIDANCE_H_ */

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

@ -12,33 +12,22 @@
#include "util/CholeskyDecomposition.h" #include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h" #include "util/MathOperations.h"
/*Initialisation of values for parameters in constructor*/ MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {}
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( ReturnValue_t MultiplicativeKalmanFilter::init(
const double *magneticField_, const bool validMagField_, const double *sunDir_, const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, 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 // check for valid mag/sun
if (validMagField_ && validSS && validSSModel && validMagModel) { if (validMagField_ && validSS && validSSModel && validMagModel) {
validInit = true;
// QUEST ALGO ----------------------------------------------------------------------- // QUEST ALGO -----------------------------------------------------------------------
double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
sigmaSun = kalmanFilterParameters->sensorNoiseSS; sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS;
sigmaMag = kalmanFilterParameters->sensorNoiseMAG; sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG;
sigmaGyro = kalmanFilterParameters->sensorNoiseGYR; sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGYR;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0}; normSunJ[3] = {0, 0, 0};
@ -192,21 +181,18 @@ ReturnValue_t MultiplicativeKalmanFilter::init(
return MEKF_INITIALIZED; return MEKF_INITIALIZED;
} else { } else {
// no initialisation possible, no valid measurements // no initialisation possible, no valid measurements
validInit = false;
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
return MEKF_UNINITIALIZED; return MEKF_UNINITIALIZED;
} }
} }
// --------------- MEKF DISCRETE TIME STEP ------------------------------- // --------------- MEKF DISCRETE TIME STEP -------------------------------
ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, const bool validSTR_, ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
const double *rateGYRs_, const bool validGYRs_, const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
const double *magneticField_, const bool validGYRs_, const double *magneticField_, const bool validMagField_,
const bool validMagField_, const double *sunDir_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
const bool validSS, const double *sunDirJ, const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
const bool validSSModel, const double *magFieldJ, AcsParameters *acsParameters) {
const bool validMagModel, double sampleTime,
acsctrl::MekfData *mekfData) {
// Check for GYR Measurements // Check for GYR Measurements
int MDF = 0; // Matrix Dimension Factor int MDF = 0; // Matrix Dimension Factor
if (!validGYRs_) { if (!validGYRs_) {
@ -248,9 +234,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
// If we are here, MEKF will perform // If we are here, MEKF will perform
double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0; double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0;
sigmaSun = kalmanFilterParameters->sensorNoiseSS; sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS;
sigmaMag = kalmanFilterParameters->sensorNoiseMAG; sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG;
sigmaStr = kalmanFilterParameters->sensorNoiseSTR; sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseSTR;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[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]; biasGYR[2] = updatedGyroBias[2];
/* ----------- PROPAGATION ----------*/ /* ----------- PROPAGATION ----------*/
double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR; double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseBsGYR;
double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR; 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}, 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}}; {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}}, covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ22[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}}; covQ12trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
if (normRotEst * sampleTime < M_PI / 10) { if (normRotEst * acsParameters->onBoardParams.sampleTime < M_PI / 10) {
double fact11 = pow(sigmaV, 2) * sampleTime + 1. / 3. * pow(sigmaU, 2) * pow(sampleTime, 3); 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); 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); MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3);
std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double)); 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); MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
} else { } else {
double fact22 = pow(sigmaU, 2) * sampleTime; double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3); 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 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); 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); MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3);
double fact12_2 = 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); pow(normRotEst, 4);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3); MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 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); MatrixOperations<double>::transpose(*covQ12, *covQ12trans, 3);
double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][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); 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); MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3);
double fact11_2 = (2 * normRotEst * sampleTime - 2 * sin(normRotEst * sampleTime) - double fact11_2 =
1. / 3. * pow(normRotEst, 3) * pow(sampleTime, 3)) / (2 * normRotEst * acsParameters->onBoardParams.sampleTime -
pow(normRotEst, 5); 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>::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3); MatrixOperations<double>::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3);
MatrixOperations<double>::subtract(*covQ11_1, *covQ11_2, *covQ11_12, 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}, 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}}; {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 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); 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>::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3); MatrixOperations<double>::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3);
MatrixOperations<double>::subtract(*identityMatrix3, *phi11_1, *phi11_01, 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 phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3];
double fact12_0 = fact11_2; double fact12_0 = fact11_2;
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3); MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi12_1, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3,
double fact12_2 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3)); 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>::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3); MatrixOperations<double>::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3);
MatrixOperations<double>::subtract(*phi12_0, *phi12_1, *phi12_01, 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 // Propagated Quaternion
double rotSin[3] = {0, 0, 0}, rotCosMat[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; 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 rotCos = cos(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime);
double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst; double sinFac = sin(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst;
VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3); VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; 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: public:
/* @brief: Constructor /* @brief: Constructor
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/ */
MultiplicativeKalmanFilter(AcsParameters *acsParameters_); MultiplicativeKalmanFilter();
virtual ~MultiplicativeKalmanFilter(); virtual ~MultiplicativeKalmanFilter();
ReturnValue_t reset(acsctrl::MekfData *mekfData); ReturnValue_t reset(acsctrl::MekfData *mekfData);
@ -33,8 +32,8 @@ class MultiplicativeKalmanFilter {
*/ */
ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_, ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel, const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel, const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
acsctrl::MekfData *mekfData); AcsParameters *acsParameters);
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
* for the current step after the initalization * for the current step after the initalization
@ -54,7 +53,8 @@ class MultiplicativeKalmanFilter {
const bool validGYRs_, const double *magneticField_, const bool validGYRs_, const double *magneticField_,
const bool validMagField_, const double *sunDir_, const bool validSS, const bool validMagField_, const double *sunDir_, const bool validSS,
const double *sunDirJ, const bool validSSModel, const double *magFieldJ, 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 { enum MekfStatus : uint8_t {
UNINITIALIZED = 0, UNINITIALIZED = 0,
@ -79,23 +79,21 @@ class MultiplicativeKalmanFilter {
private: private:
/*Parameters*/ /*Parameters*/
AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
double quaternion_STR_SB[4]; double quaternion_STR_SB[4];
bool validInit;
/*States*/ /*States*/
double initialQuaternion[4]; /*after reset?QUEST*/ double initialQuaternion[4] = {0, 0, 0, 1}; /*after reset?QUEST*/
double initialCovarianceMatrix[6][6]; /*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*/ double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
uint8_t sensorsAvail; uint8_t sensorsAvail = 0;
/*Outputs*/ /*Outputs*/
double quatBJ[4]; /* Output Quaternion */ double quatBJ[4]; /* Output Quaternion */
double biasGYR[3]; /*Between measured and estimated sat Rate*/ double biasGYR[3]; /*Between measured and estimated sat Rate*/
/*Parameter INIT*/ /*Parameter INIT*/
/*Functions*/ /*Functions*/
void loadAcsParameters(AcsParameters *acsParameters_);
void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus); void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4], void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
double satRotRate[3]); double satRotRate[3]);

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

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

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

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

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

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

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

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

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

@ -1,17 +1,16 @@
#ifndef SENSORVALUES_H_ #ifndef SENSORVALUES_H_
#define 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/gyroAdisHelpers.h>
#include <mission/devices/devicedefinitions/imtqHelpers.h> #include <mission/devices/devicedefinitions/imtqHelpers.h>
#include <mission/devices/devicedefinitions/rwHelpers.h> #include <mission/devices/devicedefinitions/rwHelpers.h>
#include <mission/devices/devicedefinitions/susMax1227Helpers.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 { namespace ACS {
class SensorValues { class SensorValues {

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

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

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

@ -12,30 +12,22 @@
class Detumble { class Detumble {
public: public:
Detumble(AcsParameters *acsParameters_); Detumble();
virtual ~Detumble(); virtual ~Detumble();
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE; static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE;
static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); 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, 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 bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField,
const bool *magFieldValid, double *magMom, double gain);
ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField,
const bool *magFieldValid, double *magMom);
private: private:
AcsParameters::DetumbleParameter *detumbleParameter;
AcsParameters::MagnetorquesParameter *magnetorquesParameter;
}; };
#endif /*ACS_CONTROL_DETUMBLE_H_*/ #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 "PtgCtrl.h"
#include <fsfw/globalfunctions/constants.h> #include <fsfw/globalfunctions/constants.h>
@ -16,16 +9,10 @@
#include "../util/MathOperations.h" #include "../util/MathOperations.h"
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
PtgCtrl::~PtgCtrl() {} PtgCtrl::~PtgCtrl() {}
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices = &(acsParameters_->rwMatrices);
}
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *errorQuat, const double *deltaRate, const double *rwPseudoInv, const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
double *torqueRws) { double *torqueRws) {
@ -62,8 +49,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
gainMatrixDiagonal[0][0] = gainVector[0]; gainMatrixDiagonal[0][0] = gainVector[0];
gainMatrixDiagonal[1][1] = gainVector[1]; gainMatrixDiagonal[1][1] = gainVector[1];
gainMatrixDiagonal[2][2] = gainVector[2]; gainMatrixDiagonal[2][2] = gainVector[2];
MatrixOperations<double>::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), MatrixOperations<double>::multiply(
*gainMatrix, 3, 3, 3); *gainMatrixDiagonal, *(acsParameters->inertiaEIVE.inertiaMatrix), *gainMatrix, 3, 3, 3);
// Inverse of gainMatrix // Inverse of gainMatrix
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; 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]; gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, MatrixOperations<double>::multiply(
3, 3); *gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrix), *pMatrix, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3); MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
//------------------------------------------------------------------------------------------------ //------------------------------------------------------------------------------------------------
@ -91,18 +78,19 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
pErrorSign[i] = pError[i]; pErrorSign[i] = pError[i];
} }
} }
// Torque for quaternion error // torque for quaternion error
double torqueQuat[3] = {0, 0, 0}; double torqueQuat[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1); MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3); VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);
// Torque for rate error // torque for rate error
double torqueRate[3] = {0, 0, 0}; 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, cInt, torqueRate, 3);
VectorOperations<double>::mulScalar(torqueRate, -1, 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}; double torque[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3); VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
@ -120,20 +108,22 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
return; 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 speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, momentumRwU, 4);
1); MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU,
momentumRw, 3, 4, 1);
double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; 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); VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
// calculating momentum error // calculating momentum error
double deltaMomentum[3] = {0, 0, 0}; double deltaMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef, VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
deltaMomentum, 3); deltaMomentum, 3);
// resulting magnetic dipole command // resulting magnetic dipole command
double crossMomentumMagField[3] = {0, 0, 0}; double crossMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField); VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0; 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 speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0}; double wheelMomentum[4] = {0, 0, 0, 0};
double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; 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(rpmOffset, factor, rpmOffset, 4);
VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
double diffRwSpeed[4] = {0, 0, 0, 0}; double diffRwSpeed[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4); VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
wheelMomentum, 4); wheelMomentum, 4);
double gainNs = pointingLawParameters->gainNullspace; double gainNs = pointingLawParameters->gainNullspace;
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; 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); *nullSpaceMatrix, 4);
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
VectorOperations<double>::mulScalar(rwTrqNs, -1, 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] = { bool rwAvailable[4] = {
(sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()), (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()),
(sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()), (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()),
(sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()), (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()),
(sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid())}; (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid())};
int32_t omegaRw[4] = {sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value, int32_t currRwSpeed[4] = {
sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value}; sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value,
sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value};
for (uint8_t i = 0; i < 4; i++) { for (uint8_t i = 0; i < 4; i++) {
if (rwAvailable[i]) { if (rwAvailable[i]) {
if (torqueMemory[i] != 0) { if (rwCmdSpeeds[i] != 0) {
if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) { if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed &&
torqueMemory[i] = 0; rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) {
} else { if (currRwSpeed[i] == 0) {
torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; if (rwCmdSpeeds[i] > 0) {
} rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
} else { } else if (rwCmdSpeeds[i] < 0) {
if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) && rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
(omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { }
if (omegaRw[i] < omegaMemory[i]) { } else if (currRwSpeed[i] < -acsParameters->rwHandlingParameters.stictionSpeed) {
torqueMemory[i] = -1; rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
} else { } else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) {
torqueMemory[i] = 1; 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_ #ifndef PTGCTRL_H_
#define PTGCTRL_H_ #define PTGCTRL_H_
@ -23,6 +10,13 @@
#include "eive/resultClassIds.h" #include "eive/resultClassIds.h"
class PtgCtrl { 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: public:
/* @brief: Constructor /* @brief: Constructor
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters * @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 uint8_t INTERFACE_ID = CLASS_ID::ACS_PTG;
static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); 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 /* @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, void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
const double *deltaRate, const double *rwPseudoInv, double *torqueRws); const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
@ -54,18 +42,12 @@ class PtgCtrl {
const int32_t *speedRw3, double *rwTrqNs); const int32_t *speedRw3, double *rwTrqNs);
/* @brief: Commands the stiction torque in case wheel speed is to low /* @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 * torqueCommand modified torque after antistiction
*/ */
void rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand); void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed);
private: private:
AcsParameters::RwHandlingParameters *rwHandlingParameters; const AcsParameters *acsParameters;
AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::RwMatrices *rwMatrices;
double torqueMemory[4] = {0, 0, 0, 0};
double omegaMemory[4] = {0, 0, 0, 0};
}; };
#endif /* ACS_CONTROL_PTGCTRL_H_ */ #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 "SafeCtrl.h"
#include <fsfw/globalfunctions/constants.h> #include <fsfw/globalfunctions/constants.h>
@ -15,19 +8,10 @@
#include "../util/MathOperations.h" #include "../util/MathOperations.h"
SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
loadAcsParameters(acsParameters_);
MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3,
3);
}
SafeCtrl::~SafeCtrl() {} SafeCtrl::~SafeCtrl() {}
void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
}
ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
double *magFieldModel, bool magFieldModelValid, double *magFieldModel, bool magFieldModelValid,
double *sunDirModel, bool sunDirModelValid, double *satRateMekf, 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; return SAFECTRL_MEKF_INPUT_INVALID;
} }
double kRate = safeModeControllerParameters->k_rate_mekf; double kRate = acsParameters->safeModeControllerParameters.k_rate_mekf;
double kAlign = safeModeControllerParameters->k_align_mekf; double kAlign = acsParameters->safeModeControllerParameters.k_align_mekf;
// Calc sunDirB ,magFieldB with mekf output and model // Calc sunDirB ,magFieldB with mekf output and model
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; 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); VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
// Adding factor of inertia for axes // 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); MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
// MagMom B (orthogonal torque) // 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 /* Only valid if angle between sun direction and magnetic field direction
* is sufficiently large */ * is sufficiently large */
double angleSunMag = acos(cosAngleSunMag); double angleSunMag = acos(cosAngleSunMag);
if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) { if (angleSunMag < acsParameters->safeModeControllerParameters.sunMagAngleMin) {
return returnvalue::FAILED; return returnvalue::FAILED;
} }
@ -135,8 +122,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3); VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
// Torque Align calculation // Torque Align calculation
double kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; double kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf;
double kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; double kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf;
double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB); double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
double crossSusSunRef[3] = {0, 0, 0}; double crossSusSunRef[3] = {0, 0, 0};
@ -155,8 +142,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
// Final torque // Final torque
double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0}; double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3); VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), torqueAlignRate,
1); torqueB, 3, 3, 1);
// Magnetic moment // Magnetic moment
double magMomB[3] = {0, 0, 0}; 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 uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE;
static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); 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, ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel,
bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
double *satRateMekf, bool rateMekfValid, double *sunDirRef, double *satRateMekf, bool rateMekfValid, double *sunDirRef,
@ -32,8 +30,7 @@ class SafeCtrl {
protected: protected:
private: private:
AcsParameters::SafeModeControllerParameters *safeModeControllerParameters; AcsParameters *acsParameters;
AcsParameters::InertiaEIVE *inertiaEIVE;
double gainMatrixInertia[3][3]; double gainMatrixInertia[3][3];
double magFieldBState[3]; double magFieldBState[3];

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

@ -86,6 +86,7 @@ enum PoolIds : lp_id_t {
// GPS Processed // GPS Processed
GC_LATITUDE, GC_LATITUDE,
GD_LONGITUDE, GD_LONGITUDE,
ALTITUDE,
GPS_POSITION, GPS_POSITION,
GPS_VELOCITY, GPS_VELOCITY,
// MEKF // 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 SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4; static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; 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 MEKF_SET_ENTRIES = 3;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; 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> 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> 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> 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); lp_vec_t<double, 3> gpsVelocity = lp_vec_t<double, 3>(sid.objectId, GPS_VELOCITY, this);