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

Higher ACS Modes STR Only #818

Merged
meggert merged 38 commits from higher-acs-modes-only-str into dev-7.5.0 2023-12-06 17:15:52 +01:00
Conversation 0 Commits 38 Files Changed 27 +786 -230
27 changed files with 786 additions and 230 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
CHANGELOG.md
View File

@ -14,7 +14,7 @@ will consitute of a breaking change warranting a new major release:
- The TMTC interface changes in any shape of form.
- The behaviour of the OBSW changes in a major shape or form relevant for operations
# [v7.5.0] 2023-12-XX
# [v7.5.0] 2023-12-06
## Changed
@ -27,6 +27,13 @@ will consitute of a breaking change warranting a new major release:
- Both the `AcsController` and the `PwrController` now use the monotonic clock to calculate
the time difference.
## Added
- Higher ACS modes can now be entered without a running `MEKF`. Higher modes will collect their
quaternion and rotational rate depending on the available sources.
- `QUEST` attitude estimation was added to the `AcsController`.
- The fused rotational rate can now be estimated from `QUEST` and the `STR`.
# [v7.4.1] 2023-12-06
## Fixed
@ -123,6 +130,16 @@ will consitute of a breaking change warranting a new major release:
during which the SUS was not working as well as the maximum amount of invalid messages.
- Updated battery internal resistance to new value
## Changed
- `Power Controller` now uses monotonic clock for calculating time difference
- `ACS Controller` now uses monotonic clock for calculating time difference and the normal clock
for model calculations. The `timeDelta` is now calculated in the controller instead of
everywhere where it is needed.
- `ACS Controller` now has the function `performAttitudeControl` which is called prior to passing
on to the relevant mode functions. It handles all telemetry relevant functions, which were
always called, regardless of the mode.
# [v7.1.0] 2023-10-11
- Bumped `eive-tmtc` to v5.8.0.

2
fsfw

@ -1 +1 @@
Subproject commit 7187f2b5cdfe163bf7ed1a8fab48900d69f4c8bf
Subproject commit 7105e199c650303ac1a48e75aebc44182630931e

32
mission/acs/defs.h
View File

