diff --git a/CHANGELOG.md b/CHANGELOG.md index e902fb35..fe6a2cb3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -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. diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b79fb714..08e8760d 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -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::norm(mekfData.satRotRateMekf.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleStart) { + acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } } else if (acsParameters.safeModeControllerParameters.useGyr) { if (gyrDataProcessed.gyrVecTot.isValid() and VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleStart) { + acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } } else if (fusedRotRateData.rotRateTotal.isValid() and VectorOperations::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 diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index c78c7e15..a7dfbf6a 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -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;