path: root/positioning_hal/src/LineSensDrv/LineSensDrv_Sensor.cpp

/*
 * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/**
* @file LineSensDrv_Snesor.cpp
*/

/*---------------------------------------------------------------------------*/
// Include files

#include "LineSensDrv_Sensor.h"
#include "LineSensDrv_Api.h"

/*---------------------------------------------------------------------------*/
// Value Define

#define LSDRV_MASK_WORD_L                   0x00FF
#define LSDRV_MASK_WORD_U                   0xFF00

#define LINE_SENS_DRV_SENSOR_DEBUG_FACTORY  0
#define LINE_SENS_DRV_SENSOR_DEBUG_DIAG     0

#define VEHICLE_SNS_INFO_PULSE_NUM          32

/*---------------------------------------------------------------------------*/
// Global variable

static LSDRV_SPEEDKMPH_DAT    g_speed_kmph_data;
static uint8_t                g_rcv_data_len;     /* Receive SYS data length */
extern uint8_t g_uc_vehicle_reverse;
/*---------------------------------------------------------------------------*/
// Functions

/*******************************************************************************
 * MODULE    : LineSensDrv_Sensor
 * ABSTRACT  : Sensor data reception processing
 * FUNCTION  : Convert sensor data to delivery format
 * ARGUMENT  : *ucRcvdata : Data pointer
 * NOTE      :
 * RETURN    : None
 ******************************************************************************/
