hal/positioning_hal/src/LineSensDrv/LineSensDrv_Sensor.cpp

   1 /*
   2  * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION.
   3  *
   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
   7  *
   8  *      http://www.apache.org/licenses/LICENSE-2.0
   9  *
  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.
  15  */
  16 /**
  17 * @file LineSensDrv_Snesor.cpp
  18 */
  19
  20 /*---------------------------------------------------------------------------*/
  21 // Include files
  22
  23 #include "LineSensDrv_Sensor.h"
  24 #include "LineSensDrv_Api.h"
  25
  26 /*---------------------------------------------------------------------------*/
  27 // Value Define
  28
  29 #define LSDRV_MASK_WORD_L                   0x00FF
  30 #define LSDRV_MASK_WORD_U                   0xFF00
  31
  32 #define LINE_SENS_DRV_SENSOR_DEBUG_FACTORY  0
  33 #define LINE_SENS_DRV_SENSOR_DEBUG_DIAG     0
  34
  35 #define VEHICLE_SNS_INFO_PULSE_NUM          32
  36
  37 /*---------------------------------------------------------------------------*/
  38 // Global variable
  39
  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 /*---------------------------------------------------------------------------*/
  44 // Functions
  45
  46 /*******************************************************************************
  47  * MODULE    : LineSensDrv_Sensor
  48  * ABSTRACT  : Sensor data reception processing
  49  * FUNCTION  : Convert sensor data to delivery format
  50  * ARGUMENT  : *ucRcvdata : Data pointer
  51  * NOTE      :
  52  * RETURN    : None
  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;
  68
  69   /* Receive sensor data top address acquisition */
  70   uc_data_pos = (uc_rcvdata);
  71
  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 ));
  77
  78     /* Sensor counter */
  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);
  86
  87     /* Gyro output */
  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);
  96     }
  97     uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20);
  98
  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);
 107     }
 108     uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20);
 109
 110     /* Reverse flag */
 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);
 124
 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);
 134     }
 135     uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20);
 136
 137     /* Vehicle speed(km/h) */
 138     p_snd_data              = p_snd_data_base + LSDRV_SPEED_PULSE;
 139     us_sp_kmph              = 0;
 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);
 143
 144     LineSensDrvSetLastSpeedPulse(us_sp_pls1, uc_sens_cnt);
 145
 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   */
 153     }
 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);
 158
 159     POSITIONING_LOG("[LOG, %d(cnt), %d(km/h), %d(pls1), %d(pls2)]  \r\n",
 160       uc_sens_cnt,
 161       us_sp_kmph,
 162       us_sp_pls1,
 163       us_sp_pls2);
 164
 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;
 176     }
 177
 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));
 188
 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;
 192     }
 193     uc_data_pos = ( uc_data_pos + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * 10) );
 194
 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 */
 200
 201     /* Clear the buffer for variable length */
 202     (void)memset(&p_snd_data->uc_data[0], 0x00, sizeof(p_snd_data->uc_data));
 203
 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;
 209       }
 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) );
 215
 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);
 220       }
 221     }
 222
 223     /* Messaging */
 224     DeliveryLineSensorDataPositioning(p_snd_buf, LSDRV_KINDS_MAX);
 225   }
 226
 227   return;
 228 }
 229
 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
 235  * NOTE      :
 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 */
 250
 251   /* #016 start */
 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. */
 255   } else {
 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;
 259     } else {
 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 */
 262     }
 263
 264     ul_work = 0;
 265     if (LSDRV_SPKMPH_AVE_TIME > uc_sens_cnt) {
 266       us_offset = LSDRV_SENSCNT_BRW_ADD;
 267     } else {
 268       us_offset = 0;
 269     }
 270
 271     us_sens_cnt_search = (u_int16)uc_sens_cnt + us_offset - LSDRV_SPKMPH_AVE_TIME;
 272
 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) {
 276         break;
 277         }
 278
 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;
 282       } else {
 283         us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt;
 284       }
 285
 286       /* Checking the sensor counter to finish search */
 287       if (us_sens_cnt_search >= us_sens_cnt_ref) {
 288         break;
 289       }
 290
 291       /* Add to calculate average value */
 292       ul_work += (u_int32)g_speed_kmph_data.st_data[uc_ptr].us_speed_pulse;
 293
 294       if (0 == uc_ptr) {
 295         uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1;                /* To the end of the data table */
 296       } else {
 297         uc_ptr--;                            /* To the previous data */
 298       }
 299     }
 300
 301     /* Averaging computation */
 302     if (0 == i) {
 303       d_work = 0;
 304     } else {
 305       if (ul_work == 1) {
 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)) {
 309             ul_work = 0;
 310             break;
 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) {
 313             ul_work = 1;
 314             break;
 315           } else {
 316             /* nop */
 317           }
 318
 319           /* Borrow actions for pointer values */
 320           if (0 == uc_ptr2) {
 321             uc_ptr2 = LSDRV_SPKMPH_TBL_NUM - 1;                /* To the end of the data table */
 322           } else {
 323             uc_ptr2--;                            /* To the previous data */
 324           }
 325         }
 326       }
 327
 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 */
 333     }
 334
 335     ul_sp_caluc = static_cast<int32>(d_work);
 336
 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;
 340     } else {
 341       us_speed = (u_int16)(LSDRV_PLSMAX - 1);
 342     }
 343   }
 344
 345   return us_speed;
 346 }
 347
 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
 352  * ARGUMENT  : None
 353  * NOTE      :
 354  * RETURN    : None
 355  ******************************************************************************/
 356 void LineSensDrvSpeedKmphDataInit(void) {
 357   int32 i = 0;
 358
 359   memset(reinterpret_cast<void*>(&g_speed_kmph_data), static_cast<int32>(0), (size_t)sizeof(g_speed_kmph_data));
 360
 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;
 364   }
 365
 366   return;
 367 }
 368
 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
 376  * NOTE      :
 377  * RETURN    : None
 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 */
 387
 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 */
 392   } else {
 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 */
 395   }
 396
 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);
 400   }
 401
 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) {
 411             uc_fstsns_cnt--;
 412           } else {
 413             uc_fstsns_cnt = LSDRV_SENSCNT_MAX;
 414           }
 415
 416           g_speed_kmph_data.st_data[i].uc_sens_cnt = uc_fstsns_cnt;
 417         }
 418       }
 419     }
 420
 421     /* Sensor data reception status  <-  "Received" */
 422     g_speed_kmph_data.uc_sns_rcv = LSDRV_PLSDATA_RCV;
 423   }
 424
 425   uc_ptr = g_speed_kmph_data.uc_ptr;
 426
 427   /* If the sensor counter is the same as the previous one, overwrite update */
 428   if (0 == uc_ptr) {
 429     us_last_ptr = LSDRV_SPKMPH_TBL_NUM - 1;
 430   } else {
 431     us_last_ptr = uc_ptr - 1;   /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
 432   }
 433
 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;
 438     } else {
 439       g_speed_kmph_data.uc_ptr--;
 440     }
 441
 442     uc_ptr = g_speed_kmph_data.uc_ptr;
 443   }
 444
 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 */
 450
 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;
 454   } else {
 455     g_speed_kmph_data.uc_ptr++;
 456   }
 457
 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 */
 461     l_data_num = 0;
 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) {
 464           l_data_num++;
 465       }
 466     }
 467
 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;
 471     }
 472   }
 473
 474   return;
 475 }
 476
 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
 482  * NOTE      :
 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 */
 492
 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 */
 497   noise_flag = 1;
 498
 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;
 502   } else {
 503     uc_ptr = g_speed_kmph_data.uc_ptr - 1;  /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
 504   }
 505
 506   if (LSDRV_SPKMPH_NOISE_TIME > uc_sens_cnt) {
 507     us_offset = LSDRV_SENSCNT_BRW_ADD;
 508   } else {
 509     us_offset = 0;
 510   }
 511
 512   us_sens_cnt_search = (u_int16)uc_sens_cnt + us_offset - LSDRV_SPKMPH_NOISE_TIME;
 513
 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;
 518     } else {
 519       us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt;  /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
 520     }
 521
 522     /* Checking the sensor Counter to Finish Search */
 523     if (us_sens_cnt_ref <= us_sens_cnt_search) {
 524       noise_flag = 1;
 525       break;
 526     } else {
 527       if (g_speed_kmph_data.st_data[uc_ptr].us_speed_pulse >= 1) {
 528         noise_flag = 0;
 529         break;
 530       }
 531     }
 532
 533     /* Borrow actions for pointer values */
 534     if (0 == uc_ptr) {
 535       uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1;                /* To the end of the data table */
 536     } else {
 537       uc_ptr--;                            /* To the previous data */
 538     }
 539   }
 540
 541   return noise_flag;
 542 }
 543
 544 /**
 545  * @brief
 546  *  Return the last (100ms ago) vehicle speed pulse
 547  *
 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
 551  *
 552  * @return  LSDRV_SPKMPH_INVALID  Vehicle speed pulse information valid<br>
 553  *      LSDRV_SPKMPH_VALID    Vehicle speed pulse information invalid
 554  */
 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  */
 560
 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;
 567     } else {
 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;
 570   }
 571
 572   /* Check if sensor counter is continuous */
 573   if (cnt_diff <= 1) {
 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;
 576   } else {
 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;
 581       } else {
 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;
 584       }
 585
 586       /* Calculate average vehicle speed pulse including skip period */
 587       sp_pls = (u_int16)(sp_pls_diff / cnt_diff);
 588
 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;
 593       } else {
 594         /* Subtracting a 100ms vehicle speed pulse from the latest one overflows */
 595         *us_sp2 = LSDRV_PLSMAX - (sp_pls - us_sp1);
 596       }
 597     }
 598
 599     ret = LSDRV_SPKMPH_VALID;
 600   }
 601
 602   return ret;
 603 }
 604
 605 /**
 606  * @brief
 607  *  Return the last (100ms ago) vehicle speed pulse
 608  *
 609  * @param[in]  us_sp      Vehicle speed pulse
 610  * @param[in]  uc_sens_cnt  Sensor counter
 611  */
 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;
 619 }
 620
 621 /*---------------------------------------------------------------------------*/
 622 /*EOF*/