@ -22,10 +22,10 @@ enum AcsMode : Mode_t {
enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 };
enum SafeModeStrategy : uint8_t {
SAFECTRL_OFF = 0,
SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
SAFECTRL_NO_SENSORS_FOR_CONTROL = 2,
enum ControlModeStrategy : uint8_t {
CTRL_OFF = 0,
CTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
CTRL_NO_SENSORS_FOR_CONTROL = 2,
// OBSW version <= v6.1.0
LEGACY_SAFECTRL_ACTIVE_MEKF = 10,
LEGACY_SAFECTRL_WITHOUT_MEKF = 11,
@ -40,14 +40,28 @@ enum SafeModeStrategy : uint8_t {
SAFECTRL_ECLIPSE_IDELING = 19,
SAFECTRL_DETUMBLE_FULL = 20,
SAFECTRL_DETUMBLE_DETERIORATED = 21,
// Added in vNext
PTGCTRL_MEKF = 100,
PTGCTRL_STR = 101,
PTGCTRL_QUEST = 102,
};
enum GpsSource : uint8_t {
namespace gps {
enum Source : uint8_t {
NONE,
GPS,
GPS_EXTRAPOLATED,
SPG4,
};
}
namespace rotrate {
enum Source : uint8_t {
NONE,
SUSMGM,
QUEST,
STR,
};
}
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
//! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
@ -64,9 +78,9 @@ static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO);
static constexpr Event MEKF_RECOVERY = MAKE_EVENT(4, severity::INFO);
//! [EXPORT] : [COMMENT] MEKF performed an automatic reset after detection of nonfinite values.
static constexpr Event MEKF_AUTOMATIC_RESET = MAKE_EVENT(5, severity::INFO);
//! [EXPORT] : [COMMENT] MEKF was not able to compute a solution during any pointing ACS mode for a
//! prolonged time.
static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIGH);
//! [EXPORT] : [COMMENT] For a prolonged time, no attitude information was available for the
//! Pointing Controller. Falling back to Safe Mode.
static constexpr Event PTG_CTRL_NO_ATTITUDE_INFORMATION = MAKE_EVENT(6, severity::HIGH);
//! [EXPORT] : [COMMENT] The ACS safe mode controller was not able to compute a solution and has
//! failed.
//! P1: Missing information about magnetic field, P2: Missing information about rotational rate

229
mission/controller/AcsController.cpp
View File

@ -4,6 +4,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMoun
: ExtendedControllerBase(objectId),
enableHkSets(enableHkSets),
sdcMan(sdcMan),
attitudeEstimation(&acsParameters),
fusedRotationEstimation(&acsParameters),
guidance(&acsParameters),
safeCtrl(&acsParameters),
@ -16,10 +17,11 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMoun
gyrDataRaw(this),
gyrDataProcessed(this),
gpsDataProcessed(this),
mekfData(this),
attitudeEstimationData(this),
ctrlValData(this),
actuatorCmdData(this),
fusedRotRateData(this) {}
fusedRotRateData(this),
fusedRotRateSourcesData(this) {}
ReturnValue_t AcsController::initialize() {
ReturnValue_t result = parameterHelper.initialize();
@ -52,7 +54,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
return HasActionsIF::EXECUTION_FINISHED;
}
case RESET_MEKF: {
navigation.resetMekf(&mekfData);
navigation.resetMekf(&attitudeEstimationData);
return HasActionsIF::EXECUTION_FINISHED;
}
case RESTORE_MEKF_NONFINITE_RECOVERY: {
@ -169,28 +171,32 @@ void AcsController::performAttitudeControl() {
sensorProcessing.process(timeAbsolute, timeDelta, &sensorValues, &mgmDataProcessed,
&susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData);
fusedRotationEstimation.estimateFusedRotationRate(
&susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
&attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
&susDataProcessed, &attitudeEstimationData, &acsParameters);
switch (mode) {
case acs::SAFE:
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
triggerEvent(acs::MEKF_INVALID_INFO,
static_cast<uint32_t>(attitudeEstimationData.mekfStatus.value));
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
navigation.resetMekf(&attitudeEstimationData);
mekfLost = true;
}
} else if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false;
}
switch (mode) {
case acs::SAFE:
switch (submode) {
case SUBMODE_NONE:
performSafe();
@ -205,35 +211,6 @@ void AcsController::performAttitudeControl() {
case acs::PTG_TARGET_GS:
case acs::PTG_NADIR:
case acs::PTG_INERTIAL:
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
mekfInvalidCounter++;
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
mekfLost = true;
}
if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
// Trigger this so STR FDIR can set the device faulty.
EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0,
0);
mekfInvalidCounter = 0;
}
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration,
cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
return;
} else {
if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false;
}
mekfInvalidCounter = 0;
}
performPointingCtrl();
break;
}
@ -245,27 +222,28 @@ void AcsController::performSafe() {
guidance.getTargetParamsSafe(sunTargetDir);
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
acsParameters.safeModeControllerParameters.useGyr,
acsParameters.safeModeControllerParameters.dampingDuringEclipse);
switch (safeCtrlStrat) {
case (acs::SafeModeStrategy::SAFECTRL_MEKF):
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
magMomMtq, errAng);
case (acs::ControlModeStrategy::SAFECTRL_MEKF):
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value,
attitudeEstimationData.satRotRateMekf.value,
susDataProcessed.sunIjkModel.value, attitudeEstimationData.quatMekf.value,
sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_GYR):
case (acs::ControlModeStrategy::SAFECTRL_GYR):
safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
case (acs::ControlModeStrategy::SAFECTRL_SUSMGM):
safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
fusedRotRateData.rotRateParallel.value,
fusedRotRateData.rotRateOrthogonal.value,
@ -273,29 +251,29 @@ void AcsController::performSafe() {
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value,
gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq,
errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq,
errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING):
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING):
errAng = NAN;
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL):
safeCtrlFailure(1, 0);
break;
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL):
safeCtrlFailure(0, 1);
break;
default:
@ -308,8 +286,8 @@ void AcsController::performSafe() {
// detumble check and switch
if (acsParameters.safeModeControllerParameters.useMekf) {
if (mekfData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
if (attitudeEstimationData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
}
@ -340,24 +318,24 @@ void AcsController::performSafe() {
}
void AcsController::performDetumble() {
acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
acs::ControlModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.isValid(),
acsParameters.detumbleParameter.useFullDetumbleLaw);
double magMomMtq[3] = {0, 0, 0};
switch (safeCtrlStrat) {
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL):
case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL):
detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
magMomMtq, acsParameters.detumbleParameter.gainFull);
break;
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
magMomMtq, acsParameters.detumbleParameter.gainBdot);
break;
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL):
safeCtrlFailure(1, 0);
break;
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL):
safeCtrlFailure(0, 1);
break;
default:
@ -369,8 +347,8 @@ void AcsController::performDetumble() {
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
if (acsParameters.safeModeControllerParameters.useMekf) {
if (mekfData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
if (attitudeEstimationData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
}
@ -402,6 +380,68 @@ void AcsController::performDetumble() {
}
void AcsController::performPointingCtrl() {
bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and
sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid());
uint8_t useMekf = false;
switch (mode) {
case acs::PTG_IDLE:
useMekf = acsParameters.idleModeControllerParameters.useMekf;
break;
case acs::PTG_TARGET:
useMekf = acsParameters.targetModeControllerParameters.useMekf;
break;
case acs::PTG_TARGET_GS:
useMekf = acsParameters.gsTargetModeControllerParameters.useMekf;
break;
case acs::PTG_NADIR:
useMekf = acsParameters.nadirModeControllerParameters.useMekf;
break;
case acs::PTG_INERTIAL:
useMekf = acsParameters.inertialModeControllerParameters.useMekf;
break;
}
acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
fusedRotRateData.rotRateSource.isValid(), useMekf);
if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
ptgCtrlLostCounter++;
if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
ptgCtrlLostCounter = 0;
}
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
return;
} else {
ptgCtrlLostCounter = 0;
}
double quatBI[4] = {0, 0, 0, 0}, rotRateB[3] = {0, 0, 0};
switch (ptgCtrlStrat) {
case acs::ControlModeStrategy::PTGCTRL_MEKF:
std::memcpy(quatBI, attitudeEstimationData.quatMekf.value, sizeof(quatBI));
std::memcpy(rotRateB, attitudeEstimationData.satRotRateMekf.value, sizeof(rotRateB));
break;
case acs::ControlModeStrategy::PTGCTRL_STR:
quatBI[0] = sensorValues.strSet.caliQx.value;
quatBI[1] = sensorValues.strSet.caliQy.value;
quatBI[2] = sensorValues.strSet.caliQz.value;
quatBI[3] = sensorValues.strSet.caliQw.value;
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
break;
case acs::ControlModeStrategy::PTGCTRL_QUEST:
std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI));
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
break;
default:
sif::error << "AcsController: Invalid pointing mode strategy for performDetumble"
<< std::endl;
break;
}
uint8_t enableAntiStiction = true;
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
@ -428,8 +468,8 @@ void AcsController::performPointingCtrl() {
case acs::PTG_IDLE:
guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
@ -440,9 +480,9 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
break;
@ -450,8 +490,8 @@ void AcsController::performPointingCtrl() {
guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef,
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
acsParameters.targetModeControllerParameters.quatRef,
acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
@ -464,17 +504,17 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break;
case acs::PTG_TARGET_GS:
guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
@ -485,9 +525,9 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
break;
@ -495,8 +535,8 @@ void AcsController::performPointingCtrl() {
guidance.targetQuatPtgNadirThreeAxes(
timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
acsParameters.nadirModeControllerParameters.quatRef,
acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
@ -509,17 +549,17 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
break;
case acs::PTG_INERTIAL:
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
sizeof(targetQuat));
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef,
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
acsParameters.inertialModeControllerParameters.quatRef,
acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
@ -532,9 +572,9 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
break;
default:
@ -673,7 +713,7 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
ctrlValData.safeStrat.value = acs::SafeModeStrategy::SAFECTRL_OFF;
ctrlValData.safeStrat.value = acs::ControlModeStrategy::CTRL_OFF;
ctrlValData.setValidity(true, true);
}
}
@ -750,11 +790,12 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource);
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
// MEKF
// Attitude Estimation
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_QUEST, &quatQuest);
poolManager.subscribeForDiagPeriodicPacket({attitudeEstimationData.getSid(), enableHkSets, 10.0});
// Ctrl Values
localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
@ -771,7 +812,15 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
// Fused Rot Rate Sources
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL_SUSMGM, &rotRateOrthogonalSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL_SUSMGM, &rotRateParallelSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SUSMGM, &rotRateTotalSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_QUEST, &rotRateTotalQuest);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_STR, &rotRateTotalStr);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateSourcesData.getSid(), false, 10.0});
return returnvalue::OK;
}
@ -792,13 +841,15 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
case acsctrl::GPS_PROCESSED_DATA:
return &gpsDataProcessed;
case acsctrl::MEKF_DATA:
return &mekfData;
return &attitudeEstimationData;
case acsctrl::CTRL_VAL_DATA:
return &ctrlValData;
case acsctrl::ACTUATOR_CMD_DATA:
return &actuatorCmdData;
case acsctrl::FUSED_ROTATION_RATE_DATA:
return &fusedRotRateData;
case acsctrl::FUSED_ROTATION_RATE_SOURCES_DATA:
return &fusedRotRateSourcesData;
default:
return nullptr;
}

