diff options
Diffstat (limited to 'positioning_hal/src/LineSensDrv')
| -rw-r--r-- | positioning_hal/src/LineSensDrv/LineSensDrv_Api.cpp | 136 | ||||
| -rw-r--r-- | positioning_hal/src/LineSensDrv/LineSensDrv_Sensor.cpp | 622 | ||||
| -rw-r--r-- | positioning_hal/src/LineSensDrv/LineSensDrv_Thread.cpp | 125 |
3 files changed, 883 insertions, 0 deletions
diff --git a/positioning_hal/src/LineSensDrv/LineSensDrv_Api.cpp b/positioning_hal/src/LineSensDrv/LineSensDrv_Api.cpp new file mode 100644 index 00000000..d363a4bd --- /dev/null +++ b/positioning_hal/src/LineSensDrv/LineSensDrv_Api.cpp @@ -0,0 +1,136 @@ +/* + * @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_Api.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "LineSensDrv_Api.h" +#include "positioning_def.h" + +/*---------------------------------------------------------------------------*/ +// Macro definitions + +#define LSDRV_GPS_DATA_TOP_TH 5 // Threshold for determining the beginning of GPS data +#define LSDRV_SENS_DATA_RCV_WAIT_TIME 400 // Sensor data wait time of GPS reception flag ON(Total) +#define LSDRV_SENS_DATA_RCV_WAIT_TERM 50 // Sensor data wait time of GPS reception flag ON(1 time) + +#define LSDRV_EST_GPS_CNT_ARRAY_NUM 3U // Number of arrays for holding estimated GPS counter +#define LSDRV_EST_GPS_CNT_ENABLE_TH 5U // Estimated GPS counter enable/disable determination threshold +#define LSDRV_EST_GPS_CNT_ADD_VALUE 10U // Estimated GPS counter update addition value + +// GPS data reception cycle:1sec sensor counters: 100ms + +// for debug +#define LINE_SENS_DRV_API_DEBUG_SWITCH 0 // 0:OFF 1:ON + +/*---------------------------------------------------------------------------*/ +// Global variable + +static HANDLE g_gps_irq_mutex = NULL; // GPS received flag Mutex handles +static BOOL g_rcv_pps_int = FALSE; // PPS interrupt reception flag (GPS->_CWORD102_) +static BOOL g_rcv_gps_irq = FALSE; // GPS reception flag (GPS->_CWORD56_) +static u_int8 g_rcv_gps_sens_cnt_tmp = 0; // Sensor counter when GPS reception flag is ON(for retention) +static u_int8 g_rcv_gps_sens_cnt = 0; // Sensor counter when GPS reception flag is ON +static u_int8 g_gps_sens_cnt_top = 0; // Sensor counter when first GPS data is received +static u_int8 g_est_gps_cnt[LSDRV_EST_GPS_CNT_ARRAY_NUM]; // Array for storing estimated GPS counter values +static BOOL g_est_gps_cnt_available = FALSE; // Estimated GPS counter value enable/disable judgment + +typedef struct VehicleSensDataMaster { + DID ul_did; // Data ID + u_int16 us_size; // Size of the data + u_int8 uc_rcv_flag; // Receive flag + u_int8 uc_sns_cnt; // Sensor counter + u_int8 uc_data[132]; // Vehicle sensor data +} VEHICLESENS_DATA_MASTER; + + +/******************************************************************************* + * MODULE : DeliveryLineSensorDataPositioning + * ABSTRACT : LineSensor vehicle signaling notification messages sending process + * FUNCTION : Send LineSensor vehicle signalling notification messages + * ARGUMENT : *pstSendBuf:Transmitted data + * : uc_data_num :Number of sent data + * NOTE : + * RETURN : None + ******************************************************************************/ +void DeliveryLineSensorDataPositioning(LSDRV_MSG_LSDATA_G* pst_send_buf, u_int8 uc_data_num) { + if (pst_send_buf != NULL) { + /* Initializing sent messages */ + memset(reinterpret_cast<void *>(&(pst_send_buf->st_head)), 0, sizeof(T_APIMSG_MSGBUF_HEADER)); + + /* Message Header Settings */ + pst_send_buf->st_head.hdr.sndpno = PNO_LINE_SENS_DRV; /* Source process number */ + pst_send_buf->st_head.hdr.cid = CID_LINESENS_VEHICLE_DATA_G; /* Command ID */ + pst_send_buf->st_head.hdr.msgbodysize = sizeof(LSDRV_MSG_LSDATA_DAT_G); /* Message data body length */ + + /* Message data is already set */ + pst_send_buf->st_para.uc_data_num = uc_data_num; + + /* Send messages */ + (void)_pb_ZcSndMsg(PNO_VEHICLE_SENSOR, sizeof( LSDRV_MSG_LSDATA_G ), 0); + } +} + +/******************************************************************************* + * MODULE : LineSensDrvApi_Initialize + * ABSTRACT : LineSensDrvApi initialization process + * FUNCTION : LineSensDrvApi initialization process + * ARGUMENT : - + * NOTE : + * RETURN : - + ******************************************************************************/ +BOOL LineSensDrvApiInitialize(void) { + BOOL ret = TRUE; + + g_gps_irq_mutex = _pb_CreateMutex(NULL, FALSE, MUTEX_GPS_IRQ_FLG); + + if (g_gps_irq_mutex == 0) { + ret = FALSE; + } else { + g_rcv_gps_irq = FALSE; + g_rcv_gps_sens_cnt_tmp = 0; + LineSensDrvApiInitEstGpsCnt(); + ret = TRUE; + } + + return (ret); +} + +/******************************************************************************* + * MODULE : LineSensDrvApi_InitEstGpsCnt + * ABSTRACT : Estimated GPS counter related parameter initialization processing + * FUNCTION : Estimated GPS counter related parameter initialization processing + * ARGUMENT : - + * NOTE : + * RETURN : - + ******************************************************************************/ +void LineSensDrvApiInitEstGpsCnt(void) { + /* Initializing process */ + g_rcv_pps_int = FALSE; + g_rcv_gps_sens_cnt = 0; + g_gps_sens_cnt_top = 0; + + (void)memset(reinterpret_cast<void *>(&g_est_gps_cnt[0]), 0, sizeof(g_est_gps_cnt)); + g_est_gps_cnt_available = FALSE; + + return; +} + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/positioning_hal/src/LineSensDrv/LineSensDrv_Sensor.cpp b/positioning_hal/src/LineSensDrv/LineSensDrv_Sensor.cpp new file mode 100644 index 00000000..78ae4881 --- /dev/null +++ b/positioning_hal/src/LineSensDrv/LineSensDrv_Sensor.cpp @@ -0,0 +1,622 @@ +/* + * @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*/ diff --git a/positioning_hal/src/LineSensDrv/LineSensDrv_Thread.cpp b/positioning_hal/src/LineSensDrv/LineSensDrv_Thread.cpp new file mode 100644 index 00000000..9fb1c299 --- /dev/null +++ b/positioning_hal/src/LineSensDrv/LineSensDrv_Thread.cpp @@ -0,0 +1,125 @@ +/* + * @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_Thread.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "LineSensDrv_Thread.h" +#include <native_service/frameworkunified_types.h> +#include <native_service/frameworkunified_framework_if.h> +#include "positioning_def.h" +#include "positioning_common.h" +/*---------------------------------------------------------------------------*/ +// Global variable + +static u_int8 g_sys_recv_flg = 0; // For debugging +char g_threadname[] = "POS_Sens"; +uint8_t g_uc_vehicle_reverse = 0; +uint8_t g_uc_reverse_state = 0; +static BOOL g_line_sens_thread_stop = FALSE; + +#define POS_REV_STATE_PROCESSING 0 +#define POS_REV_STATE_IDLE 1 + +/******************************************************************************* + * MODULE : LineSensDrv_Thread + * ABSTRACT : LineSensor driver thread main process + * FUNCTION : Main processing + * ARGUMENT : lpvPara : + * NOTE : + * RETURN : + ******************************************************************************/ +EFrameworkunifiedStatus LineSensDrvThread(HANDLE h_app) { + int32 l_ret = RET_LSDRV_SUCCESS; + EFrameworkunifiedStatus l_status = eFrameworkunifiedStatusOK; + + + (void)PosSetupThread(h_app, ETID_POS_SENS); + + /* Execute the initialization processes */ + l_ret = LineSensDrvMainThreadInit(h_app); + if (RET_LSDRV_SUCCESS != l_ret) { + l_status = eFrameworkunifiedStatusFail; + } + + return l_status; +} + +/******************************************************************************* + * MODULE : LineSensDrv_MainThread_Init + * ABSTRACT : Thread initialization process + * FUNCTION : Initialize thread + * ARGUMENT : None + * NOTE : + * RETURN : RET_LSDRV_SUCCESS:Success in initialization + * RET_LSDRV_ERROR :Initialization failed + ******************************************************************************/ +int32 LineSensDrvMainThreadInit(HANDLE h_app) { + int32 l_ret = RET_LSDRV_SUCCESS; + BOOL b_ret = TRUE; + + /****** Global variable initialization **********/ + LineSensDrvParamInit(); + + /****** LineSensDrvApi initialization **********/ + b_ret = LineSensDrvApiInitialize(); + + if (TRUE != b_ret) { + l_ret = RET_LSDRV_ERROR; + } + + return l_ret; +} + +/******************************************************************************* + * MODULE : LineSensDrv_Param_Init + * ABSTRACT : Global variable initialization processing + * FUNCTION : Initialize global variables + * ARGUMENT : None + * NOTE : + * RETURN : None + ******************************************************************************/ +void LineSensDrvParamInit(void) { + LineSensDrvSpeedKmphDataInit(); // Data table initialization process for vehicle speed calculation +} + +/** + * @brief + * Pos_Sens thread stop processing + */ +void LineSensDrvThreadStopProcess(void) { + g_line_sens_thread_stop = TRUE; + + if (POS_REV_STATE_IDLE == g_uc_reverse_state) { + PosTeardownThread(ETID_POS_SENS); + } + + return; +} + +/** + * @brief + * Get initial sensor data reception flag + */ +u_int8 LineSensDrvGetSysRecvFlag(void) { + return g_sys_recv_flg; +} + +/*---------------------------------------------------------------------------*/ +/*EOF*/ |