From 5512605cd7aedff8b1fd52680d2582c602385de6 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 14 Nov 2023 16:23:12 +0100 Subject: [PATCH] param to enable/disable STR for rot rate calc --- mission/controller/acs/AcsParameters.h | 1 + mission/controller/acs/FusedRotationEstimation.cpp | 5 +++-- 2 files changed, 4 insertions(+), 2 deletions(-) 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) {