void LineSensDrvSensor(u_int8* uc_rcvdata) {
  u_int8              uc_sens_cnt = 0;
  u_int16             us_sp_kmph = 0;         /* Vehicle speed(km/h)      */
  u_int16             us_sp_pls1 = 0;         /* Total vehicle speed pulse(Latest)  */
  u_int16             us_sp_pls2 = 0;         /* Total vehicle speed pulse(Last time)  */
  u_int8              us_sp_ret = 0;          /* Last vehicle speed information acquisition result  */
  u_int8              uc_size = 0;            /* Size of the data      */
  u_int16             us_cnt = 0;             /* Data counter    */
  u_int16             us_cnt2 = 0;            /* Data counter    */
  u_int8*             uc_data_pos = NULL;     /* Data storage location    */
  LSDRV_MSG_LSDATA_G* p_snd_buf = NULL;
  LSDRV_LSDATA_G*     p_snd_data_base = NULL;
  LSDRV_LSDATA_G*     p_snd_data = NULL;
  RET_API             ret = RET_NORMAL;

  /* Receive sensor data top address acquisition */
  uc_data_pos = (uc_rcvdata);

  /* Create send buffer/delivery all received data */
  ret = _pb_GetZcSndBuf(PNO_VEHICLE_SENSOR, reinterpret_cast<void**>(&p_snd_buf));
  if ((ret == RET_NORMAL) && (p_snd_buf != NULL)) {
    p_snd_data_base = p_snd_buf->st_para.st_data;
    (void)memset(p_snd_data_base, 0, LSDRV_KINDS_MAX * sizeof( LSDRV_LSDATA_G ));

    /* Sensor counter */
    p_snd_data              = p_snd_data_base + LSDRV_SNS_COUNTER;
    uc_sens_cnt             = (u_int8)*(uc_data_pos);
    p_snd_data->ul_did      = VEHICLE_DID_SNS_COUNTER;    /* Data ID       */
    p_snd_data->uc_size     = LSDRV_SNDMSG_DTSIZE_1;      /* Size of the data   */
    p_snd_data->uc_sns_cnt  = uc_sens_cnt;                /* Sensor counter */
    p_snd_data->uc_data[0]  = uc_sens_cnt;                /* Data content     */
    uc_data_pos             = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_1);

    /* Gyro output */
    p_snd_data              = p_snd_data_base + LSDRV_GYRO_EXT;
    p_snd_data->ul_did      = VEHICLE_DID_GYRO_EXT;        /* Data ID       */
    p_snd_data->uc_size     = LSDRV_SNDMSG_DTSIZE_20;      /* Size of the data   */
    p_snd_data->uc_sns_cnt  = uc_sens_cnt;                 /* Sensor counter */
    for (us_cnt = 0; us_cnt < LSDRV_SNDMSG_DTSIZE_20; us_cnt++) {
      /* Since [0] is older and [9] is newer, the order of received data is switched. */
      /* Be in reverse order for endian conversion */
      p_snd_data->uc_data[LSDRV_SNDMSG_DTSIZE_20 - (us_cnt + 1)] = (u_int8)*(uc_data_pos + us_cnt);
    }
    uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20);

    p_snd_data              = p_snd_data_base + LSDRV_GYRO_X;
    p_snd_data->ul_did      = VEHICLE_DID_GYRO;             /* Data ID       */
    p_snd_data->uc_size     = LSDRV_SNDMSG_DTSIZE_20;       /* Size of the data   */
    p_snd_data->uc_sns_cnt  = uc_sens_cnt;                  /* Sensor counter */
    for (us_cnt = 0; us_cnt < LSDRV_SNDMSG_DTSIZE_20; us_cnt++) {
      /* Since [0] is older and [9] is newer, the order of received data is switched. */
      /* Be in reverse order for endian conversion */
      p_snd_data->uc_data[LSDRV_SNDMSG_DTSIZE_20 - (us_cnt + 1)] = (u_int8)*(uc_data_pos + us_cnt);
    }
    uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20);

    /* Reverse flag */
    p_snd_data              = p_snd_data_base + LSDRV_REV;
    p_snd_data->ul_did      = VEHICLE_DID_REV;              /* Data ID       */
    p_snd_data->uc_size     = LSDRV_SNDMSG_DTSIZE_1;        /* Size of the data   */
    p_snd_data->uc_sns_cnt  = uc_sens_cnt;                  /* Sensor Counter */
    p_snd_data->uc_data[0] = g_uc_vehicle_reverse;
    /* Gyro Temperature */
    p_snd_data              = p_snd_data_base + LSDRV_GYRO_TEMP;
    p_snd_data->ul_did      = VEHICLE_DID_GYRO_TEMP;        /* Data ID       */
    p_snd_data->uc_size     = LSDRV_SNDMSG_DTSIZE_2;        /* Size of the data   */
    p_snd_data->uc_sns_cnt  = uc_sens_cnt;                  /* Sensor counter */
    p_snd_data->uc_data[1]  = (u_int8)(*(uc_data_pos    ) & (u_int8)(LSDRV_TEMP_MASK >> 8));
    p_snd_data->uc_data[0]  = (u_int8)(*(uc_data_pos + 1) & (u_int8)(LSDRV_TEMP_MASK));
    uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_2);

    /* Vehicle speed pulse */
    p_snd_data              = p_snd_data_base + LSDRV_SPEED_PULSE;
    p_snd_data->ul_did      = VEHICLE_DID_SPEED_PULSE;      /* Data ID       */
    p_snd_data->uc_size     = LSDRV_SNDMSG_DTSIZE_20;       /* Size of the data   */
    p_snd_data->uc_sns_cnt  = uc_sens_cnt;                  /* Sensor counter */
    for (us_cnt = 0; us_cnt < LSDRV_SNDMSG_DTSIZE_20; us_cnt++) {
      /* Since [0] is older and [9] is newer, the order of received data is switched. */
      /* Be in reverse order for endian conversion */
      p_snd_data->uc_data[LSDRV_SNDMSG_DTSIZE_20 - (us_cnt + 1)] = (u_int8)*(uc_data_pos + us_cnt);
    }
    uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20);

    /* Vehicle speed(km/h) */
    p_snd_data              = p_snd_data_base + LSDRV_SPEED_PULSE;
    us_sp_kmph              = 0;
    us_sp_pls1              = (u_int16)p_snd_data->uc_data[1];
    us_sp_pls1              = (u_int16)((us_sp_pls1 << 8) | p_snd_data->uc_data[0]);
    us_sp_ret               = LineSensDrvGetLastSpeedPulse(&us_sp_pls2, us_sp_pls1, uc_sens_cnt);

    LineSensDrvSetLastSpeedPulse(us_sp_pls1, uc_sens_cnt);

    p_snd_data            = p_snd_data_base + LSDRV_SPEED_KMPH;
    if (us_sp_ret != LSDRV_SPKMPH_INVALID) {
      /* Vehicle speed pulse before 100 ms is valid */
      LineSensDrvSpeedPulseSave(us_sp_pls1, us_sp_pls2, uc_sens_cnt);
      us_sp_kmph = LineSensDrvSpeedCalc(uc_sens_cnt);
      /* 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. */
      p_snd_data->uc_size  = LSDRV_SNDMSG_DTSIZE_2;      /* Size of the data   */
    }
    p_snd_data->ul_did      = VEHICLE_DID_SPEED_KMPH;       /* Data ID       */
    p_snd_data->uc_sns_cnt  = uc_sens_cnt;                  /* Sensor counter */
    p_snd_data->uc_data[0]  = (u_int8)(us_sp_kmph & 0x00FF);
    p_snd_data->uc_data[1]  = (u_int8)(us_sp_kmph >> 8);

    POSITIONING_LOG("[LOG, %d(cnt), %d(km/h), %d(pls1), %d(pls2)]  \r\n",
      uc_sens_cnt,
      us_sp_kmph,
      us_sp_pls1,
      us_sp_pls2);

    /* G-Sensor X-axes */
    p_snd_data              = p_snd_data_base + LSDRV_GSENSOR_X;
    p_snd_data->ul_did      = VEHICLE_DID_GSNS_X;           /* Data ID       */
    p_snd_data->uc_size     = LSDRV_SNDMSG_DTSIZE_20;       /* Size of the data   */
    p_snd_data->uc_sns_cnt  = uc_sens_cnt;                  /* Sensor counter */
    us_cnt2                 = p_snd_data->uc_size - 1;
    for (us_cnt = 0; us_cnt < 10; us_cnt++) {
      /* Since [0] is older and [9] is newer, the order of received data is switched. */
      p_snd_data->uc_data[us_cnt2    ] = (u_int8)*( uc_data_pos + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt)    );
      p_snd_data->uc_data[us_cnt2 - 1] = (u_int8)*( uc_data_pos + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt) + 1);
      us_cnt2 = us_cnt2 - 2;
    }

    /* G-Sensor Y-axes */
    p_snd_data              = p_snd_data_base + LSDRV_GSENSOR_Y;
    p_snd_data->ul_did      = VEHICLE_DID_GSNS_Y;           /* Data ID       */
    p_snd_data->uc_size     = LSDRV_SNDMSG_DTSIZE_20;       /* Size of the data   */
    p_snd_data->uc_sns_cnt  = uc_sens_cnt;                  /* Sensor counter */
    us_cnt2                 = p_snd_data->uc_size - 1;
    for (us_cnt = 0; us_cnt < 10; us_cnt++) {
      /* Since [0] is older and [9] is newer, the order of received data is switched. */
      p_snd_data->uc_data[us_cnt2] = (u_int8)*( (uc_data_pos + sizeof(u_int16)) \
                                           + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt));

      p_snd_data->uc_data[us_cnt2-1] = (u_int8)*( (uc_data_pos + sizeof(u_int16)) \
                                           + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt) + 1);
      us_cnt2 = us_cnt2 - 2;
    }
    uc_data_pos = ( uc_data_pos + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * 10) );

    /* Inter-Pulse time */
    p_snd_data              = p_snd_data_base + LSDRV_PULSE_TIME;
    p_snd_data->ul_did      = VEHICLE_DID_PULSE_TIME;       /* Data ID       */
    p_snd_data->uc_size     = LSDRV_SNDMSG_DTSIZE_132;      /* Size of the data   */
    p_snd_data->uc_sns_cnt  = uc_sens_cnt;                  /* Sensor counter */

    /* Clear the buffer for variable length */
    (void)memset(&p_snd_data->uc_data[0], 0x00, sizeof(p_snd_data->uc_data));

    if (g_rcv_data_len == LSDRV_PLSTIME_LEN) {
      /* Inter-Pulse time (number of items + time). The number of items is stored at the beginning. */
      uc_size = (u_int8)*uc_data_pos;
      if (uc_size > VEHICLE_SNS_INFO_PULSE_NUM) {
        uc_size = VEHICLE_SNS_INFO_PULSE_NUM;
      }
      p_snd_data->uc_data[0] = uc_size;
      p_snd_data->uc_data[1] = 0x00;
      p_snd_data->uc_data[2] = 0x00;
      p_snd_data->uc_data[3] = 0x00;
      uc_data_pos = ( uc_data_pos + sizeof(u_int8) );

      /* Since [0] is old and [31] is new in the received time, the order is changed. */
      /* Be in reverse order for endian conversion */
      for (us_cnt = 0; us_cnt < (uc_size * sizeof(u_int32)); us_cnt++) {
        p_snd_data->uc_data[(uc_size * sizeof(u_int32)) - (us_cnt + 1) + 4] = (u_int8)*(uc_data_pos + us_cnt);
      }
    }

    /* Messaging */
    DeliveryLineSensorDataPositioning(p_snd_buf, LSDRV_KINDS_MAX);
  }

  return;
}