18
mission/controller/AcsController.h
View File

@ -17,6 +17,7 @@
#include <mission/acs/susMax1227Helpers.h>
#include <mission/config/torquer.h>
#include <mission/controller/acs/ActuatorCmd.h>
#include <mission/controller/acs/AttitudeEstimation.h>
#include <mission/controller/acs/FusedRotationEstimation.h>
#include <mission/controller/acs/Guidance.h>
#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
@ -67,6 +68,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
AcsParameters acsParameters;
SensorProcessing sensorProcessing;
AttitudeEstimation attitudeEstimation;
FusedRotationEstimation fusedRotationEstimation;
Navigation navigation;
ActuatorCmd actuatorCmd;
@ -82,7 +84,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
uint8_t detumbleCounter = 0;
uint8_t multipleRwUnavailableCounter = 0;
bool mekfInvalidFlag = false;
uint16_t mekfInvalidCounter = 0;
uint16_t ptgCtrlLostCounter = 0;
bool safeCtrlFailureFlag = false;
uint8_t safeCtrlFailureCounter = 0;
uint8_t resetMekfCount = 0;
@ -239,11 +241,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
PoolEntry<uint8_t> gpsSource = PoolEntry<uint8_t>();
// MEKF
acsctrl::MekfData mekfData;
// Attitude Estimation
acsctrl::AttitudeEstimationData attitudeEstimationData;
PoolEntry<double> quatMekf = PoolEntry<double>(4);
PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);
PoolEntry<uint8_t> mekfStatus = PoolEntry<uint8_t>();
PoolEntry<double> quatQuest = PoolEntry<double>(4);
// Ctrl Values
acsctrl::CtrlValData ctrlValData;
@ -264,6 +267,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<double> rotRateOrthogonal = PoolEntry<double>(3);
PoolEntry<double> rotRateParallel = PoolEntry<double>(3);
PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
PoolEntry<uint8_t> rotRateSource = PoolEntry<uint8_t>();
// Fused Rot Rate Sources
acsctrl::FusedRotRateSourcesData fusedRotRateSourcesData;
PoolEntry<double> rotRateOrthogonalSusMgm = PoolEntry<double>(3);
PoolEntry<double> rotRateParallelSusMgm = PoolEntry<double>(3);
PoolEntry<double> rotRateTotalSusMgm = PoolEntry<double>(3);
PoolEntry<double> rotRateTotalQuest = PoolEntry<double>(3);
PoolEntry<double> rotRateTotalStr = PoolEntry<double>(3);
// Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(INIT_DELAY);

1
mission/controller/PowerController.cpp
View File

@ -216,6 +216,7 @@ void PowerController::calculateStateOfCharge() {
pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
}
}
return;
}
// commit to dataset

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

@ -24,11 +24,20 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(onBoardParams.sampleTime);
break;
case 0x1:
parameterWrapper->set(onBoardParams.mekfViolationTimer);
parameterWrapper->set(onBoardParams.ptgCtrlLostTimer);
break;
case 0x2:
parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse);
break;
case 0x3:
parameterWrapper->set(onBoardParams.fusedRateFromStr);
break;
case 0x4:
parameterWrapper->set(onBoardParams.fusedRateFromQuest);
break;
case 0x5:
parameterWrapper->set(onBoardParams.questFilterWeight);
break;
default:
return INVALID_IDENTIFIER_ID;
}
@ -425,6 +434,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x9:
parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->set(idleModeControllerParameters.useMekf);
break;
default:
return INVALID_IDENTIFIER_ID;
}
@ -462,36 +474,39 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
parameterWrapper->set(targetModeControllerParameters.useMekf);
break;
case 0xB:
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
break;
case 0xC:
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
break;
case 0xD:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
break;
case 0xE:
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
break;
case 0xF:
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
break;
case 0x10:
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
break;
case 0x11:
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
break;
case 0x12:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
break;
case 0x13:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
break;
case 0x14:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
break;
case 0x15:
parameterWrapper->set(targetModeControllerParameters.blindRotRate);
break;
default:
@ -531,18 +546,21 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
parameterWrapper->set(gsTargetModeControllerParameters.useMekf);
break;
case 0xB:
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
break;
case 0xC:
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
break;
case 0xD:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
break;
case 0xE:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
break;
case 0xF:
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
break;
default:
@ -582,15 +600,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
parameterWrapper->set(nadirModeControllerParameters.useMekf);
break;
case 0xB:
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
break;
case 0xC:
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
break;
case 0xD:
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
break;
case 0xE:
parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
break;
default:
@ -630,12 +651,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
break;
case 0xA:
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
parameterWrapper->set(inertialModeControllerParameters.useMekf);
break;
case 0xB:
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
break;
case 0xC:
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
break;
case 0xD:
parameterWrapper->setVector(inertialModeControllerParameters.quatRef);
break;
default:

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

@ -18,8 +18,11 @@ class AcsParameters : public HasParametersIF {
struct OnBoardParams {
double sampleTime = 0.4; // [s]
uint16_t mekfViolationTimer = 750;
uint16_t ptgCtrlLostTimer = 750;
uint8_t fusedRateSafeDuringEclipse = true;
uint8_t fusedRateFromStr = false;
uint8_t fusedRateFromQuest = false;
double questFilterWeight = 0.0;
} onBoardParams;
struct InertiaEIVE {
@ -861,6 +864,7 @@ class AcsParameters : public HasParametersIF {
double deSatGainFactor = 1000;
uint8_t desatOn = true;
uint8_t enableAntiStiction = true;
uint8_t useMekf = false;
} pointingLawParameters;
struct IdleModeControllerParameters : PointingLawParameters {

111
mission/controller/acs/AttitudeEstimation.cpp Normal file
View File

@ -0,0 +1,111 @@
#include "AttitudeEstimation.h"
AttitudeEstimation::AttitudeEstimation(AcsParameters *acsParameters_) {
acsParameters = acsParameters_;
}
AttitudeEstimation::~AttitudeEstimation() {}
void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
meggert marked this conversation as resolved
muellerr commented 2023-12-06 15:42:23 +01:00
Review
Copy Link

some subroutines might be a good idea, a lot of if else if else blocks in this function..

some subroutines might be a good idea, a lot of if else if else blocks in this function..
acsctrl::MgmDataProcessed *mgmData,
acsctrl::AttitudeEstimationData *attitudeEstimation) {
if (not(susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and
mgmData->mgmVecTot.value and mgmData->magIgrfModel.isValid())) {
{
meggert marked this conversation as resolved Outdated
muellerr commented 2023-12-06 15:35:49 +01:00
Outdated
Review
Copy Link

maybe this block can be put into a subfunction?

maybe this block can be put into a subfunction?
PoolReadGuard pg{attitudeEstimation};
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(attitudeEstimation->quatQuest.value, ZERO_VEC4, 4 * sizeof(double));
attitudeEstimation->quatQuest.setValid(false);
}
}
return;
}
// Normalize Data
double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0},
normSusI[3] = {0, 0, 0};
VectorOperations<double>::normalize(susData->susVecTot.value, normMgmB, 3);
VectorOperations<double>::normalize(susData->sunIjkModel.value, normMgmI, 3);
VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normSusB, 3);
VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normSusI, 3);
// Create Helper Vectors
double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
helperSum[3] = {0, 0, 0};
VectorOperations<double>::cross(normSusB, normMgmB, normHelperB);
VectorOperations<double>::cross(normSusI, normMgmI, normHelperI);
VectorOperations<double>::normalize(normHelperB, normHelperB, 3);
VectorOperations<double>::normalize(normHelperI, normHelperI, 3);
VectorOperations<double>::cross(normHelperB, normHelperI, helperCross);
VectorOperations<double>::add(normHelperB, normHelperI, helperSum, 3);
// Sensor Weights
double kSus = 0, kMgm = 0;
kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2);
kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2);
// Weighted Vectors
double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},
kMgmVec[3] = {0, 0, 0}, kSumVec[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(normSusB, kSus, weightedSusB, 3);
VectorOperations<double>::mulScalar(normMgmB, kMgm, weightedMgmB, 3);
VectorOperations<double>::cross(weightedSusB, normSusI, kSusVec);
VectorOperations<double>::cross(weightedMgmB, normMgmI, kMgmVec);
VectorOperations<double>::add(kSusVec, kMgmVec, kSumVec, 3);
meggert marked this conversation as resolved Outdated
muellerr commented 2023-12-06 15:34:43 +01:00
Outdated
Review
Copy Link

imagine me

imagine me
// Some weird Angles
double alpha = (1 + VectorOperations<double>::dot(normHelperB, normHelperI)) *
(VectorOperations<double>::dot(weightedSusB, normSusI) +
VectorOperations<double>::dot(weightedMgmB, normMgmI)) +
VectorOperations<double>::dot(helperCross, kSumVec);
double beta = VectorOperations<double>::dot(helperSum, kSumVec);
double gamma = std::sqrt(std::pow(alpha, 2) + std::pow(beta, 2));
// I don't even know what this is supposed to be
double constPlus =
1. / (2 * std::sqrt(gamma * (gamma + alpha) *
(1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
double constMinus =
1. / (2 * std::sqrt(gamma * (gamma - alpha) *
(1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
// Calculate Quaternion
double qBI[4] = {0, 0, 0, 0}, qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0},
qRotVecPt1[3] = {0, 0, 0};
if (alpha >= 0) {
// Scalar Part
qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
// Rotational Vector Part
VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3);
VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3);
QuaternionOperations::normalize(qBI, qBI);
} else {
// Scalar Part
qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
// Rotational Vector Part
VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3);
VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
QuaternionOperations::normalize(qBI, qBI);
}
// Low Pass
if (VectorOperations<double>::norm(qOld, 4) != 0.0) {
QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI);
}
{
PoolReadGuard pg{attitudeEstimation};
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(attitudeEstimation->quatQuest.value, qBI, 4 * sizeof(double));
attitudeEstimation->quatQuest.setValid(true);
}
}
}

