![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LUIS
Revision 0:fcc396b03bbe, committed 2018-10-05
- Comitter:
- pirottealex
- Date:
- Fri Oct 05 14:55:14 2018 +0000
- Commit message:
- a;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/digital_filter.cpp Fri Oct 05 14:55:14 2018 +0000 @@ -0,0 +1,122 @@ +/** + * 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; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/digital_filter.h Fri Oct 05 14:55:14 2018 +0000 @@ -0,0 +1,47 @@ +/** + * 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 + */ + +#ifndef DEF_digital_filter +#define DEF_digital_filter + + /** + * Structure stockant les coefficients du filtre + */ + typedef struct digital_filter_coefficient + { + double * coef_a; // Les coefficients a1..aN + double * coef_b; // Les coefficients b0..bM + int numberOfCoef_a; // Le nombre de coefficients a + int numberOfCoef_b; // Le nombre de coefficients b + } digital_filter_coefficient; + + /** + * Structure stockant les coefficiants du filtre et les échantillons et résultats nécessaires + */ + typedef struct digital_filter + { + double * samples; // Les échantillons en entrées du filtre + double * results; // Le résultat de la sortie du filtre + int current_sample; // L'indice du tableau où se trouvera le prochain échantillon + int previous_result; // L'indice du tableau où se trouve la sortie du filtre pour l'échantillon courrant + struct digital_filter_coefficient transfert_function; // La structure stockant les coefficients du filtre + } digital_filter; + + void init_digital_filter_coefficient(struct digital_filter_coefficient *, double *, int, double * , int); // Initialise les coefficients du filtre + void init_digital_filter(struct digital_filter *, double *, int, double * , int); // Initialise le filtre + double filter_next_sample(digital_filter *, double); // Calcule la sortie du filtre pour un nouvel échantillon + + int modulo(int, int); // Fonction modulo pour buffer circulaire (modulo(-1, 3) = 2) + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/fct.h Fri Oct 05 14:55:14 2018 +0000 @@ -0,0 +1,204 @@ +#include "digital_filter.h" + +#define PERIOD 0.00005 +#define GET_INFO 0x50 +#define GET_HEALTH 0x52 +#define RESET 0x40 +#define SYNC 0xA5 +#define SCAN 0x20 +#define STOP 0x25 +#define BOFF 7 + +#define TTAB 1800 +#define TTMES 500 + +#define TLONGUEUR 4000 +#define TLARGEUR 4000 + +Serial Lidar (PC_10,PC_11); +Serial pc(USBTX,USBRX); + +PwmOut LIDAR(A3); + +uint16_t taille_tab=1; +uint16_t* Angle=(uint16_t*) malloc(taille_tab*sizeof(uint16_t)); +uint16_t* Distance=(uint16_t*) malloc(taille_tab*sizeof(uint16_t)); + +// FILTRAGE +double coef_b[4] = {0.1448, 0.1, 0.0608, 0.0208}; // Les 4 coefficients bj +double coef_a[3] = {-1.2978, 0.7875, -0.1632}; // Les 3 coefficients ai +digital_filter mon_filtre; + +void get_scan (void); +void init_lidar(void); +void get_info(void); +void get_health(void); +void get_position(void); +void init (void); + +void init(void) +{ + + init_digital_filter(&mon_filtre, coef_a, 3, coef_b, 4); + filter_next_sample(&mon_filtre, 600); + pc.baud(460800); + pc.printf("ALIVE\n\r"); + init_lidar(); +} + +void init_lidar(void) +{ + Lidar.baud(115200); + Lidar.putc(RESET); + wait(0.001); + get_info(); + wait(0.001); + get_health(); + LIDAR.period(PERIOD); + LIDAR.pulsewidth(PERIOD*0.6); + +} + +void get_health(void) +{ + char info[27] = {0}; + Lidar.putc(SYNC); + Lidar.putc(GET_HEALTH); + for(int k=0; k<10; k++) + { + info[k]=Lidar.getc(); + } + int statut=info[BOFF]; + if( statut==0) { + pc.printf("Statut = 0 : GOOD"); + } else if( statut==1) { + pc.printf("Statut = 1 : WARNING"); + } else if( statut==2) { + pc.printf("Statut = 2 : ERROR"); + } + pc.printf("\n\r"); +} + +void get_info(void) +{ + char info[27] = {0}; + Lidar.putc(SYNC); + Lidar.putc(GET_INFO); + for(int k=0; k<27; k++) + { + info[k]=Lidar.getc(); + } + + pc.printf("LIDAR MODEL : %d, LIDAR FIRMWARE : %d.%d, NUMERO DE SERIE : ",info[BOFF],info[BOFF+2],info[BOFF+1]); + for(int j=BOFF+3; j<=16+BOFF+3; j++) { + pc.printf("%x",info[j]); + } + pc.printf("\n\r"); +} + +void get_scan (void) +{ + taille_tab=1; + char msg[TTAB] = {0}; + + Lidar.putc(SYNC); + Lidar.putc(SCAN); + + for(uint16_t k=0; k<TTAB; k++) + { + msg[k]=Lidar.getc(); + } + Lidar.putc(SYNC); + Lidar.putc(STOP); + + char flag=0; + for(int i=10; i<TTAB-1; i=i+5) + { + char qualite= msg[i-3]>>2; + char check = msg[i-2]&0x01; + + if((msg[i-3]&0x01)&&(check)) + { + flag++; + } + if(flag==2) + { + taille_tab--; + break; + } + if((qualite>=8)&&(check)&&(flag)) + { + uint16_t dist=((msg[i]) + ((msg[i+1]) << 8)) >>2; + if(dist>0 && dist<700){ + + Angle = (uint16_t*)realloc(Angle,taille_tab*sizeof(uint16_t)); + Distance = (uint16_t*)realloc(Distance,taille_tab*sizeof(uint16_t)); + Angle[taille_tab-1]=((msg[i-2] >> 1) + (msg[i-1] << 7)) >>6; + Distance[taille_tab-1]=dist; + taille_tab++; + } + } + } + uint16_t max =taille_tab,buf; + for(uint16_t count1 = 0; count1 < max; count1++) + { + for(uint16_t count2 = count1; count2 < (max); count2++) + { + if(Angle[count2] < Angle[count1]) + { + buf = Angle[count2]; + Angle[count2] = Angle[count1]; + Angle[count1]= buf; + buf = Distance[count2]; + Distance[count2] = Distance[count1]; + Distance[count1]= buf; + + } + } + } + + /*for (uint16_t k=0; k<taille_tab-1;k++) + { + pc.printf("%d, %d, %d\n\r",k,Angle[k],Distance[k]); + } + */ + +} + +void get_position(void) +{ + get_scan(); + pc.printf("%d\n\r", taille_tab); + uint16_t dist_filtre[taille_tab]; + int16_t dist_deriv[taille_tab-1]; + uint8_t indice[20]; + uint8_t indice_count=0; + for (uint16_t j=0; j<taille_tab;j++) + { + dist_filtre[j]=filter_next_sample(&mon_filtre, Distance[j]); + //pc.printf("%d, %d, %d\n\r",Angle[j], Distance[j], dist_filtre[j]); + //pc.printf("%d, %d\n\r",j, taille_tab); + } + for (uint16_t j=1; j<taille_tab;j++) + { + dist_deriv[j-1]=(int16_t)(dist_filtre[j-1]-dist_filtre[j]); + //pc.printf("%d\n\r",dist_deriv[j-1]); + } + + for (uint16_t j=0; j<taille_tab-2;j++) + { + if((dist_deriv[j]<=0)&&(dist_deriv[j+1]>0)&&(abs(dist_deriv[j]-dist_deriv[j+1]))>5) + { + indice[indice_count]=j; + indice_count++; + } + } + + for (uint16_t j=0; j<indice_count;j++) + { + pc.printf("indice %d Angle %d\n\r", indice[j],Angle[indice[j]]); + } +} + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Oct 05 14:55:14 2018 +0000 @@ -0,0 +1,24 @@ +#include "mbed.h" +#include "fct.h" +#define LAVABRE 5 + +int main() { + + init(); + wait(LAVABRE); + Timer t1; + t1.start(); + int count=0; + while(count<5) + { + pc.printf("TOUR %0.3f\n\r",t1.read()); + t1.reset(); + get_position(); + wait(2); + count++; + } +} + + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Oct 05 14:55:14 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/5aab5a7997ee \ No newline at end of file