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 1800
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 // FILTRAGE 
00028 double coef_b[4] = {0.1448, 0.1, 0.0608, 0.0208};    // Les 4 coefficients bj
00029 double coef_a[3] = {-1.2978, 0.7875, -0.1632};          // Les 3 coefficients ai
00030 digital_filter mon_filtre;
00031 
00032 void get_scan (void);
00033 void init_lidar(void);
00034 void get_info(void);
00035 void get_health(void);
00036 void get_position(void);
00037 void init (void);
00038 
00039 void init(void)
00040 {
00041     
00042     init_digital_filter(&mon_filtre, coef_a, 3, coef_b, 4);
00043     filter_next_sample(&mon_filtre, 600);
00044     pc.baud(460800);
00045     pc.printf("ALIVE\n\r");
00046     init_lidar();    
00047 }
00048 
00049 void init_lidar(void)
00050 {
00051     Lidar.baud(115200);
00052     Lidar.putc(RESET);
00053     wait(0.001);
00054     get_info();
00055     wait(0.001);
00056     get_health();
00057     LIDAR.period(PERIOD);
00058     LIDAR.pulsewidth(PERIOD*0.6);
00059 
00060 }
00061 
00062 void get_health(void)
00063 {
00064     char info[27] = {0};
00065     Lidar.putc(SYNC);
00066     Lidar.putc(GET_HEALTH);
00067     for(int k=0; k<10; k++)
00068     {
00069         info[k]=Lidar.getc();
00070     }
00071     int statut=info[BOFF];
00072     if( statut==0) {
00073         pc.printf("Statut = 0 : GOOD");
00074     } else if( statut==1) {
00075         pc.printf("Statut = 1 : WARNING");
00076     } else if( statut==2) {
00077         pc.printf("Statut = 2 : ERROR");
00078     }
00079     pc.printf("\n\r");
00080 }
00081 
00082 void get_info(void)
00083 {
00084     char info[27] = {0};
00085     Lidar.putc(SYNC);
00086     Lidar.putc(GET_INFO);
00087     for(int k=0; k<27; k++)
00088     {
00089         info[k]=Lidar.getc();
00090     }
00091     
00092     pc.printf("LIDAR MODEL : %d, LIDAR FIRMWARE : %d.%d, NUMERO DE SERIE : ",info[BOFF],info[BOFF+2],info[BOFF+1]);
00093     for(int j=BOFF+3; j<=16+BOFF+3; j++) {
00094         pc.printf("%x",info[j]);
00095     }
00096     pc.printf("\n\r");
00097 }
00098 
00099 void get_scan (void)
00100 {
00101     taille_tab=1;
00102     char msg[TTAB] = {0};
00103     
00104     Lidar.putc(SYNC);
00105     Lidar.putc(SCAN);
00106     
00107     for(uint16_t k=0; k<TTAB; k++)
00108     {
00109         msg[k]=Lidar.getc();
00110     }
00111     Lidar.putc(SYNC);
00112     Lidar.putc(STOP);
00113     
00114     char flag=0;
00115     for(int i=10; i<TTAB-1; i=i+5) 
00116     {
00117         char qualite= msg[i-3]>>2;
00118         char check = msg[i-2]&0x01;
00119         
00120         if((msg[i-3]&0x01)&&(check))
00121         {
00122             flag++;
00123         }
00124         if(flag==2)
00125         {
00126             taille_tab--;
00127             break;
00128         }        
00129         if((qualite>=8)&&(check)&&(flag))
00130         {
00131             uint16_t dist=((msg[i]) + ((msg[i+1]) << 8)) >>2;
00132             if(dist>0 && dist<700){
00133                 
00134                 Angle = (uint16_t*)realloc(Angle,taille_tab*sizeof(uint16_t));
00135                 Distance = (uint16_t*)realloc(Distance,taille_tab*sizeof(uint16_t));
00136                 Angle[taille_tab-1]=((msg[i-2] >> 1) + (msg[i-1] << 7)) >>6;
00137                 Distance[taille_tab-1]=dist;
00138                 taille_tab++;
00139             }
00140         }
00141     }
00142     uint16_t max =taille_tab,buf;
00143     for(uint16_t count1 = 0; count1 < max; count1++)
00144     {
00145         for(uint16_t count2 = count1; count2 < (max); count2++)
00146         {
00147             if(Angle[count2] < Angle[count1])
00148             {   
00149                 buf = Angle[count2];
00150                 Angle[count2] = Angle[count1];
00151                 Angle[count1]= buf;
00152                 buf = Distance[count2];
00153                 Distance[count2] = Distance[count1];
00154                 Distance[count1]= buf;
00155 
00156             }
00157         }
00158     }
00159     
00160     /*for (uint16_t k=0; k<taille_tab-1;k++)
00161     {
00162         pc.printf("%d, %d, %d\n\r",k,Angle[k],Distance[k]);
00163     }
00164     */
00165     
00166 }
00167 
00168 void get_position(void)
00169 {
00170     get_scan();
00171     pc.printf("%d\n\r", taille_tab);
00172     uint16_t dist_filtre[taille_tab];
00173     int16_t dist_deriv[taille_tab-1];
00174     uint8_t indice[20];
00175     uint8_t indice_count=0;
00176     for (uint16_t j=0; j<taille_tab;j++)
00177     {
00178         dist_filtre[j]=filter_next_sample(&mon_filtre, Distance[j]);
00179         //pc.printf("%d, %d, %d\n\r",Angle[j], Distance[j], dist_filtre[j]);
00180         //pc.printf("%d, %d\n\r",j, taille_tab);
00181     }
00182     for (uint16_t j=1; j<taille_tab;j++)
00183     {
00184         dist_deriv[j-1]=(int16_t)(dist_filtre[j-1]-dist_filtre[j]);
00185         //pc.printf("%d\n\r",dist_deriv[j-1]);
00186     }
00187 
00188     for (uint16_t j=0; j<taille_tab-2;j++)
00189     {
00190         if((dist_deriv[j]<=0)&&(dist_deriv[j+1]>0)&&(abs(dist_deriv[j]-dist_deriv[j+1]))>5)
00191         {
00192             indice[indice_count]=j;
00193             indice_count++;
00194         }
00195     }
00196 
00197     for (uint16_t j=0; j<indice_count;j++)
00198     {
00199         pc.printf("indice %d Angle %d\n\r", indice[j],Angle[indice[j]]);
00200     }
00201 }   
00202     
00203 
00204