diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index eea30389..57135dc1 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -105,6 +105,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setVector(mgmHandlingParameters.mgm4variance); break; case 0x12: + parameterWrapper->set(mgmHandlingParameters.mgmVectorFilterWeight); + break; + case 0x13: parameterWrapper->set(mgmHandlingParameters.mgmDerivativeFilterWeight); break; default: @@ -224,6 +227,12 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x24: parameterWrapper->set(susHandlingParameters.susBrightnessThreshold); break; + case 0x25: + parameterWrapper->set(susHandlingParameters.susVectorFilterWeight); + break; + case 0x26: + parameterWrapper->set(susHandlingParameters.susRateFilterWeight); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 9e13070f..661c7adc 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -77,7 +77,8 @@ class AcsParameters : public HasParametersIF { float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)}; float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)}; float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)}; - float mgmDerivativeFilterWeight = 0.5; + float mgmVectorFilterWeight = 0.85; + float mgmDerivativeFilterWeight = 0.85; } mgmHandlingParameters; struct SusHandlingParameters { @@ -767,6 +768,8 @@ class AcsParameters : public HasParametersIF { 0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513, -0.000889232196185857, -0.00168429567131815}}; float susBrightnessThreshold = 0.7; + float susVectorFilterWeight = .85; + float susRateFilterWeight = .85; } susHandlingParameters; struct GyrHandlingParameters { diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 0279215f..511cae35 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -132,6 +132,10 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const for (uint8_t i = 0; i < 3; i++) { mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; } + if (VectorOperations::norm(mgmVecTot, 3) != 0 and mgmDataProcessed->mgmVecTot.isValid()) { + lowPassFilter(mgmVecTot, mgmDataProcessed->mgmVecTot.value, + mgmParameters->mgmVectorFilterWeight); + } //-----------------------Mgm Rate Computation --------------------------------------------------- double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; @@ -351,6 +355,11 @@ void SensorProcessing::processSus( double susVecTot[3] = {0.0, 0.0, 0.0}; VectorOperations::normalize(susMeanValue, susVecTot, 3); + if (VectorOperations::norm(susVecTot, 3) != 0 and susDataProcessed->susVecTot.isValid()) { + lowPassFilter(susVecTot, susDataProcessed->susVecTot.value, + susParameters->susVectorFilterWeight); + } + /* -------- Sun Derivatiative --------------------- */ double susVecTotDerivative[3] = {0.0, 0.0, 0.0}; @@ -363,6 +372,11 @@ void SensorProcessing::processSus( susVecTotDerivativeValid = true; } } + if (VectorOperations::norm(susVecTotDerivative, 3) != 0 and + susDataProcessed->susVecTotDerivative.isValid()) { + lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value, + susParameters->susRateFilterWeight); + } timeOfSavedSusDirEst = timeOfSusMeasurement; { PoolReadGuard pg(susDataProcessed);