/*******************************************************************************
 * MODULE    : LineSensDrv_SpeedCalc
 * ABSTRACT  : Vehicle speed calculation processing for sensor data
 * FUNCTION  : Calculate vehicle speed for sensor data
 * ARGUMENT  : uc_sens_cnt : Sensor counter
 * NOTE      :
 * RETURN    : Vehicle speed(0.01km/h)
 ******************************************************************************/
u_int16 LineSensDrvSpeedCalc(u_int8 uc_sens_cnt) {
  u_int32 ul_sp_caluc = 0;          /* Vehicle speed(2^-8km/h)  */
  u_int16 us_speed = 0;             /* Vehicle speed(km/h)    */
  u_int8  uc_ptr = 0;               /* Data storage pointer for sensor data  */  /* #010 */
  u_int8  uc_ptr2 = 0;              /* Data storage pointer for sensor data  */  /* #010 */
  u_int32 ul_work = 0;              /* Variables for unsigned 32-bit type calculations      */  /* #010 */
  double  d_work = 0.0;             /* Variables for calculating double types          */  /* #010 */  /* Ignore -> MISRA-C++:2008 Rule 3-9-2 */
  u_int16 us_sens_cnt_search = 0;   /* Sensor counter to be searched        */  /* #010 */
  u_int16 us_sens_cnt_ref = 0;      /* Sensor counters to be compared      */  /* #010 */
  int32   i = 0;                    /* Generic counters              */  /* #010 */
  int32   j = 0;                    /* Generic counters              */  /* #010 */
  u_int16 us_offset = 0;            /* Offset value              */  /* #010 */

  /* #016 start */
  /* Is the number of data that can be calculated at the vehicle speed already received? */
  if (LSDRV_PLSDATA_NRCV == g_speed_kmph_data.uc_calc_start) {
    /* Do not compute if there is not enough data. */
  } else {
    if (0 == g_speed_kmph_data.uc_ptr) {
      uc_ptr  = LSDRV_SPKMPH_TBL_NUM - 1;
      uc_ptr2 = LSDRV_SPKMPH_TBL_NUM - 1;
    } else {
      uc_ptr  = g_speed_kmph_data.uc_ptr - 1;  /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
      uc_ptr2 = g_speed_kmph_data.uc_ptr - 1;  /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
    }

    ul_work = 0;
    if (LSDRV_SPKMPH_AVE_TIME > uc_sens_cnt) {
      us_offset = LSDRV_SENSCNT_BRW_ADD;
    } else {
      us_offset = 0;
    }

    us_sens_cnt_search = (u_int16)uc_sens_cnt + us_offset - LSDRV_SPKMPH_AVE_TIME;

    for (i = 0; i < LSDRV_SPKMPH_TBL_NUM; i++) {
      /* Invalid data is detected, and the search is completed. */
      if (LSDRV_SPKMPH_DATA_DIS == g_speed_kmph_data.st_data[uc_ptr].uc_flag) {
        break;
        }

      /* When the sensor counter is 29 or less, the borrow is considered. */
      if (LSDRV_SPKMPH_AVE_TIME > g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt) {
        us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt + us_offset;
      } else {
        us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt;
      }

      /* Checking the sensor counter to finish search */
      if (us_sens_cnt_search >= us_sens_cnt_ref) {
        break;
      }

      /* Add to calculate average value */
      ul_work += (u_int32)g_speed_kmph_data.st_data[uc_ptr].us_speed_pulse;

      if (0 == uc_ptr) {
        uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1;                /* To the end of the data table */
      } else {
        uc_ptr--;                            /* To the previous data */
      }
    }

    /* Averaging computation */
    if (0 == i) {
      d_work = 0;
    } else {
      if (ul_work == 1) {
        for (j = 0; j < LSDRV_SPKMPH_TBL_NUM; j++) {
          if ((g_speed_kmph_data.st_data[uc_ptr2].us_speed_pulse == 1)
            && (g_speed_kmph_data.st_data[uc_ptr2].uc_noise_flag == 1)) {
            ul_work = 0;
            break;
          } else if (g_speed_kmph_data.st_data[uc_ptr2].us_speed_pulse == 1 &&
            g_speed_kmph_data.st_data[uc_ptr2].uc_noise_flag == 0) {
            ul_work = 1;
            break;
          } else {
            /* nop */
          }

          /* Borrow actions for pointer values */
          if (0 == uc_ptr2) {
            uc_ptr2 = LSDRV_SPKMPH_TBL_NUM - 1;                /* To the end of the data table */
          } else {
            uc_ptr2--;                            /* To the previous data */
          }
        }
      }

      d_work = static_cast<double>(ul_work);
      d_work = d_work / static_cast<double>(i);
      d_work = d_work * static_cast<double>(LSDRV_SENS_COEFFICIENT);
      d_work = d_work * 100;               /* [1km/h] -> [0.01km/h] */
      d_work = d_work + 0.5;              /* Preparation for rounding */
    }

    ul_sp_caluc = static_cast<int32>(d_work);

    /* When the vehicle speed calculation result overflows, the upper limit value is used for clipping. */
    if (LSDRV_PLSMAX - 1 >= ul_sp_caluc) {
      us_speed = (u_int16)ul_sp_caluc;
    } else {
      us_speed = (u_int16)(LSDRV_PLSMAX - 1);
    }
  }

  return us_speed;
}

