#include "bp_iir.h"
#include <math.h>
#define BP_MAX_COEFS 120
#define PI 3.1415926
/*This is an array of the filter parameters object
defined in the br_iir.h file*/
static struct bp_coeffs bp_coeff_arr[BP_MAX_COEFS];
/*This initialization function will create the
band pass filter coefficients array, you have to specify:
fsfilt = Sampling Frequency
gb = Gain at cut frequencies
Q = Q factor, Higher Q gives narrower band
fstep = Frequency step to increase center frequencies
in the array
fmin = Minimum frequency for the range of center frequencies
*/
void bp_iir_init(double fsfilt,double gb,double Q,short fstep, short fmin) {
int i;
double damp;
double wo;
damp = gb/sqrt(1 - pow(gb,2));
for (i=0;i<BP_MAX_COEFS;i++) {
wo = 2*PI*(fstep*i + fmin)/fsfilt;
bp_coeff_arr[i].e = 1/(1 + damp*tan(wo/(Q*2)));
bp_coeff_arr[i].p = cos(wo);
bp_coeff_arr[i].d[0] = (1-bp_coeff_arr[i].e);
bp_coeff_arr[i].d[1] = 2*bp_coeff_arr[i].e*bp_coeff_arr[i].p;
bp_coeff_arr[i].d[2] = (2*bp_coeff_arr[i].e-1);
}
}
/*This function loads a given set of band pass filter coefficients acording to a center frequency index
into a band pass filter object
H = filter object
ind = index of the array mapped to a center frequency
*/
void bp_iir_setup(struct bp_filter * H,int ind) {
H->e = bp_coeff_arr[ind].e;
H->p = bp_coeff_arr[ind].p;
H->d[0] = bp_coeff_arr[ind].d[0];
H->d[1] = bp_coeff_arr[ind].d[1];
H->d[2] = bp_coeff_arr[ind].d[2];
}
/*This function loads a given set of band pass filter coefficients acording to a center frequency index
into a band pass filter object
H = filter object
ind = index of the array mapped to a center frequency
*/
double bp_iir_filter(double yin,struct bp_filter * H) {
double yout;
H->x[0] = H->x[1];
H->x[1] = H->x[2];
H->x[2] = yin;
H->y[0] = H->y[1];
H->y[1] = H->y[2];
H->y[2] = H->d[0]* H->x[2] - H->d[0]* H->x[0] + (H->d[1]* H->y[1]) - H->d[2]* H->y[0];
yout = H->y[2];
return yout;
}
/******************
bp_iir.h
**********************/
#ifndef __BP_IIR_H__
#define __BP_IIR_H__
struct bp_coeffs{
double e;
double p;
double d[3];
};
struct bp_filter{
double e;
double p;
double d[3];
double x[3];
double y[3];
};
extern void bp_iir_init(double fsfilt,double gb,double Q,short fstep, short fmin);
extern void bp_iir_setup(struct bp_filter * H,int index);
extern double bp_iir_filter(double yin,struct bp_filter * H);
#endif
/* **********************************************************
* Purpose::
* heuristic multi-user waterfilling algorithm implementation
* example invocation included; compile and run with
* gcc -Wall (thisFile.c); ./a.out
*
* PLEASE NOTE:
* This is a quick rewrite that hasn't been extensively tested.
* Please verify independently that the code does what it says.
*/
/* **********************************************************
* Header file
* **********************************************************/
#ifndef WATERFILL_H
#define WATERFILL_H
#include <stdio.h>
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
/* **********************************************************
* Waterfilling algorithm data structure
* In C++ this would become an object.
* Usage:
* wfalg_new(...);
* while (need to invoke it multiple times on changing data){
* wfalg_run(...);
* (process results)
* }
* wfalg_free(...); now the results become invalid
* **********************************************************/
typedef struct wfalg_t_ wfalg_t;
/* Purpose:
* Create data structure for multiuser waterfiller "object"
* Use for multiple invocations on varying data
*
* Parameters:
* nRB: Number of "resource blocks" (frequency bins)
*
* nUser: number of simultaneous transmissions to allocate
*
* pmode: 0 conventional multi-user waterfilling: Per-resource power based on
* channel quality estimate
* otherwise: select resources based on channel quality, but distribute the
* per-user power evenly
*/
wfalg_t* wfalg_new(int /*nRB*/, int /*nUser*/, int /*mode*/);
/* Purpose:
* Invoke multi-user waterfilling for the following parameters:
* Rmax: maximum number of RBs that may be allocated to each individual user.
* in "ideal" waterfilling, a single user with a very good channel hogs up
* most of the resources. While this (ideally) maximizes sum throughput, there are
* usually other criteria that are equally or more important.
* The caller can limit the number of resources for a particular user, and thus
* control the "fairness" of allocation.
* Set Rmax >= nRB for each user to disable this limitation.
*
* SNR_threshold: Do not allocate a resource if the signal-to-noise ratio falls below the
* threshold. For example, even heavily coded QPSK doesn't lead to any meaningful
* throughput when the signal-to-noise ratio is below -3 dB
* if so, choose SNR_threshold=10*log10(-3/10) = 0.5
*
* Tinv: inverse of channel quality, noise-to-signal ratio N/S where "signal" includes
* the channel |H(f)| with f the frequency of a resource.
* Matrix: first user resource 0..nRB-1, second user resources 0..nRB-1, last user
*
* porder: Process users in the given order (array of user indices 0..nUser-1 with length nUser)
* The order is important in pathological cases, such as "more users than RBs."
* If so, the scheduling decision is effectively made by the caller, via the processing order:
* first come, first served, and the later ones get nothing.
*
* After execution,
* - resultAlloc points to a size-nRB array containing the index of the allocated user for each resource block.
- resultPower points to a size-nRB array with the power allocated to a resource.
* All powers for user j sum up to pUser[j]
* The number of resources where resultAlloc==j is <= Rmax[j].
* There is only one storage space per wfalg_t "object". Earlier results become invalid with
* the next invocation.
*/
void wfalg_run(wfalg_t* /*obj*/, const double* /*pUser*/, const int* /*Rmax*/, const int* /*porder*/, const double* /*TinvMatrix*/, double /*SNR_threshold*/, int** /*resultAlloc*/, double** /*resultPower*/);
/* Purpose:
* Release allocated memory.
* obj becomes invalid.
* any space provided by wfalg_run becomes invalid.
*/
void wfalg_free(wfalg_t* /* obj */);
#endif
/* **********************************************************
* .c file
* **********************************************************/
/* Data structure for multiple invocations without unnecessary malloc / free */
struct wfalg_t_ {
/* parameters */
int nRB;
int nUser;
/* Storage space for results */
int* RB_alloc;
double* RB_power;
/* Internal temporary storage space
* all Tinv values for the single user that is
* processed by waterfill() */
double* temp_singleUserTinv;
/* internal temporary storage: How many resources are allocated per user? */
int* temp_userNResources;
int* order;
/* Mode switch parameter */
int mode;
};
wfalg_t* wfalg_new(int nRB, int nUser, int mode){
wfalg_t* obj=(wfalg_t*)malloc(sizeof(wfalg_t)); assert(obj);
obj->nUser=nUser;
obj->nRB=nRB;
obj->mode=mode;
obj->RB_alloc=(int*)malloc(sizeof(int) * nRB); assert(obj->RB_alloc);
obj->RB_power=(double*)malloc(sizeof(double) * nRB); assert(obj->RB_power);
obj->temp_singleUserTinv=(double*)malloc(sizeof(double) * nRB); assert(obj->temp_singleUserTinv);
obj->temp_userNResources=(int*)malloc(sizeof(int) * nUser); assert(obj->temp_userNResources);
memset((void*)obj->RB_power, 0, nRB * sizeof(double));
return obj;
}
void wfalg_free(wfalg_t* obj){
free(obj->RB_alloc);
free(obj->RB_power);
free(obj->temp_singleUserTinv);
free(obj->temp_userNResources);
free(obj);
}
/* ************************************************************************************
* Conventional multi-user waterfilling on the set of resources where
* RB_alloc[]==userindex
* expects temp_singleUserTinv to be filled with the Tinv for the allocating user!
* ************************************************************************************/
static void waterfill(wfalg_t* obj, int userindex, double pUser){
const int nRB=obj->nRB;
const double E0=pUser;
const double threshold=1e-12* E0;
/* ************************************************************************************
* Count resource allocated to this user
* ************************************************************************************/
int nRB_user=0;
int j_RB;
for(j_RB=0; j_RB < nRB; ++j_RB){
if (*(obj->RB_alloc+j_RB)==userindex){
++nRB_user;
}
}
if (nRB_user == 0){
/* no resources allocated - can't waterfill */
return;
}
double E=E0;
double waterlevel=0;
while (E > threshold){
/* ************************************************************************************
* Distribute remaining energy evenly over all RBs
* since some of them have greater-than-zero Tinv, this approximation will always stay
* below target (standard waterfilling algorithm)
* ************************************************************************************/
E *= 0.999;
waterlevel += E/(double)nRB_user;
/* ************************************************************************************
* Determine energy that remains with current allocation
* ************************************************************************************/
E=E0;
/* Note: temp_singleUserTinv contains the Tinv for the user who allocates the RB.
* This avoids many repeated table lookups from the nUser*nRB Tinv matrix. */
double* pTinv=obj->temp_singleUserTinv;
double* pRB_power=obj->RB_power;
int* palloc=obj->RB_alloc;
int j_RB;
for(j_RB=0; j_RB < nRB; ++j_RB){
int alloc=*(palloc++);
double Tinv=*(pTinv++);
/* resource is allocated to current user */
if (alloc==userindex){
/* determine actual per-resource energy */
double d=waterlevel - Tinv;
d=(d > 0.0 ? d : 0.0);
E -= d;
*(pRB_power)=d;
} /* if allocated */
++pRB_power;
} /* for RB */
} /* while energy remains */
assert(E >= 0.0);
}
/* picks the best unallocated RB for the given user */
static int find_best_RB(wfalg_t* obj, int userindex, const double* thisUserTinv){
/* find RB with lowest Tinv for this user */
int i_RB;
int bestRB_index=-1;
double bestRB_Tinv=9e99;
int valid=0;
const double* pTinv=thisUserTinv;
for (i_RB=0; i_RB < obj->nRB; ++i_RB){
double Tinv=*(pTinv++);
int alloc=*(obj->RB_alloc+i_RB);
if ((alloc < 0) && (Tinv < bestRB_Tinv)){
bestRB_index=i_RB;
bestRB_Tinv=Tinv;
valid=1;
}
}
if (!valid){
bestRB_index=-1;
}
return bestRB_index; /* -1 means: none */
}
static int try_alloc_one_RB(wfalg_t* obj, int userindex, double pThisUser, const double* thisUserTinv, double SNR_threshold){
int RBindex=find_best_RB(obj, userindex, thisUserTinv);
if (RBindex >= 0){
/* channel quality on resource RBindex */
double Tinv=*(thisUserTinv+RBindex);
/* allocate RB, at least temporarily */
*(obj->RB_alloc+RBindex)=userindex;
double Tinv2=Tinv;
if (obj->mode){
/* Equal power allocation - disregard "floor" level in waterfilling
* We'll use the original Tinv for the SNR check later */
Tinv2=0.0;
}
*(obj->temp_singleUserTinv+RBindex) = Tinv2;
waterfill(obj, userindex, pThisUser);
/* check SNR of last RB */
double P=*(obj->RB_power + RBindex);
double SNR=P/Tinv;
if (SNR >= SNR_threshold){
return 1; /* success */
} else {
/* too low power to be useful.
* undo allocation. */
*(obj->RB_alloc+RBindex)=-1;
}
}
return 0; /* failure */
}
static void zero_unallocated_RBs(const wfalg_t* obj){
int* ap=obj->RB_alloc;
double* pp=obj->RB_power;
int iRB;
for (iRB=0; iRB < obj->nRB; ++iRB){
*pp=(*ap >= 0 ? *pp : 0.0);
++pp;
++ap;
}
}
void wfalg_run(wfalg_t* obj, const double* pUser, const int* Rmax, const int* porder, const double* TinvMatrix, double SNR_threshold, int** resultAlloc, double** resultPower){
/* Deallocate all RBs */
int* p=obj->RB_alloc;
int i_RB;
for (i_RB=0; i_RB < obj->nRB; ++i_RB){
*(p++) = -1;
}
memset((void*)obj->temp_userNResources, 0, obj->nUser * sizeof(int));
int nAllocResourcesThisRound=-1; /* any nonzero value to enter the loop */
/* Repeat until we fail to allocate another resource */
while (nAllocResourcesThisRound != 0){
nAllocResourcesThisRound=0;
/* Iterate over all users ... */
int jj_user;
for (jj_user=0; jj_user < obj->nUser; ++jj_user){
/* ... in the specified order */
int j_user=*(porder+jj_user);
/* power budget of current user */
double p_jUser=*(pUser+j_user);
/* Are we allowed to add another resource to this user? (Rmax constraint) */
if (*(obj->temp_userNResources+j_user) < *(Rmax+j_user)){
/* Look up the channel quality array for j_user */
const double* thisUserTinv=TinvMatrix + j_user * obj->nRB;
/* Try to allocate one more resource to this user */
if (try_alloc_one_RB(obj, j_user, p_jUser, thisUserTinv, SNR_threshold)){
/* Successfully allocated */
++ *(obj->temp_userNResources+j_user); /* count resources allocated to this user */
++nAllocResourcesThisRound; /* continue iterating */
} else {
/* this user can't allocate any more resources
* There is no need to try again in future iterations,
* disregard this user by making the Rmax test fail
*
* note, power allocation is not valid at this point */
*(obj->temp_userNResources+j_user) = *(Rmax+j_user);
}
}
} /* for j_user */
} /* while allocations succeed */
/* The previous step fails exactly once per user, making the
* power allocation invalid.
*
* Recalculate.
* This is single-user waterfilling without any interaction, therefore order does not matter
* Note that waterfill() requires a valid temp_singleUserTinv (which is the case at this point):
* For each resource, it contains the Tinv of the allocating user
*/
int j_user;
for (j_user=0; j_user < obj->nUser; ++j_user){
double p_jUser=*(pUser+j_user);
waterfill(obj, j_user, p_jUser);
}
/* Set zero power allocation for resources that aren't allocated to any user */
zero_unallocated_RBs(obj);
*resultAlloc=obj->RB_alloc;
*resultPower=obj->RB_power;
}
/* **********************************************************
* Example use (still .c file)
* **********************************************************/
#if 1
int main(void){
const int nUser=5;
const int nRB=40;
/* allocate and create random multi-user channel data */
double* Tinv=(double*)malloc(nUser*nRB*sizeof(double));
int i; int j;
double* p=Tinv;
for (i=0; i < nUser; ++i){
for (j=0; j < nRB; ++j){
*(p++)=(double)random() / (double)RAND_MAX;
}
}
/* power per user */
double pUser[]={1, 2, 3, 4, 5};
/* processing order */
int order[]={0, 1, 2, 3, 4};
/* maximum number of resources per user */
const int RMax[]={nRB, nRB, nRB, nRB, nRB};
/* create waterfiller "object" */
wfalg_t* wfalg=wfalg_new(nRB, nUser, /* mode */0);
/* Invoke waterfilling */
int* RB_alloc;
double* RB_power;
wfalg_run(wfalg, pUser, RMax, order, Tinv, /* SNR_threshold */1.0, &RB_alloc, &RB_power);
/* print result */
int j_user; int i_RB;
for (j_user=0; j_user < nUser; ++j_user){
printf("user %i ", j_user);
double sum=0;
for (i_RB=0; i_RB < nRB; ++i_RB){
if (*(RB_alloc+i_RB)==j_user){
double p=*(RB_power+i_RB);
printf("%i=%lf ", i_RB, p);
sum += p;
}
}
printf("sum=%lf\n", sum);
}
/* clean up */
wfalg_free(wfalg);
free(Tinv);
return EXIT_SUCCESS;
}
#endif
/* ****************************************************
* Farrow resampler (with optional bank switching)
* M. Nentwig, 2011
* Input stream is taken from stdin
* Output stream goes to stdout
* Main target was readability, is not optimized for efficiency.
* ****************************************************/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#if 1
/* **************************************************************
* example coefficients.
* Each column [c0; c1; c2; ...] describes a polynomial for one tap coefficent in fractional time ft [0, 1]:
* tapCoeff = c0 + c1 * ft + c2 * ft ^ 2 + ...
* Each column corresponds to one tap.
* The example filters is based on a 6th order Chebyshev Laplace-domain prototype.
* This version uses three sub-segments per tap (NBANKS = 3)
* **************************************************************/
const double cMatrix[] = {
2.87810386e-4, 4.70096244e-3, 7.93412570e-2, 4.39824536e-1, 1.31192924e+000, 2.67892232e+000, 4.16465421e+000, 5.16499621e+000, 5.15592605e+000, 3.99000369e+000, 2.00785470e+000, -7.42377060e-2, -1.52569354e+000, -1.94402804e+000, -1.40915797e+000, -3.86484652e-1, 5.44712939e-1, 9.77559688e-1, 8.32191447e-1, 3.22691788e-1, -2.13133045e-1, -5.08501962e-1, -4.82928807e-1, -2.36313854e-1, 4.76034568e-2, 2.16891966e-1, 2.20894063e-1, 1.08361553e-1, -2.63421832e-2, -1.06276015e-1, -1.07491548e-1, -5.53793711e-2, 4.86314061e-3, 3.94357182e-2, 4.06217506e-2, 2.17199064e-2, 1.60318761e-3, -8.40370106e-3, -8.10525279e-3, -3.62112499e-3, -4.13413072e-4, 2.33101911e-4,
-3.26760325e-3, -6.46028234e-3, 1.46793247e-1, 5.90235537e-1, 1.18931309e+000, 1.57853546e+000, 1.40402774e+000, 5.76506323e-1, -6.33522788e-1, -1.74564700e+000, -2.24153717e+000, -1.91309453e+000, -9.55568978e-1, 1.58239169e-1, 9.36193787e-1, 1.10969783e+000, 7.33284446e-1, 1.06542194e-1, -4.15412084e-1, -6.06616434e-1, -4.54898908e-1, -1.20841199e-1, 1.82941623e-1, 3.12543429e-1, 2.49935829e-1, 8.05376898e-2, -7.83213666e-2, -1.47769751e-1, -1.18735248e-1, -3.70656555e-2, 3.72608374e-2, 6.71425397e-2, 5.17812605e-2, 1.55564930e-2, -1.40896327e-2, -2.35058137e-2, -1.59635057e-2, -3.44701792e-3, 4.14108065e-3, 4.56234829e-3, 1.59503132e-3, -3.17301882e-4,
5.64310141e-3, 7.74786707e-2, 2.11791763e-1, 2.84703201e-1, 1.85158633e-1, -8.41118142e-2, -3.98497442e-1, -5.86821615e-1, -5.40397941e-1, -2.47558080e-1, 1.50864737e-1, 4.59312895e-1, 5.41539400e-1, 3.84673917e-1, 9.39576331e-2, -1.74932542e-1, -3.01635463e-1, -2.56239225e-1, -9.87146864e-2, 6.82216764e-2, 1.59795852e-1, 1.48668245e-1, 6.62563431e-2, -2.71234898e-2, -8.07045577e-2, -7.76841351e-2, -3.55333136e-2, 1.23206602e-2, 3.88535040e-2, 3.64199073e-2, 1.54608563e-2, -6.59814558e-3, -1.72735099e-2, -1.46307777e-2, -5.04363288e-3, 3.31049461e-3, 6.01267607e-3, 3.83904192e-3, 3.92549958e-4, -1.36315264e-3, -9.76017430e-4, 7.46699178e-5};
#define NTAPS (14)
#define NBANKS (3)
#define ORDER (2)
#else
/* Alternative example
* Similar impulse response as above
* A conventional Farrow design (no subdivisions => NBANKS = 1), order 3
*/
const double cMatrix[] = {
-8.57738278e-3, 7.82989032e-1, 7.19303539e+000, 6.90955718e+000, -2.62377450e+000, -6.85327127e-1, 1.44681608e+000, -8.79147907e-1, 7.82633997e-2, 1.91318985e-1, -1.88573400e-1, 6.91790782e-2, 3.07723786e-3, -6.74800912e-3,
2.32448021e-1, 2.52624309e+000, 7.67543936e+000, -8.83951796e+000, -5.49838636e+000, 6.07298348e+000, -2.16053205e+000, -7.59142947e-1, 1.41269409e+000, -8.17735712e-1, 1.98119464e-1, 9.15904145e-2, -9.18092030e-2, 2.74136108e-2,
-1.14183319e+000, 6.86126458e+000, -6.86015957e+000, -6.35135894e+000, 1.10745051e+001, -3.34847578e+000, -2.22405694e+000, 3.14374725e+000, -1.68249886e+000, 2.54083065e-1, 3.22275037e-1, -3.04794927e-1, 1.29393976e-1, -3.32026332e-2,
1.67363115e+000, -2.93090391e+000, -1.13549165e+000, 5.65274939e+000, -3.60291782e+000, -6.20715544e-1, 2.06619782e+000, -1.42159644e+000, 3.75075865e-1, 1.88433333e-1, -2.64135123e-1, 1.47117661e-1, -4.71871047e-2, 1.24921920e-2};
#define NTAPS (14)
#define NBANKS (1)
#define ORDER (3)
#endif
/* Set here the ratio between output and input sample rate.
* It could be changed even during runtime! */
#define INCR (1.0 / 6.28 / 3)
/* delay line storage */
double delayLine[NTAPS];
/* Coefficient lookup "table" */
static double c(int ixTap, int ixBank, int ixPower){
return cMatrix[ixPower * (NTAPS * NBANKS) + ixTap * NBANKS + ixBank];
}
int main (void){
/* clear delay line */
int ix;
for (ix = 0; ix < NTAPS; ++ix)
delayLine[ix] = 0;
/* Index of last input sample that was read
* First valid sample starts at 0 */
int sampleIndexInput = -1;
/* Position of next output sample in input stream */
double sampleIndexOutput = 0.0;
/* loop forever. Terminate, once out of input data. */
while (1){
/* Split output sample location into integer and fractional part:
* Integer part corresponds to sample index in input stream
* fractional part [0, 1[ spans one tap (covering NBANKS segments) */
int sio_int = floor(sampleIndexOutput);
double sio_fracInTap = sampleIndexOutput - (double)sio_int;
assert((sio_fracInTap >= 0) && (sio_fracInTap < 1));
/* Further split the fractional part into
* - bank index
* - fractional part within the bank */
int sio_intBank = floor(sio_fracInTap * (double) NBANKS);
double sio_fracInBank = sio_fracInTap * (double) NBANKS - (double)sio_intBank;
assert((sio_fracInBank >= 0) && (sio_fracInBank < 1));
/* ****************************************************
* load new samples into the delay line, until the last
* processed input sample (sampleIndexInput) catches
* up with the integer part of the output stream position (sio_int)
* ***************************************************/
while (sampleIndexInput < sio_int){
/* Advance the delay line one step */
++sampleIndexInput;
for (ix = NTAPS-1; ix > 0; --ix)
delayLine[ix] = delayLine[ix-1];
/* Read one input sample */
int flag = scanf("%lf", &delayLine[0]);
if (flag != 1)
goto done; /* there's nothing wrong with "goto" as "break" for multiple loops ... */
} /* while delay line behind output data */
/* ****************************************************
* Calculate one output sample:
* "out" sums up the contribution of each tap
* ***************************************************/
double out = 0;
int ixTap;
for (ixTap = 0; ixTap < NTAPS; ++ixTap){
/* ****************************************************
* Contribution of tap "ixTap" to output:
* ***************************************************/
/* Evaluate polynomial in sio_fracInBank:
* c(ixTap, sio_intBank, 0) is the constant coefficient
* c(ixTap, sio_intBank, 1) is the linear coefficient etc
*/
double hornerSum = c(ixTap, sio_intBank, ORDER);
int ixPower;
for (ixPower = ORDER-1; ixPower >= 0; --ixPower){
hornerSum *= sio_fracInBank;
hornerSum += c(ixTap, sio_intBank, ixPower);
} /* for ixPower */
/* ****************************************************
* Weigh the delay line sample of this tap with the
* polynomial result and add to output
* ***************************************************/
out += hornerSum * delayLine[ixTap];
} /* for ixTap */
/* ****************************************************
* Generate output sample and advance the position of
* the next output sample in the input data stream
* ***************************************************/
printf("%1.12le\n", out);
sampleIndexOutput += INCR;
} /* loop until out of input data */
done: /* out of input data => break loops and continue here */
return 0;
} /* main */
typedef signed long s32;
#define ABS(x) ((x) < 0 ? -(x) : (x))
#define CLAMP(x,min,max) {if ((x) <= (min)) (x) = (min); else if ((x) >= (max)) (x) = (max);}
#define KLPF_MAX_ADAPT 232L
#define KLPF_MIN_ADAPT 0L
#define KLPF_DEFAULT_ADAPT 160L
#define ACCEL_MIN_ADAPT 5
#define ACCEL_MAX_ADAPT 15
s32 gnKLpf = KLPF_DEFAULT_ADAPT; // IIR filter coefficient, 0 to 255
s32 gnKLpfMax = KLPF_MAX_ADAPT;
s32 gnAccel = 0; // rate of change of climbrate
s32 gnCpsx256 = 0; // climbrate in centimeters per second, with 8 bit fractional precision
s32 gnCps; // climbrate in centimeters per second
// IIR low pass filter using fixed point integer arithmetic. 8bits of fractional precision for IIR coefficient K.
// So the actual floating point coefficient is k = K/256, 0.0 <= k < 1.0. The filter computes
// climbRateFiltered = climbRateFiltered*k + newClimbRate*(1.0 - k). So K values close to 256 will result in
// heavy damping, K values close to 0 will result in almost no damping. The IIR low pass filtered output gnCps
// is the rounded up integer value, the 8bit fraction precision value gnCpsx256 is a global variable, so is
// maintained between calls to the filter. No integer divides required.
void sns_LPFClimbRate(void) {
s32 tmp;
tmp = gnCpsx256*gnKLpf + (gnCps<<8)*(256L-gnKLpf);
gnCpsx256 = (tmp >= 0 ? ((tmp + 128L)>>8) : ((tmp - 128L)>>8));
gnCps = (gnCpsx256>>8);
}
// Adapt the IIR filter coeffient to the rate of change. If samples are not changing much, increase the damping.
// If the samples are changing by a large amount, reduce the damping. This is done to reduce the response delay to a
// a step change, while still being able to heavily damp out jitter in steady state noisy samples.
// This function basically linearly interpolates KLpf between KLPF_MAX and KLPF_MIN based on the acceleration.
// Values of acceleration outside experimentally determined limits are clamped to the limits, depends on the
// application. A variable is used for KLpfMax to allow user to adjust the maximum allowed damping.
s32 sns_AdaptKLpf(void) {
s32 klpf,nAccel;
nAccel = ABS(gnAccel);
CLAMP(nAccel, ACCEL_MIN_ADAPT, ACCEL_MAX_ADAPT);
klpf = gnKLpfMax + ((KLPF_MIN_ADAPT-gnKLpfMax)*(nAccel - ACCEL_MIN_ADAPT))/(ACCEL_MAX_ADAPT-ACCEL_MIN_ADAPT);
CLAMP(klpf, KLPF_MIN_ADAPT, KLPF_MAX_ADAPT);
return klpf;
}
// usage :
// For each new data sample that arrives
// 1. calculate new unfiltered climbrate nCps from sample data buffer
// 2. gnAccel = (nCps - gnCps);
// 3. (optionally low pass filter gnAccel using same type of IIR filter, if required)
// 4. gnCps = nCps;
// 5. gnKLpf = sns_AdaptKLpf();
// 6. sns_LPFClimbRate();
/*************** AutoWah.c ********************************/
#include "bp_iir.h"
#include "AutoWah.h"
static short center_freq;
static short samp_freq;
static short counter;
static short counter_limit;
static short control;
static short max_freq;
static short min_freq;
static short f_step;
static struct bp_filter H;
/*
This is the auto wah effect initialization function.
This initializes the band pass filter and the effect control variables
*/
void AutoWah_init(short effect_rate,short sampling,short maxf,short minf,short Q,double gainfactor,short freq_step) {
double C;
/*Process variables*/
center_freq = 0;
samp_freq = sampling;
counter = effect_rate;
control = 0;
/*User Parametters*/
counter_limit = effect_rate;
/*Convert frequencies to index ranges*/
min_freq = 0;
max_freq = (maxf - minf)/freq_step;
bp_iir_init(sampling,gainfactor,Q,freq_step,minf);
f_step = freq_step;
}
/*
This function generates the current output value
Note that if the input and output signal are integer
unsigned types, we need to add a half scale offset
*/
double AutoWah_process(int xin) {
double yout;
yout = bp_iir_filter(xin,&H);
#ifdef INPUT_UNSIGNED
yout += 32767;
#endif
return yout;
}
/*
This function will emulate a LFO that will vary according
to the effect_rate parameter set in the AutoWah_init function.
*/
void AutoWah_sweep(void) {
double yout;
if (!--counter) {
if (!control) {
bp_iir_setup(&H,(center_freq+=f_step));
if (center_freq > max_freq) {
control = 1;
}
}
else if (control) {
bp_iir_setup(&H,(center_freq-=f_step));
if (center_freq == min_freq) {
control = 0;
}
}
counter = counter_limit;
}
}
/*************** AutoWah.h ****************************/
#ifndef __AUTOWAH_H__
#define __AUTOWAH_H__
extern void AutoWah_init(short effect_rate,short sampling,short maxf,short minf,short Q,double gainfactor,short freq_step);
extern double AutoWah_process(int xin);
extern void AutoWah_sweep(void);
#endif
/************** Usage Example **************************/
#include "AutoWah.h"
void main(void) {
short xin;
short yout;
AutoWah_init(2000, /*Effect rate 2000*/
16000, /*Sampling Frequency*/
1000, /*Maximum frequency*/
500, /*Minimum frequency*/
4, /*Q*/
0.707, /*Gain factor*/
10 /*Frequency increment*/
);
while(1) {
if (new_sample_flag()) {
/*When there's new sample at your ADC or CODEC input*/
/*Read the sample*/
xin = read_sample();
/*Apply AutoWah_process function to the sample*/
yout =AutoWah_process(xin);
/*Send the output value to your DAC or codec output*/
write_output(yout);
/*Makes the LFO vary*/
AutoWah_sweep();
}
}
}
/* ****************************************************
* Farrow resampler (with optional bank switching)
* M. Nentwig, 2011
* Input stream is taken from stdin
* Output stream goes to stdout
* Main target was readability, is not optimized for efficiency.
* ****************************************************/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#if 1
/* **************************************************************
* example coefficients.
* Each column [c0; c1; c2; ...] describes a polynomial for one tap coefficent in fractional time ft [0, 1]:
* tapCoeff = c0 + c1 * ft + c2 * ft ^ 2 + ...
* Each column corresponds to one tap.
* The example filters is based on a 6th order Chebyshev Laplace-domain prototype.
* This version uses three sub-segments per tap (NBANKS = 3)
* **************************************************************/
const double cMatrix[] = {
2.87810386e-4, 4.70096244e-3, 7.93412570e-2, 4.39824536e-1, 1.31192924e+000, 2.67892232e+000, 4.16465421e+000, 5.16499621e+000, 5.15592605e+000, 3.99000369e+000, 2.00785470e+000, -7.42377060e-2, -1.52569354e+000, -1.94402804e+000, -1.40915797e+000, -3.86484652e-1, 5.44712939e-1, 9.77559688e-1, 8.32191447e-1, 3.22691788e-1, -2.13133045e-1, -5.08501962e-1, -4.82928807e-1, -2.36313854e-1, 4.76034568e-2, 2.16891966e-1, 2.20894063e-1, 1.08361553e-1, -2.63421832e-2, -1.06276015e-1, -1.07491548e-1, -5.53793711e-2, 4.86314061e-3, 3.94357182e-2, 4.06217506e-2, 2.17199064e-2, 1.60318761e-3, -8.40370106e-3, -8.10525279e-3, -3.62112499e-3, -4.13413072e-4, 2.33101911e-4,
-3.26760325e-3, -6.46028234e-3, 1.46793247e-1, 5.90235537e-1, 1.18931309e+000, 1.57853546e+000, 1.40402774e+000, 5.76506323e-1, -6.33522788e-1, -1.74564700e+000, -2.24153717e+000, -1.91309453e+000, -9.55568978e-1, 1.58239169e-1, 9.36193787e-1, 1.10969783e+000, 7.33284446e-1, 1.06542194e-1, -4.15412084e-1, -6.06616434e-1, -4.54898908e-1, -1.20841199e-1, 1.82941623e-1, 3.12543429e-1, 2.49935829e-1, 8.05376898e-2, -7.83213666e-2, -1.47769751e-1, -1.18735248e-1, -3.70656555e-2, 3.72608374e-2, 6.71425397e-2, 5.17812605e-2, 1.55564930e-2, -1.40896327e-2, -2.35058137e-2, -1.59635057e-2, -3.44701792e-3, 4.14108065e-3, 4.56234829e-3, 1.59503132e-3, -3.17301882e-4,
5.64310141e-3, 7.74786707e-2, 2.11791763e-1, 2.84703201e-1, 1.85158633e-1, -8.41118142e-2, -3.98497442e-1, -5.86821615e-1, -5.40397941e-1, -2.47558080e-1, 1.50864737e-1, 4.59312895e-1, 5.41539400e-1, 3.84673917e-1, 9.39576331e-2, -1.74932542e-1, -3.01635463e-1, -2.56239225e-1, -9.87146864e-2, 6.82216764e-2, 1.59795852e-1, 1.48668245e-1, 6.62563431e-2, -2.71234898e-2, -8.07045577e-2, -7.76841351e-2, -3.55333136e-2, 1.23206602e-2, 3.88535040e-2, 3.64199073e-2, 1.54608563e-2, -6.59814558e-3, -1.72735099e-2, -1.46307777e-2, -5.04363288e-3, 3.31049461e-3, 6.01267607e-3, 3.83904192e-3, 3.92549958e-4, -1.36315264e-3, -9.76017430e-4, 7.46699178e-5};
#define NTAPS (14)
#define NBANKS (3)
#define ORDER (2)
#else
/* Alternative example
* Similar impulse response as above
* A conventional Farrow design (no subdivisions => NBANKS = 1), order 3
*/
const double cMatrix[] = {
-8.57738278e-3, 7.82989032e-1, 7.19303539e+000, 6.90955718e+000, -2.62377450e+000, -6.85327127e-1, 1.44681608e+000, -8.79147907e-1, 7.82633997e-2, 1.91318985e-1, -1.88573400e-1, 6.91790782e-2, 3.07723786e-3, -6.74800912e-3,
2.32448021e-1, 2.52624309e+000, 7.67543936e+000, -8.83951796e+000, -5.49838636e+000, 6.07298348e+000, -2.16053205e+000, -7.59142947e-1, 1.41269409e+000, -8.17735712e-1, 1.98119464e-1, 9.15904145e-2, -9.18092030e-2, 2.74136108e-2,
-1.14183319e+000, 6.86126458e+000, -6.86015957e+000, -6.35135894e+000, 1.10745051e+001, -3.34847578e+000, -2.22405694e+000, 3.14374725e+000, -1.68249886e+000, 2.54083065e-1, 3.22275037e-1, -3.04794927e-1, 1.29393976e-1, -3.32026332e-2,
1.67363115e+000, -2.93090391e+000, -1.13549165e+000, 5.65274939e+000, -3.60291782e+000, -6.20715544e-1, 2.06619782e+000, -1.42159644e+000, 3.75075865e-1, 1.88433333e-1, -2.64135123e-1, 1.47117661e-1, -4.71871047e-2, 1.24921920e-2};
#define NTAPS (14)
#define NBANKS (1)
#define ORDER (3)
#endif
/* Set here the ratio between output and input sample rate.
* It could be changed even during runtime! */
#define INCR (1.0 / 6.28 / 3)
/* delay line storage */
double delayLine[NTAPS];
/* Coefficient lookup "table" */
static double c(int ixTap, int ixBank, int ixPower){
return cMatrix[ixPower * (NTAPS * NBANKS) + ixTap * NBANKS + ixBank];
}
int main (void){
/* clear delay line */
int ix;
for (ix = 0; ix < NTAPS; ++ix)
delayLine[ix] = 0;
/* Index of last input sample that was read
* First valid sample starts at 0 */
int sampleIndexInput = -1;
/* Position of next output sample in input stream */
double sampleIndexOutput = 0.0;
/* loop forever. Terminate, once out of input data. */
while (1){
/* Split output sample location into integer and fractional part:
* Integer part corresponds to sample index in input stream
* fractional part [0, 1[ spans one tap (covering NBANKS segments) */
int sio_int = floor(sampleIndexOutput);
double sio_fracInTap = sampleIndexOutput - (double)sio_int;
assert((sio_fracInTap >= 0) && (sio_fracInTap < 1));
/* Further split the fractional part into
* - bank index
* - fractional part within the bank */
int sio_intBank = floor(sio_fracInTap * (double) NBANKS);
double sio_fracInBank = sio_fracInTap * (double) NBANKS - (double)sio_intBank;
assert((sio_fracInBank >= 0) && (sio_fracInBank < 1));
/* ****************************************************
* load new samples into the delay line, until the last
* processed input sample (sampleIndexInput) catches
* up with the integer part of the output stream position (sio_int)
* ***************************************************/
while (sampleIndexInput < sio_int){
/* Advance the delay line one step */
++sampleIndexInput;
for (ix = NTAPS-1; ix > 0; --ix)
delayLine[ix] = delayLine[ix-1];
/* Read one input sample */
int flag = scanf("%lf", &delayLine[0]);
if (flag != 1)
goto done; /* there's nothing wrong with "goto" as "break" for multiple loops ... */
} /* while delay line behind output data */
/* ****************************************************
* Calculate one output sample:
* "out" sums up the contribution of each tap
* ***************************************************/
double out = 0;
int ixTap;
for (ixTap = 0; ixTap < NTAPS; ++ixTap){
/* ****************************************************
* Contribution of tap "ixTap" to output:
* ***************************************************/
/* Evaluate polynomial in sio_fracInBank:
* c(ixTap, sio_intBank, 0) is the constant coefficient
* c(ixTap, sio_intBank, 1) is the linear coefficient etc
*/
double hornerSum = c(ixTap, sio_intBank, ORDER);
int ixPower;
for (ixPower = ORDER-1; ixPower >= 0; --ixPower){
hornerSum *= sio_fracInBank;
hornerSum += c(ixTap, sio_intBank, ixPower);
} /* for ixPower */
/* ****************************************************
* Weigh the delay line sample of this tap with the
* polynomial result and add to output
* ***************************************************/
out += hornerSum * delayLine[ixTap];
} /* for ixTap */
/* ****************************************************
* Generate output sample and advance the position of
* the next output sample in the input data stream
* ***************************************************/
printf("%1.12le\n", out);
sampleIndexOutput += INCR;
} /* loop until out of input data */
done: /* out of input data => break loops and continue here */
return 0;
} /* main */
#include "bp_iir.h"
#include <math.h>
#define BP_MAX_COEFS 120
#define PI 3.1415926
/*This is an array of the filter parameters object
defined in the br_iir.h file*/
static struct bp_coeffs bp_coeff_arr[BP_MAX_COEFS];
/*This initialization function will create the
band pass filter coefficients array, you have to specify:
fsfilt = Sampling Frequency
gb = Gain at cut frequencies
Q = Q factor, Higher Q gives narrower band
fstep = Frequency step to increase center frequencies
in the array
fmin = Minimum frequency for the range of center frequencies
*/
void bp_iir_init(double fsfilt,double gb,double Q,short fstep, short fmin) {
int i;
double damp;
double wo;
damp = gb/sqrt(1 - pow(gb,2));
for (i=0;i<BP_MAX_COEFS;i++) {
wo = 2*PI*(fstep*i + fmin)/fsfilt;
bp_coeff_arr[i].e = 1/(1 + damp*tan(wo/(Q*2)));
bp_coeff_arr[i].p = cos(wo);
bp_coeff_arr[i].d[0] = (1-bp_coeff_arr[i].e);
bp_coeff_arr[i].d[1] = 2*bp_coeff_arr[i].e*bp_coeff_arr[i].p;
bp_coeff_arr[i].d[2] = (2*bp_coeff_arr[i].e-1);
}
}
/*This function loads a given set of band pass filter coefficients acording to a center frequency index
into a band pass filter object
H = filter object
ind = index of the array mapped to a center frequency
*/
void bp_iir_setup(struct bp_filter * H,int ind) {
H->e = bp_coeff_arr[ind].e;
H->p = bp_coeff_arr[ind].p;
H->d[0] = bp_coeff_arr[ind].d[0];
H->d[1] = bp_coeff_arr[ind].d[1];
H->d[2] = bp_coeff_arr[ind].d[2];
}
/*This function loads a given set of band pass filter coefficients acording to a center frequency index
into a band pass filter object
H = filter object
ind = index of the array mapped to a center frequency
*/
double bp_iir_filter(double yin,struct bp_filter * H) {
double yout;
H->x[0] = H->x[1];
H->x[1] = H->x[2];
H->x[2] = yin;
H->y[0] = H->y[1];
H->y[1] = H->y[2];
H->y[2] = H->d[0]* H->x[2] - H->d[0]* H->x[0] + (H->d[1]* H->y[1]) - H->d[2]* H->y[0];
yout = H->y[2];
return yout;
}
/******************
bp_iir.h
**********************/
#ifndef __BP_IIR_H__
#define __BP_IIR_H__
struct bp_coeffs{
double e;
double p;
double d[3];
};
struct bp_filter{
double e;
double p;
double d[3];
double x[3];
double y[3];
};
extern void bp_iir_init(double fsfilt,double gb,double Q,short fstep, short fmin);
extern void bp_iir_setup(struct bp_filter * H,int index);
extern double bp_iir_filter(double yin,struct bp_filter * H);
#endif