Alexandre Pirotte / Mbed 2 deprecated NUCLEO_LIDAR_scan_interrupt

LUIS

Dependencies:   mbed

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?

UserRevisionLine numberNew 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