/*******************************************************************************
 * MODULE    : LineSensDrv_SpeedKmphDataInit
 * ABSTRACT  : Data table initialization process for vehicle speed calculation
 * FUNCTION  : Initialize the data table for calculating the vehicle speed
 * ARGUMENT  : None
 * NOTE      :
 * RETURN    : None
 ******************************************************************************/
void LineSensDrvSpeedKmphDataInit(void) {
  int32 i = 0;

  memset(reinterpret_cast<void*>(&g_speed_kmph_data), static_cast<int32>(0), (size_t)sizeof(g_speed_kmph_data));

  /* Disable all data storage flags */
  for (i = 0; i < LSDRV_SPKMPH_TBL_NUM; i++) {
    g_speed_kmph_data.st_data[i].uc_flag = LSDRV_SPKMPH_DATA_DIS;
  }

  return;
}

/*******************************************************************************
 * MODULE    : LineSensDrv_SpeedPulseSave
 * ABSTRACT  : Sensor data vehicle speed pulse save processing
 * FUNCTION  : Saving the vehicle speed pulse of the sensor data to the data table for calculating the vehicle speed 
 * ARGUMENT  : us_sp1     : Vehicle speed pulse of the latest information
 *           : us_sp2     : Vehicle speed pulse of the previous information
 *           : uc_sens_cnt : Sensor counter
 * NOTE      :
 * RETURN    : None
 ******************************************************************************/
