LUIS

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
pirottealex
Date:
Fri Oct 05 14:55:30 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 43101a0b7a4c digital_filter.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/digital_filter.cpp	Fri Oct 05 14:55:30 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 43101a0b7a4c digital_filter.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/digital_filter.h	Fri Oct 05 14:55:30 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 43101a0b7a4c fct.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fct.h	Fri Oct 05 14:55:30 2018 +0000
@@ -0,0 +1,232 @@
+#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 1000
+#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));
+
+char flag_reception=0;
+uint16_t i=0;
+char buffer[TTAB];
+
+// 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 start_scan(void);
+void stop_scan(void);
+void reception_lidar(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)
+{
+    
+    //pc.printf("coucou");
+    taille_tab=1;
+    char msg[TTAB] = {0};
+    stop_scan();
+    memcpy(&msg,&buffer,TTAB);   
+    //start_scan();
+    
+    
+    char flag=0;
+    for(int ii=0; ii<TTAB-5; ii=ii+5) 
+    {
+        pc.printf("%d\n\r",(buffer[ii]&0x01));
+        char qualite= msg[ii]>>2;
+        char check = msg[ii+1]&0x01;
+        
+        if((msg[ii]&0x01)&&(check))
+        {
+            flag++;
+        }
+        /*if(flag==2)
+        {
+        pc.printf("FIN\n\r");
+
+            taille_tab--;
+            break;
+        }    */    
+        if((qualite>=8)&&(check)&&(flag))
+        {
+            uint16_t dist=((msg[ii+3]) + ((msg[ii+4]) << 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[ii+1] >> 1) + (msg[ii+2] << 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 start_scan(void)
+{
+    Lidar.attach(&reception_lidar);
+    flag_reception=0;
+    i=0;
+    Lidar.putc(SYNC);
+    Lidar.putc(SCAN);
+}
+
+void stop_scan(void)
+{
+    Lidar.attach(0);
+    Lidar.putc(SYNC);
+    Lidar.putc(STOP);
+    flag_reception=0;
+    i=0;
+}    
+
+
+void reception_lidar(void)
+{
+    if(flag_reception==0)
+    {
+        
+        if(i<7)
+        {
+            Lidar.getc();
+            i++     ;       
+        }
+        else
+        {
+            pc.printf("\n\r");
+            flag_reception=1;
+            i=0;
+        }
+    }
+   if(flag_reception==1)
+    {
+        if(i==999)
+        {
+            //pc.printf("%d\n\r",i);
+            for (uint16_t k=0; k<TTAB-5; k++)
+            {
+                buffer[k]=buffer[k+5];
+            }
+            i=TTAB-5;
+        }  
+        else
+        {
+           buffer[i]=Lidar.getc();
+           i++;
+        } 
+       
+        
+    }
+}
+
diff -r 000000000000 -r 43101a0b7a4c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 05 14:55:30 2018 +0000
@@ -0,0 +1,20 @@
+#include "mbed.h"
+#include "fct.h"
+#define LAVABRE 5
+
+int main() {
+
+    init();
+    wait(LAVABRE);
+    start_scan();
+    wait(2);
+    get_scan();
+    while(1)
+    {
+        wait(1);
+    }
+}
+
+
+
+
diff -r 000000000000 -r 43101a0b7a4c mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Oct 05 14:55:30 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/5aab5a7997ee
\ No newline at end of file