Filtrage numerique

/*!
 * \fn			s32 AMDF_SOS_Filter_4_Order(const float CST_FLT_SOS[L_SOS][C_SOS], const float flt_G, float (*Yn)[C_SOS>>1], float* pflt_SampleValueBuf)
 *				Fonction de traitement d'un filtre IIR représenté par son gain G et sa matrice SOS.
 * \param		float* pflt_SampleValueBuf = [X(n) X(n-1) X(n-2)], pointeur sur tableau d'échantillon.
 * \param		const float (*CST_FLT_SOS)[C_SOS], pointeur sur la SOS du filtre IIR.
 * \param		const float flt_G, gain du filtre IIR.
 * \param		u8 u8_Order = ordre du filtre.
 * \param		float (*Yn)[C_SOS>>1] : pointeur sur la matrice d'historique des sorties du filtre. Le nombre de ligne doit etre de u8_Order / 2. 
 *		Matrice historique sorties (nombre de ligne maximum (MAX_ORDER/2).
 *		Yn = |y1(n) y1(n-1) y1(n-2)|
 *			 |y2(n) y2(n-1) y2(n-2)|
 *			 |y3(n) y3(n-1) y3(n-2)|
 *			 |y4(n) y4(n-1) y4(n-2)|
 *			 |y5(n) y5(n-1) y5(n-2)|
 * \warning     Toutes les opérations de cette fonction doivent avoir des opérateurs de type flotant.
 * \warning     ATTENTION à la spécification de l'ordre !! Voir \param u8 u8_Order.
 * \return		Echantillon filtré
*/
#define C_SOS		6 //Nb colonne de la SOS
#define MAX_ORDER	10
s32 IIR_Filter(const float (*CST_FLT_SOS)[C_SOS], const float flt_G, u8 u8_Order, float (*Yn)[C_SOS>>1], float* pflt_SampleValueBuf)
{
// w = [w1(n) w2(n) w3(n) w4(n) w5(n)]
float w[MAX_ORDER>>1] = {0.0,0.0,0.0,0.0,0.0};
u8 u8_NbLigneSOS = (u8)((u8_Order>>1) + (u8_Order % 2));//Calcul du nombre de ligne de la SOS
	
//Traitement 1ere structure avec report du gain
	w[0] += ((*CST_FLT_SOS)[0] * (*pflt_SampleValueBuf)) + ((*CST_FLT_SOS)[1] * (*(pflt_SampleValueBuf + 1))) + ((*CST_FLT_SOS)[2] * (*(pflt_SampleValueBuf + 2)));
	w[0] *= flt_G;
	(*Yn)[0] = w[0] - ((*Yn)[1] * (*CST_FLT_SOS)[4]) - ((*Yn)[2] * (*CST_FLT_SOS)[5]);

//Traitement des autres structures
	for (u8 ul_sos = 1; ul_sos < u8_NbLigneSOS; ul_sos++)
	{
		w[ul_sos] += ((*(CST_FLT_SOS + ul_sos))[0] * (*(Yn + ul_sos - 1))[0]) + ((*(CST_FLT_SOS + ul_sos))[1] * (*(Yn + ul_sos - 1))[1]) + ((*(CST_FLT_SOS + ul_sos))[2] * (*(Yn + ul_sos - 1))[2]);
		(*(Yn + ul_sos))[0] = w[ul_sos] - ((*(Yn + ul_sos))[1] * (*(CST_FLT_SOS + ul_sos))[4]) - ((*(Yn + ul_sos))[2] * (*(CST_FLT_SOS + ul_sos))[5]);
	}

//Mise à jour matrice Yn pour le prochain echantillon
	for (u8 ul_Yn = 0; ul_Yn < u8_NbLigneSOS; ul_Yn++)
	{
		(*(Yn + ul_Yn))[2] = (*(Yn + ul_Yn))[1];
		(*(Yn + ul_Yn))[1] = (*(Yn + ul_Yn))[0];
	}
	
	return (s32)((*(Yn + (u8_NbLigneSOS - 1)))[0]);
}
#undef C_SOS
#undef MAX_ORDER

/*!
 * \fn			void AMDF_ConditionnementDebitEnPas(u8 u8_SampleNumber)
 *				Met en oeuvre la chaine de conditionnement du signal de débit patient en pas acq pour l'analyse AMDF
 * \param		u8 u8_SampleNumber : Numéro de l'échantillon à traiter.
 * \warning		u8_SampleNumber suit la valeur de g_u8_Indice_Acquisitions
 * \note		g_u16_Acquisitions_Debits_Patient_En_Pas est l'entrée de la chaîne de conditionnement
*/
void ConditionnementDebitEnPas(u8 u8_SampleNumber)
{	
//Filtre HP
static float floatBufMemoHP[3];//Historiques des entrées du filtre HP [X(n), X(n-1), X(n-2)]
static float Yn_HP[3][3] = {{0.0,0.0,0.0},//Historiques de sorties du filtre HP
							{0.0,0.0,0.0},
							{0.0,0.0,0.0}};
s32 s32_SignalRecentre;

//Filtre LP
static float floatBufMemoLP[3];//Historiques des entrées du filtre LP [X(n), X(n-1), X(n-2)]
static float Yn_LP[3][3] = {{0.0,0.0,0.0},
							{0.0,0.0,0.0},
							{0.0,0.0,0.0}};//Historiques de sorties du filtre LP

//1er étage conditionnement (recentrage)
	floatBufMemoHP[2] = floatBufMemoHP[1];
	floatBufMemoHP[1] = floatBufMemoHP[0];
	floatBufMemoHP[0] = (float)g_u16_Acquisitions_Debits_Patient_En_Pas[u8_SampleNumber];
	s32_SignalRecentre = IIR_Filter(HP_SOS, HP_G, 5, Yn_HP, floatBufMemoHP);

//2nd étage conditionnement (élimination bruit HF)
	floatBufMemoLP[2] = floatBufMemoLP[1];
	floatBufMemoLP[1] = floatBufMemoLP[0];
	floatBufMemoLP[0] = (float)s32_SignalRecentre;
	g_s32_Acq_Debit_Pas_Filtre[u8_SampleNumber] = IIR_Filter(LP_SOS, LP_G, 2, Yn_LP, floatBufMemoLP);
}

// Filtre passe-bas FS = 200HZ, Fc = 2.5Hz, Ordre 2 fixe, Ripple = 1dB.
// -- Type chebyshev 1 --
const float LP_SOS[][6] = {1.0,2.0,1.0,1.0,-1.91093,0.91744};
const float LP_G = 0.00145179;


//IIR HIGHPASS CHEBYSHEV TYPE I - Fc = 0.6Hz - ordre 5 - optimal.
const float HP_SOS[][6] = {{1.0, -1.0, 0.0, 1.0, -0.93694, 0.0},
						{1.0, -2.0, 1.0, 1.0, -1.97883, 0.97965},
						{1.0, -2.0, 1.0, 1.0, -1.99623, 0.99659}};
const float HP_G = 0.95669671;