Different Rot Rate Source for Detumble #902

Merged
muellerr merged 14 commits from use-different-rot-rate-for-detumble into main 2024-06-26 15:36:32 +02:00
8 changed files with 67 additions and 53 deletions

View File

@ -20,6 +20,10 @@ will consitute of a breaking change warranting a new major release:
- STR quaternions are now used by the `MEKF` by default - STR quaternions are now used by the `MEKF` by default
- Changed nominal `SUS Assembly` side to `B Side`. - Changed nominal `SUS Assembly` side to `B Side`.
- Changed source for state machine of detumbling to SUS and MGM only.
- Changed `FusedRotRateData` dataset to always display rotation rate from SUS and MGM.
- Solution from `QUEST` will be set to invalid if sun vector and magnetic field vector are too close
to each other.
- Changed collection intervals of dataset collection - Changed collection intervals of dataset collection
- `GPS Controller`: `GPS Set` to 60s - `GPS Controller`: `GPS Set` to 60s
- `MTQ Handler`: `HK with Torque`, `HK without Torque` to 60s - `MTQ Handler`: `HK with Torque`, `HK without Torque` to 60s

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"
@ -555,8 +557,8 @@ void AcsController::performPointingCtrl() {
void AcsController::handleDetumbling() { void AcsController::handleDetumbling() {
switch (detumbleState) { switch (detumbleState) {
case DetumbleState::NO_DETUMBLE: case DetumbleState::NO_DETUMBLE:
if (fusedRotRateData.rotRateTotal.isValid() and if (fusedRotRateData.rotRateTotalSusMgm.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) > VectorOperations<double>::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) { acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++; detumbleCounter++;
} else if (detumbleCounter > 0) { } else if (detumbleCounter > 0) {
@ -599,8 +601,8 @@ void AcsController::handleDetumbling() {
detumbleState = DetumbleState::NO_DETUMBLE; detumbleState = DetumbleState::NO_DETUMBLE;
break; break;
case DetumbleState::IN_DETUMBLE: case DetumbleState::IN_DETUMBLE:
if (fusedRotRateData.rotRateTotal.isValid() and if (fusedRotRateData.rotRateTotalSusMgm.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) < VectorOperations<double>::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) { acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++; detumbleCounter++;
} else if (detumbleCounter > 0) { } else if (detumbleCounter > 0) {
@ -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, 30.0}); poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 30.0});
// Fused Rot Rate // Fused Rot Rate
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SUSMGM, &rotRateTotSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SOURCE, &rotRateTotSource);
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, 30.0}); poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 30.0});
// Fused Rot Rate Sources // Fused Rot Rate Sources

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> rotRateTotSusMgm = PoolEntry<double>(3);
PoolEntry<double> rotRateParallel = PoolEntry<double>(3); PoolEntry<double> rotRateTotSource = 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

View File

@ -38,6 +38,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x5: case 0x5:
parameterWrapper->set(onBoardParams.questFilterWeight); parameterWrapper->set(onBoardParams.questFilterWeight);
break; break;
case 0x6:
parameterWrapper->set(onBoardParams.questAngleLimit);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }

View File

@ -26,6 +26,7 @@ class AcsParameters : public HasParametersIF {
uint8_t fusedRateFromStr = true; uint8_t fusedRateFromStr = true;
uint8_t fusedRateFromQuest = true; uint8_t fusedRateFromQuest = true;
double questFilterWeight = 0.9; double questFilterWeight = 0.9;
double questAngleLimit = 5 * DEG2RAD;
} onBoardParams; } onBoardParams;
struct InertiaEIVE { struct InertiaEIVE {

View File

@ -29,6 +29,20 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normMgmB, 3); VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normMgmB, 3);
VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normMgmI, 3); VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normMgmI, 3);
if ((std::acos(VectorOperations<double>::dot(normSusB, normMgmB)) <
acsParameters->onBoardParams.questAngleLimit) or
(std::acos(VectorOperations<double>::dot(normSusI, normMgmI)) <
acsParameters->onBoardParams.questAngleLimit)) {
{
PoolReadGuard pg{attitudeEstimationData};
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(attitudeEstimationData->quatQuest.value, ZERO_VEC4, 4 * sizeof(double));
attitudeEstimationData->quatQuest.setValid(false);
}
}
return;
}
// Create Helper Vectors // Create Helper Vectors
double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0}, double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
helperSum[3] = {0, 0, 0}; helperSum[3] = {0, 0, 0};

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,38 @@ 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->rotRateTotalSource.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(true);
} 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(

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_TOT_SUSMGM,
ROT_RATE_PARALLEL, ROT_RATE_TOT_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_TOT_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_TOT_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: