Alexandre Pirotte / Mbed 2 deprecated NUCLEO_LIDAR_scan

LUIS

Dependencies:   mbed

digital_filter.cpp@0:fcc396b03bbe, 2018-10-05 (annotated)

Committer:
pirottealex
Date:
Fri Oct 05 14:55:14 2018 +0000
Revision:
0:fcc396b03bbe
a;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pirottealex 0:fcc396b03bbe 1 /**
pirottealex 0:fcc396b03bbe 2 * digital_filter.h
pirottealex 0:fcc396b03bbe 3 *
pirottealex 0:fcc396b03bbe 4 * Auteur : Ferdinand Piette (Avril 2011)
pirottealex 0:fcc396b03bbe 5 * Version : 1.0b
pirottealex 0:fcc396b03bbe 6 *
pirottealex 0:fcc396b03bbe 7 * Fourni des fonctions permettant d'appliquer des filtres numériques
pirottealex 0:fcc396b03bbe 8 * ayant pour fonction de transfert :
pirottealex 0:fcc396b03bbe 9 *
pirottealex 0:fcc396b03bbe 10 * b0*z + b1*z^-1 + ... + bN*z^-N
pirottealex 0:fcc396b03bbe 11 * H(z) = --------------------------------
pirottealex 0:fcc396b03bbe 12 * 1 + a1*z^-1 + ... + aM*z^-M
pirottealex 0:fcc396b03bbe 13 */
pirottealex 0:fcc396b03bbe 14
pirottealex 0:fcc396b03bbe 15 #include <stdlib.h>
pirottealex 0:fcc396b03bbe 16 #include <stdio.h>
pirottealex 0:fcc396b03bbe 17
pirottealex 0:fcc396b03bbe 18 #include "digital_filter.h"
pirottealex 0:fcc396b03bbe 19
pirottealex 0:fcc396b03bbe 20 /**
pirottealex 0:fcc396b03bbe 21 * Initialise les coefficiants du filtre
pirottealex 0:fcc396b03bbe 22 * @param coef_a : les coefficients ai de la fonction de transfert du filtre
pirottealex 0:fcc396b03bbe 23 * @param coef_b : les coefficients bj de la fonction de transfert du filtre
pirottealex 0:fcc396b03bbe 24 * @param coef_a_size : le nombre de coefficients ai. Correspond à M dans la fonction de transfert du filtre
pirottealex 0:fcc396b03bbe 25 * @param coef_b_size : le nombre de coefficients bi. Correspond à N dans la fonction de transfert du filtre
pirottealex 0:fcc396b03bbe 26 * @param transfert_function : un pointeur vers la structure digital_filter_coefficient à initialiser
pirottealex 0:fcc396b03bbe 27 * Si coef_a_size = 0, alors on a la fonction de transfert d'un filtre RIF, sinon, on a celle d'un filtre RII
pirottealex 0:fcc396b03bbe 28 */
pirottealex 0:fcc396b03bbe 29 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)
pirottealex 0:fcc396b03bbe 30 {
pirottealex 0:fcc396b03bbe 31 int i;
pirottealex 0:fcc396b03bbe 32
pirottealex 0:fcc396b03bbe 33 // Création du tableau des coefficients a
pirottealex 0:fcc396b03bbe 34 transfert_function->numberOfCoef_a = coef_a_size;
pirottealex 0:fcc396b03bbe 35 transfert_function->coef_a = (double *)malloc(transfert_function->numberOfCoef_a * sizeof * transfert_function->coef_a);
pirottealex 0:fcc396b03bbe 36 for(i = 0; i < transfert_function->numberOfCoef_a; i++)
pirottealex 0:fcc396b03bbe 37 transfert_function->coef_a[i] = coef_a[i];
pirottealex 0:fcc396b03bbe 38
pirottealex 0:fcc396b03bbe 39 // Création du tableau des coefficients b
pirottealex 0:fcc396b03bbe 40 transfert_function->numberOfCoef_b = coef_b_size;
pirottealex 0:fcc396b03bbe 41 transfert_function->coef_b = (double *)malloc(transfert_function->numberOfCoef_b * sizeof * transfert_function->coef_b);
pirottealex 0:fcc396b03bbe 42 for(i = 0; i < transfert_function->numberOfCoef_b; i++)
pirottealex 0:fcc396b03bbe 43 transfert_function->coef_b[i] = coef_b[i];
pirottealex 0:fcc396b03bbe 44 }
pirottealex 0:fcc396b03bbe 45
pirottealex 0:fcc396b03bbe 46 /**
pirottealex 0:fcc396b03bbe 47 * Initialise le filtre
pirottealex 0:fcc396b03bbe 48 * @param coef_a : les coefficients ai de la fonction de transfert du filtre
pirottealex 0:fcc396b03bbe 49 * @param coef_b : les coefficients bj de la fonction de transfert du filtre
pirottealex 0:fcc396b03bbe 50 * @param coef_a_size : le nombre de coefficients ai. Correspond à M dans la fonction de transfert du filtre
pirottealex 0:fcc396b03bbe 51 * @param coef_b_size : le nombre de coefficients bi. Correspond à N dans la fonction de transfert du filtre
pirottealex 0:fcc396b03bbe 52 * @param filter : un pointeur vers la structure digital_filter à initialiser
pirottealex 0:fcc396b03bbe 53 * Si coef_a_size = 0, alors on se ramène à un filtre RIF, sinon, on a un filtre RII
pirottealex 0:fcc396b03bbe 54 */
pirottealex 0:fcc396b03bbe 55 void init_digital_filter(struct digital_filter * filter, double * coef_a, int coef_a_size, double * coef_b, int coef_b_size)
pirottealex 0:fcc396b03bbe 56 {
pirottealex 0:fcc396b03bbe 57 // Initialise les coefficients du filtre
pirottealex 0:fcc396b03bbe 58 init_digital_filter_coefficient(&filter->transfert_function, coef_a, coef_a_size, coef_b, coef_b_size);
pirottealex 0:fcc396b03bbe 59
pirottealex 0:fcc396b03bbe 60 // Alloue l'espace mémoire pour les échantillons et les résultats
pirottealex 0:fcc396b03bbe 61 filter->samples = (double *)calloc(filter->transfert_function.numberOfCoef_b, sizeof * filter->samples);
pirottealex 0:fcc396b03bbe 62 filter->results = (double *)calloc(filter->transfert_function.numberOfCoef_a, sizeof * filter->results);
pirottealex 0:fcc396b03bbe 63
pirottealex 0:fcc396b03bbe 64 // Initialise les pointeurs des tableaux d'échantillons et de résultats
pirottealex 0:fcc396b03bbe 65 filter->current_sample = 0;
pirottealex 0:fcc396b03bbe 66 filter->previous_result = filter->transfert_function.numberOfCoef_a-1;
pirottealex 0:fcc396b03bbe 67 }
pirottealex 0:fcc396b03bbe 68
pirottealex 0:fcc396b03bbe 69 /**
pirottealex 0:fcc396b03bbe 70 * Calcul l'échantillon après filtrage
pirottealex 0:fcc396b03bbe 71 * @param sample : le nouvel échantillon que l'on veut filtrer
pirottealex 0:fcc396b03bbe 72 * @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)
pirottealex 0:fcc396b03bbe 73 * @return l'échantillon après filtrage
pirottealex 0:fcc396b03bbe 74 */
pirottealex 0:fcc396b03bbe 75 double filter_next_sample(digital_filter * filter, double sample)
pirottealex 0:fcc396b03bbe 76 {
pirottealex 0:fcc396b03bbe 77 double result = 0;
pirottealex 0:fcc396b03bbe 78 int i;
pirottealex 0:fcc396b03bbe 79
pirottealex 0:fcc396b03bbe 80 // Stocke en mémoire le nouvel échantillon
pirottealex 0:fcc396b03bbe 81 filter->samples[filter->current_sample] = sample;
pirottealex 0:fcc396b03bbe 82
pirottealex 0:fcc396b03bbe 83 // Calcul le résultat du filtrage pour un filtre RIF ou RII
pirottealex 0:fcc396b03bbe 84 // (Prise en compte des échantillons précédent)
pirottealex 0:fcc396b03bbe 85 for(i = 0; i < filter->transfert_function.numberOfCoef_b; i++)
pirottealex 0:fcc396b03bbe 86 result += filter->transfert_function.coef_b[i] * filter->samples[modulo((filter->current_sample-i),filter->transfert_function.numberOfCoef_b)];
pirottealex 0:fcc396b03bbe 87
pirottealex 0:fcc396b03bbe 88 // Calcul le résultat du filtrage pour un filtre RII
pirottealex 0:fcc396b03bbe 89 // (Correction avec le résultat du filtrage des échantillons précédents)
pirottealex 0:fcc396b03bbe 90 for(i = 0; i < filter->transfert_function.numberOfCoef_a; i++)
pirottealex 0:fcc396b03bbe 91 result -= filter->transfert_function.coef_a[i] * filter->results[modulo((filter->previous_result-i),filter->transfert_function.numberOfCoef_a)];
pirottealex 0:fcc396b03bbe 92
pirottealex 0:fcc396b03bbe 93 // Met à jours les pointeurs des tableaux d'échantillons et de résultats
pirottealex 0:fcc396b03bbe 94 filter->current_sample = (filter->current_sample + 1) % filter->transfert_function.numberOfCoef_b;
pirottealex 0:fcc396b03bbe 95 if(filter->transfert_function.numberOfCoef_a > 0)
pirottealex 0:fcc396b03bbe 96 {
pirottealex 0:fcc396b03bbe 97 filter->previous_result = (filter->previous_result + 1) % filter->transfert_function.numberOfCoef_a;
pirottealex 0:fcc396b03bbe 98 filter->results[filter->previous_result] = result; // Garde en mémoire le résultat pour l'échantillon en cours
pirottealex 0:fcc396b03bbe 99 }
pirottealex 0:fcc396b03bbe 100
pirottealex 0:fcc396b03bbe 101 return result;
pirottealex 0:fcc396b03bbe 102 }
pirottealex 0:fcc396b03bbe 103
pirottealex 0:fcc396b03bbe 104 /**
pirottealex 0:fcc396b03bbe 105 * Définie l'opération de modulo dans les négatifs pour manipuler des buffers circulaires
pirottealex 0:fcc396b03bbe 106 * @param number : le nombre modulé
pirottealex 0:fcc396b03bbe 107 * @param n : le nombre modulant
pirottealex 0:fcc396b03bbe 108 * @return number modulo n
pirottealex 0:fcc396b03bbe 109 * modulo(-1, 3) = 2
pirottealex 0:fcc396b03bbe 110 */
pirottealex 0:fcc396b03bbe 111 int modulo(int number, int n)
pirottealex 0:fcc396b03bbe 112 {
pirottealex 0:fcc396b03bbe 113 if(number >= 0)
pirottealex 0:fcc396b03bbe 114 return number%n;
pirottealex 0:fcc396b03bbe 115
pirottealex 0:fcc396b03bbe 116 int result = (-1*number)%n;
pirottealex 0:fcc396b03bbe 117
pirottealex 0:fcc396b03bbe 118 if(result > 0)
pirottealex 0:fcc396b03bbe 119 return n-result;
pirottealex 0:fcc396b03bbe 120
pirottealex 0:fcc396b03bbe 121 return 0;
pirottealex 0:fcc396b03bbe 122 }