diff --git a/synchronization.c b/synchronization.c index 61ec962..b1c56a5 100755 --- a/synchronization.c +++ b/synchronization.c @@ -66,10 +66,17 @@ void LeadLagSetCoeff(lead_lag_coeff_t *pCoeff, radio_float_t lead, radio_float_t { pCoeff->lead = lead; pCoeff->lag = lag; + pCoeff->rho = (radio_float_t)1.0; +} + +void LeadLagSetLeakiness(lead_lag_coeff_t *pCoeff, radio_float_t leakiness) +{ + pCoeff->rho = (radio_float_t)1.0-leakiness; } radio_float_t LeadLagProcess(lead_lag_filter_t *pObj, lead_lag_coeff_t *pCoeff, radio_float_t x) { + pObj->lag_state *= pCoeff->rho; pObj->lag_state += LEAD_LAG_SCALING*pCoeff->lag * x; return pObj->lag_state/LEAD_LAG_SCALING + pCoeff->lead * x; diff --git a/synchronization.h b/synchronization.h index 0cc1655..64e063c 100755 --- a/synchronization.h +++ b/synchronization.h @@ -26,6 +26,7 @@ typedef struct _lead_lag_coeff_t { radio_float_t lead; radio_float_t lag; + radio_float_t rho; } lead_lag_coeff_t; @@ -75,6 +76,7 @@ radio_float_t PfdProcess(pfd_t *pObj, radio_float_t x, radio_float_t k); void LeadLagInit(lead_lag_filter_t *pObj, radio_float_t state); void LeadLagSetCoeff(lead_lag_coeff_t *pCoeff, radio_float_t lead, radio_float_t lag); +void LeadLagSetLeakiness(lead_lag_coeff_t *pCoeff, radio_float_t leakiness); radio_float_t LeadLagProcess(lead_lag_filter_t *pObj, lead_lag_coeff_t *pCoeff, radio_float_t x); radio_float_t LeadLagGetState(lead_lag_filter_t *pObj); void LeadLagSetState(lead_lag_filter_t *pObj, radio_float_t state);