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

added fused rotation calculation to detumble and write detumble strat to dataset
Some checks are pending
EIVE/eive-obsw/pipeline/head Build started...
Details
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-08-01 16:37:42 +02:00
parent 1f61f7d2e8
commit bea918e861
2 changed files with 21 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
mission/controller/AcsController.cpp
View File

@ -273,6 +273,8 @@ void AcsController::performDetumble() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters); &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters); &susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
@ -345,7 +347,7 @@ void AcsController::performDetumble() {
startTransition(mode, acs::SafeSubmode::DEFAULT); startTransition(mode, acs::SafeSubmode::DEFAULT);
} }
disableCtrlValData(); updateCtrlValData(safeCtrlStrat);
updateActuatorCmdData(cmdDipoleMtqs); updateActuatorCmdData(cmdDipoleMtqs);
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
acsParameters.magnetorquerParameter.torqueDuration); acsParameters.magnetorquerParameter.torqueDuration);
@ -616,6 +618,23 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
} }
} }
void AcsController::updateCtrlValData(uint8_t safeModeStrat) {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = 0;
ctrlValData.errAng.setValid(false);
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
ctrlValData.tgtRotRate.setValid(false);
ctrlValData.safeStrat.value = safeModeStrat;
ctrlValData.safeStrat.setValid(true);
ctrlValData.setValidity(true, false);
}
}
void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) { void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
PoolReadGuard pg(&ctrlValData); PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
@ -646,17 +665,6 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu
} }
} }
void AcsController::disableCtrlValData() {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double));
ctrlValData.errAng.value = 0;
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
ctrlValData.setValidity(false, true);
}
}
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) { LocalDataPoolManager &poolManager) {
// MGM Raw // MGM Raw

2
mission/controller/AcsController.h
View File

@ -117,10 +117,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void updateActuatorCmdData(const int16_t* mtqTargetDipole); void updateActuatorCmdData(const int16_t* mtqTargetDipole);
void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed, void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
const int16_t* mtqTargetDipole); const int16_t* mtqTargetDipole);
void updateCtrlValData(uint8_t safeModeStrat);
void updateCtrlValData(double errAng, uint8_t safeModeStrat); void updateCtrlValData(double errAng, uint8_t safeModeStrat);
void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng, void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
const double* tgtRotRate); const double* tgtRotRate);
void disableCtrlValData();
/* ACS Sensor Values */ /* ACS Sensor Values */
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;