查看 | 历史 | 提交 | 提交对比
QuakeGod
2024-07-27 842bb64195f958b050867c50db66fc0aa413dafb
提交 | 用户 | age
842bb6 1 #include "YDLidar.h"
Q 2
3 int nPosX;
4 int nPosY;
5 int nPosZ;
6
7 int nPosZ1,nPosZ2;
8
9 int pCount1=0;
10 int pCount2=0;
11
12 int dCount1=0;
13 int dCount2=0;
14
15 int vCount1=0;
16 int vCount2=0;
17
18
19 void Uart3SendPacket(char * str, int len);
20 void Uart5SendPacket(char * str, int len);
21
22 //正弦表，0 - 360 代表 0 - 2PI,结果0 - 1000 表示0 - 1，表中只有0 - PI/2即1/4周期的数据，其他的数据用对称和镜像得到。
23 const int SinTable[] =  
24 {
25 0,17,35, 52, 70, 87, 105, 122, 139, 156, 174, 191, 208, 225, 242, 259, 276, 292,
26 309, 326, 342, 358, 375, 391, 407, 423, 438, 454, 469, 485, 500, 515, 530, 545,
27 559, 574, 588, 602, 616, 629, 643, 656, 669, 682, 695, 707, 719, 731, 743, 755,
28 766, 777, 788, 799, 809, 819, 829, 839, 848, 857, 866, 875, 883, 891, 899, 906,
29 914, 921, 927, 934, 940, 946, 951, 956, 961, 966, 970, 974, 978, 982, 985, 988,
30 990, 993, 995, 996, 998, 999, 999, 1000, 1000,
31 1000, 999, 999, 998, 996, 995,
32 993, 990, 988, 985, 982, 978, 974, 970, 966, 961, 956, 951, 946, 940, 934, 927,
33 921, 914, 906, 899, 891, 883, 875, 866, 857, 848, 839, 829, 819, 809, 799, 788,
34 777, 766, 755, 743, 731, 719, 707, 695, 682, 669, 656, 643, 629, 616, 602, 588,
35 574, 559, 545, 530, 515, 500, 485, 469, 454, 438, 423, 407, 391, 375, 358, 342,
36 326, 309, 292, 276, 259, 242, 225, 208, 191, 174, 156, 139, 122, 105, 87, 70,
37 52, 35, 17, 0, -17, -35, -52, -70, -87, -105, -122, -139, -156, -174, -191,
38 -208, -225, -242, -259, -276, -292, -309, -326, -342, -358, -375, -391, -407,
39 -423, -438, -454, -469, -485, -500, -515, -530, -545, -559, -574, -588, -602,
40 -616, -629, -643, -656, -669, -682, -695, -707, -719, -731, -743, -755, -766,
41 -777, -788, -799, -809, -819, -829, -839, -848, -857, -866, -875, -883, -891,
42 -899, -906, -914, -921, -927, -934, -940, -946, -951, -956, -961, -966, -970,
43 -974, -978, -982, -985, -988, -990, -993, -995, -996, -998, -999, -999, -1000,
44 -1000, -1000, -999, -999, -998, -996, -995, -993, -990, -988, -985, -982, -978,
45 -974, -970, -966, -961, -956, -951, -946, -940, -934, -927, -921, -914, -906,
46 -899, -891, -883, -875, -866, -857, -848, -839, -829, -819, -809, -799, -788,
47 -777, -766, -755, -743, -731, -719, -707, -695, -682, -669, -656, -643, -629,
48 -616, -602, -588, -574, -559, -545, -530, -515, -500, -485, -469, -454, -438,
49 -423, -407, -391, -375, -358, -342, -326, -309, -292, -276, -259, -242, -225,
50 -208, -191, -174, -156, -139, -122, -105, -87, -70, -52, -35, -17
51 };
52 const int CosTable[] =
53 {
54     1000 ,999 ,999 ,998 ,996 ,995 ,993 ,990 ,988 ,985 ,982 ,978 ,974 ,970 ,966 ,961
55 ,956 ,951 ,946 ,940 ,934 ,927 ,921 ,914 ,906 ,899 ,891 ,883 ,875 ,866 ,857 ,848
56 ,839 ,829 ,819 ,809 ,799 ,788 ,777 ,766 ,755 ,743 ,731 ,719 ,707 ,695 ,682 ,669
57 ,656 ,643 ,629 ,616 ,602 ,588 ,574 ,559 ,545 ,530 ,515 ,500 ,485 ,469 ,454 ,438
58 ,423 ,407 ,391 ,375 ,358 ,342 ,326 ,309 ,292 ,276 ,259 ,242 ,225 ,208 ,191 ,174
59 ,156 ,139 ,122 ,105 ,87 ,70 ,52 ,35 ,17 ,0 ,-17 ,-35 ,-52 ,-70 ,-87 ,-105 ,-122
60 ,-139 ,-156 ,-174 ,-191 ,-208 ,-225 ,-242 ,-259 ,-276 ,-292 ,-309 ,-326 ,-342
61 ,-358 ,-375 ,-391 ,-407 ,-423 ,-438 ,-454 ,-469 ,-485 ,-500 ,-515 ,-530 ,-545
62 ,-559 ,-574 ,-588 ,-602 ,-616 ,-629 ,-643 ,-656 ,-669 ,-682 ,-695 ,-707 ,-719
63 ,-731 ,-743 ,-755 ,-766 ,-777 ,-788 ,-799 ,-809 ,-819 ,-829 ,-839 ,-848 ,-857
64 ,-866 ,-875 ,-883 ,-891 ,-899 ,-906 ,-914 ,-921 ,-927 ,-934 ,-940 ,-946 ,-951
65 ,-956 ,-961 ,-966 ,-970 ,-974 ,-978 ,-982 ,-985 ,-988 ,-990 ,-993 ,-995 ,-996
66 ,-998 ,-999 ,-999 ,-1000 ,-1000 ,-1000 ,-999 ,-999 ,-998 ,-996 ,-995 ,-993 ,-990
67 ,-988 ,-985 ,-982 ,-978 ,-974 ,-970 ,-966 ,-961 ,-956 ,-951 ,-946 ,-940 ,-934
68 ,-927 ,-921 ,-914 ,-906 ,-899 ,-891 ,-883 ,-875 ,-866 ,-857 ,-848 ,-839 ,-829
69 ,-819 ,-809 ,-799 ,-788 ,-777 ,-766 ,-755 ,-743 ,-731 ,-719 ,-707 ,-695 ,-682
70 ,-669 ,-656 ,-643 ,-629 ,-616 ,-602 ,-588 ,-574 ,-559 ,-545 ,-530 ,-515 ,-500
71 ,-485 ,-469 ,-454 ,-438 ,-423 ,-407 ,-391 ,-375 ,-358 ,-342 ,-326 ,-309 ,-292
72 ,-276 ,-259 ,-242 ,-225 ,-208 ,-191 ,-174 ,-156 ,-139 ,-122 ,-105 ,-87 ,-70 ,-52
73 ,-35 ,-17 ,0 ,17 ,35 ,52 ,70 ,87 ,105 ,122 ,139 ,156 ,174 ,191 ,208 ,225 ,242
74 ,259 ,276 ,292 ,309 ,326 ,342 ,358 ,375 ,391 ,407 ,423 ,438 ,454 ,469 ,485 ,500
75 ,515 ,530 ,545 ,559 ,574 ,588 ,602 ,616 ,629 ,643 ,656 ,669 ,682 ,695 ,707 ,719
76 ,731 ,743 ,755 ,766 ,777 ,788 ,799 ,809 ,819 ,829 ,839 ,848 ,857 ,866 ,875 ,883
77 ,891 ,899 ,906 ,914 ,921 ,927 ,934 ,940 ,946 ,951 ,956 ,961 ,966 ,970 ,974 ,978
78 ,982 ,985 ,988 ,990 ,993 ,995 ,996 ,998 ,999 ,999 ,1000 ,1000
79 };
80
81
82 const unsigned char sintab[256]={    //正弦表，0 - 1024代表 0 - 2PI,结果0 - 255表示0 - 1，表中只有0 - PI/2即1/4周期的数据，其他的数据用对称和镜像得到。
83  0, 1, 3, 4, 6, 7, 9, 10, 12, 14, 15, 17, 18, 20, 21, 23, 25, 26, 28, 29, 31, 32, 34, 36, 37, 39, 40, 42, 43, 45, 46,
84  48, 49, 51, 53, 54, 56, 57, 59, 60, 62, 63, 65, 66, 68, 69, 71, 72, 74, 75, 77, 78, 80, 81, 83, 84, 86, 87, 89, 90,
85  92, 93, 95, 96, 97, 99, 100, 102, 103, 105, 106, 108, 109, 110, 112, 113, 115, 116, 117, 119, 120, 122, 123, 124,
86  126, 127, 128, 130, 131, 132, 134, 135, 136, 138, 139, 140, 142, 143, 144, 146, 147, 148, 149, 151, 152, 153, 155,
87  156, 157, 158, 159, 161, 162, 163, 164, 166, 167, 168, 169, 170, 171, 173, 174, 175, 176, 177, 178, 179, 181, 182,
88  183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204,
89  205, 206, 207, 208, 209, 210, 211, 211, 212, 213, 214, 215, 216, 217, 217, 218, 219, 220, 221, 221, 222, 223,
90  224, 225, 225, 226, 227, 227, 228, 229, 230, 230, 231, 232, 232, 233, 234, 234, 235, 235, 236, 237, 237, 238,
91  238, 239, 239, 240, 241, 241, 242, 242, 243, 243, 244, 244, 244, 245, 245, 246, 246, 247, 247, 247, 248, 248, 
92  249, 249, 249, 250, 250, 250, 251, 251, 251, 251, 252, 252, 252, 252, 253, 253, 253, 253, 254, 254, 254, 254, 
93  254, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,// 255 
94 };
95
96     
97
98
99 char StartCMD[2] = { 0xA5,0x60 };
100 char StopCMD[2] = { 0xA5,0x65 };
101
102
103 int StartAngle;
104 int EndAngle;
105
106 int YdLidarStart(int nIdx)
107 {
108     if (nIdx == 0 || nIdx == -1 ) {
109         Uart3SendPacket(StartCMD,sizeof(StartCMD));
110     }
111     if (nIdx == 1 || nIdx == -1) {
112         Uart5SendPacket(StartCMD,sizeof(StartCMD));
113     }
114         return 0;
115 };
116
117
118 int YdLidarStop(int nIdx){
119     if (nIdx == 0 || nIdx == -1 ) {
120         Uart3SendPacket(StopCMD,sizeof(StopCMD));
121     }
122     if (nIdx == 1 || nIdx == -1) {
123         Uart5SendPacket(StopCMD,sizeof(StopCMD));
124     }
125     return 0;
126 };
127
128 char startFlag0 = 0;
129 char startFlag1 = 0;
130
131 #define VALID_DATA_MAX 200
132 stLidarDot validData0[200];
133 stLidarDot validData1[200];
134
135 int nValidCount0 = 0;
136 int nValidCount1 = 0;
137
138 #define SUB_DEGREE 64
139
140 #define START_DEGREE 135
141 #define END_DEGREE 225
142
143 #define MAX_DISTANCE 500                //mm
144
145 //整数求SIN值，带线性插值功能， 输入0 - 360*64 为 0 - 2PI,一个周期;输出-1000 至 +1000,代表-1到+1;
146 int sini(int a)
147 {
148     
149     a = a % (360 * SUB_DEGREE);
150     int b;
151     b=a / SUB_DEGREE;
152     int xx = a % SUB_DEGREE;
153     if (b< 90 )
154     {
155         int d1= SinTable[b ];
156         int d2 =SinTable[b+1 ];
157         int d;
158         d = ((SUB_DEGREE-xx) * d1 + xx * d2 ) / SUB_DEGREE;
159         
160         return (d);
161     }
162     else if (b<180 )
163     {    
164         int c;
165         c=180-b;
166         
167         int d1= SinTable[c ];
168         int d2 =SinTable[c-1 ];
169         int d;
170         d = ((SUB_DEGREE-xx) * d1 + xx * d2 ) / SUB_DEGREE;
171         
172         return (d);
173     }
174     
175     else if (b<270)
176     {
177         int c;
178         c=b-180;
179         int d1= SinTable[c ];
180         int d2 =SinTable[c+1 ];
181         int d;
182         d = ((SUB_DEGREE-xx) * d1 + xx * d2 ) / SUB_DEGREE;
183         
184         return (-d);
185     }
186     else 
187     {
188         int c;
189         c=360 -b;
190         int d1= SinTable[c ];
191         int d2 =SinTable[c-1 ];
192         int d;
193         d = ((SUB_DEGREE-xx) * d1 + xx * d2 ) / SUB_DEGREE;
194         
195         return (-d);
196     }
197     
198 //    return a;
199 }
200
201 int cosi(int a)
202 {
203     return sini(a+90*SUB_DEGREE);
204 }
205
206 int YdLidarParsePkt(int nLidarIdx, stLidarDotsPkt * pLindarPkt, int len1)
207 {
208     int iRet = 0;
209     if (pLindarPkt->StSign != 0x55AA) return 0;        // check for start sign
210     if ((pLindarPkt->CT & 0x01) != 0x00) return 0;        // check for CT bit0. 
211     if (pLindarPkt->LSN<2) return 0;
212     
213     int startAngle = (pLindarPkt->FSA >> 1);    // degree * 64;
214     int endAngle = (pLindarPkt->LSA >> 1);         // degree * 64;
215     if (startAngle >= endAngle)return 0;
216     int nCount = pLindarPkt->LSN;    
217     int diffAngle = (endAngle - startAngle);
218     int eachAngle = diffAngle / (nCount - 1);    
219     
220     if (nLidarIdx == 0) 
221     {
222         pCount1++;
223         for (int i = 0;i < nCount && i < 40 ;i++)
224         {
225             float intensity = pLindarPkt->LidarDotDatas[i].intensity;
226             int angle = eachAngle * i + startAngle;    // degree * 64
227             dCount1++;
228             
229             if (angle < START_DEGREE * SUB_DEGREE )    startFlag0 = 1;
230             if (angle > START_DEGREE * SUB_DEGREE && angle < END_DEGREE * SUB_DEGREE) // &&x>100000&&x<250000)
231             {
232                 int value = (pLindarPkt->LidarDotDatas[i].s2 << 6) + (pLindarPkt->LidarDotDatas[i].s1 >> 2); // distance
233                 
234                 int x = value * sini(angle - 90 * SUB_DEGREE );            //     uM
235                 int y = value * cosi(angle - 90 * SUB_DEGREE );            //  uM
236                 
237                 //vectorX.push_back(x);
238                 //vectorY.push_back(y);
239                 validData0[nValidCount0].distance = value;
240                 validData0[nValidCount0].x = x;
241                 validData0[nValidCount0].y = y;
242                 if (nValidCount0 < 200 - 1 ) nValidCount0 ++;
243                 vCount1++;
244             }
245
246             if (angle > END_DEGREE * SUB_DEGREE)
247             {
248                 if (startFlag0 == 1 && nValidCount0 > 0)
249                 {
250
251                         int min = 1000000, max = -1000000;
252                         int minX = 1000000;
253                         for (int i = 0;i < nValidCount0;i++)
254                         {
255                             if (validData0[i].distance < 5000) continue;
256                             
257                             if (validData0[i].y > 0)
258                             {
259                                 if (validData0[i].y < min)
260                                     min = validData0[i].y;
261                             }
262                             if (validData0[i].y < 0)
263                             {
264                                 if (validData0[i].y > max)
265                                     max = validData0[i].y;
266                             }
267                         }
268                         float avg=0;
269                         for (int i = 0;i < nValidCount0;i++)
270                         {
271                             if (validData0[i].x < minX) 
272                             {
273                                 minX = validData0[i].x;
274                             }
275                         }
276                         if (min != 0 && max != 0)
277                         {
278                             avg = (min + max) / 2000;
279                         }
280                         
281                         nPosX = avg;
282                         nPosZ1 = minX/1000;
283                 }
284                 startFlag0 = 0;
285                 nValidCount0 = 0;
286             }
287         }
288     }
289
290     if (nLidarIdx == 1) 
291     {
292         pCount2++;
293         for (int i = 0;i < nCount && i < 40 ;i++)
294         {
295             float intensity = pLindarPkt->LidarDotDatas[i].intensity;
296             int angle = eachAngle * i + startAngle;
297
298             dCount2++;
299             
300             if (angle < START_DEGREE * SUB_DEGREE)    startFlag1 = 1;
301             if (angle > START_DEGREE * SUB_DEGREE && angle < END_DEGREE * SUB_DEGREE ) // &&x>100000&&x<250000)
302             {
303                 int value = (pLindarPkt->LidarDotDatas[i].s2 << 6) + (pLindarPkt->LidarDotDatas[i].s1 >> 2);
304
305                 int x = value * sini(angle - 90 * SUB_DEGREE );            //    uM
306                 int y = value * cosi(angle - 90 * SUB_DEGREE );            //    uM //vectorX.push_back(x);
307                 //vectorY.push_back(y);
308                 validData1[nValidCount1].distance = value; 
309                 validData1[nValidCount1].x = x;
310                 validData1[nValidCount1].y = y;
311                 nValidCount1 ++;
312                 vCount2 ++;
313             }
314
315             if (angle > END_DEGREE * SUB_DEGREE)
316             {
317                 if (startFlag1 == 1 && nValidCount1 > 0 )
318                 {
319                         int minDisIndex = 0;
320                         int minDistance = 99999999;
321                         
322                         int min = 1000000, max = -1000000;
323                         int minX = 1000000;
324                         for (int i = 0;i < nValidCount1;i++)
325                         {
326                             if (validData1[i].distance < 30) continue;        // skip too small points;
327                             
328                             if (validData1[i].distance < minDistance) {
329                                 minDisIndex = i;
330                                 minDistance = validData1[i].distance;
331                             }
332
333                             if (validData1[i].x < minX) 
334                             {
335                                 minX = validData1[i].x;
336                             }
337                             
338                             if (validData1[i].y > 0)
339                             {
340                                 if (validData1[i].y < min)
341                                     min = validData1[i].y;
342                             }
343                             if (validData1[i].y < 0)
344                             {
345                                 if (validData1[i].y > max)
346                                     max = validData1[i].y;
347                             }
348                             
349                         }
350                         int avg=0;
351                         int leftcount = 0;
352                         int midcount = 0;
353                         int rightcount =0;
354                         for (int i = 0;i < nValidCount1;i++)
355                         {
356                             if (validData1[i].distance < 30) continue;        // skip too small points;
357                             // 找中间空洞;
358                             if (validData1[i].x > minX + 50000) {
359                                             // points, 50mm higher that minX;
360                                 if (leftcount>5 && rightcount <5) midcount++;
361                                 
362                                 continue;        //skip 
363                             }else {
364                                     if (midcount ==0) {leftcount++;}
365                                     else {rightcount++;}
366                             }
367                         }
368                         if (min != 0 && max != 0)
369                         {
370                             avg = (min + max) / 2000;
371                         }
372                         
373                         nPosY = avg;
374                         nPosZ1 = minX/1000;
375                         nPosY = validData1[minDisIndex].y /1000;
376 //                        nPosZ2 = validData1[minDisIndex].x /1000;
377                         nPosZ2 = midcount;
378                 }
379                 startFlag1 = 0;
380                 nValidCount1 = 0;
381             }
382         }
383     }    
384     nPosZ = (nPosZ1 + nPosZ2) /2;
385     
386     return iRet;
387 }
388
389