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

diff -r 000000000000 -r fcc396b03bbe digital_filter.cpp
--- /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;
+}

diff -r 000000000000 -r fcc396b03bbe digital_filter.h
--- /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

diff -r 000000000000 -r fcc396b03bbe fct.h
--- /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]]);
+    }
+}   
+    
+
+

diff -r 000000000000 -r fcc396b03bbe main.cpp
--- /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++;
+    }
+}
+
+
+
+

diff -r 000000000000 -r fcc396b03bbe mbed.bld
--- /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