F030C8T6_Kbus_MIX.git

QuakeGod

2024-07-27 842bb64195f958b050867c50db66fc0aa413dafb

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