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

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
Conversation 0 Commits 14 Files Changed 8 +67 -53
8 changed files with 67 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CHANGELOG.md
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
- 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
- `GPS Controller`: `GPS Set` to 60s
- `MTQ Handler`: `HK with Torque`, `HK without Torque` to 60s

33
mission/controller/AcsController.cpp
View File

@ -232,7 +232,8 @@ void AcsController::performSafe() {
acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
fusedRotRateSourcesData.rotRateTotalSusMgm.isValid(),
acsParameters.safeModeControllerParameters.useMekf,
acsParameters.safeModeControllerParameters.useGyr,
acsParameters.safeModeControllerParameters.dampingDuringEclipse);
switch (safeCtrlStrat) {
@ -251,9 +252,10 @@ void AcsController::performSafe() {
safeCtrlFailureCounter = 0;
break;
case (acs::ControlModeStrategy::SAFECTRL_SUSMGM):
safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
fusedRotRateData.rotRateParallel.value,
fusedRotRateData.rotRateOrthogonal.value,
safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value,
fusedRotRateSourcesData.rotRateTotalSusMgm.value,
fusedRotRateSourcesData.rotRateParallelSusMgm.value,
fusedRotRateSourcesData.rotRateOrthogonalSusMgm.value,
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
@ -267,8 +269,8 @@ void AcsController::performSafe() {
break;
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq,
errAng);
fusedRotRateSourcesData.rotRateTotalSusMgm.value, sunTargetDir,
magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
@ -355,7 +357,7 @@ void AcsController::performPointingCtrl() {
}
acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotalSource.isValid(),
fusedRotRateData.rotRateSource.value, useMekf);
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[2] = sensorValues.strSet.caliQz.value;
quatBI[3] = sensorValues.strSet.caliQw.value;
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
std::memcpy(rotRateB, fusedRotRateData.rotRateTotalSource.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));
std::memcpy(rotRateB, fusedRotRateData.rotRateTotalSource.value, sizeof(rotRateB));
break;
default:
sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl"
@ -555,8 +557,8 @@ void AcsController::performPointingCtrl() {
void AcsController::handleDetumbling() {
switch (detumbleState) {
case DetumbleState::NO_DETUMBLE:
if (fusedRotRateData.rotRateTotal.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
if (fusedRotRateData.rotRateTotalSusMgm.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
} else if (detumbleCounter > 0) {
@ -599,8 +601,8 @@ void AcsController::handleDetumbling() {
detumbleState = DetumbleState::NO_DETUMBLE;
break;
case DetumbleState::IN_DETUMBLE:
if (fusedRotRateData.rotRateTotal.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
if (fusedRotRateData.rotRateTotalSusMgm.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
} else if (detumbleCounter > 0) {
@ -829,9 +831,8 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 30.0});
// Fused Rot Rate
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_TOT_SUSMGM, &rotRateTotSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SOURCE, &rotRateTotSource);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 30.0});
// Fused Rot Rate Sources

5
mission/controller/AcsController.h
View File

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

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

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

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

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

14
mission/controller/acs/AttitudeEstimation.cpp
View File

@ -29,6 +29,20 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normMgmB, 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
double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
helperSum[3] = {0, 0, 0};

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

@ -19,13 +19,9 @@ void FusedRotationEstimation::estimateFusedRotationRate(
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,
std::memcpy(fusedRotRateData->rotRateTotalSource.value,
fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
fusedRotRateData->rotRateTotalSource.setValid(true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
fusedRotRateData->rotRateSource.setValid(true);
}
@ -34,41 +30,38 @@ void FusedRotationEstimation::estimateFusedRotationRate(
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,
std::memcpy(fusedRotRateData->rotRateTotalSource.value,
fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
fusedRotRateData->rotRateTotalSource.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,
std::memcpy(fusedRotRateData->rotRateTotalSource.value,
fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
fusedRotRateData->rotRateTotalSource.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);
std::memcpy(fusedRotRateData->rotRateTotalSource.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateTotalSource.setValid(false);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE;
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(

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

@ -129,9 +129,8 @@ enum PoolIds : lp_id_t {
RW_TARGET_SPEED,
MTQ_TARGET_DIPOLE,
// Fused Rotation Rate
ROT_RATE_ORTHOGONAL,
ROT_RATE_PARALLEL,
ROT_RATE_TOTAL,
ROT_RATE_TOT_SUSMGM,
ROT_RATE_TOT_SOURCE,
ROT_RATE_SOURCE,
// Fused Rotation Rate Sources
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 CTRL_VAL_SET_ENTRIES = 5;
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;
/**
@ -318,10 +317,10 @@ class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
FusedRotRateData(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_DATA) {}
lp_vec_t<double, 3> rotRateOrthogonal =
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_vec_t<double, 3> rotRateTotalSusMgm =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOT_SUSMGM, this);
lp_vec_t<double, 3> rotRateTotalSource =
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);
private: