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

make detumble relevant rotational rate always observable

Browse Source
This commit is contained in:
Marius Eggert 2024-06-12 16:32:47 +02:00
parent f19b129609
commit abbbb0cabb
4 changed files with 40 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
mission/controller/AcsController.cpp
View File

@ -232,7 +232,8 @@ void AcsController::performSafe() {
acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf, fusedRotRateSourcesData.rotRateTotalSusMgm.isValid(),
acsParameters.safeModeControllerParameters.useMekf,
acsParameters.safeModeControllerParameters.useGyr, acsParameters.safeModeControllerParameters.useGyr,
acsParameters.safeModeControllerParameters.dampingDuringEclipse); acsParameters.safeModeControllerParameters.dampingDuringEclipse);
switch (safeCtrlStrat) { switch (safeCtrlStrat) {
@ -251,9 +252,10 @@ void AcsController::performSafe() {
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;
case (acs::ControlModeStrategy::SAFECTRL_SUSMGM): case (acs::ControlModeStrategy::SAFECTRL_SUSMGM):
safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value, safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value,
fusedRotRateData.rotRateParallel.value, fusedRotRateSourcesData.rotRateTotalSusMgm.value,
fusedRotRateData.rotRateOrthogonal.value, fusedRotRateSourcesData.rotRateParallelSusMgm.value,
fusedRotRateSourcesData.rotRateOrthogonalSusMgm.value,
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
@ -267,8 +269,8 @@ void AcsController::performSafe() {
break; break;
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM): case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value, safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq, fusedRotRateSourcesData.rotRateTotalSusMgm.value, sunTargetDir,
errAng); magMomMtq, errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;
@ -355,7 +357,7 @@ void AcsController::performPointingCtrl() {
} }
acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy( acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid, mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(), attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotalSource.isValid(),
fusedRotRateData.rotRateSource.value, useMekf); fusedRotRateData.rotRateSource.value, useMekf);
if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL or if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL or
@ -387,11 +389,11 @@ void AcsController::performPointingCtrl() {
quatBI[1] = sensorValues.strSet.caliQy.value; quatBI[1] = sensorValues.strSet.caliQy.value;
quatBI[2] = sensorValues.strSet.caliQz.value; quatBI[2] = sensorValues.strSet.caliQz.value;
quatBI[3] = sensorValues.strSet.caliQw.value; quatBI[3] = sensorValues.strSet.caliQw.value;
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB)); std::memcpy(rotRateB, fusedRotRateData.rotRateTotalSource.value, sizeof(rotRateB));
break; break;
case acs::ControlModeStrategy::PTGCTRL_QUEST: case acs::ControlModeStrategy::PTGCTRL_QUEST:
std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI)); std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI));
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB)); std::memcpy(rotRateB, fusedRotRateData.rotRateTotalSource.value, sizeof(rotRateB));
break; break;
default: default:
sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl" sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl"
@ -829,9 +831,8 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0}); poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0});
// Fused Rot Rate // Fused Rot Rate
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SUSMGM, &rotRateTotalSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SOURCE, &rotRateTotalSource);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0}); poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
// Fused Rot Rate Sources // Fused Rot Rate Sources

5
mission/controller/AcsController.h
View File

@ -271,9 +271,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
// Fused Rot Rate // Fused Rot Rate
acsctrl::FusedRotRateData fusedRotRateData; acsctrl::FusedRotRateData fusedRotRateData;
PoolEntry<double> rotRateOrthogonal = PoolEntry<double>(3); PoolEntry<double> rotRateTotalSusMgm = PoolEntry<double>(3);
PoolEntry<double> rotRateParallel = PoolEntry<double>(3); PoolEntry<double> rotRateTotalSource = PoolEntry<double>(3);
PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
PoolEntry<uint8_t> rotRateSource = PoolEntry<uint8_t>(); PoolEntry<uint8_t> rotRateSource = PoolEntry<uint8_t>();
// Fused Rot Rate Sources // Fused Rot Rate Sources

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

@ -19,13 +19,9 @@ void FusedRotationEstimation::estimateFusedRotationRate(
acsParameters->onBoardParams.fusedRateFromStr)) { acsParameters->onBoardParams.fusedRateFromStr)) {
PoolReadGuard pg(fusedRotRateData); PoolReadGuard pg(fusedRotRateData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateTotalSource.value,
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)); fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true); fusedRotRateData->rotRateTotalSource.setValid(true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR; fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
fusedRotRateData->rotRateSource.setValid(true); fusedRotRateData->rotRateSource.setValid(true);
} }
@ -34,41 +30,37 @@ void FusedRotationEstimation::estimateFusedRotationRate(
acsParameters->onBoardParams.fusedRateFromQuest)) { acsParameters->onBoardParams.fusedRateFromQuest)) {
PoolReadGuard pg(fusedRotRateData); PoolReadGuard pg(fusedRotRateData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateTotalSource.value,
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)); fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true); fusedRotRateData->rotRateTotalSource.setValid(true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST; fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST;
fusedRotRateData->rotRateSource.setValid(true); fusedRotRateData->rotRateSource.setValid(true);
} }
} else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) { } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, std::memcpy(fusedRotRateData->rotRateTotalSource.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)); fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM; fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM;
fusedRotRateData->rotRateSource.setValid(true); fusedRotRateData->rotRateSource.setValid(true);
} else { } else {
PoolReadGuard pg(fusedRotRateData); PoolReadGuard pg(fusedRotRateData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateTotalSource.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateData->rotRateTotalSource.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE; fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE;
fusedRotRateData->rotRateSource.setValid(true); fusedRotRateData->rotRateSource.setValid(true);
} }
} }
if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
std::memcpy(fusedRotRateData->rotRateTotalSusMgm.value,
fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateTotalSusMgm.setValid(false);
} else {
PoolReadGuard pg(fusedRotRateData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateTotalSusMgm.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateTotalSusMgm.setValid(false);
}
}
} }
void FusedRotationEstimation::estimateFusedRotationRateStr( void FusedRotationEstimation::estimateFusedRotationRateStr(

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

@ -129,9 +129,8 @@ enum PoolIds : lp_id_t {
RW_TARGET_SPEED, RW_TARGET_SPEED,
MTQ_TARGET_DIPOLE, MTQ_TARGET_DIPOLE,
// Fused Rotation Rate // Fused Rotation Rate
ROT_RATE_ORTHOGONAL, ROT_RATE_TOTAL_SUSMGM,
ROT_RATE_PARALLEL, ROT_RATE_TOTAL_SOURCE,
ROT_RATE_TOTAL,
ROT_RATE_SOURCE, ROT_RATE_SOURCE,
// Fused Rotation Rate Sources // Fused Rotation Rate Sources
ROT_RATE_ORTHOGONAL_SUSMGM, ROT_RATE_ORTHOGONAL_SUSMGM,
@ -151,7 +150,7 @@ static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6;
static constexpr uint8_t ATTITUDE_ESTIMATION_SET_ENTRIES = 4; static constexpr uint8_t ATTITUDE_ESTIMATION_SET_ENTRIES = 4;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 4; static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3;
static constexpr uint8_t FUSED_ROT_RATE_SOURCES_SET_ENTRIES = 5; static constexpr uint8_t FUSED_ROT_RATE_SOURCES_SET_ENTRIES = 5;
/** /**
@ -318,10 +317,10 @@ class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
FusedRotRateData(HasLocalDataPoolIF* hkOwner) FusedRotRateData(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_DATA) {} : StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_DATA) {}
lp_vec_t<double, 3> rotRateOrthogonal = lp_vec_t<double, 3> rotRateTotalSusMgm =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL, this); lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_SUSMGM, this);
lp_vec_t<double, 3> rotRateParallel = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL, this); lp_vec_t<double, 3> rotRateTotalSource =
lp_vec_t<double, 3> rotRateTotal = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL, this); lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_SOURCE, this);
lp_var_t<uint8_t> rotRateSource = lp_var_t<uint8_t>(sid.objectId, ROT_RATE_SOURCE, this); lp_var_t<uint8_t> rotRateSource = lp_var_t<uint8_t>(sid.objectId, ROT_RATE_SOURCE, this);
private: private: