Alexandre Pirotte / Mbed 2 deprecated NUCLEO_LIDAR_scan_interrupt

LUIS

Dependencies:   mbed

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

Committer:
pirottealex
Date:
Fri Oct 05 14:55:30 2018 +0000
Revision:
0:43101a0b7a4c
a

Who changed what in which revision?

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