31
mission/controller/acs/AttitudeEstimation.h Normal file
View File

@ -0,0 +1,31 @@
#ifndef MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_
#define MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <cmath>
#include <iostream>
class AttitudeEstimation {
public:
AttitudeEstimation(AcsParameters *acsParameters_);
virtual ~AttitudeEstimation();
;
void quest(acsctrl::SusDataProcessed *susData, acsctrl::MgmDataProcessed *mgmData,
acsctrl::AttitudeEstimationData *attitudeEstimation);
protected:
private:
AcsParameters *acsParameters;
double qOld[4] = {0, 0, 0, 0};
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
};
#endif /* MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ */

1
mission/controller/acs/CMakeLists.txt
View File

@ -2,6 +2,7 @@ target_sources(
${LIB_EIVE_MISSION}
PRIVATE AcsParameters.cpp
ActuatorCmd.cpp
AttitudeEstimation.cpp
FusedRotationEstimation.cpp
Guidance.cpp
Igrf13Model.cpp

282
mission/controller/acs/FusedRotationEstimation.cpp
View File

@ -4,19 +4,220 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_)
acsParameters = acsParameters_;
}
void FusedRotationEstimation::estimateFusedRotationRateSafe(
void FusedRotationEstimation::estimateFusedRotationRate(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues,
acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
acsctrl::FusedRotRateData *fusedRotRateData) {
estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData);
estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData);
estimateFusedRotationRateSusMgm(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
fusedRotRateSourcesData);
if (fusedRotRateSourcesData->rotRateTotalStr.isValid() and
acsParameters->onBoardParams.fusedRateFromStr) {
PoolReadGuard pg(fusedRotRateData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value,
fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
fusedRotRateData->rotRateSource.setValid(true);
}
} else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and
acsParameters->onBoardParams.fusedRateFromQuest) {
PoolReadGuard pg(fusedRotRateData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value,
fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST;
fusedRotRateData->rotRateSource.setValid(true);
}
} else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(
fusedRotRateSourcesData->rotRateOrthogonalSusMgm.isValid());
std::memcpy(fusedRotRateData->rotRateParallel.value,
fusedRotRateSourcesData->rotRateParallelSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(
fusedRotRateSourcesData->rotRateParallelSusMgm.isValid());
std::memcpy(fusedRotRateData->rotRateTotal.value,
fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM;
fusedRotRateData->rotRateSource.setValid(true);
} else {
PoolReadGuard pg(fusedRotRateData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE;
fusedRotRateData->rotRateSource.setValid(true);
}
}
}
void FusedRotationEstimation::estimateFusedRotationRateStr(
ACS::SensorValues *sensorValues, const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if (not(sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
meggert marked this conversation as resolved Outdated
muellerr commented 2023-12-06 15:37:39 +01:00
Outdated
Review
Copy Link

maybe use the negation of all conditions here, move the invalid setting handling into the block, and put the block below without a return?

maybe use the negation of all conditions here, move the invalid setting handling into the block, and put the block below without a return?
sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
}
}
std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr));
return;
}
double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
if (VectorOperations<double>::norm(quatOldStr, 4) != 0 and timeDelta != 0) {
double quatOldInv[4] = {0, 0, 0, 0};
double quatDelta[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatOldStr, quatOldInv);
QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
QuaternionOperations::normalize(quatDelta);
}
double rotVec[3] = {0, 0, 0};
double angle = QuaternionOperations::getAngle(quatDelta);
if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
}
}
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
return;
}
VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
}
}
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
return;
}
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
}
}
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
return;
}
void FusedRotationEstimation::estimateFusedRotationRateQuest(
acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
meggert marked this conversation as resolved
muellerr commented 2023-12-06 15:41:12 +01:00
Review
Copy Link

same as above: Put the error handling code with the pre-emptive return at the top, save one level of indentation for the regular code.

same as above: Put the error handling code with the pre-emptive return at the top, save one level of indentation for the regular code.
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if (not attitudeEstimationData->quatQuest.isValid()) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
}
}
std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest));
}
if (VectorOperations<double>::norm(quatOldQuest, 4) != 0 and timeDelta != 0) {
double quatOldInv[4] = {0, 0, 0, 0};
double quatDelta[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatOldQuest, quatOldInv);
QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, quatDelta);
if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
QuaternionOperations::normalize(quatDelta);
}
double rotVec[3] = {0, 0, 0};
double angle = QuaternionOperations::getAngle(quatDelta);
if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
}
}
std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
return;
}
VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
}
}
std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
return;
}
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
}
}
std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
return;
}
void FusedRotationEstimation::estimateFusedRotationRateSusMgm(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and
not fusedRotRateData->rotRateTotal.isValid()) or
not fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) or
(not susDataProcessed->susVecTotDerivative.isValid() and
not mgmDataProcessed->mgmVecTotDerivative.isValid())) {
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false);
}
}
// store for calculation of angular acceleration
if (gyrDataProcessed->gyrVecTot.isValid()) {
@ -25,7 +226,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
return;
}
if (not susDataProcessed->susVecTot.isValid()) {
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData);
// store for calculation of angular acceleration
if (gyrDataProcessed->gyrVecTot.isValid()) {
std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
@ -49,7 +250,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
VectorOperations<double>::mulScalar(susDataProcessed->susVecTot.value, omegaParallel,
fusedRotRateParallel, 3);
} else {
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData);
// store for calculation of angular acceleration
if (gyrDataProcessed->gyrVecTot.isValid()) {
std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
@ -71,12 +272,18 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
VectorOperations<double>::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal);
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal,
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, fusedRotRateOrthogonal,
3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, fusedRotRateParallel, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
fusedRotRateData->setValidity(true, true);
fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(true);
std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, fusedRotRateParallel,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(true);
std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true);
}
}
// store for calculation of angular acceleration
@ -86,31 +293,44 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
}
void FusedRotationEstimation::estimateFusedRotationRateEclipse(
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if (not acsParameters->onBoardParams.fusedRateSafeDuringEclipse or
not gyrDataProcessed->gyrVecTot.isValid() or
VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) {
VectorOperations<double>::norm(fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3) == 0) {
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false);
}
}
return;
}
double angAccelB[3] = {0, 0, 0};
VectorOperations<double>::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3);
double fusedRotRateTotal[3] = {0, 0, 0};
VectorOperations<double>::add(fusedRotRateData->rotRateTotal.value, angAccelB, fusedRotRateTotal,
3);
VectorOperations<double>::add(fusedRotRateSourcesData->rotRateTotalSusMgm.value, angAccelB,
fusedRotRateTotal, 3);
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true);
}
}
}