void LineSensDrvSpeedPulseSave(u_int16 us_sp1, u_int16 us_sp2, u_int8 uc_sens_cnt) {
  u_int16 us_sp_diff = 0;     /* Vehicle speed pulse difference  */
  u_int8  uc_ptr = 0;         /* Data storage pointer for sensor data  */  /* #010 */
  u_int8  us_last_ptr = 0;    /* Last pointer              */  /* #010 */
  int32   i = 0;              /* Generic counters              */  /* #010 */
  u_int8  uc_fstsns_cnt = 0;  /* Initial sensor data sensor counter value  */  /* #016 */
  int32   l_data_num = 0;     /* Number of registered data              */  /* #016 */
  u_int8  uc_noise_flag = 0;  /* Noise flag              */  /* #017 */

  /* Calculate the vehicle speed pulse difference between the latest and last 100ms information */
  if (us_sp1 >= us_sp2) {
    /* The cumulative pulse number of the latest information is larger. */
    us_sp_diff = us_sp1 - us_sp2;  /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
  } else {
    /* The cumulative pulse number of the latest information is smaller (the accumulated pulse overflows) */
    us_sp_diff = (LSDRV_PLSMAX - us_sp2) + us_sp1;  /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
  }

  /* Call noise check only if us_sp_diff is 1 */
  if (us_sp_diff == 1) {
    uc_noise_flag = LineSensDrvCheckNoise(uc_sens_cnt);
  }

  /* Saving sensor data vehicle speed pulse in data table for vehicle speed calculation */
  if (LSDRV_PLSDATA_NRCV == g_speed_kmph_data.uc_sns_rcv) {
    if (LSDRV_PLSDATA_RCV == g_speed_kmph_data.uc_fstsns_rcv) {
      /* 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. */
      uc_fstsns_cnt = uc_sens_cnt;
      for (i = (LSDRV_SPKMPH_TBL_NUM - 1); i >= 0 ; i--) {
        if (LSDRV_SPKMPH_DATA_EN == g_speed_kmph_data.st_data[i].uc_flag) {
          /* Data storage flag is valid */
          if (0 != uc_fstsns_cnt) {
            uc_fstsns_cnt--;
          } else {
            uc_fstsns_cnt = LSDRV_SENSCNT_MAX;
          }

          g_speed_kmph_data.st_data[i].uc_sens_cnt = uc_fstsns_cnt;
        }
      }
    }

    /* Sensor data reception status  <-  "Received" */
    g_speed_kmph_data.uc_sns_rcv = LSDRV_PLSDATA_RCV;
  }

  uc_ptr = g_speed_kmph_data.uc_ptr;

  /* If the sensor counter is the same as the previous one, overwrite update */
  if (0 == uc_ptr) {
    us_last_ptr = LSDRV_SPKMPH_TBL_NUM - 1;
  } else {
    us_last_ptr = uc_ptr - 1;   /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
  }

  if (g_speed_kmph_data.st_data[us_last_ptr].uc_sens_cnt == uc_sens_cnt) {
    /* Next update of the data storage location */
    if (0 == g_speed_kmph_data.uc_ptr) {
      g_speed_kmph_data.uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1;
    } else {
      g_speed_kmph_data.uc_ptr--;
    }

    uc_ptr = g_speed_kmph_data.uc_ptr;
  }

  /* Input into data table for calculation of vehicle speed */
  g_speed_kmph_data.st_data[uc_ptr].uc_flag         = LSDRV_SPKMPH_DATA_EN; /* Data validity      */
  g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt     = uc_sens_cnt;          /* Sensor counter input  */
  g_speed_kmph_data.st_data[uc_ptr].us_speed_pulse  = us_sp_diff;           /* Vehicle speed pulse difference    */
  g_speed_kmph_data.st_data[uc_ptr].uc_noise_flag   = uc_noise_flag;        /* Noise flag      */ /* #017 */

  /* Next update of the data storage location */
  if ((LSDRV_SPKMPH_TBL_NUM - 1) <= g_speed_kmph_data.uc_ptr) {
    g_speed_kmph_data.uc_ptr = 0;
  } else {
    g_speed_kmph_data.uc_ptr++;
  }

  /* Determine whether the vehicle speed can be calculated. */
  if (g_speed_kmph_data.uc_calc_start == LSDRV_PLSDATA_NRCV) {
    /* Data count detection */
    l_data_num = 0;
    for (i = 0; i < LSDRV_SPKMPH_TBL_NUM; i++) {
      if (LSDRV_SPKMPH_DATA_EN == g_speed_kmph_data.st_data[i].uc_flag) {
          l_data_num++;
      }
    }

    if (LSDRV_SPKMPH_MIN_DATA_N <= l_data_num) {
      /* Vehicle Speed Calculation Required Data Number Received */
      g_speed_kmph_data.uc_calc_start = LSDRV_PLSDATA_RCV;
    }
  }

  return;
}

