Skip to content

S
Software for White Rabbit PTP Core
Commit dc619b92 authored by Grzegorz Daniluk's avatar Grzegorz Daniluk
Browse files
Options

moved from switch-sw: SPLL, Tom's biquad filter implementation

parent 8e5c3e8d
Hide whitespace changes
Inline Side-by-side
Showing with 50 additions and 1 deletion
softpll/spll_common.c
View file @ dc619b92
......@@ -138,3 +138,31 @@ void spll_enable_tagger(int channel, int enable)
// TRACE("%s: ch %d, OCER 0x%x, RCER 0x%x\n", __FUNCTION__, channel, SPLL->OCER, SPLL->RCER);
}
void biquad_init(spll_biquad_t *bq, const int *coefs, int shift)
{
memset(bq, 0, sizeof(spll_biquad_t));
memcpy(bq->coefs, coefs, 5 * sizeof(int));
bq->shift = shift;
}
int biquad_update(spll_biquad_t *bq, int x)
{
register int y = 0;
y += bq->coefs[0] * x;
y += bq->coefs[1] * bq->xd[0];
y += bq->coefs[2] * bq->xd[1];
y -= bq->coefs[3] * bq->yd[0];
y -= bq->coefs[4] * bq->yd[1];
y >>= bq->shift;
bq->xd[1] = bq->xd[0];
bq->xd[0] = x;
bq->yd[1] = bq->yd[0];
bq->yd[0] = y;
return y;
}
softpll/spll_common.h
View file @ dc619b92
......@@ -60,6 +60,12 @@ typedef struct {
int y_d;
} spll_lowpass_t;
typedef struct {
int coefs[5]; /* Biquad coefficients: b0 b1 b2 a1 a2 */
int shift; /* bit shift for the coeffs / output */
int yd[2], xd[2]; /* I/O delay lines */
} spll_biquad_t;
/* initializes the PI controller state. Currently almost a stub. */
void pi_init(spll_pi_t *pi);
......@@ -74,4 +80,7 @@ int lowpass_update(spll_lowpass_t *lp, int x);
void spll_enable_tagger(int channel, int enable);
void biquad_init(spll_biquad_t *bq, const int *coefs, int shift);
int biquad_update(spll_biquad_t *bq, int x);
#endif // __SPLL_COMMON_H
softpll/spll_helper.c
View file @ dc619b92
......@@ -11,7 +11,14 @@
#include "spll_helper.h"
#include "spll_debug.h"
const int helper_precomp_coefs [] =
{ /*b0*/ 60648,
/*b1*/ 60648,
/*b2*/ 0,
/*a1*/ 55760,
/*a2*/ 0};
void helper_init(struct spll_helper_state *s, int ref_channel)
{
......@@ -59,6 +66,8 @@ int helper_update(struct spll_helper_state *s, int tag,
err = HELPER_ERROR_CLAMP;
}
// err = biquad_update(&s->precomp, err);
if ((tag + s->p_adder) > HELPER_TAG_WRAPAROUND
&& s->p_setpoint > HELPER_TAG_WRAPAROUND) {
s->p_adder -= HELPER_TAG_WRAPAROUND;
......@@ -95,6 +104,8 @@ void helper_start(struct spll_helper_state *s)
pi_init((spll_pi_t *)&s->pi);
ld_init((spll_lock_det_t *)&s->ld);
biquad_init(&s->precomp, helper_precomp_coefs, 16);
spll_enable_tagger(s->ref_src, 1);
spll_debug(DBG_EVENT | DBG_HELPER, DBG_EVT_START, 1);
}
softpll/spll_helper.h
View file @ dc619b92
......@@ -33,6 +33,7 @@ struct spll_helper_state {
int delock_count;
spll_pi_t pi;
spll_lock_det_t ld;
spll_biquad_t precomp;
};
void helper_init(struct spll_helper_state *s, int ref_channel);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment