Diff: fct.h

Revision:

0:43101a0b7a4c

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++;
+        } 
+       
+        
+    }
+}
+