Skip to content

W
White Rabbit Switch - Software
Commit fe7251ba authored by Tomasz Wlostowski's avatar Tomasz Wlostowski Committed by Alessandro Rubini
Browse files
Options

softpll: biquad filter implementation

parent 1c5873ff
Hide whitespace changes
Inline Side-by-side
Showing with 37 additions and 0 deletions
rt/softpll/spll_common.c
View file @ fe7251ba
......@@ -139,3 +139,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;
}
rt/softpll/spll_common.h
View file @ fe7251ba
......@@ -58,6 +58,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);
......@@ -72,4 +78,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
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