Alexandre Pirotte
/
NUCLEO_LIDAR_scan_interrupt
LUIS
digital_filter.cpp
- Committer:
- pirottealex
- Date:
- 2018-10-05
- Revision:
- 0:43101a0b7a4c
File content as of revision 0:43101a0b7a4c:
/** * digital_filter.h * * Auteur : Ferdinand Piette (Avril 2011) * Version : 1.0b * * Fourni des fonctions permettant d'appliquer des filtres numériques * ayant pour fonction de transfert : * * b0*z + b1*z^-1 + ... + bN*z^-N * H(z) = -------------------------------- * 1 + a1*z^-1 + ... + aM*z^-M */ #include <stdlib.h> #include <stdio.h> #include "digital_filter.h" /** * Initialise les coefficiants du filtre * @param coef_a : les coefficients ai de la fonction de transfert du filtre * @param coef_b : les coefficients bj de la fonction de transfert du filtre * @param coef_a_size : le nombre de coefficients ai. Correspond à M dans la fonction de transfert du filtre * @param coef_b_size : le nombre de coefficients bi. Correspond à N dans la fonction de transfert du filtre * @param transfert_function : un pointeur vers la structure digital_filter_coefficient à initialiser * Si coef_a_size = 0, alors on a la fonction de transfert d'un filtre RIF, sinon, on a celle d'un filtre RII */ void init_digital_filter_coefficient(struct digital_filter_coefficient * transfert_function, double * coef_a, int coef_a_size, double * coef_b, int coef_b_size) { int i; // Création du tableau des coefficients a transfert_function->numberOfCoef_a = coef_a_size; transfert_function->coef_a = (double *)malloc(transfert_function->numberOfCoef_a * sizeof * transfert_function->coef_a); for(i = 0; i < transfert_function->numberOfCoef_a; i++) transfert_function->coef_a[i] = coef_a[i]; // Création du tableau des coefficients b transfert_function->numberOfCoef_b = coef_b_size; transfert_function->coef_b = (double *)malloc(transfert_function->numberOfCoef_b * sizeof * transfert_function->coef_b); for(i = 0; i < transfert_function->numberOfCoef_b; i++) transfert_function->coef_b[i] = coef_b[i]; } /** * Initialise le filtre * @param coef_a : les coefficients ai de la fonction de transfert du filtre * @param coef_b : les coefficients bj de la fonction de transfert du filtre * @param coef_a_size : le nombre de coefficients ai. Correspond à M dans la fonction de transfert du filtre * @param coef_b_size : le nombre de coefficients bi. Correspond à N dans la fonction de transfert du filtre * @param filter : un pointeur vers la structure digital_filter à initialiser * Si coef_a_size = 0, alors on se ramène à un filtre RIF, sinon, on a un filtre RII */ void init_digital_filter(struct digital_filter * filter, double * coef_a, int coef_a_size, double * coef_b, int coef_b_size) { // Initialise les coefficients du filtre init_digital_filter_coefficient(&filter->transfert_function, coef_a, coef_a_size, coef_b, coef_b_size); // Alloue l'espace mémoire pour les échantillons et les résultats filter->samples = (double *)calloc(filter->transfert_function.numberOfCoef_b, sizeof * filter->samples); filter->results = (double *)calloc(filter->transfert_function.numberOfCoef_a, sizeof * filter->results); // Initialise les pointeurs des tableaux d'échantillons et de résultats filter->current_sample = 0; filter->previous_result = filter->transfert_function.numberOfCoef_a-1; } /** * Calcul l'échantillon après filtrage * @param sample : le nouvel échantillon que l'on veut filtrer * @param filter : un pointeur vers la structure digital_filter contenant les informations du filtre (fonction de transfert et mémoire des échantillons précédents) * @return l'échantillon après filtrage */ double filter_next_sample(digital_filter * filter, double sample) { double result = 0; int i; // Stocke en mémoire le nouvel échantillon filter->samples[filter->current_sample] = sample; // Calcul le résultat du filtrage pour un filtre RIF ou RII // (Prise en compte des échantillons précédent) for(i = 0; i < filter->transfert_function.numberOfCoef_b; i++) result += filter->transfert_function.coef_b[i] * filter->samples[modulo((filter->current_sample-i),filter->transfert_function.numberOfCoef_b)]; // Calcul le résultat du filtrage pour un filtre RII // (Correction avec le résultat du filtrage des échantillons précédents) for(i = 0; i < filter->transfert_function.numberOfCoef_a; i++) result -= filter->transfert_function.coef_a[i] * filter->results[modulo((filter->previous_result-i),filter->transfert_function.numberOfCoef_a)]; // Met à jours les pointeurs des tableaux d'échantillons et de résultats filter->current_sample = (filter->current_sample + 1) % filter->transfert_function.numberOfCoef_b; if(filter->transfert_function.numberOfCoef_a > 0) { filter->previous_result = (filter->previous_result + 1) % filter->transfert_function.numberOfCoef_a; filter->results[filter->previous_result] = result; // Garde en mémoire le résultat pour l'échantillon en cours } return result; } /** * Définie l'opération de modulo dans les négatifs pour manipuler des buffers circulaires * @param number : le nombre modulé * @param n : le nombre modulant * @return number modulo n * modulo(-1, 3) = 2 */ int modulo(int number, int n) { if(number >= 0) return number%n; int result = (-1*number)%n; if(result > 0) return n-result; return 0; }