fct.h

00001 #include "digital_filter.h"
00002 
00003 #define PERIOD 0.00005
00004 #define GET_INFO 0x50
00005 #define GET_HEALTH 0x52
00006 #define RESET 0x40
00007 #define SYNC 0xA5
00008 #define SCAN 0x20
00009 #define STOP 0x25
00010 #define BOFF 7
00011 
00012 #define TTAB 1000
00013 #define TTMES 500
00014 
00015 #define TLONGUEUR 4000
00016 #define TLARGEUR 4000
00017 
00018 Serial Lidar (PC_10,PC_11);
00019 Serial pc(USBTX,USBRX);
00020 
00021 PwmOut LIDAR(A3);
00022 
00023 uint16_t taille_tab=1;
00024 uint16_t* Angle=(uint16_t*) malloc(taille_tab*sizeof(uint16_t));
00025 uint16_t* Distance=(uint16_t*) malloc(taille_tab*sizeof(uint16_t));
00026 
00027 char flag_reception=0;
00028 uint16_t i=0;
00029 char buffer[TTAB];
00030 
00031 // FILTRAGE 
00032 double coef_b[4] = {0.1448, 0.1, 0.0608, 0.0208};    // Les 4 coefficients bj
00033 double coef_a[3] = {-1.2978, 0.7875, -0.1632};          // Les 3 coefficients ai
00034 digital_filter mon_filtre;
00035 
00036 void get_scan (void);
00037 void init_lidar(void);
00038 void get_info(void);
00039 void get_health(void);
00040 void get_position(void);
00041 void init (void);
00042 void start_scan(void);
00043 void stop_scan(void);
00044 void reception_lidar(void);
00045 
00046 void init(void)
00047 {
00048     
00049     init_digital_filter(&mon_filtre, coef_a, 3, coef_b, 4);
00050     filter_next_sample(&mon_filtre, 600);
00051     pc.baud(460800);
00052     pc.printf("ALIVE\n\r");
00053     init_lidar();    
00054 }
00055 
00056 void init_lidar(void)
00057 {
00058     Lidar.baud(115200);
00059     Lidar.putc(RESET);
00060     wait(0.001);
00061     get_info();
00062     wait(0.001);
00063     get_health();
00064     LIDAR.period(PERIOD);
00065     LIDAR.pulsewidth(PERIOD*0.6);
00066 
00067 }
00068 
00069 void get_health(void)
00070 {
00071     char info[27] = {0};
00072     Lidar.putc(SYNC);
00073     Lidar.putc(GET_HEALTH);
00074     for(int k=0; k<10; k++)
00075     {
00076         info[k]=Lidar.getc();
00077     }
00078     int statut=info[BOFF];
00079     if( statut==0) {
00080         pc.printf("Statut = 0 : GOOD");
00081     } else if( statut==1) {
00082         pc.printf("Statut = 1 : WARNING");
00083     } else if( statut==2) {
00084         pc.printf("Statut = 2 : ERROR");
00085     }
00086     pc.printf("\n\r");
00087 }
00088 
00089 void get_info(void)
00090 {
00091     char info[27] = {0};
00092     Lidar.putc(SYNC);
00093     Lidar.putc(GET_INFO);
00094     for(int k=0; k<27; k++)
00095     {
00096         info[k]=Lidar.getc();
00097     }
00098     
00099     pc.printf("LIDAR MODEL : %d, LIDAR FIRMWARE : %d.%d, NUMERO DE SERIE : ",info[BOFF],info[BOFF+2],info[BOFF+1]);
00100     for(int j=BOFF+3; j<=16+BOFF+3; j++) {
00101         pc.printf("%x",info[j]);
00102     }
00103     pc.printf("\n\r");
00104 }
00105 
00106 void get_scan (void)
00107 {
00108     
00109     //pc.printf("coucou");
00110     taille_tab=1;
00111     char msg[TTAB] = {0};
00112     stop_scan();
00113     memcpy(&msg,&buffer,TTAB);   
00114     //start_scan();
00115     
00116     
00117     char flag=0;
00118     for(int ii=0; ii<TTAB-5; ii=ii+5) 
00119     {
00120         pc.printf("%d\n\r",(buffer[ii]&0x01));
00121         char qualite= msg[ii]>>2;
00122         char check = msg[ii+1]&0x01;
00123         
00124         if((msg[ii]&0x01)&&(check))
00125         {
00126             flag++;
00127         }
00128         /*if(flag==2)
00129         {
00130         pc.printf("FIN\n\r");
00131 
00132             taille_tab--;
00133             break;
00134         }    */    
00135         if((qualite>=8)&&(check)&&(flag))
00136         {
00137             uint16_t dist=((msg[ii+3]) + ((msg[ii+4]) << 8)) >>2;
00138             if(dist>0 && dist<700){
00139                 
00140                 Angle = (uint16_t*)realloc(Angle,taille_tab*sizeof(uint16_t));
00141                 Distance = (uint16_t*)realloc(Distance,taille_tab*sizeof(uint16_t));
00142                 Angle[taille_tab-1]=((msg[ii+1] >> 1) + (msg[ii+2] << 7)) >>6;
00143                 Distance[taille_tab-1]=dist;
00144                 taille_tab++;
00145             }
00146         }
00147     }
00148     uint16_t max =taille_tab,buf;
00149     for(uint16_t count1 = 0; count1 < max; count1++)
00150     {
00151         for(uint16_t count2 = count1; count2 < (max); count2++)
00152         {
00153             if(Angle[count2] < Angle[count1])
00154             {   
00155                 buf = Angle[count2];
00156                 Angle[count2] = Angle[count1];
00157                 Angle[count1]= buf;
00158                 buf = Distance[count2];
00159                 Distance[count2] = Distance[count1];
00160                 Distance[count1]= buf;
00161 
00162             }
00163         }
00164     }
00165     
00166     for (uint16_t k=0; k<taille_tab-1;k++)
00167     {
00168         pc.printf("%d, %d, %d\n\r",k,Angle[k],Distance[k]);
00169     }
00170     
00171     
00172 }
00173  
00174 
00175 
00176 void start_scan(void)
00177 {
00178     Lidar.attach(&reception_lidar);
00179     flag_reception=0;
00180     i=0;
00181     Lidar.putc(SYNC);
00182     Lidar.putc(SCAN);
00183 }
00184 
00185 void stop_scan(void)
00186 {
00187     Lidar.attach(0);
00188     Lidar.putc(SYNC);
00189     Lidar.putc(STOP);
00190     flag_reception=0;
00191     i=0;
00192 }    
00193 
00194 
00195 void reception_lidar(void)
00196 {
00197     if(flag_reception==0)
00198     {
00199         
00200         if(i<7)
00201         {
00202             Lidar.getc();
00203             i++     ;       
00204         }
00205         else
00206         {
00207             pc.printf("\n\r");
00208             flag_reception=1;
00209             i=0;
00210         }
00211     }
00212    if(flag_reception==1)
00213     {
00214         if(i==999)
00215         {
00216             //pc.printf("%d\n\r",i);
00217             for (uint16_t k=0; k<TTAB-5; k++)
00218             {
00219                 buffer[k]=buffer[k+5];
00220             }
00221             i=TTAB-5;
00222         }  
00223         else
00224         {
00225            buffer[i]=Lidar.getc();
00226            i++;
00227         } 
00228        
00229         
00230     }
00231 }
00232