--- /dev/null
+/*
+ * @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*/
Code Review / staging / basesystem.git / blobdiff