24
mission/controller/acs/FusedRotationEstimation.h
View File

@ -2,28 +2,46 @@
#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
class FusedRotationEstimation {
public:
FusedRotationEstimation(AcsParameters *acsParameters_);
void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed,
void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
ACS::SensorValues *sensorValues,
acsctrl::AttitudeEstimationData *attitudeEstimationData,
const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
acsctrl::FusedRotRateData *fusedRotRateData);
protected:
private:
static constexpr double ZERO_VEC[3] = {0, 0, 0};
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
AcsParameters *acsParameters;
double quatOldQuest[4] = {0, 0, 0, 0};
double quatOldStr[4] = {0, 0, 0, 0};
double rotRateOldB[3] = {0, 0, 0};
void estimateFusedRotationRateSusMgm(acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateData *fusedRotRateData);
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
void estimateFusedRotationRateQuest(acsctrl::AttitudeEstimationData *attitudeEstimationData,
const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
void estimateFusedRotationRateStr(ACS::SensorValues *sensorValues, const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
};
#endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */

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

@ -495,24 +495,24 @@ void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double time
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
double *rwPseudoInv) {
bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid());
bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid());
bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
meggert marked this conversation as resolved
muellerr commented 2023-12-06 15:40:04 +01:00
Review
Copy Link

value is a ptr?

