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

Fused Rotation Rate for Safe Mode #867

Merged
meggert merged 5 commits from fused-rot-rate-safe into main 2024-02-26 13:11:42 +01:00
Conversation 2 Commits 5 Files Changed 5 +8 -12
4 changed files with 8 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 96acca4847 - Show all commits

5
mission/controller/AcsController.cpp
View File

@ -173,7 +173,7 @@ void AcsController::performAttitudeControl() {
&susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData); attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData);
fusedRotationEstimation.estimateFusedRotationRate( fusedRotationEstimation.estimateFusedRotationRate(
modeIsSafe, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
&attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData); &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &attitudeEstimationData, &acsParameters); &susDataProcessed, &attitudeEstimationData, &acsParameters);
@ -914,9 +914,6 @@ void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
PoolReadGuard pg(&rw4SpeedSet); PoolReadGuard pg(&rw4SpeedSet);
rw4SpeedSet.setRwSpeed(0, 10); rw4SpeedSet.setRwSpeed(0, 10);
} }
modeIsSafe = true;
} else {
modeIsSafe = false;
} }
meggert marked this conversation as resolved Outdated
muellerr commented 2024-02-26 10:02:36 +01:00
Outdated
Review
Copy Link

can we use a member function which simply does mode == SAFE to avoid another state variable or would that not work for some reason?

can we use a member function which simply does `mode == SAFE` to avoid another state variable or would that not work for some reason?
meggert commented 2024-02-26 10:15:15 +01:00
Outdated
Review
Copy Link

how about this

how about this
muellerr commented 2024-02-26 10:23:09 +01:00
Outdated
Review
Copy Link

nice

nice
if (submode == acs::SafeSubmode::DETUMBLE) { if (submode == acs::SafeSubmode::DETUMBLE) {
detumbleState = DetumbleState::IN_DETUMBLE; detumbleState = DetumbleState::IN_DETUMBLE;

2
mission/controller/AcsController.h
View File

@ -62,8 +62,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
double timeDelta = 0.0; double timeDelta = 0.0;
timeval oldTimeRelative; timeval oldTimeRelative;
bool modeIsSafe = true;
AcsParameters acsParameters; AcsParameters acsParameters;
SensorProcessing sensorProcessing; SensorProcessing sensorProcessing;
AttitudeEstimation attitudeEstimation; AttitudeEstimation attitudeEstimation;

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

@ -5,7 +5,7 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_)
} }
void FusedRotationEstimation::estimateFusedRotationRate( void FusedRotationEstimation::estimateFusedRotationRate(
const bool modeIsSafe, acsctrl::SusDataProcessed *susDataProcessed, const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
ACS::SensorValues *sensorValues, acsctrl::AttitudeEstimationData *attitudeEstimationData, ACS::SensorValues *sensorValues, acsctrl::AttitudeEstimationData *attitudeEstimationData,
const double timeDelta, acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData, const double timeDelta, acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
@ -15,8 +15,8 @@ void FusedRotationEstimation::estimateFusedRotationRate(
estimateFusedRotationRateSusMgm(susDataProcessed, mgmDataProcessed, gyrDataProcessed, estimateFusedRotationRateSusMgm(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
fusedRotRateSourcesData); fusedRotRateSourcesData);
if (not modeIsSafe and fusedRotRateSourcesData->rotRateTotalStr.isValid() and if (not(mode == acs::AcsMode::SAFE) and (fusedRotRateSourcesData->rotRateTotalStr.isValid() and
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->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
@ -29,8 +29,9 @@ void FusedRotationEstimation::estimateFusedRotationRate(
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR; fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
fusedRotRateData->rotRateSource.setValid(true); fusedRotRateData->rotRateSource.setValid(true);
} }
} else if (not modeIsSafe and fusedRotRateSourcesData->rotRateTotalQuest.isValid() and } else if (not(mode == acs::AcsMode::SAFE) and
acsParameters->onBoardParams.fusedRateFromQuest) { (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and
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->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));

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

@ -12,7 +12,7 @@ class FusedRotationEstimation {
public: public:
FusedRotationEstimation(AcsParameters *acsParameters_); FusedRotationEstimation(AcsParameters *acsParameters_);
void estimateFusedRotationRate(const bool modeIsSafe, acsctrl::SusDataProcessed *susDataProcessed, void estimateFusedRotationRate(const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
ACS::SensorValues *sensorValues, ACS::SensorValues *sensorValues,