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
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
#include "Data_VSpeed.h"
#include "Components.h"
#include "CAN_CH0_CAN_Communication_Matrix.h"
uint16_t DataVSpeedActual;
uint16_t DataVSpeedDisp;
uint16_t DataVSpeedHysteresis;
uint8_t DataVSpeedValid;
// static uint16_t DataVSpeedDisp_Mile;
static DataVSpeedSamplerStruct DataVSpeedSampler;
static DataVSPeedDampingStruct DataVSPeedDamping;
#define DATA_APPR_DIR_INC 0x01
#define DATA_APPR_DIR_DEC 0x02
/******************************************************************************
Data_Vehicle_Speed_KL30_Wakeup_Init
功 能：车速数据KL30_Wakeup_Init
参 数：无
返回值：无
******************************************************************************
注 意：该函数KL30_Wakeup_Init被调用一次
******************************************************************************/
void Data_Vehicle_Speed_KL30_Wakeup_Init(void)
{
DataVSpeedActual = 0;
DataVSpeedDisp = 0;
DataVSpeedHysteresis = 0;
DataVSpeedValid = 1;
DataVSpeedSampler.Cnt = 0;
DataVSPeedDamping.Speed = 0;
DataVSPeedDamping.Delta = 0;
DataVSPeedDamping.Dir = DATA_APPR_DIR_INC;
DataVSpeedSampler.Timer1 = 0;
DataVSpeedSampler.Timer = 0;
}
/******************************************************************************
函数名：Data_Vehicle_Speed_Wakeup_Init
功 能：车速数据唤醒初始化
参 数：无
返回值：无
******************************************************************************
注 意：该函数唤醒初始化被调用一次
******************************************************************************/
void Data_Vehicle_Speed_Wakeup_Init(void)
{
DataVSpeedDisp = 0;
DataVSpeedHysteresis = 0;
DataVSPeedDamping.Speed = 0;
DataVSPeedDamping.Delta = 0;
DataVSPeedDamping.Dir = DATA_APPR_DIR_INC;
DataVSpeedSampler.Timer1 = 0;
DataVSpeedSampler.Timer = 0;
}
/******************************************************************************
函数名：Data_Vehicle_Speed_Processing_Service
功 能：车速数据处理函数
参 数：无
返回值：无
******************************************************************************
注 意：该函数必须每20ms被调用一次
******************************************************************************/
void Data_Vehicle_Speed_Processing_Service(void)
{
uint8_t i;
uint16_t Delta;
uint32_t VSpeed;
uint32_t VSpeedCal;
// uint8_t ESC_VehicleSpeedvalid;
uint32_t Vehicle_Speed;
Vehicle_Speed = Get_CAN_CH0_ID_101_Sig_ECU_Vehicle_Speed();
//ESC_VehicleSpeedvalid = Get_CAN_CH0_ID_330_Sig_ESC_Vehicle_speed_valid( );
// 第1步：获取实际车速值及车速有效性
if( Common_Get_IG_Sts( ) == COMMON_POWER_ON )
{
if ((CAN_MSG_Status(&CAN_CH0_CanMsgOp, CAN_CH0_ID_CH0_101_Msg_Count) == CAN_SIG_LOST) || (Vehicle_Speed > 0XFF))
{
/* 先判断掉线 */
if (CAN_MSG_Status(&CAN_CH0_CanMsgOp, CAN_CH0_ID_CH0_101_Msg_Count) == CAN_SIG_LOST)
{
DataVSpeedValid = 0;
DataVSpeedActual = 0;
DataVSpeedSampler.Backup = 0;
}
#if 0
else if ( ESC_VehicleSpeedvalid == 1 )//发送can无效标志位
{
if ( DataVSpeedSampler.Timer1 <= 10 ) /* 10帧无效 */
{
if ( DataVSpeedValid )//判断车速是否有效,有效采集10次,车速等于上次值
{
DataVSpeedSampler.Timer1++;
DataVSpeedActual = DataVSpeedSampler.Backup;
}
else//无效,不采集,车速=0
{
DataVSpeedSampler.Timer1 = 10;
DataVSpeedActual = 0;
DataVSpeedSampler.Backup = 0;
}
}
else//不采集就,车速=0
{
DataVSpeedValid = 0;
DataVSpeedActual = 0;
DataVSpeedSampler.Backup = 0;
}
}
#endif
else /* 收到无效值 */
{
if ( DataVSpeedSampler.Timer <= 100 ) /* 100帧无效值 */
{
if ( DataVSpeedValid )
{
DataVSpeedSampler.Timer++;
DataVSpeedActual = DataVSpeedSampler.Backup;
}
else
{
DataVSpeedSampler.Timer = 100;
DataVSpeedActual = 0;
DataVSpeedSampler.Backup = 0;
}
}
else // 接收到100帧无效值
{
DataVSpeedValid = 0;
DataVSpeedActual = 0;
DataVSpeedSampler.Backup = 0;
}
}
}
else //车速在线且有效
{
if ( DataVSpeedValid == 0 )
{
DataVSpeedValid = 1;
VSpeed = Vehicle_Speed;
// VSpeed *= 9;
// VSpeed >>= 4;
DataVSpeedActual = VSpeed * 10;
}
else
{
DataVSpeedSampler.Buffer [ DataVSpeedSampler.Cnt ] = Vehicle_Speed;
i = DataVSpeedSampler.Cnt;
while ( (i > 0) && (DataVSpeedSampler.Buffer [ i ] < DataVSpeedSampler.Buffer [ i - 1 ]) )
{
VSpeed = DataVSpeedSampler.Buffer [ i ];
DataVSpeedSampler.Buffer [ i ] = DataVSpeedSampler.Buffer [ i - 1 ];
DataVSpeedSampler.Buffer [ i - 1 ] = VSpeed;
i--;
}
DataVSpeedSampler.Cnt++;
if ( DataVSpeedSampler.Cnt >= 3 )
{
DataVSpeedSampler.Cnt = 0;
VSpeed = DataVSpeedSampler.Buffer [ 1 ];
//VSpeed *= 9;
//VSpeed >>= 4;
DataVSpeedActual = VSpeed * 10;
}
DataVSpeedSampler.Timer = 0;
DataVSpeedSampler.Timer1 = 0;
DataVSpeedSampler.Backup = DataVSpeedActual;
}
}
}
else
{
DataVSpeedValid = 0;
DataVSpeedActual = 0;
DataVSpeedSampler.Timer = 0;
DataVSpeedSampler.Timer1 = 0;
DataVSpeedSampler.Cnt = 0;
DataVSpeedSampler.Backup = 0;
}
// 第2步：车速的阻尼处理
VSpeed = DataVSpeedActual;
if ( VSpeed > DataVSPeedDamping.Speed ) // 实际值比当前显示值大时
{
Delta = ( uint16_t )VSpeed - DataVSPeedDamping.Speed;
if ( DataVSPeedDamping.Dir == DATA_APPR_DIR_INC ) // 显示值正在向实际值增加,则维持当前方向调节增加速度
{
if ( DataVSPeedDamping.Delta < Delta ) // 当前增量小于显示值与实际值的差值,则提升增加的速度
{
DataVSPeedDamping.Delta += DATA_VSPEED_INC_STEP;
if ( DataVSPeedDamping.Delta > Delta )
DataVSPeedDamping.Delta = Delta;
}
else // 当前增量大于等于显示值与实际值的差值,保持增量与当前差值同步,增加的速度即逐步减小
DataVSPeedDamping.Delta = Delta;
Delta = DataVSPeedDamping.Delta;
Delta /= DATA_VSPEED_DAMPING_FACTOR;
if ( Delta < DATA_VSPEED_APPR_SPEED_MIN )
Delta = DATA_VSPEED_APPR_SPEED_MIN;
DataVSPeedDamping.Speed += Delta;
if ( DataVSPeedDamping.Speed > VSpeed )
DataVSPeedDamping.Speed = ( uint16_t )VSpeed;
}
else // 显示值正在减小,则尽快减速至速度为最小时更换方向
{
if ( DataVSPeedDamping.Delta > DATA_VSPEED_DEC_STEP )
DataVSPeedDamping.Delta -= DATA_VSPEED_DEC_STEP;
else
DataVSPeedDamping.Delta = 0;
Delta = DataVSPeedDamping.Delta;
Delta /= DATA_VSPEED_DAMPING_FACTOR;
if ( Delta < DATA_VSPEED_APPR_SPEED_MIN ) // 已减速至最小速度
DataVSPeedDamping.Dir = DATA_APPR_DIR_INC; // 更换方向
else
{
if ( DataVSPeedDamping.Speed > Delta )
DataVSPeedDamping.Speed -= Delta;
else
DataVSPeedDamping.Speed = 0;
}
}
}
else if ( VSpeed < DataVSPeedDamping.Speed ) // 实际值比当前显示值小时
{
Delta = DataVSPeedDamping.Speed - ( uint16_t )VSpeed;
if ( DataVSPeedDamping.Dir == DATA_APPR_DIR_INC ) // 显示值仍在增加,则尽快减速至速度为最小时更换方向
{
if ( DataVSPeedDamping.Delta > DATA_VSPEED_DEC_STEP )
DataVSPeedDamping.Delta -= DATA_VSPEED_DEC_STEP;
else
DataVSPeedDamping.Delta = 0;
Delta = DataVSPeedDamping.Delta;
Delta /= DATA_VSPEED_DAMPING_FACTOR;
if ( Delta < DATA_VSPEED_APPR_SPEED_MIN ) // 已减速至最小速度
DataVSPeedDamping.Dir = DATA_APPR_DIR_DEC; // 更换方向
else
{
DataVSPeedDamping.Speed += Delta;
if ( DataVSPeedDamping.Speed > 1990 )
DataVSPeedDamping.Speed = 1990;
}
}
else // 显示值正在向实际值减小,则维持当前方向调节增加速度
{
if ( DataVSPeedDamping.Delta < Delta ) // 当前(负)增量小于显示值与实际值的差值,则提升减小的速度
{
DataVSPeedDamping.Delta += DATA_VSPEED_INC_STEP;
if ( DataVSPeedDamping.Delta > Delta )
DataVSPeedDamping.Delta = Delta;
}
else // 当前(负)增量大于等于显示值与实际值的差值,保持(负)增量与当前差值同步,减小的速度即逐步减小
DataVSPeedDamping.Delta = Delta;
Delta = DataVSPeedDamping.Delta;
Delta /= DATA_VSPEED_DAMPING_FACTOR;
if ( Delta < DATA_VSPEED_APPR_SPEED_MIN )
Delta = DATA_VSPEED_APPR_SPEED_MIN;
if ( DataVSPeedDamping.Speed < VSpeed + Delta )
DataVSPeedDamping.Speed = ( uint16_t )VSpeed;
else
DataVSPeedDamping.Speed -= Delta;
}
}
else // 实际值与当前显示值相等时
{
Delta = DataVSPeedDamping.Delta;
Delta /= DATA_VSPEED_DAMPING_FACTOR;
if ( Delta > DATA_VSPEED_APPR_SPEED_MIN ) // 当前的速度不是最小,说明数值正在增加/减小中,则继续原过程
{
if ( DataVSPeedDamping.Delta > DATA_VSPEED_DEC_STEP ) // 显示值越过了实际值,必然要先减速至最小速度,再改变方向返回实际值
DataVSPeedDamping.Delta -= DATA_VSPEED_DEC_STEP;
else
DataVSPeedDamping.Delta = 0;
Delta = DataVSPeedDamping.Delta;
Delta /= DATA_VSPEED_DAMPING_FACTOR;
if ( DataVSPeedDamping.Dir == DATA_APPR_DIR_INC ) // 显示值当前是增加方向
{
if ( Delta < DATA_VSPEED_APPR_SPEED_MIN ) // 已减速至最小速度
DataVSPeedDamping.Dir = DATA_APPR_DIR_DEC; // 更换方向
else
{
DataVSPeedDamping.Speed += Delta;
if ( DataVSPeedDamping.Speed > 1990 )
DataVSPeedDamping.Speed = 1990;
}
}
else // 显示值当前是减小方向
{
if ( Delta < DATA_VSPEED_APPR_SPEED_MIN ) // 已减速至最小速度
DataVSPeedDamping.Dir = DATA_APPR_DIR_INC; // 更换方向
else
{
if ( DataVSPeedDamping.Speed > Delta )
DataVSPeedDamping.Speed -= Delta;
else
DataVSPeedDamping.Speed = 0;
}
}
}
}
// 第3步：生成显示车速
if ( (DataVSPeedDamping.Speed >= DataVSpeedHysteresis) || (DataVSPeedDamping.Speed < DATA_VSPEED_HYSTERESIS) )
{
DataVSpeedHysteresis = DataVSPeedDamping.Speed;
}
else
{
if ( DataVSpeedHysteresis - DataVSPeedDamping.Speed >= DATA_VSPEED_HYSTERESIS )
{
DataVSpeedHysteresis = DataVSPeedDamping.Speed;
}
}
VSpeedCal = ( uint32_t )DataVSpeedHysteresis;
if ( DataVSpeedValid )
{
VSpeedCal *= 105;
VSpeedCal /= 100;
if((VSpeedCal % 10) >= 5)
{
VSpeedCal += 5; /* 四舍五入 */
}
DataVSpeedDisp = ( uint16_t )VSpeedCal;
}
else
{
DataVSpeedDisp = 0;
}
}
/*车速有效位：有效=1，无效=0*/
uint8_t Get_VechileSpeedValid(void)
{
return DataVSpeedValid;
}
/*真实的车速，精度10倍,*/
uint16_t Get_ActualVechileSpeed(void)
{
return DataVSpeedActual;
}
/*显示车速，精度10倍，取整，舍去小数点*/
uint16_t Get_DispVechileSpeed(void)
{
uint16_t Vspeed = 0;
if(Get_Dis_Unit() == 0) /* KM */
{
if ( DataVSpeedDisp >= 1990 )
{
DataVSpeedDisp = 1990;
}
Vspeed = DataVSpeedDisp;
}
else //if (Get_Dis_Unit() == 1) /* MILE */
{
Vspeed = Data_Km_To_Mile(DataVSpeedDisp);
if ( Vspeed >= 1230 )
{
Vspeed = 1230;
}
}
// else
// {
// ;
// }
return Vspeed;
}