/*******************************************************************************
 * MODULE    : LineSensDrv_CheckNoise
 * ABSTRACT  : Sensor data noise check processing
 * FUNCTION  : The vehicle speed pulse is saved in the data table for the vehicle speed calculation. 
 * ARGUMENT  : uc_sens_cnt     : Sensor counter of the latest data
 * NOTE      :
 * RETURN    : Noise flag
 ******************************************************************************/
u_int8 LineSensDrvCheckNoise(u_int8 uc_sens_cnt) {
  int32   i = 0;                  /* Generic counters */
  u_int16 us_sens_cnt_search = 0; /* Sensor counter to be searched*/
  u_int8  uc_ptr = 0;             /* Data storage pointer */
  u_int16 us_offset = 0;          /* Offset value  */
  u_int8  noise_flag = 0;         /* Noise flag  */
  u_int16 us_sens_cnt_ref = 0;    /* Sensor counters to be compared */

  /* If there is no point where the difference in vehicle speed pulse is 1 or more 
     between -1 and -14 of sensor counter of the latest data, 
     set the noise flag of the latest data to 1. */
  /* Set the noise flag to 1 */
  noise_flag = 1;

  /* The Target is the one before the storage location of the latest data. */
  if (0 == g_speed_kmph_data.uc_ptr) {
    uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1;
  } else {
    uc_ptr = g_speed_kmph_data.uc_ptr - 1;  /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
  }

  if (LSDRV_SPKMPH_NOISE_TIME > uc_sens_cnt) {
    us_offset = LSDRV_SENSCNT_BRW_ADD;
  } else {
    us_offset = 0;
  }

  us_sens_cnt_search = (u_int16)uc_sens_cnt + us_offset - LSDRV_SPKMPH_NOISE_TIME;

  for (i = 0; i < LSDRV_SPKMPH_NOISE_TIME; i++) {
    /* When the sensor counter is 15 or less, the borrow is considered. */
    if (LSDRV_SPKMPH_NOISE_TIME > g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt) {
      us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt + us_offset;
    } else {
      us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt;  /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */
    }

    /* Checking the sensor Counter to Finish Search */
    if (us_sens_cnt_ref <= us_sens_cnt_search) {
      noise_flag = 1;
      break;
    } else {
      if (g_speed_kmph_data.st_data[uc_ptr].us_speed_pulse >= 1) {
        noise_flag = 0;
        break;
      }
    }

    /* Borrow actions for pointer values */
    if (0 == uc_ptr) {
      uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1;                /* To the end of the data table */
    } else {
      uc_ptr--;                            /* To the previous data */
    }
  }

  return noise_flag;
}

