Merge pull request 'Bugfixes for Detumble Mode' (#764) from acs-detumble-bugfixes into main
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good

Reviewed-on: #764
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
This commit is contained in:
Robin Müller 2023-08-02 10:43:59 +02:00
commit 7f3c71f4dc
3 changed files with 27 additions and 16 deletions

View File

@ -20,6 +20,9 @@ will consitute of a breaking change warranting a new major release:
- Small SCEX fix: The temperatur check option was not passed
on for commands with a user data size larger than 1.
- ACS Controller strategy is now actually written to the dataset for detumbling
- During detumble the fused rotation rate is now calculated
- Detumbling is now exited when its exit value is undercut and not its entry value
- `EiveSystem`: Add a small delay between triggering an event for FDIR reboots and sending the
command to the core controller.

View File

@ -273,6 +273,8 @@ void AcsController::performDetumble() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
@ -321,18 +323,18 @@ void AcsController::performDetumble() {
if (acsParameters.safeModeControllerParameters.useMekf) {
if (mekfData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleStart) {
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
}
} else if (acsParameters.safeModeControllerParameters.useGyr) {
if (gyrDataProcessed.gyrVecTot.isValid() and
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleStart) {
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
}
} else if (fusedRotRateData.rotRateTotal.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleStart) {
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
} else if (detumbleCounter > 0) {
detumbleCounter -= 1;
@ -345,7 +347,7 @@ void AcsController::performDetumble() {
startTransition(mode, acs::SafeSubmode::DEFAULT);
}
disableCtrlValData();
updateCtrlValData(safeCtrlStrat);
updateActuatorCmdData(cmdDipoleMtqs);
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
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) {
PoolReadGuard pg(&ctrlValData);
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,
LocalDataPoolManager &poolManager) {
// MGM Raw

View File

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