value is a ptr?
bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
if (rw1valid && rw2valid && rw3valid && rw4valid) {
if (rw1valid and rw2valid and rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
return returnvalue::OK;
} else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
} else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
} else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && rw2valid && !rw3valid && rw4valid) {
} else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
} else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
return returnvalue::OK;
} else {

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

@ -19,7 +19,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
ReturnValue_t MultiplicativeKalmanFilter::init(
const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, acsctrl::MekfData *mekfData,
const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters) { // valids for "model measurements"?
// check for valid mag/sun
if (validMagField_ && validSS && validSSModel && validMagModel) {
@ -191,7 +191,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
const bool validGYRs_, const double *magneticField_, const bool validMagField_,
const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
const double *magFieldJ, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters) {
// Check for GYR Measurements
int MDF = 0; // Matrix Dimension Factor
@ -1090,7 +1090,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
return MEKF_RUNNING;
}
ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::AttitudeEstimationData *mekfData) {
double resetQuaternion[4] = {0, 0, 0, 1};
double resetCovarianceMatrix[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}};
@ -1100,7 +1100,7 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
return MEKF_UNINITIALIZED;
}
void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,
void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData,
MekfStatus mekfStatus) {
{
PoolReadGuard pg(mekfData);
@ -1115,7 +1115,7 @@ void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mek
}
}
void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus,
void MultiplicativeKalmanFilter::updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus,
double quat[4], double satRotRate[3]) {
{
PoolReadGuard pg(mekfData);

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

@ -21,7 +21,7 @@ class MultiplicativeKalmanFilter {
MultiplicativeKalmanFilter();
virtual ~MultiplicativeKalmanFilter();
ReturnValue_t reset(acsctrl::MekfData *mekfData);
ReturnValue_t reset(acsctrl::AttitudeEstimationData *mekfData);
/* @brief: init() - This function initializes the Kalman Filter and will provide the first
* quaternion through the QUEST algorithm
@ -32,7 +32,7 @@ class MultiplicativeKalmanFilter {
*/
ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
const double *magFieldJ, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters);
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
@ -53,7 +53,7 @@ class MultiplicativeKalmanFilter {
const bool validGYRs_, const double *magneticField_,
const bool validMagField_, const double *sunDir_, const bool validSS,
const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, acsctrl::MekfData *mekfData,
const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters);
enum MekfStatus : uint8_t {
@ -99,8 +99,8 @@ class MultiplicativeKalmanFilter {
double biasGYR[3]; /*Between measured and estimated sat Rate*/
/*Parameter INIT*/
/*Functions*/
void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus);
void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus, double quat[4],
double satRotRate[3]);
};

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

@ -16,7 +16,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MekfData *mekfData, AcsParameters *acsParameters) {
acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters) {
double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;
@ -41,7 +42,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
}
}
void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
void Navigation::resetMekf(acsctrl::AttitudeEstimationData *mekfData) {
mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
}
@ -54,7 +55,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
{
PoolReadGuard pg(gpsDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->source = acs::GpsSource::SPG4;
gpsDataProcessed->source = acs::gps::Source::SPG4;
gpsDataProcessed->source.setValid(true);
std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
gpsDataProcessed->gpsPosition.setValid(true);
@ -66,7 +67,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
{
PoolReadGuard pg(gpsDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->source = acs::GpsSource::NONE;
gpsDataProcessed->source = acs::gps::Source::NONE;
gpsDataProcessed->source.setValid(true);
std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
gpsDataProcessed->gpsPosition.setValid(false);

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

@ -17,9 +17,9 @@ class Navigation {
ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters);
void resetMekf(acsctrl::MekfData *mekfData);
void resetMekf(acsctrl::AttitudeEstimationData *mekfData);
ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);

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

@ -1,7 +1,5 @@
#include "SensorProcessing.h"
#include <fsfw/timemanager/Stopwatch.h>
SensorProcessing::SensorProcessing() {}
SensorProcessing::~SensorProcessing() {}
@ -17,7 +15,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
// ------------------------------------------------
double magIgrfModel[3] = {0.0, 0.0, 0.0};
bool gpsValid = false;
if (gpsDataProcessed->source.value != acs::GpsSource::NONE) {
if (gpsDataProcessed->source.value != acs::gps::Source::NONE) {
// There seems to be a bug here, which causes the model vector to drift until infinity, if the
// model class is not initialized new every time. Works for now, but should be investigated.
Igrf13Model igrf13;
@ -527,9 +525,9 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
// init variables
double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
gpsVelocityE[3] = {0, 0, 0};
uint8_t gpsSource = acs::GpsSource::NONE;
uint8_t gpsSource = acs::gps::Source::NONE;
// We do not trust the GPS and therefore it shall die here if SPG4 is running
if (gpsDataProcessed->source.value == acs::GpsSource::SPG4 and gpsParameters->useSpg4) {
if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) {
MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude,
gdLongitude, altitude);
double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
@ -572,7 +570,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
validSavedPosSatE = true;
gpsSource = acs::GpsSource::GPS;
gpsSource = acs::gps::Source::GPS;
}
{
PoolReadGuard pg(gpsDataProcessed);

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

@ -7,18 +7,18 @@ Detumble::Detumble() {}
Detumble::~Detumble() {}
acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
acs::ControlModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
const bool satRotRateValid,
const bool magFieldRateValid,
const bool useFullDetumbleLaw) {
if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (satRotRateValid and useFullDetumbleLaw) {
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL;
return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL;
} else if (magFieldRateValid) {
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
}

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

@ -11,7 +11,7 @@ class Detumble {
Detumble();
virtual ~Detumble();
acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
acs::ControlModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
const bool magFieldRateValid,
const bool useFullDetumbleLaw);

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

@ -10,6 +10,22 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_
PtgCtrl::~PtgCtrl() {}
acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
const bool magFieldValid, const bool mekfValid, const bool strValid, const bool questValid,
const bool fusedRateValid, const uint8_t rotRateSource, const uint8_t mekfEnabled) {
if (not magFieldValid) {
return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) {
return acs::ControlModeStrategy::PTGCTRL_MEKF;
} else if (strValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
return acs::ControlModeStrategy::PTGCTRL_STR;
} else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
return acs::ControlModeStrategy::PTGCTRL_QUEST;
} else {
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
}
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
double *torqueRws) {

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

@ -2,6 +2,7 @@
#define PTGCTRL_H_
#include <math.h>
#include <mission/acs/defs.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <stdio.h>
@ -9,7 +10,7 @@
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
* torque for the reaction wheels, and magnetic Field strength for magnetorquer 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
@ -21,6 +22,12 @@ class PtgCtrl {
PtgCtrl(AcsParameters *acsParameters_);
virtual ~PtgCtrl();
acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool strValid, const bool questValid,
const bool fusedRateValid,
const uint8_t rotRateSource,
const uint8_t mekfEnabled);
/* @brief: Calculates the needed torque for the pointing control mechanism
*/
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
@ -36,7 +43,7 @@ class PtgCtrl {
const int32_t speedRw3, double *mgtDpDes);
/* @brief: Commands the stiction torque in case wheel speed is to low
* torqueCommand modified torque after antistiction
* torqueCommand modified torque after anti-stiction
*/
void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed);

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

@ -9,40 +9,40 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
SafeCtrl::~SafeCtrl() {}
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const bool fusedRateTotalValid,
const uint8_t mekfEnabled,
const uint8_t gyrEnabled,
const uint8_t dampingEnabled) {
if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) {
return acs::SafeModeStrategy::SAFECTRL_MEKF;
return acs::ControlModeStrategy::SAFECTRL_MEKF;
} else if (sunDirValid) {
if (gyrEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_GYR;
return acs::ControlModeStrategy::SAFECTRL_GYR;
} else if (not gyrEnabled and fusedRateTotalValid) {
return acs::SafeModeStrategy::SAFECTRL_SUSMGM;
return acs::ControlModeStrategy::SAFECTRL_SUSMGM;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
} else if (not sunDirValid) {
if (dampingEnabled) {
if (gyrEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
} else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
} else if (not dampingEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING;
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
}

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

@ -12,7 +12,7 @@ class SafeCtrl {
SafeCtrl(AcsParameters *acsParameters_);
virtual ~SafeCtrl();
acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
acs::ControlModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const bool fusedRateTotalValid, const uint8_t mekfEnabled,
const uint8_t gyrEnabled, const uint8_t dampingEnabled);

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

@ -20,6 +20,7 @@ enum SetIds : uint32_t {
CTRL_VAL_DATA,
ACTUATOR_CMD_DATA,
FUSED_ROTATION_RATE_DATA,
FUSED_ROTATION_RATE_SOURCES_DATA,
TLE_SET,
};
@ -96,6 +97,7 @@ enum PoolIds : lp_id_t {
SAT_ROT_RATE_MEKF,
QUAT_MEKF,
MEKF_STATUS,
QUAT_QUEST,
// Ctrl Values
SAFE_STRAT,
TGT_QUAT,
@ -110,6 +112,13 @@ enum PoolIds : lp_id_t {
ROT_RATE_ORTHOGONAL,
ROT_RATE_PARALLEL,
ROT_RATE_TOTAL,
ROT_RATE_SOURCE,
// Fused Rotation Rate Sources
ROT_RATE_ORTHOGONAL_SUSMGM,
ROT_RATE_PARALLEL_SUSMGM,
ROT_RATE_TOTAL_SUSMGM,
ROT_RATE_TOTAL_QUEST,
ROT_RATE_TOTAL_STR,
};
static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6;
@ -119,10 +128,11 @@ static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6;
static constexpr uint8_t MEKF_SET_ENTRIES = 3;
static constexpr uint8_t ATTITUDE_ESTIMATION_SET_ENTRIES = 4;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3;
static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 4;
static constexpr uint8_t FUSED_ROT_RATE_SOURCES_SET_ENTRIES = 5;
/**
* @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status.
@ -246,13 +256,14 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
private:
};
class MekfData : public StaticLocalDataSet<MEKF_SET_ENTRIES> {
class AttitudeEstimationData : public StaticLocalDataSet<ATTITUDE_ESTIMATION_SET_ENTRIES> {
public:
MekfData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {}
AttitudeEstimationData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {}
lp_vec_t<double, 4> quatMekf = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this);
lp_vec_t<double, 3> satRotRateMekf = lp_vec_t<double, 3>(sid.objectId, SAT_ROT_RATE_MEKF, this);
lp_var_t<uint8_t> mekfStatus = lp_var_t<uint8_t>(sid.objectId, MEKF_STATUS, this);
lp_vec_t<double, 4> quatQuest = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this);
private:
};
@ -291,6 +302,25 @@ class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL, this);
lp_vec_t<double, 3> rotRateParallel = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL, this);
lp_vec_t<double, 3> rotRateTotal = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL, this);
lp_var_t<uint8_t> rotRateSource = lp_var_t<uint8_t>(sid.objectId, ROT_RATE_SOURCE, this);
private:
};
class FusedRotRateSourcesData : public StaticLocalDataSet<FUSED_ROT_RATE_SOURCES_SET_ENTRIES> {
public:
FusedRotRateSourcesData(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_SOURCES_DATA) {}
lp_vec_t<double, 3> rotRateOrthogonalSusMgm =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL_SUSMGM, this);
lp_vec_t<double, 3> rotRateParallelSusMgm =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL_SUSMGM, this);
lp_vec_t<double, 3> rotRateTotalSusMgm =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_SUSMGM, this);
lp_vec_t<double, 3> rotRateTotalQuest =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_QUEST, this);
lp_vec_t<double, 3> rotRateTotalStr = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_STR, this);
private:
};

2
mission/system/acs/StrFdir.cpp
View File

@ -6,7 +6,7 @@ StrFdir::StrFdir(object_id_t strObject)
: DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {}
ReturnValue_t StrFdir::eventReceived(EventMessage* event) {
if (event->getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) {
if (event->getEvent() == acs::PTG_CTRL_NO_ATTITUDE_INFORMATION) {
setFaulty(event->getEvent());
return returnvalue::OK;
}