diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index a9c9ab5d..f62928a0 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -910,6 +910,7 @@ class AcsParameters : public HasParametersIF { struct StrParameters { double exclusionAngle = 20 * M_PI / 180; double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame + uint8_t useStrForRotRate = true; } strParameters; struct GpsParameters { diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 1b2df526..cf91b951 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -8,8 +8,9 @@ void FusedRotationEstimation::estimateFusedRotationRate( acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) { - if (sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and - sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid()) { + if (acsParameters->strParameters.useStrForRotRate and + (sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and + sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) { double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; if (VectorOperations::norm(quatOld, 4) != 0 and timeDelta != 0) {