LUIS

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
pirottealex
Date:
Fri Oct 05 14:55:14 2018 +0000
Commit message:
a;

Changed in this revision

digital_filter.cpp Show annotated file Show diff for this revision Revisions of this file
digital_filter.h Show annotated file Show diff for this revision Revisions of this file
fct.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
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