Alexandre Pirotte / Mbed 2 deprecated NUCLEO_LIDAR_scan

LUIS

Dependencies:   mbed

fct.h@0:fcc396b03bbe, 2018-10-05 (annotated)

Committer:
pirottealex
Date:
Fri Oct 05 14:55:14 2018 +0000
Revision:
0:fcc396b03bbe
a;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pirottealex 0:fcc396b03bbe 1 #include "digital_filter.h"
pirottealex 0:fcc396b03bbe 2
pirottealex 0:fcc396b03bbe 3 #define PERIOD 0.00005
pirottealex 0:fcc396b03bbe 4 #define GET_INFO 0x50
pirottealex 0:fcc396b03bbe 5 #define GET_HEALTH 0x52
pirottealex 0:fcc396b03bbe 6 #define RESET 0x40
pirottealex 0:fcc396b03bbe 7 #define SYNC 0xA5
pirottealex 0:fcc396b03bbe 8 #define SCAN 0x20
pirottealex 0:fcc396b03bbe 9 #define STOP 0x25
pirottealex 0:fcc396b03bbe 10 #define BOFF 7
pirottealex 0:fcc396b03bbe 11
pirottealex 0:fcc396b03bbe 12 #define TTAB 1800
pirottealex 0:fcc396b03bbe 13 #define TTMES 500
pirottealex 0:fcc396b03bbe 14
pirottealex 0:fcc396b03bbe 15 #define TLONGUEUR 4000
pirottealex 0:fcc396b03bbe 16 #define TLARGEUR 4000
pirottealex 0:fcc396b03bbe 17
pirottealex 0:fcc396b03bbe 18 Serial Lidar (PC_10,PC_11);
pirottealex 0:fcc396b03bbe 19 Serial pc(USBTX,USBRX);
pirottealex 0:fcc396b03bbe 20
pirottealex 0:fcc396b03bbe 21 PwmOut LIDAR(A3);
pirottealex 0:fcc396b03bbe 22
pirottealex 0:fcc396b03bbe 23 uint16_t taille_tab=1;
pirottealex 0:fcc396b03bbe 24 uint16_t* Angle=(uint16_t*) malloc(taille_tab*sizeof(uint16_t));
pirottealex 0:fcc396b03bbe 25 uint16_t* Distance=(uint16_t*) malloc(taille_tab*sizeof(uint16_t));
pirottealex 0:fcc396b03bbe 26
pirottealex 0:fcc396b03bbe 27 // FILTRAGE
pirottealex 0:fcc396b03bbe 28 double coef_b[4] = {0.1448, 0.1, 0.0608, 0.0208}; // Les 4 coefficients bj
pirottealex 0:fcc396b03bbe 29 double coef_a[3] = {-1.2978, 0.7875, -0.1632}; // Les 3 coefficients ai
pirottealex 0:fcc396b03bbe 30 digital_filter mon_filtre;
pirottealex 0:fcc396b03bbe 31
pirottealex 0:fcc396b03bbe 32 void get_scan (void);
pirottealex 0:fcc396b03bbe 33 void init_lidar(void);
pirottealex 0:fcc396b03bbe 34 void get_info(void);
pirottealex 0:fcc396b03bbe 35 void get_health(void);
pirottealex 0:fcc396b03bbe 36 void get_position(void);
pirottealex 0:fcc396b03bbe 37 void init (void);
pirottealex 0:fcc396b03bbe 38
pirottealex 0:fcc396b03bbe 39 void init(void)
pirottealex 0:fcc396b03bbe 40 {
pirottealex 0:fcc396b03bbe 41
pirottealex 0:fcc396b03bbe 42 init_digital_filter(&mon_filtre, coef_a, 3, coef_b, 4);
pirottealex 0:fcc396b03bbe 43 filter_next_sample(&mon_filtre, 600);
pirottealex 0:fcc396b03bbe 44 pc.baud(460800);
pirottealex 0:fcc396b03bbe 45 pc.printf("ALIVE\n\r");
pirottealex 0:fcc396b03bbe 46 init_lidar();
pirottealex 0:fcc396b03bbe 47 }
pirottealex 0:fcc396b03bbe 48
pirottealex 0:fcc396b03bbe 49 void init_lidar(void)
pirottealex 0:fcc396b03bbe 50 {
pirottealex 0:fcc396b03bbe 51 Lidar.baud(115200);
pirottealex 0:fcc396b03bbe 52 Lidar.putc(RESET);
pirottealex 0:fcc396b03bbe 53 wait(0.001);
pirottealex 0:fcc396b03bbe 54 get_info();
pirottealex 0:fcc396b03bbe 55 wait(0.001);
pirottealex 0:fcc396b03bbe 56 get_health();
pirottealex 0:fcc396b03bbe 57 LIDAR.period(PERIOD);
pirottealex 0:fcc396b03bbe 58 LIDAR.pulsewidth(PERIOD*0.6);
pirottealex 0:fcc396b03bbe 59
pirottealex 0:fcc396b03bbe 60 }
pirottealex 0:fcc396b03bbe 61
pirottealex 0:fcc396b03bbe 62 void get_health(void)
pirottealex 0:fcc396b03bbe 63 {
pirottealex 0:fcc396b03bbe 64 char info[27] = {0};
pirottealex 0:fcc396b03bbe 65 Lidar.putc(SYNC);
pirottealex 0:fcc396b03bbe 66 Lidar.putc(GET_HEALTH);
pirottealex 0:fcc396b03bbe 67 for(int k=0; k<10; k++)
pirottealex 0:fcc396b03bbe 68 {
pirottealex 0:fcc396b03bbe 69 info[k]=Lidar.getc();
pirottealex 0:fcc396b03bbe 70 }
pirottealex 0:fcc396b03bbe 71 int statut=info[BOFF];
pirottealex 0:fcc396b03bbe 72 if( statut==0) {
pirottealex 0:fcc396b03bbe 73 pc.printf("Statut = 0 : GOOD");
pirottealex 0:fcc396b03bbe 74 } else if( statut==1) {
pirottealex 0:fcc396b03bbe 75 pc.printf("Statut = 1 : WARNING");
pirottealex 0:fcc396b03bbe 76 } else if( statut==2) {
pirottealex 0:fcc396b03bbe 77 pc.printf("Statut = 2 : ERROR");
pirottealex 0:fcc396b03bbe 78 }
pirottealex 0:fcc396b03bbe 79 pc.printf("\n\r");
pirottealex 0:fcc396b03bbe 80 }
pirottealex 0:fcc396b03bbe 81
pirottealex 0:fcc396b03bbe 82 void get_info(void)
pirottealex 0:fcc396b03bbe 83 {
pirottealex 0:fcc396b03bbe 84 char info[27] = {0};
pirottealex 0:fcc396b03bbe 85 Lidar.putc(SYNC);
pirottealex 0:fcc396b03bbe 86 Lidar.putc(GET_INFO);
pirottealex 0:fcc396b03bbe 87 for(int k=0; k<27; k++)
pirottealex 0:fcc396b03bbe 88 {
pirottealex 0:fcc396b03bbe 89 info[k]=Lidar.getc();
pirottealex 0:fcc396b03bbe 90 }
pirottealex 0:fcc396b03bbe 91
pirottealex 0:fcc396b03bbe 92 pc.printf("LIDAR MODEL : %d, LIDAR FIRMWARE : %d.%d, NUMERO DE SERIE : ",info[BOFF],info[BOFF+2],info[BOFF+1]);
pirottealex 0:fcc396b03bbe 93 for(int j=BOFF+3; j<=16+BOFF+3; j++) {
pirottealex 0:fcc396b03bbe 94 pc.printf("%x",info[j]);
pirottealex 0:fcc396b03bbe 95 }
pirottealex 0:fcc396b03bbe 96 pc.printf("\n\r");
pirottealex 0:fcc396b03bbe 97 }
pirottealex 0:fcc396b03bbe 98
pirottealex 0:fcc396b03bbe 99 void get_scan (void)
pirottealex 0:fcc396b03bbe 100 {
pirottealex 0:fcc396b03bbe 101 taille_tab=1;
pirottealex 0:fcc396b03bbe 102 char msg[TTAB] = {0};
pirottealex 0:fcc396b03bbe 103
pirottealex 0:fcc396b03bbe 104 Lidar.putc(SYNC);
pirottealex 0:fcc396b03bbe 105 Lidar.putc(SCAN);
pirottealex 0:fcc396b03bbe 106
pirottealex 0:fcc396b03bbe 107 for(uint16_t k=0; k<TTAB; k++)
pirottealex 0:fcc396b03bbe 108 {
pirottealex 0:fcc396b03bbe 109 msg[k]=Lidar.getc();
pirottealex 0:fcc396b03bbe 110 }
pirottealex 0:fcc396b03bbe 111 Lidar.putc(SYNC);
pirottealex 0:fcc396b03bbe 112 Lidar.putc(STOP);
pirottealex 0:fcc396b03bbe 113
pirottealex 0:fcc396b03bbe 114 char flag=0;
pirottealex 0:fcc396b03bbe 115 for(int i=10; i<TTAB-1; i=i+5)
pirottealex 0:fcc396b03bbe 116 {
pirottealex 0:fcc396b03bbe 117 char qualite= msg[i-3]>>2;
pirottealex 0:fcc396b03bbe 118 char check = msg[i-2]&0x01;
pirottealex 0:fcc396b03bbe 119
pirottealex 0:fcc396b03bbe 120 if((msg[i-3]&0x01)&&(check))
pirottealex 0:fcc396b03bbe 121 {
pirottealex 0:fcc396b03bbe 122 flag++;
pirottealex 0:fcc396b03bbe 123 }
pirottealex 0:fcc396b03bbe 124 if(flag==2)
pirottealex 0:fcc396b03bbe 125 {
pirottealex 0:fcc396b03bbe 126 taille_tab--;
pirottealex 0:fcc396b03bbe 127 break;
pirottealex 0:fcc396b03bbe 128 }
pirottealex 0:fcc396b03bbe 129 if((qualite>=8)&&(check)&&(flag))
pirottealex 0:fcc396b03bbe 130 {
pirottealex 0:fcc396b03bbe 131 uint16_t dist=((msg[i]) + ((msg[i+1]) << 8)) >>2;
pirottealex 0:fcc396b03bbe 132 if(dist>0 && dist<700){
pirottealex 0:fcc396b03bbe 133
pirottealex 0:fcc396b03bbe 134 Angle = (uint16_t*)realloc(Angle,taille_tab*sizeof(uint16_t));
pirottealex 0:fcc396b03bbe 135 Distance = (uint16_t*)realloc(Distance,taille_tab*sizeof(uint16_t));
pirottealex 0:fcc396b03bbe 136 Angle[taille_tab-1]=((msg[i-2] >> 1) + (msg[i-1] << 7)) >>6;
pirottealex 0:fcc396b03bbe 137 Distance[taille_tab-1]=dist;
pirottealex 0:fcc396b03bbe 138 taille_tab++;
pirottealex 0:fcc396b03bbe 139 }
pirottealex 0:fcc396b03bbe 140 }
pirottealex 0:fcc396b03bbe 141 }
pirottealex 0:fcc396b03bbe 142 uint16_t max =taille_tab,buf;
pirottealex 0:fcc396b03bbe 143 for(uint16_t count1 = 0; count1 < max; count1++)
pirottealex 0:fcc396b03bbe 144 {
pirottealex 0:fcc396b03bbe 145 for(uint16_t count2 = count1; count2 < (max); count2++)
pirottealex 0:fcc396b03bbe 146 {
pirottealex 0:fcc396b03bbe 147 if(Angle[count2] < Angle[count1])
pirottealex 0:fcc396b03bbe 148 {
pirottealex 0:fcc396b03bbe 149 buf = Angle[count2];
pirottealex 0:fcc396b03bbe 150 Angle[count2] = Angle[count1];
pirottealex 0:fcc396b03bbe 151 Angle[count1]= buf;
pirottealex 0:fcc396b03bbe 152 buf = Distance[count2];
pirottealex 0:fcc396b03bbe 153 Distance[count2] = Distance[count1];
pirottealex 0:fcc396b03bbe 154 Distance[count1]= buf;
pirottealex 0:fcc396b03bbe 155
pirottealex 0:fcc396b03bbe 156 }
pirottealex 0:fcc396b03bbe 157 }
pirottealex 0:fcc396b03bbe 158 }
pirottealex 0:fcc396b03bbe 159
pirottealex 0:fcc396b03bbe 160 /*for (uint16_t k=0; k<taille_tab-1;k++)
pirottealex 0:fcc396b03bbe 161 {
pirottealex 0:fcc396b03bbe 162 pc.printf("%d, %d, %d\n\r",k,Angle[k],Distance[k]);
pirottealex 0:fcc396b03bbe 163 }
pirottealex 0:fcc396b03bbe 164 */
pirottealex 0:fcc396b03bbe 165
pirottealex 0:fcc396b03bbe 166 }
pirottealex 0:fcc396b03bbe 167
pirottealex 0:fcc396b03bbe 168 void get_position(void)
pirottealex 0:fcc396b03bbe 169 {
pirottealex 0:fcc396b03bbe 170 get_scan();
pirottealex 0:fcc396b03bbe 171 pc.printf("%d\n\r", taille_tab);
pirottealex 0:fcc396b03bbe 172 uint16_t dist_filtre[taille_tab];
pirottealex 0:fcc396b03bbe 173 int16_t dist_deriv[taille_tab-1];
pirottealex 0:fcc396b03bbe 174 uint8_t indice[20];
pirottealex 0:fcc396b03bbe 175 uint8_t indice_count=0;
pirottealex 0:fcc396b03bbe 176 for (uint16_t j=0; j<taille_tab;j++)
pirottealex 0:fcc396b03bbe 177 {
pirottealex 0:fcc396b03bbe 178 dist_filtre[j]=filter_next_sample(&mon_filtre, Distance[j]);
pirottealex 0:fcc396b03bbe 179 //pc.printf("%d, %d, %d\n\r",Angle[j], Distance[j], dist_filtre[j]);
pirottealex 0:fcc396b03bbe 180 //pc.printf("%d, %d\n\r",j, taille_tab);
pirottealex 0:fcc396b03bbe 181 }
pirottealex 0:fcc396b03bbe 182 for (uint16_t j=1; j<taille_tab;j++)
pirottealex 0:fcc396b03bbe 183 {
pirottealex 0:fcc396b03bbe 184 dist_deriv[j-1]=(int16_t)(dist_filtre[j-1]-dist_filtre[j]);
pirottealex 0:fcc396b03bbe 185 //pc.printf("%d\n\r",dist_deriv[j-1]);
pirottealex 0:fcc396b03bbe 186 }
pirottealex 0:fcc396b03bbe 187
pirottealex 0:fcc396b03bbe 188 for (uint16_t j=0; j<taille_tab-2;j++)
pirottealex 0:fcc396b03bbe 189 {
pirottealex 0:fcc396b03bbe 190 if((dist_deriv[j]<=0)&&(dist_deriv[j+1]>0)&&(abs(dist_deriv[j]-dist_deriv[j+1]))>5)
pirottealex 0:fcc396b03bbe 191 {
pirottealex 0:fcc396b03bbe 192 indice[indice_count]=j;
pirottealex 0:fcc396b03bbe 193 indice_count++;
pirottealex 0:fcc396b03bbe 194 }
pirottealex 0:fcc396b03bbe 195 }
pirottealex 0:fcc396b03bbe 196
pirottealex 0:fcc396b03bbe 197 for (uint16_t j=0; j<indice_count;j++)
pirottealex 0:fcc396b03bbe 198 {
pirottealex 0:fcc396b03bbe 199 pc.printf("indice %d Angle %d\n\r", indice[j],Angle[indice[j]]);
pirottealex 0:fcc396b03bbe 200 }
pirottealex 0:fcc396b03bbe 201 }
pirottealex 0:fcc396b03bbe 202
pirottealex 0:fcc396b03bbe 203
pirottealex 0:fcc396b03bbe 204