2 * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION.
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
8 * http://www.apache.org/licenses/LICENSE-2.0
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
17 * @file LineSensDrv_Snesor.cpp
20 /*---------------------------------------------------------------------------*/
23 #include "LineSensDrv_Sensor.h"
24 #include "LineSensDrv_Api.h"
26 /*---------------------------------------------------------------------------*/
29 #define LSDRV_MASK_WORD_L 0x00FF
30 #define LSDRV_MASK_WORD_U 0xFF00
32 #define LINE_SENS_DRV_SENSOR_DEBUG_FACTORY 0
33 #define LINE_SENS_DRV_SENSOR_DEBUG_DIAG 0
35 #define VEHICLE_SNS_INFO_PULSE_NUM 32
37 /*---------------------------------------------------------------------------*/
40 static LSDRV_SPEEDKMPH_DAT g_speed_kmph_data;
41 static uint8_t g_rcv_data_len; /* Receive SYS data length */
42 extern uint8_t g_uc_vehicle_reverse;
43 /*---------------------------------------------------------------------------*/
46 /*******************************************************************************
47 * MODULE : LineSensDrv_Sensor
48 * ABSTRACT : Sensor data reception processing
49 * FUNCTION : Convert sensor data to delivery format
50 * ARGUMENT : *ucRcvdata : Data pointer
53 ******************************************************************************/
54 void LineSensDrvSensor(u_int8* uc_rcvdata) {
55 u_int8 uc_sens_cnt = 0;
56 u_int16 us_sp_kmph = 0; /* Vehicle speed(km/h) */
57 u_int16 us_sp_pls1 = 0; /* Total vehicle speed pulse(Latest) */
58 u_int16 us_sp_pls2 = 0; /* Total vehicle speed pulse(Last time) */
59 u_int8 us_sp_ret = 0; /* Last vehicle speed information acquisition result */
60 u_int8 uc_size = 0; /* Size of the data */
61 u_int16 us_cnt = 0; /* Data counter */
62 u_int16 us_cnt2 = 0; /* Data counter */
63 u_int8* uc_data_pos = NULL; /* Data storage location */
64 LSDRV_MSG_LSDATA_G* p_snd_buf = NULL;
65 LSDRV_LSDATA_G* p_snd_data_base = NULL;
66 LSDRV_LSDATA_G* p_snd_data = NULL;
67 RET_API ret = RET_NORMAL;
69 /* Receive sensor data top address acquisition */
70 uc_data_pos = (uc_rcvdata);
72 /* Create send buffer/delivery all received data */
73 ret = _pb_GetZcSndBuf(PNO_VEHICLE_SENSOR, reinterpret_cast<void**>(&p_snd_buf));
74 if ((ret == RET_NORMAL) && (p_snd_buf != NULL)) {
75 p_snd_data_base = p_snd_buf->st_para.st_data;
76 (void)memset(p_snd_data_base, 0, LSDRV_KINDS_MAX * sizeof( LSDRV_LSDATA_G ));
79 p_snd_data = p_snd_data_base + LSDRV_SNS_COUNTER;
80 uc_sens_cnt = (u_int8)*(uc_data_pos);
81 p_snd_data->ul_did = VEHICLE_DID_SNS_COUNTER; /* Data ID */
82 p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_1; /* Size of the data */
83 p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */
84 p_snd_data->uc_data[0] = uc_sens_cnt; /* Data content */
85 uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_1);
88 p_snd_data = p_snd_data_base + LSDRV_GYRO_EXT;
89 p_snd_data->ul_did = VEHICLE_DID_GYRO_EXT; /* Data ID */
90 p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */
91 p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */
92 for (us_cnt = 0; us_cnt < LSDRV_SNDMSG_DTSIZE_20; us_cnt++) {
93 /* Since [0] is older and [9] is newer, the order of received data is switched. */
94 /* Be in reverse order for endian conversion */
95 p_snd_data->uc_data[LSDRV_SNDMSG_DTSIZE_20 - (us_cnt + 1)] = (u_int8)*(uc_data_pos + us_cnt);
97 uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20);
99 p_snd_data = p_snd_data_base + LSDRV_GYRO_X;
100 p_snd_data->ul_did = VEHICLE_DID_GYRO; /* Data ID */
101 p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */
102 p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */
103 for (us_cnt = 0; us_cnt < LSDRV_SNDMSG_DTSIZE_20; us_cnt++) {
104 /* Since [0] is older and [9] is newer, the order of received data is switched. */
105 /* Be in reverse order for endian conversion */
106 p_snd_data->uc_data[LSDRV_SNDMSG_DTSIZE_20 - (us_cnt + 1)] = (u_int8)*(uc_data_pos + us_cnt);
108 uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20);
111 p_snd_data = p_snd_data_base + LSDRV_REV;
112 p_snd_data->ul_did = VEHICLE_DID_REV; /* Data ID */
113 p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_1; /* Size of the data */
114 p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor Counter */
115 p_snd_data->uc_data[0] = g_uc_vehicle_reverse;
116 /* Gyro Temperature */
117 p_snd_data = p_snd_data_base + LSDRV_GYRO_TEMP;
118 p_snd_data->ul_did = VEHICLE_DID_GYRO_TEMP; /* Data ID */
119 p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_2; /* Size of the data */
120 p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */
121 p_snd_data->uc_data[1] = (u_int8)(*(uc_data_pos ) & (u_int8)(LSDRV_TEMP_MASK >> 8));
122 p_snd_data->uc_data[0] = (u_int8)(*(uc_data_pos + 1) & (u_int8)(LSDRV_TEMP_MASK));
123 uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_2);
125 /* Vehicle speed pulse */
126 p_snd_data = p_snd_data_base + LSDRV_SPEED_PULSE;
127 p_snd_data->ul_did = VEHICLE_DID_SPEED_PULSE; /* Data ID */
128 p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */
129 p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */
130 for (us_cnt = 0; us_cnt < LSDRV_SNDMSG_DTSIZE_20; us_cnt++) {
131 /* Since [0] is older and [9] is newer, the order of received data is switched. */
132 /* Be in reverse order for endian conversion */
133 p_snd_data->uc_data[LSDRV_SNDMSG_DTSIZE_20 - (us_cnt + 1)] = (u_int8)*(uc_data_pos + us_cnt);
135 uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20);
137 /* Vehicle speed(km/h) */
138 p_snd_data = p_snd_data_base + LSDRV_SPEED_PULSE;
140 us_sp_pls1 = (u_int16)p_snd_data->uc_data[1];
141 us_sp_pls1 = (u_int16)((us_sp_pls1 << 8) | p_snd_data->uc_data[0]);
142 us_sp_ret = LineSensDrvGetLastSpeedPulse(&us_sp_pls2, us_sp_pls1, uc_sens_cnt);
144 LineSensDrvSetLastSpeedPulse(us_sp_pls1, uc_sens_cnt);
146 p_snd_data = p_snd_data_base + LSDRV_SPEED_KMPH;
147 if (us_sp_ret != LSDRV_SPKMPH_INVALID) {
148 /* Vehicle speed pulse before 100 ms is valid */
149 LineSensDrvSpeedPulseSave(us_sp_pls1, us_sp_pls2, uc_sens_cnt);
150 us_sp_kmph = LineSensDrvSpeedCalc(uc_sens_cnt);
151 /* The size can be set only when the vehicle speed [km/h] is calculated. "0" is notified to the vehicle sensor when the size cannot be set. */
152 p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_2; /* Size of the data */
154 p_snd_data->ul_did = VEHICLE_DID_SPEED_KMPH; /* Data ID */
155 p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */
156 p_snd_data->uc_data[0] = (u_int8)(us_sp_kmph & 0x00FF);
157 p_snd_data->uc_data[1] = (u_int8)(us_sp_kmph >> 8);
159 POSITIONING_LOG("[LOG, %d(cnt), %d(km/h), %d(pls1), %d(pls2)] \r\n",
165 /* G-Sensor X-axes */
166 p_snd_data = p_snd_data_base + LSDRV_GSENSOR_X;
167 p_snd_data->ul_did = VEHICLE_DID_GSNS_X; /* Data ID */
168 p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */
169 p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */
170 us_cnt2 = p_snd_data->uc_size - 1;
171 for (us_cnt = 0; us_cnt < 10; us_cnt++) {
172 /* Since [0] is older and [9] is newer, the order of received data is switched. */
173 p_snd_data->uc_data[us_cnt2 ] = (u_int8)*( uc_data_pos + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt) );
174 p_snd_data->uc_data[us_cnt2 - 1] = (u_int8)*( uc_data_pos + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt) + 1);
175 us_cnt2 = us_cnt2 - 2;
178 /* G-Sensor Y-axes */
179 p_snd_data = p_snd_data_base + LSDRV_GSENSOR_Y;
180 p_snd_data->ul_did = VEHICLE_DID_GSNS_Y; /* Data ID */
181 p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */
182 p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */
183 us_cnt2 = p_snd_data->uc_size - 1;
184 for (us_cnt = 0; us_cnt < 10; us_cnt++) {
185 /* Since [0] is older and [9] is newer, the order of received data is switched. */
186 p_snd_data->uc_data[us_cnt2] = (u_int8)*( (uc_data_pos + sizeof(u_int16)) \
187 + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt));
189 p_snd_data->uc_data[us_cnt2-1] = (u_int8)*( (uc_data_pos + sizeof(u_int16)) \
190 + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt) + 1);
191 us_cnt2 = us_cnt2 - 2;
193 uc_data_pos = ( uc_data_pos + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * 10) );
195 /* Inter-Pulse time */
196 p_snd_data = p_snd_data_base + LSDRV_PULSE_TIME;
197 p_snd_data->ul_did = VEHICLE_DID_PULSE_TIME; /* Data ID */
198 p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_132; /* Size of the data */
199 p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */
201 /* Clear the buffer for variable length */
202 (void)memset(&p_snd_data->uc_data[0], 0x00, sizeof(p_snd_data->uc_data));
204 if (g_rcv_data_len == LSDRV_PLSTIME_LEN) {
205 /* Inter-Pulse time (number of items + time). The number of items is stored at the beginning. */
206 uc_size = (u_int8)*uc_data_pos;
207 if (uc_size > VEHICLE_SNS_INFO_PULSE_NUM) {
208 uc_size = VEHICLE_SNS_INFO_PULSE_NUM;
210 p_snd_data->uc_data[0] = uc_size;
211 p_snd_data->uc_data[1] = 0x00;
212 p_snd_data->uc_data[2] = 0x00;
213 p_snd_data->uc_data[3] = 0x00;
214 uc_data_pos = ( uc_data_pos + sizeof(u_int8) );
216 /* Since [0] is old and [31] is new in the received time, the order is changed. */
217 /* Be in reverse order for endian conversion */
218 for (us_cnt = 0; us_cnt < (uc_size * sizeof(u_int32)); us_cnt++) {
219 p_snd_data->uc_data[(uc_size * sizeof(u_int32)) - (us_cnt + 1) + 4] = (u_int8)*(uc_data_pos + us_cnt);
224 DeliveryLineSensorDataPositioning(p_snd_buf, LSDRV_KINDS_MAX);
230 /*******************************************************************************
231 * MODULE : LineSensDrv_SpeedCalc
232 * ABSTRACT : Vehicle speed calculation processing for sensor data
233 * FUNCTION : Calculate vehicle speed for sensor data
234 * ARGUMENT : uc_sens_cnt : Sensor counter
236 * RETURN : Vehicle speed(0.01km/h)
237 ******************************************************************************/
238 u_int16 LineSensDrvSpeedCalc(u_int8 uc_sens_cnt) {
239 u_int32 ul_sp_caluc = 0; /* Vehicle speed(2^-8km/h) */
240 u_int16 us_speed = 0; /* Vehicle speed(km/h) */
241 u_int8 uc_ptr = 0; /* Data storage pointer for sensor data */ /* #010 */
242 u_int8 uc_ptr2 = 0; /* Data storage pointer for sensor data */ /* #010 */
243 u_int32 ul_work = 0; /* Variables for unsigned 32-bit type calculations */ /* #010 */
244 double d_work = 0.0; /* Variables for calculating double types */ /* #010 */ /* Ignore -> MISRA-C++:2008 Rule 3-9-2 */
245 u_int16 us_sens_cnt_search = 0; /* Sensor counter to be searched */ /* #010 */
246 u_int16 us_sens_cnt_ref = 0; /* Sensor counters to be compared */ /* #010 */
247 int32 i = 0; /* Generic counters */ /* #010 */
248 int32 j = 0; /* Generic counters */ /* #010 */
249 u_int16 us_offset = 0; /* Offset value */ /* #010 */
252 /* Is the number of data that can be calculated at the vehicle speed already received? */
253 if (LSDRV_PLSDATA_NRCV == g_speed_kmph_data.uc_calc_start) {
254 /* Do not compute if there is not enough data. */
256 if (0 == g_speed_kmph_data.uc_ptr) {
257 uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1;
258 uc_ptr2 = LSDRV_SPKMPH_TBL_NUM - 1;
260 uc_ptr = g_speed_kmph_data.uc_ptr - 1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
261 uc_ptr2 = g_speed_kmph_data.uc_ptr - 1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
265 if (LSDRV_SPKMPH_AVE_TIME > uc_sens_cnt) {
266 us_offset = LSDRV_SENSCNT_BRW_ADD;
271 us_sens_cnt_search = (u_int16)uc_sens_cnt + us_offset - LSDRV_SPKMPH_AVE_TIME;
273 for (i = 0; i < LSDRV_SPKMPH_TBL_NUM; i++) {
274 /* Invalid data is detected, and the search is completed. */
275 if (LSDRV_SPKMPH_DATA_DIS == g_speed_kmph_data.st_data[uc_ptr].uc_flag) {
279 /* When the sensor counter is 29 or less, the borrow is considered. */
280 if (LSDRV_SPKMPH_AVE_TIME > g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt) {
281 us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt + us_offset;
283 us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt;
286 /* Checking the sensor counter to finish search */
287 if (us_sens_cnt_search >= us_sens_cnt_ref) {
291 /* Add to calculate average value */
292 ul_work += (u_int32)g_speed_kmph_data.st_data[uc_ptr].us_speed_pulse;
295 uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1; /* To the end of the data table */
297 uc_ptr--; /* To the previous data */
301 /* Averaging computation */
306 for (j = 0; j < LSDRV_SPKMPH_TBL_NUM; j++) {
307 if ((g_speed_kmph_data.st_data[uc_ptr2].us_speed_pulse == 1)
308 && (g_speed_kmph_data.st_data[uc_ptr2].uc_noise_flag == 1)) {
311 } else if (g_speed_kmph_data.st_data[uc_ptr2].us_speed_pulse == 1 &&
312 g_speed_kmph_data.st_data[uc_ptr2].uc_noise_flag == 0) {
319 /* Borrow actions for pointer values */
321 uc_ptr2 = LSDRV_SPKMPH_TBL_NUM - 1; /* To the end of the data table */
323 uc_ptr2--; /* To the previous data */
328 d_work = static_cast<double>(ul_work);
329 d_work = d_work / static_cast<double>(i);
330 d_work = d_work * static_cast<double>(LSDRV_SENS_COEFFICIENT);
331 d_work = d_work * 100; /* [1km/h] -> [0.01km/h] */
332 d_work = d_work + 0.5; /* Preparation for rounding */
335 ul_sp_caluc = static_cast<int32>(d_work);
337 /* When the vehicle speed calculation result overflows, the upper limit value is used for clipping. */
338 if (LSDRV_PLSMAX - 1 >= ul_sp_caluc) {
339 us_speed = (u_int16)ul_sp_caluc;
341 us_speed = (u_int16)(LSDRV_PLSMAX - 1);
348 /*******************************************************************************
349 * MODULE : LineSensDrv_SpeedKmphDataInit
350 * ABSTRACT : Data table initialization process for vehicle speed calculation
351 * FUNCTION : Initialize the data table for calculating the vehicle speed
355 ******************************************************************************/
356 void LineSensDrvSpeedKmphDataInit(void) {
359 memset(reinterpret_cast<void*>(&g_speed_kmph_data), static_cast<int32>(0), (size_t)sizeof(g_speed_kmph_data));
361 /* Disable all data storage flags */
362 for (i = 0; i < LSDRV_SPKMPH_TBL_NUM; i++) {
363 g_speed_kmph_data.st_data[i].uc_flag = LSDRV_SPKMPH_DATA_DIS;
369 /*******************************************************************************
370 * MODULE : LineSensDrv_SpeedPulseSave
371 * ABSTRACT : Sensor data vehicle speed pulse save processing
372 * FUNCTION : Saving the vehicle speed pulse of the sensor data to the data table for calculating the vehicle speed
373 * ARGUMENT : us_sp1 : Vehicle speed pulse of the latest information
374 * : us_sp2 : Vehicle speed pulse of the previous information
375 * : uc_sens_cnt : Sensor counter
378 ******************************************************************************/
379 void LineSensDrvSpeedPulseSave(u_int16 us_sp1, u_int16 us_sp2, u_int8 uc_sens_cnt) {
380 u_int16 us_sp_diff = 0; /* Vehicle speed pulse difference */
381 u_int8 uc_ptr = 0; /* Data storage pointer for sensor data */ /* #010 */
382 u_int8 us_last_ptr = 0; /* Last pointer */ /* #010 */
383 int32 i = 0; /* Generic counters */ /* #010 */
384 u_int8 uc_fstsns_cnt = 0; /* Initial sensor data sensor counter value */ /* #016 */
385 int32 l_data_num = 0; /* Number of registered data */ /* #016 */
386 u_int8 uc_noise_flag = 0; /* Noise flag */ /* #017 */
388 /* Calculate the vehicle speed pulse difference between the latest and last 100ms information */
389 if (us_sp1 >= us_sp2) {
390 /* The cumulative pulse number of the latest information is larger. */
391 us_sp_diff = us_sp1 - us_sp2; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
393 /* The cumulative pulse number of the latest information is smaller (the accumulated pulse overflows) */
394 us_sp_diff = (LSDRV_PLSMAX - us_sp2) + us_sp1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
397 /* Call noise check only if us_sp_diff is 1 */
398 if (us_sp_diff == 1) {
399 uc_noise_flag = LineSensDrvCheckNoise(uc_sens_cnt);
402 /* Saving sensor data vehicle speed pulse in data table for vehicle speed calculation */
403 if (LSDRV_PLSDATA_NRCV == g_speed_kmph_data.uc_sns_rcv) {
404 if (LSDRV_PLSDATA_RCV == g_speed_kmph_data.uc_fstsns_rcv) {
405 /* If the sensor data has already been received for the first time, set the temporary sensor counter value when the sensor data has been saved for the first time. */
406 uc_fstsns_cnt = uc_sens_cnt;
407 for (i = (LSDRV_SPKMPH_TBL_NUM - 1); i >= 0 ; i--) {
408 if (LSDRV_SPKMPH_DATA_EN == g_speed_kmph_data.st_data[i].uc_flag) {
409 /* Data storage flag is valid */
410 if (0 != uc_fstsns_cnt) {
413 uc_fstsns_cnt = LSDRV_SENSCNT_MAX;
416 g_speed_kmph_data.st_data[i].uc_sens_cnt = uc_fstsns_cnt;
421 /* Sensor data reception status <- "Received" */
422 g_speed_kmph_data.uc_sns_rcv = LSDRV_PLSDATA_RCV;
425 uc_ptr = g_speed_kmph_data.uc_ptr;
427 /* If the sensor counter is the same as the previous one, overwrite update */
429 us_last_ptr = LSDRV_SPKMPH_TBL_NUM - 1;
431 us_last_ptr = uc_ptr - 1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
434 if (g_speed_kmph_data.st_data[us_last_ptr].uc_sens_cnt == uc_sens_cnt) {
435 /* Next update of the data storage location */
436 if (0 == g_speed_kmph_data.uc_ptr) {
437 g_speed_kmph_data.uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1;
439 g_speed_kmph_data.uc_ptr--;
442 uc_ptr = g_speed_kmph_data.uc_ptr;
445 /* Input into data table for calculation of vehicle speed */
446 g_speed_kmph_data.st_data[uc_ptr].uc_flag = LSDRV_SPKMPH_DATA_EN; /* Data validity */
447 g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt = uc_sens_cnt; /* Sensor counter input */
448 g_speed_kmph_data.st_data[uc_ptr].us_speed_pulse = us_sp_diff; /* Vehicle speed pulse difference */
449 g_speed_kmph_data.st_data[uc_ptr].uc_noise_flag = uc_noise_flag; /* Noise flag */ /* #017 */
451 /* Next update of the data storage location */
452 if ((LSDRV_SPKMPH_TBL_NUM - 1) <= g_speed_kmph_data.uc_ptr) {
453 g_speed_kmph_data.uc_ptr = 0;
455 g_speed_kmph_data.uc_ptr++;
458 /* Determine whether the vehicle speed can be calculated. */
459 if (g_speed_kmph_data.uc_calc_start == LSDRV_PLSDATA_NRCV) {
460 /* Data count detection */
462 for (i = 0; i < LSDRV_SPKMPH_TBL_NUM; i++) {
463 if (LSDRV_SPKMPH_DATA_EN == g_speed_kmph_data.st_data[i].uc_flag) {
468 if (LSDRV_SPKMPH_MIN_DATA_N <= l_data_num) {
469 /* Vehicle Speed Calculation Required Data Number Received */
470 g_speed_kmph_data.uc_calc_start = LSDRV_PLSDATA_RCV;
477 /*******************************************************************************
478 * MODULE : LineSensDrv_CheckNoise
479 * ABSTRACT : Sensor data noise check processing
480 * FUNCTION : The vehicle speed pulse is saved in the data table for the vehicle speed calculation.
481 * ARGUMENT : uc_sens_cnt : Sensor counter of the latest data
483 * RETURN : Noise flag
484 ******************************************************************************/
485 u_int8 LineSensDrvCheckNoise(u_int8 uc_sens_cnt) {
486 int32 i = 0; /* Generic counters */
487 u_int16 us_sens_cnt_search = 0; /* Sensor counter to be searched*/
488 u_int8 uc_ptr = 0; /* Data storage pointer */
489 u_int16 us_offset = 0; /* Offset value */
490 u_int8 noise_flag = 0; /* Noise flag */
491 u_int16 us_sens_cnt_ref = 0; /* Sensor counters to be compared */
493 /* If there is no point where the difference in vehicle speed pulse is 1 or more
494 between -1 and -14 of sensor counter of the latest data,
495 set the noise flag of the latest data to 1. */
496 /* Set the noise flag to 1 */
499 /* The Target is the one before the storage location of the latest data. */
500 if (0 == g_speed_kmph_data.uc_ptr) {
501 uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1;
503 uc_ptr = g_speed_kmph_data.uc_ptr - 1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
506 if (LSDRV_SPKMPH_NOISE_TIME > uc_sens_cnt) {
507 us_offset = LSDRV_SENSCNT_BRW_ADD;
512 us_sens_cnt_search = (u_int16)uc_sens_cnt + us_offset - LSDRV_SPKMPH_NOISE_TIME;
514 for (i = 0; i < LSDRV_SPKMPH_NOISE_TIME; i++) {
515 /* When the sensor counter is 15 or less, the borrow is considered. */
516 if (LSDRV_SPKMPH_NOISE_TIME > g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt) {
517 us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt + us_offset;
519 us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
522 /* Checking the sensor Counter to Finish Search */
523 if (us_sens_cnt_ref <= us_sens_cnt_search) {
527 if (g_speed_kmph_data.st_data[uc_ptr].us_speed_pulse >= 1) {
533 /* Borrow actions for pointer values */
535 uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1; /* To the end of the data table */
537 uc_ptr--; /* To the previous data */
546 * Return the last (100ms ago) vehicle speed pulse
548 * @param[out] us_sp2 Last vehicle speed pulse
549 * @param[in] us_sp1 The latest vehicle speed pulse
550 * @param[in] uc_sens_cnt Latest sensor counter
552 * @return LSDRV_SPKMPH_INVALID Vehicle speed pulse information valid<br>
553 * LSDRV_SPKMPH_VALID Vehicle speed pulse information invalid
555 u_int8 LineSensDrvGetLastSpeedPulse(u_int16* us_sp2, u_int16 us_sp1, u_int8 uc_sens_cnt) {
556 u_int8 ret = LSDRV_SPKMPH_INVALID; /* Return value */
557 u_int16 sp_pls_diff = 0; /* Vehicle speed pulse difference */
558 u_int16 sp_pls = 0; /* Vehicle speed pulse every 100 ms */
559 u_int8 cnt_diff = 0; /* Sensor counter difference */
561 /* Check if the last vehicle speed pulse has been set */
562 if (g_speed_kmph_data.st_last_data.uc_flag == LSDRV_SPKMPH_DATA_EN) {
563 /* Differential calculation of sensor counter */
564 if (uc_sens_cnt >= g_speed_kmph_data.st_last_data.uc_sens_cnt) {
565 /* Latest sensor counter is larger */
566 cnt_diff = uc_sens_cnt - g_speed_kmph_data.st_last_data.uc_sens_cnt;
568 /* Last sensor counter is larger(sensor counter overflows) */
569 cnt_diff = (LSDRV_SENSCNT_MAX - g_speed_kmph_data.st_last_data.uc_sens_cnt) + uc_sens_cnt + 1;
572 /* Check if sensor counter is continuous */
574 /* Continuous or same as the previous one, so the previous (100ms previous) vehicle speed pulse is set as it is */
575 *us_sp2 = g_speed_kmph_data.st_last_data.us_speed_pulse;
577 /* Determine the vehicle speed pulse 100ms ago from the average considering the skipped portion because it is not consecutive. */
578 if (us_sp1 >= g_speed_kmph_data.st_last_data.us_speed_pulse) {
579 /* Larger latest cumulative vehicle speed pulse */
580 sp_pls_diff = us_sp1 - g_speed_kmph_data.st_last_data.us_speed_pulse;
582 /* Last cumulative vehicle speed pulse is larger(Cumulative vehicle speed pulse overflows) */
583 sp_pls_diff = (LSDRV_PLSMAX - g_speed_kmph_data.st_last_data.us_speed_pulse) + us_sp1;
586 /* Calculate average vehicle speed pulse including skip period */
587 sp_pls = (u_int16)(sp_pls_diff / cnt_diff);
589 /* Calculate the vehicle speed pulse 100ms ahead from the vehicle speed average */
590 if (us_sp1 >= sp_pls) {
591 /* Does not overflow even if the 100ms vehicle speed pulse is pulled from the latest one. */
592 *us_sp2 = us_sp1 - sp_pls;
594 /* Subtracting a 100ms vehicle speed pulse from the latest one overflows */
595 *us_sp2 = LSDRV_PLSMAX - (sp_pls - us_sp1);
599 ret = LSDRV_SPKMPH_VALID;
607 * Return the last (100ms ago) vehicle speed pulse
609 * @param[in] us_sp Vehicle speed pulse
610 * @param[in] uc_sens_cnt Sensor counter
612 void LineSensDrvSetLastSpeedPulse(u_int16 us_sp, u_int8 uc_sens_cnt) {
613 /* Vehicle speed pulse information valid setting */
614 g_speed_kmph_data.st_last_data.uc_flag = LSDRV_SPKMPH_DATA_EN;
615 /* Sensor counter setting */
616 g_speed_kmph_data.st_last_data.uc_sens_cnt = uc_sens_cnt;
617 /* Vehicle speed pulse setting */
618 g_speed_kmph_data.st_last_data.us_speed_pulse = us_sp;
621 /*---------------------------------------------------------------------------*/