Slam.c
5.2 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
#include "includes.h"
#include "hardware.h"
#include "all_value.h"
#include "Slam.h"
# define Ang_SlamY_X 90.0f //雷达原始坐标系起始偏角
double X_SlamLaser;//雷达中心的X位置
double Y_SlamLaser;//雷达中心的Y位置
float Ang_SlamLaser;//雷达偏角其角度范围是0-360
int Status_SlamLaser;//置信度
Str_SlamPoint AgvSP;//车中心XY位置
float Ang_SlamAgv;//车中心偏角。其角度范围是0-360
u8 SlamMess_Fresh;//SLAM信息的包收到状态，位0->收到1号包；位1->收到2号包；位2->收到3号包；位3->收到4号包；位4->收到4号包；
void SlamPos_Ananlysis(CanRxMsg *Message)//SLAM原始信息解析
{
u8 sw[8];
double Va64;
float Va32f;
u8 i;
switch(Message->StdId)
{ //
case 0x716://SLAM软件传回的置信度（位置比较可信度）
for(i=0;i<8;i++){sw[i]=Message->Data[i];}
Status_SlamLaser=*(int*)&sw[0];
/*用此量的剩余空间来存放原始采集信息*/
VaIndexW_W(&(In_1_W),14,Message->Data[6]);//远
VaIndexW_W(&(In_1_W),15,Message->Data[5]);//中
VaIndexW_W(&(In_3_W),15,Message->Data[4]);//近
/*将采集的信息具体映射*/
IN_LRadarF_Far=VaIndexW_R(&In_1_W,14);
IN_LRadarF_Middle=VaIndexW_R(&In_1_W,15);
IN_LRadarF_Near=VaIndexW_R(&In_3_W,15);
VaIndexB_W(&SlamMess_Fresh,0,1);
break;
case 0x717://SLAM传回的雷达中心X位置
for(i=0;i<8;i++){sw[i]=Message->Data[i];}
Va64=*(double*)&sw[0];//此处的换算是因为工控机那部分程序对数据做了处理所致
X_SlamLaser=Va64*1000;
VaIndexB_W(&SlamMess_Fresh,1,1);
break;
case 0x718://SLAM传回的雷达中心Y位置
for(i=0;i<8;i++){sw[i]=Message->Data[i];}
Va64=*(double*)&sw[0];
Y_SlamLaser=Va64*1000;
VaIndexB_W(&SlamMess_Fresh,2,1);
break;
case 0x719://SLAM传回的雷达角度
for(i=0;i<8;i++){sw[i]=Message->Data[i];}
Va64=*(double*)&sw[0];
Va32f=Ang_SlamY_X+Va64*Du_Pi;
AngDeal(&Va32f);
Ang_SlamLaser=Va32f;
VaIndexB_W(&SlamMess_Fresh,3,1);
VaIndexB_W(&SlamMess_Fresh,4,1);
Tim4_Slam1Nums=0;
break;
}
}
void SLAM_SendMess(u8 id)//向工控机发信息，询问位置
{
CanTxMsg tx_message1;
tx_message1.IDE = CAN_ID_STD; //标准帧
tx_message1.RTR = CAN_RTR_DATA; //数据帧0
tx_message1.DLC = 0x08; //帧长度
tx_message1.StdId = 0x700+id; //帧ID为传入参数的CAN_ID
tx_message1.ExtId = 0;
tx_message1.Data[0] = Area_LRadarF;//
tx_message1.Data[1] = 0;
CAN_Transmit(CAN1,&tx_message1);
OSTimeDly(1);//必要通信间隔
}
u8 SlamVaCalEn=1;
u8 Tim4_Slam1Nums;//发送区域检测时间间隔计时
u8 Tim4_Slam2Nums;//获取雷达信息时间计时
u8 Tim5_Slam3Nums;//置信度丢失计时
float Ang_LaToAgvInStall;//雷达安装中心与车中心连线相对车中垂线夹角
int F_Dx_AgvCToMap;// 地图X偏差
int F_Dy_AgvCToMap;// 地图Y偏差
u16 F_DAng_AgvCToMap;// 地图角度偏差
void Slam_Pro(void)
{
double X_InitSlamAgv;//车中心相对于雷达坐标系下的X位置
double Y_InitSlamAgv;//车中心相对于雷达坐标系下的Y位置
float Ang_InitSlamAgv;//车本体相对雷达坐标系下的偏角
double Dis_XY1_Agv;//车中心相对雷达坐标系原点的距离
float Ang_XY1_Agv;//雷达中心到车中心连线相对雷达坐标系下偏角
float X_Slam2Agv;//
float Y_Slam2Agv;
float Ang_Slam2Agv;//
float Ang;//
if (SlamVaCalEn)//第一次计算下位置相对信息
{
// Dis_LaToAgvInStall=(float)sqrt( pow(DX_ScanLaInstall,2)+pow(DY_ScanLaInstall,2) );//雷达相对车中心距离
Ang_LaToAgvInStall=atan(F_DX_ScanLaInstall/F_DY_ScanLaInstall)*Du_Pi;//雷达中心与车心连线相对于车前中垂线夹角，逆为正
SlamVaCalEn=0;
}
if ( ((SlamMess_Fresh&0x0F)==0x0F) && ((Status_SlamLaser==2)|(KXDPB_Slam==1)) )//SlamMess_Fresh=15表示数据分4次都取到
{
Ang_InitSlamAgv=Ang_SlamLaser-Ang_LaInstall;//
AngDeal(&Ang_InitSlamAgv);
Ang_XY1_Agv=Ang_InitSlamAgv+Ang_LaToAgvInStall;
X_InitSlamAgv=X_SlamLaser-Dis_LaToAgvInStall*cos(Ang_XY1_Agv*Pi_Du);
Y_InitSlamAgv=Y_SlamLaser-Dis_LaToAgvInStall*sin(Ang_XY1_Agv*Pi_Du);
/*计算车中心在大地图坐标系下的位置*/
Ang_Slam2Agv=Ang_InitSlamAgv+Ang_LaS_AllS;//
AngDeal(&Ang_Slam2Agv);
Dis_XY1_Agv=sqrt( pow(X_InitSlamAgv,2)+pow(Y_InitSlamAgv,2) );
if (X_InitSlamAgv!=0)
{ Ang_XY1_Agv=atan(Y_InitSlamAgv/X_InitSlamAgv)*Du_Pi;
if(X_InitSlamAgv<0)Ang_XY1_Agv=Ang_XY1_Agv+180;
}
else if (Y_InitSlamAgv>0) Ang_XY1_Agv=90;
else Ang_XY1_Agv=270;
Ang_XY1_Agv=Ang_XY1_Agv+Ang_LaS_AllS;
X_Slam2Agv=Dis_XY1_Agv*cos(Ang_XY1_Agv*Pi_Du)-F_DX_ScanLaInstall;
Y_Slam2Agv=Dis_XY1_Agv*sin(Ang_XY1_Agv*Pi_Du)+F_DY_ScanLaInstall;
if(VaIndexDW_R(&Touch_Contr,11)&&(SysKey==3005))//
{ AgvSP.X_Slam=(float)X_SlamLaser;
AgvSP.Y_Slam=(float)Y_SlamLaser;
Ang_SlamAgv=Ang_SlamLaser;
}
else
{
Ang_SlamAgv=Ang_Slam2Agv+(float)F_DAng_AgvCToMap*0.01f;
AngDeal(&Ang_SlamAgv);
Ang=(float)F_DAng_AgvCToMap*0.01f*Pi_Du;
AgvSP.Ang=(short)(Ang_SlamAgv*10);
AgvSP.X_Slam=(float)F_Dx_AgvCToMap+X_Slam2Agv*cos(Ang)-Y_Slam2Agv*sin(Ang);
AgvSP.Y_Slam=(float)F_Dy_AgvCToMap+X_Slam2Agv*sin(Ang)+Y_Slam2Agv*cos(Ang);
}
SlamMess_Fresh=SlamMess_Fresh&(~0x0F); //清除收到SLAM所有信息包的状态
if (Status_SlamLaser==2)Tim5_Slam3Nums=0;
}
if (Tim4_Slam2Nums>3&&(Electrify_Step==100))
{ SLAM_SendMess(6);
Tim4_Slam2Nums=0;
}
if (Tim4_Slam1Nums>100)
{ VaIndexW_W(&NodeOnLine,8,0);}
else VaIndexW_W(&NodeOnLine,8,1);
}