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