/**
 * @brief
 *  Return the last (100ms ago) vehicle speed pulse 
 *
 * @param[out]  us_sp2    Last vehicle speed pulse
 * @param[in]  us_sp1    The latest vehicle speed pulse
 * @param[in]  uc_sens_cnt  Latest sensor counter
 *
 * @return  LSDRV_SPKMPH_INVALID  Vehicle speed pulse information valid<br>
 *      LSDRV_SPKMPH_VALID    Vehicle speed pulse information invalid
 */
u_int8 LineSensDrvGetLastSpeedPulse(u_int16* us_sp2, u_int16 us_sp1, u_int8 uc_sens_cnt) {
  u_int8  ret = LSDRV_SPKMPH_INVALID;   /* Return value        */
  u_int16 sp_pls_diff = 0;              /* Vehicle speed pulse difference    */
  u_int16 sp_pls = 0;                   /* Vehicle speed pulse every 100 ms  */
  u_int8  cnt_diff = 0;                 /* Sensor counter difference  */

  /* Check if the last vehicle speed pulse has been set  */
  if (g_speed_kmph_data.st_last_data.uc_flag == LSDRV_SPKMPH_DATA_EN) {
    /* Differential calculation of sensor counter */
    if (uc_sens_cnt >= g_speed_kmph_data.st_last_data.uc_sens_cnt) {
      /* Latest sensor counter is larger */
      cnt_diff = uc_sens_cnt - g_speed_kmph_data.st_last_data.uc_sens_cnt;
    } else {
      /* Last sensor counter is larger(sensor counter overflows) */
      cnt_diff = (LSDRV_SENSCNT_MAX - g_speed_kmph_data.st_last_data.uc_sens_cnt) + uc_sens_cnt + 1;
  }

  /* Check if sensor counter is continuous */
  if (cnt_diff <= 1) {
    /* Continuous or same as the previous one, so the previous (100ms previous) vehicle speed pulse is set as it is */
    *us_sp2 = g_speed_kmph_data.st_last_data.us_speed_pulse;
  } else {
    /* Determine the vehicle speed pulse 100ms ago from the average considering the skipped portion because it is not consecutive. */
    if (us_sp1 >= g_speed_kmph_data.st_last_data.us_speed_pulse) {
      /* Larger latest cumulative vehicle speed pulse */
      sp_pls_diff = us_sp1 - g_speed_kmph_data.st_last_data.us_speed_pulse;
      } else {
        /* Last cumulative vehicle speed pulse is larger(Cumulative vehicle speed pulse overflows) */
        sp_pls_diff = (LSDRV_PLSMAX - g_speed_kmph_data.st_last_data.us_speed_pulse) + us_sp1;
      }

      /* Calculate average vehicle speed pulse including skip period */
      sp_pls = (u_int16)(sp_pls_diff / cnt_diff);

      /* Calculate the vehicle speed pulse 100ms ahead from the vehicle speed average */
      if (us_sp1 >= sp_pls) {
        /* Does not overflow even if the 100ms vehicle speed pulse is pulled from the latest one. */
        *us_sp2 = us_sp1 - sp_pls;
      } else {
        /* Subtracting a 100ms vehicle speed pulse from the latest one overflows */
        *us_sp2 = LSDRV_PLSMAX - (sp_pls - us_sp1);
      }
    }

    ret = LSDRV_SPKMPH_VALID;
  }

  return ret;
}

/**
 * @brief
 *  Return the last (100ms ago) vehicle speed pulse 
 *
 * @param[in]  us_sp      Vehicle speed pulse
 * @param[in]  uc_sens_cnt  Sensor counter
 */
void LineSensDrvSetLastSpeedPulse(u_int16 us_sp, u_int8 uc_sens_cnt) {
  /* Vehicle speed pulse information valid setting */
  g_speed_kmph_data.st_last_data.uc_flag = LSDRV_SPKMPH_DATA_EN;
  /* Sensor counter setting */
  g_speed_kmph_data.st_last_data.uc_sens_cnt = uc_sens_cnt;
  /* Vehicle speed pulse setting */
  g_speed_kmph_data.st_last_data.us_speed_pulse = us_sp;
}

/*---------------------------------------------------------------------------*/
/*EOF*/