diff --git a/CHANGELOG.md b/CHANGELOG.md index 247eddd4..08faf254 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,6 +24,10 @@ will consitute of a breaking change warranting a new major release: - 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. +- Rotation rate of last cycle is now stored in all cases for the fused rotational rate + calculation. +- Fused rotation rate estimation during eclipse can be disabled. This will also prevent + detumbling during eclipse, as no relevant rotational rate is available for now. - `EiveSystem`: Add a small delay between triggering an event for FDIR reboots and sending the command to the core controller. - PL PDU: Fixed bounds checking logic. Bound checks will only be performed for modules which are diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 9b0755cd..bcb1a7f4 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -26,6 +26,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x1: parameterWrapper->set(onBoardParams.mekfViolationTimer); break; + case 0x2: + parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index cdaf4899..0f2b53c9 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -19,6 +19,7 @@ class AcsParameters : public HasParametersIF { struct OnBoardParams { double sampleTime = 0.4; // [s] uint16_t mekfViolationTimer = 750; + uint8_t fusedRateSafeDuringEclipse = true; } onBoardParams; struct InertiaEIVE { diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 20f12d05..4f1dad45 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -18,10 +18,18 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double)); fusedRotRateData->setValidity(false, true); } + // store for calculation of angular acceleration + if (gyrDataProcessed->gyrVecTot.isValid()) { + std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); + } return; } if (not susDataProcessed->susVecTot.isValid()) { estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); + // store for calculation of angular acceleration + if (gyrDataProcessed->gyrVecTot.isValid()) { + std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); + } return; } @@ -42,6 +50,10 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( fusedRotRateParallel, 3); } else { estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); + // store for calculation of angular acceleration + if (gyrDataProcessed->gyrVecTot.isValid()) { + std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); + } return; } @@ -58,11 +70,6 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( double fusedRotRateTotal[3] = {0, 0, 0}; VectorOperations::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal); - // store for calculation of angular acceleration - if (gyrDataProcessed->gyrVecTot.isValid()) { - std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); - } - { PoolReadGuard pg(fusedRotRateData); std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal, @@ -71,11 +78,17 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); fusedRotRateData->setValidity(true, true); } + + // store for calculation of angular acceleration + if (gyrDataProcessed->gyrVecTot.isValid()) { + std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); + } } void FusedRotationEstimation::estimateFusedRotationRateEclipse( acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { - if (not gyrDataProcessed->gyrVecTot.isValid() or + if (not acsParameters->onBoardParams.fusedRateSafeDuringEclipse or + not gyrDataProcessed->gyrVecTot.isValid() or VectorOperations::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) { { PoolReadGuard pg(fusedRotRateData);