From 947c78887e791596d4a5ec2d1079f8b1a049628b Mon Sep 17 00:00:00 2001 From: takeshi_hoshina Date: Tue, 27 Oct 2020 11:16:21 +0900 Subject: basesystem 0.1 --- positioning_hal/src/Common/positioning_common.cpp | 388 ++++ positioning_hal/src/Common/positioning_hal.cpp | 49 + positioning_hal/src/GpsCommon/MDev_GpsRecv.cpp | 608 ++++++ positioning_hal/src/GpsCommon/MDev_GpsRollOver.cpp | 275 +++ positioning_hal/src/GpsCommon/MDev_Gps_API.cpp | 491 +++++ positioning_hal/src/GpsCommon/MDev_Gps_Common.cpp | 2105 ++++++++++++++++++++ positioning_hal/src/GpsCommon/MDev_Gps_Main.cpp | 362 ++++ positioning_hal/src/GpsCommon/MDev_Gps_Mtrx.cpp | 866 ++++++++ positioning_hal/src/GpsCommon/MDev_Gps_Nmea.cpp | 928 +++++++++ .../src/GpsCommon/MDev_Gps_TimerCtrl.cpp | 293 +++ .../src/LineSensDrv/LineSensDrv_Api.cpp | 136 ++ .../src/LineSensDrv/LineSensDrv_Sensor.cpp | 622 ++++++ .../src/LineSensDrv/LineSensDrv_Thread.cpp | 125 ++ 13 files changed, 7248 insertions(+) create mode 100644 positioning_hal/src/Common/positioning_common.cpp create mode 100644 positioning_hal/src/Common/positioning_hal.cpp create mode 100644 positioning_hal/src/GpsCommon/MDev_GpsRecv.cpp create mode 100644 positioning_hal/src/GpsCommon/MDev_GpsRollOver.cpp create mode 100644 positioning_hal/src/GpsCommon/MDev_Gps_API.cpp create mode 100644 positioning_hal/src/GpsCommon/MDev_Gps_Common.cpp create mode 100644 positioning_hal/src/GpsCommon/MDev_Gps_Main.cpp create mode 100644 positioning_hal/src/GpsCommon/MDev_Gps_Mtrx.cpp create mode 100644 positioning_hal/src/GpsCommon/MDev_Gps_Nmea.cpp create mode 100644 positioning_hal/src/GpsCommon/MDev_Gps_TimerCtrl.cpp create mode 100644 positioning_hal/src/LineSensDrv/LineSensDrv_Api.cpp create mode 100644 positioning_hal/src/LineSensDrv/LineSensDrv_Sensor.cpp create mode 100644 positioning_hal/src/LineSensDrv/LineSensDrv_Thread.cpp (limited to 'positioning_hal/src') diff --git a/positioning_hal/src/Common/positioning_common.cpp b/positioning_hal/src/Common/positioning_common.cpp new file mode 100644 index 00000000..00b3a8bc --- /dev/null +++ b/positioning_hal/src/Common/positioning_common.cpp @@ -0,0 +1,388 @@ +/* + * @copyright Copyright (c) 2017-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 positioning_common.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "positioning_common.h" +#include "MDev_Gps_Common.h" +#include "MDev_GpsRecv.h" +#include "LineSensDrv_Thread.h" + +/*---------------------------------------------------------------------------*/ +// Value define + +#define DATMOD_RETRY (3) /* Number of shared memory generation retries */ +#define DATMOD_PREINIT (0) /* Shared Memory State [Before initialization] */ +#define PRIM_NAME_MAX (32) /* Maximum Name Size */ +#define VEHICLE_SHARE_NAME ("POS_VEHICLE_SHARE_MEMORY") /* Shared memory name */ + +/* Mask for managing various notification reception conditions */ +#define NTFY_MSK_NONE (0x00) +/* Service availability notification */ +#define NTFY_MSK_COMMUNICATION_AVAILABILITY (0x01) +#define NTFY_MSK_PS_COMMUSB_AVAILABILITY (0x02) +#define NTFY_MSK_PS_PSMSHADOW_AVAILABILITY (0x04) +#define NTFY_MSK_CLOCK_AVAILABILITY (0x08) +#define NTFY_MSK_NS_BACKUPMGR_AVAILABILITY (0x10) +#define NTFY_MSK_SS_DEVDETSRV_AVAILABILITY (0x20) +/* Other Notices */ +#define NTFY_MSK_PS_PSMSHADOW_INIT_COMP (0x01) /* PSMShadow startup completion notice */ +#define NTFY_MSK_PS_LANSERVER_DEVICE_UPDATE (0x02) /* LanServer device configuration change notification */ +#define NTFY_MSK_PS_LANSERVER_WAKEUP_COMP (0x04) /* LanServer start completion notice */ + +/* Thread state */ +#define THREAD_STS_NOEXIST (0x00) +#define THREAD_STS_CREATING (0x01) +#define THREAD_STS_CREATED (0x02) + +/*---------------------------------------------------------------------------*/ +// ENUMERATION + +/*---------------------------------------------------------------------------*/ +// STRUCTURE + +typedef struct StThreadInfo { + EnumTID_POS e_id; /**< Thread ID */ + const int8_t* p_name; /**< Thread name */ + PNO p_no; /**< Process number */ + CbFuncPtr cb_routine; /**< Start routine */ + uint8_t msk_available; /**< Dependent services Availability */ + uint8_t msk_ntfy; /**< Dependent notification */ + uint8_t msk_thread; /**< Dependent threads */ + BOOL b_is_depended; /**< Positioning/Availability->TRUE change dependency */ + uint8_t uc_status; /**< Thread activation state */ + uint8_t uc_order; /**< Boot Sequence(Performance) */ + uint8_t u_reserve[2]; +} ST_THREAD_CREATE_INFO; + +typedef struct { + char share_data_name[PRIM_NAME_MAX]; /**< Shared data name */ + u_int32 data_size; /**< Shared data size */ +} ST_SHAREDATA; + +static const int8_t kThreadNamePosMain[15] = "POS_Main"; +static const int8_t kThreadNamePosSens[15] = "POS_Sens"; +static const int8_t kThreadNamePosGps[15] = "POS_Gps"; +static const int8_t kThreadNamePosGpsRecv[15] = "POS_Gps_Recv"; +static const int8_t kThreadNamePosGpsRollover[15] = "POS_Gps_Rolovr"; + +static ST_THREAD_CREATE_INFO g_pos_thread_create_info[] = { + { /* POS_Main */ + ETID_POS_MAIN, /* (1) */ + NULL, /* (2) */ + 0, /* (3) */ + NULL, /* (4) */ + (NTFY_MSK_NONE), /* (5) */ + (NTFY_MSK_NONE), /* (6) */ + 0, /* (7) */ + FALSE, /* (8) */ + THREAD_STS_NOEXIST, /* (9) */ + 0 /* (10) */ + }, + { /* POS_sens */ + ETID_POS_SENS, /* (1) */ + kThreadNamePosSens, /* (2) */ + PNO_LINE_SENS_DRV, /* (3) */ + &LineSensDrvThread, /* (4) */ + (NTFY_MSK_NONE), /* (5) */ + (NTFY_MSK_NONE), /* (6) */ + 0, /* (7) */ + FALSE, /* (8) */ + THREAD_STS_NOEXIST, /* (9) */ + 0 /* (10) */ + }, + { /* POS_Gps */ + ETID_POS_GPS, /* (1) */ + kThreadNamePosGps, /* (2) */ + PNO_NAVI_GPS_MAIN, /* (3) */ + &DevGpsMainThread, /* (4) */ + (NTFY_MSK_NONE), /* (5) */ + (NTFY_MSK_NONE), /* (6) */ + 0, /* (7) */ + TRUE, /* (8) */ + THREAD_STS_NOEXIST, /* (9) */ + 0 /* (10) */ + }, + { /* POS_Gps_Recv */ + ETID_POS_GPS_RECV, /* (1) */ + kThreadNamePosGpsRecv, /* (2) */ + PNO_NAVI_GPS_RCV, /* (3) */ + &DevGpsRecvThread, /* (4) */ + (NTFY_MSK_NONE), /* (5) */ + (NTFY_MSK_NONE), /* (6) */ + THREAD_STS_MSK_POS_GPS, /* (7) */ + FALSE, /* (8) */ + THREAD_STS_NOEXIST, /* (9) */ + 0 /* (10) */ + }, + { /* POS_Gps_Rolovr */ + ETID_POS_GPS_ROLLOVER, /* (1) */ + NULL, /* (2) */ + 0, /* (3) */ + NULL, /* (4) */ + (NTFY_MSK_NONE), /* (5) */ + (NTFY_MSK_NONE), /* (6) */ + 0, /* (7) */ + FALSE, /* (8) */ + THREAD_STS_NOEXIST, /* (9) */ + 0 /* (10) */ + }, + { /* Termination */ + ETID_POS_MAX, /* (1) */ + NULL, /* (2) */ + 0, /* (3) */ + NULL, /* (4) */ + NTFY_MSK_NONE, /* (5) */ + NTFY_MSK_NONE, /* (6) */ + 0, /* (7) */ + FALSE, /* (8) */ + THREAD_STS_NOEXIST, /* (9) */ + 0 /* (10) */ + }, +}; + +static ST_SHAREDATA g_sharedata_tbl[] = { + /* Shared data name to be generated, Shared data size */ + { {VEHICLE_SHARE_NAME}, 512 * 11 }, /* Vehicle sensor information acquisition */ +#if 0 /* Not used in _CWORD71_ below */ + { {"SENSOR_SHARE_MEMORY"}, 512 * 11 }, /* Vehicle sensor information Pkg acquisition */ + { {"GPS_INT_SIGNAL_SHARE_MEMORY"}, 4 }, /* GPS interrupt signal acquisition */ + { {"LOG_SETTING_SHARE_MEMORY"}, 36 }, /* DR feature log acquisition */ + { {"GYRO_CONNECT_STTS_SHARE_MEMORY"}, 4 }, /* Gyro connection status acquisition */ + { {"EPHEMERIS_NUM_SHARE_MEMORY"}, 4 }, /* Effective ephemeris count acquisition at shutdown */ + { {"LOCALTIME_SHARE_MEMORY"}, 12 }, /* Local time acquisition at shutdown */ + { {"LONLAT_SHARE_MEMORY"}, 8 }, /* Location acquisition at shutdown */ +#endif + { {(int8)NULL}, 0 } /* Termination */ +}; + + +/*---------------------------------------------------------------------------*/ +// Functions + +static EFrameworkunifiedStatus PosStopThreadDummy(HANDLE h_app) { + return eFrameworkunifiedStatusOK; +} + +static uint32_t PosGetMsg(HANDLE h_app, void** p_buf, uint32_t ul_size) { + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusOK; + uint32_t tmp_size = 0; + void* p_rev_buf = NULL; + + if ((h_app == NULL) || (p_buf == NULL)) { + POSITIONING_LOG("Argument ERROR!! [h_app = %p, pBuf = %p]", h_app , p_buf); + } else { + /* Check the size of received data */ + tmp_size = FrameworkunifiedGetMsgLength(h_app); + if (tmp_size > ul_size) { + POSITIONING_LOG("Message ul_size ERROR!! [tmp_size = %d, maxsize = %d]", tmp_size, ul_size); + tmp_size = 0; + } else { + /* Obtain data */ + e_status = FrameworkunifiedGetDataPointer(h_app, &p_rev_buf); + + if (e_status == eFrameworkunifiedStatusOK) { + *p_buf = p_rev_buf; + } else if (e_status == eFrameworkunifiedStatusInvldBufSize) { + e_status = FrameworkunifiedGetMsgDataOfSize(h_app, *p_buf, tmp_size); + + if (e_status != eFrameworkunifiedStatusOK) { + POSITIONING_LOG("FrameworkunifiedGetMsgDataOfSize ERROR [e_status:%d]", e_status); + tmp_size = 0; + } + } else { + POSITIONING_LOG("FrameworkunifiedGetDataPointer ERROR [e_status:%d]", e_status); + tmp_size = 0; + } + } + } + + return tmp_size; +} + +static void PosCreateSharedMemory(void) { + RET_API ret_api = RET_NORMAL; + void *mod_exec_dmy; /* Module data pointer(dummy) */ + int retry; /* Retry counter */ + ST_SHAREDATA *p_shm_tbl; + + /* Configure Shared Data Generating Tables */ + p_shm_tbl = g_sharedata_tbl; + + while (*(p_shm_tbl->share_data_name) != (int8)NULL) { + for (retry = 0; retry < DATMOD_RETRY; retry++) { + /* Shared Memory Generation */ + ret_api = _pb_CreateShareData(p_shm_tbl->share_data_name, p_shm_tbl->data_size, &mod_exec_dmy); + if (ret_api == RET_NORMAL) { + /* Set the shared memory status flag to "Before initialization (0)" */ + *reinterpret_cast(mod_exec_dmy) = DATMOD_PREINIT; + break; + } else { + /* Error Handling */ + POSITIONING_LOG("_pb_CreateShareData ERROR [ret_api:%d]", ret_api); + } + } + + if (retry >= DATMOD_RETRY) { + POSITIONING_LOG("_pb_CreateShareData failed more %d times.", DATMOD_RETRY); + _pb_Exit(); + /* don't arrive here. */ + } + + /* Next shared memory generation */ + p_shm_tbl++; + } + return; +} + +EFrameworkunifiedStatus PosInitialize(HANDLE h_app) { + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusOK; + RET_API ret_api; + ret_api = _pb_Setup_CWORD64_API(h_app); + if (ret_api != RET_NORMAL) { + POSITIONING_LOG("_pb_Setup_CWORD64_API ERROR!! [ret_api = %d]", ret_api); + + e_status = eFrameworkunifiedStatusFail; + } else { + PosCreateSharedMemory(); + } + return e_status; +} + +/** + * @brief + * Common processing after thread startup + * + * Thread naming, message queue creation, thread startup response + * + * @param[in] h_app Application handle + * @param[in] eTid Thread ID + * + * @return Thread startup mode + */ +EnumSetupMode_POS PosSetupThread(HANDLE h_app, EnumTID_POS e_tid) { + RET_API ret = RET_NORMAL; + ST_THREAD_CREATE_INFO* p_thread_info = g_pos_thread_create_info; + ST_THREAD_CREATE_INFO* p_info = p_thread_info + e_tid; + ST_THREAD_SETUP_INFO st_setup_info; + ST_THREAD_SETUP_INFO* pst_setup_info = &st_setup_info; + /* Application handle setting */ + _pb_SetAppHandle(h_app); + /* Create Message Queue */ + _pb_CreateMsg(p_info->p_no); + /* Get thread startup information */ + pst_setup_info->mode = EPOS_SETUP_MODE_NORMAL; + (void)PosGetMsg(h_app, reinterpret_cast(&pst_setup_info), sizeof(ST_THREAD_SETUP_INFO)); + + POSITIONING_LOG("[mode = %d]", pst_setup_info->mode); + + /* Issue thread creation completion notice */ + ret = _pb_SndMsg_Ext(POS_THREAD_NAME, + CID_THREAD_CREATE_COMP, + sizeof(EnumTID_POS), + reinterpret_cast(&e_tid), + 0); + + if (ret != RET_NORMAL) { + POSITIONING_LOG("_pb_SndMsg_Ext ERROR!! [ret = %d]", ret); + } + + return pst_setup_info->mode; +} + +/** + * @brief + * Common processing at thread stop + * + * Thread stop response, thread destruction + * + * @param[in] e_tid Thread ID + */ +void PosTeardownThread(EnumTID_POS e_tid) { + RET_API e_ret = RET_NORMAL; + ST_THREAD_CREATE_INFO* p_thread_info = g_pos_thread_create_info; + + (p_thread_info + e_tid)->uc_status = THREAD_STS_NOEXIST; + /* Issue thread stop completion notice */ + e_ret = _pb_SndMsg_Ext(POS_THREAD_NAME, + CID_THREAD_STOP_COMP, + sizeof(EnumTID_POS), + reinterpret_cast(&e_tid), + 0); + + if (e_ret != RET_NORMAL) { + POSITIONING_LOG("_pb_SndMsg_Ext ERROR!! [e_ret = %d]", e_ret); + } + + /* Thread destruction */ + _pb_ExitThread((DWORD)0); + + return; +} + +/** + * @brief + * Positioning in-process thread creation + * + * @param[in] hApp Application handle + */ +EFrameworkunifiedStatus PosCreateThread(HANDLE h_app, int8_t index) { + HANDLE thread_handle; + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusOK; + ST_THREAD_CREATE_INFO* p_thread_info = g_pos_thread_create_info; + + p_thread_info += index; + + static EnumSetupMode_POS g_setup_mode; + + if (NULL == h_app) { + return eFrameworkunifiedStatusInvldHandle; + } + + if (index < ETID_POS_MAX) { + if ((p_thread_info->uc_status == THREAD_STS_NOEXIST) && (p_thread_info->cb_routine != NULL)) { + /* Thread creation */ + thread_handle = FrameworkunifiedCreateChildThread(h_app, + (PCSTR)(p_thread_info->p_name), + p_thread_info->cb_routine, + &PosStopThreadDummy); + + if (thread_handle == NULL) { + POSITIONING_LOG("FrameworkunifiedCreateChildThread ERROR!! [tHandle=%p]", thread_handle); + } else { + e_status = FrameworkunifiedStartChildThread(h_app, + thread_handle, + sizeof(EnumSetupMode_POS), + &g_setup_mode); + + if (e_status != eFrameworkunifiedStatusOK) { + POSITIONING_LOG("FrameworkunifiedStartChildThread ERROR!! [e_status=%d, name=%s]", e_status, p_thread_info->p_name); + } else { + p_thread_info->uc_status = THREAD_STS_CREATING; + POSITIONING_LOG("name=%s\n", p_thread_info->p_name); + } + } + } + } + return e_status; +} + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/positioning_hal/src/Common/positioning_hal.cpp b/positioning_hal/src/Common/positioning_hal.cpp new file mode 100644 index 00000000..94f79c4a --- /dev/null +++ b/positioning_hal/src/Common/positioning_hal.cpp @@ -0,0 +1,49 @@ +/* + * @copyright Copyright (c) 2017-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 positioning_hal.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include + +#include "positioning_hal.h" +#include "positioning_def.h" +#include "positioning_common.h" + +/*---------------------------------------------------------------------------*/ +// Functions + +EFrameworkunifiedStatus StartGpsMainThreadPositioning(HANDLE h_app) { + return PosCreateThread(h_app, ETID_POS_GPS); +} + +EFrameworkunifiedStatus StartGpsRecvThreadPositioning(HANDLE h_app) { + return PosCreateThread(h_app, ETID_POS_GPS_RECV); +} + +EFrameworkunifiedStatus StartLineSensorThreadPositioning(HANDLE h_app) { + return PosCreateThread(h_app, ETID_POS_SENS); +} + +EFrameworkunifiedStatus StartGpsRolloverThreadPositioning(HANDLE h_app) { + return eFrameworkunifiedStatusErrOther; +} + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/positioning_hal/src/GpsCommon/MDev_GpsRecv.cpp b/positioning_hal/src/GpsCommon/MDev_GpsRecv.cpp new file mode 100644 index 00000000..13159b47 --- /dev/null +++ b/positioning_hal/src/GpsCommon/MDev_GpsRecv.cpp @@ -0,0 +1,608 @@ +/* + * @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 MDev_GpsRecv.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "MDev_GpsRecv.h" +#include "MDev_Gps_Common.h" +#include "positioning_common.h" +#include "MDev_Gps_API.h" +//extern "C" { +//#include +//} + +/*---------------------------------------------------------------------------*/ +// Global values + +#define FTEN_DRIVER 0 +#define TEST_HAL2 + +#if FTEN_DRIVER +static GpsMngApiObj g_gps_mng_obj; +#endif + +/*---------------------------------------------------------------------------*/ +// Functions + +/******************************************************************************* + * MODULE : DEV_Gps_RecvThread + * ABSTRACT : GPS communication management reception thread + * FUNCTION : GPS communication management reception thread main + * ARGUMENT : PVOID pv....Thread creation arguments + * NOTE : - + * RETURN : TRUE + ******************************************************************************/ +EFrameworkunifiedStatus DevGpsRecvThread(HANDLE h_app) { + int32 ret = GPS_RCV_RET_NML; + int32 gps_ret = 0; + BOOL* p_thread_stop_req = &g_gps_rcv_thread_stop; + + (void)PosSetupThread(h_app, ETID_POS_GPS_RECV); + + /* Initializing process */ + DevGpsRecvInit(); + +#if FTEN_DRIVER + gps_ret = GpsMngApiOpen(&g_gps_mng_obj); + if (gps_ret != GPS_CTL_RET_SUCCESS) { + printf("[For test] GpsMngApiOpen open failed\n"); + } +#endif + + while (1) { + /* Thread stop request is received */ + if (TRUE == *p_thread_stop_req) { + /* Thread stop processing */ + DevGpsRecvThreadStopProcess(); + } + + ret = DevGpsRecvReadRcvData(&g_gps_rcvdata); + + if (GPS_RCV_RET_NML == ret) { + /* For reception (normal) */ + DevGpsRecvRcvNormal(); + } + } + + return eFrameworkunifiedStatusOK; +} + +/******************************************************************************* + * MODULE : DEV_Gps_Recv_Init + * ABSTRACT : Receive thread initialization processing + * FUNCTION : Initialize the receive thread + * ARGUMENT : - + * NOTE : - + * RETURN : - + ******************************************************************************/ +void DevGpsRecvInit(void) { + /* Clears the receive buffer */ + DevGpsRecvClrRcvBuf(); + + /* Clear receive error count */ + g_wrecv_err = 0; + + return; +} + +/******************************************************************************* + * MODULE : DEV_Gps_Recv_ClrRcvBuf + * ABSTRACT : Buffer clear processing + * FUNCTION : Clears the receive buffer for serial reception. + * ARGUMENT : - + * NOTE : - + * RETURN : - + ******************************************************************************/ +void DevGpsRecvClrRcvBuf(void) { + /*-----------------------------------------------------------------------*/ + /* Clear serial receive data storage buffer */ + /*-----------------------------------------------------------------------*/ + memset(&g_gps_rcvdata, 0, sizeof(TG_GPS_RECV_RcvData)); + + /*-----------------------------------------------------------------------*/ + /* Clear receive data storage buffer */ + /*-----------------------------------------------------------------------*/ + memset(&g_gps_rcvbuf, 0, sizeof(TG_GPS_RECV_RcvBuf)); + + /*-----------------------------------------------------------------------*/ + /* Clear analysis data buffer */ + /*-----------------------------------------------------------------------*/ + memset(&g_gps_ana_databuf, 0, sizeof(TG_GPS_RECV_AnaDataBuf)); + + /*-----------------------------------------------------------------------*/ + /* Clear receive frame buffer */ + /*-----------------------------------------------------------------------*/ + memset(&g_gps_rcv_framebuf, 0, sizeof(TG_GPS_RECV_RcvFrameBuf)); +} + +/******************************************************************************* + * MODULE : DevGpsRecvJudgeStartSeqence + * ABSTRACT : Start sequence determination process + * FUNCTION : Determine whether or not the beginning of the data is the start sequence of the _CWORD82_ command + * ARGUMENT : const u_char *buf_p ... Pointer to analysis data + * u_int16 ana_size ... Size of data to be analyzed + * NOTE : + * RETURN : GPS__CWORD82__RET_START_SEQ_NONE ... No start sequence + * GPS__CWORD82__RET_START_SEQ_NMEA ... NMEA Start Sequences + * GPS__CWORD82__RET_START_SEQ_FULL ... Full Customization Start Sequence + ******************************************************************************/ +static int32 DevGpsRecvJudgeStartSeqence(const u_char *buf_p, u_int16 ana_size) { + int32 ret = GPS__CWORD82__RET_START_SEQ_NONE; + + if ((ana_size >= 1) && (buf_p[0] == 0x24u)) { + /* Ignore -> No applicable rules for MISRA-C */ /* 0x24: '$' QAC 285 */ + ret = GPS__CWORD82__RET_START_SEQ_NMEA; + } else if ((ana_size >= 1) && (buf_p[0] == 0xB0)) { + ret = GPS__CWORD82__RET_START_SEQ_FULL; + } else if ((ana_size >= 1) && (buf_p[0] == 0xC6)) { + /* #GPF_60_024 */ + ret = GPS__CWORD82__RET_START_SEQ_BIN; + } else if ((ana_size >= 1) && (buf_p[0] == 0xB5)) { + /* #GPF_60_024 */ + ret = GPS_UBLOX_RET_START_SEQ_UBX; + } else { + ret = GPS__CWORD82__RET_START_SEQ_NONE; + } + + return ret; +} + +/******************************************************************************* + * MODULE : DEV_Gps_Recv_SearchFrameData + * ABSTRACT : Frame detection processing + * FUNCTION : Find if a frame exists in the data + * ARGUMENT : TG_GPS_RECV_AnaDataBuf *adbuf_p ... Pointer to the analysis data storage buffer + * u_int16 *ana_size; ... Analysis completion data size + * NOTE : Since it is assumed that the beginning of the frame is stored in the analysis start offset, + * when the beginning of the frame is detected in the middle of the buffer, + * the processing is terminated with the data up to that point as abnormal format data. + * RETURN : GPS_RCV_FRMSRCH_ERR_FORMAT ... Frame format error + * GPS_RCV_FRMSRCH_FIXED ... Frame determination + * GPS_RCV_FRMSRCH_NOT_FIXED ... Frame undetermined + * GPS_RCV_FRMSRCH_NO_DATA ... No analysis data + ******************************************************************************/ +int32 DevGpsRecvSearchFrameData(const TG_GPS_RECV_AnaDataBuf *adbuf_p, u_int16 *ana_size) { + int32 ret = GPS_RCV_FRMSRCH_ERR_FORMAT; /* Return value(Frame format error) */ + /* ++ GPS _CWORD82_ support */ + int32 start_seq_type = GPS__CWORD82__RET_START_SEQ_NONE; /* Fully customized or NMEA */ + /* -- GPS _CWORD82_ support */ + u_int16 start_offset = 0; /* Analysis start offset */ + u_int16 i = 0; + u_int16 d_size = 0; /* Unanalyzed data size */ + + /* Offset Initialization */ + start_offset = adbuf_p->offset; /* Start of analysis */ /* Ignore -> No applicable rules for MISRA-C */ + + /* Setting of unanalyzed data size */ + d_size = adbuf_p->datsize - start_offset; + + /* For size 0 */ + if (d_size == 0) { + /* No analysis data */ + ret = GPS_RCV_FRMSRCH_NO_DATA; + /* Set the analysis completion size to 0. */ + *ana_size = 0; /* Ignore -> No applicable rules for MISRA-C */ + } else { + /* Since it is assumed that beginning of the frame is stored in the analysis start offset, */ + /* determine if the start sequence is the first one. */ + + /* ++ GPS _CWORD82_ support */ + start_seq_type = DevGpsRecvJudgeStartSeqence(&(adbuf_p->datbuf[start_offset]), d_size); + + if (start_seq_type != GPS__CWORD82__RET_START_SEQ_NONE) { + /* -- GPS _CWORD82_ support */ + /* Set the frame as undetermined */ + ret = GPS_RCV_FRMSRCH_NOT_FIXED; + + /* ++ GPS _CWORD82_ support */ + /* Find end sequence */ + if (start_seq_type == GPS__CWORD82__RET_START_SEQ_NMEA) { + for (i = 0; i < d_size; i++) { + if (adbuf_p->datbuf[(start_offset + i)] == 0x0A) { + /* If the end sequence is found, */ + /* end as frame fix. */ + ret = GPS_RCV_FRMSRCH_FIXED; + + /* Set the frame size for the analysis completion size. */ + *ana_size = i + 1; + + break; + } + } + + if (i == d_size) { + if (i >= GPS__CWORD82__CMD_LEN_MAX) { + /* If no end sequence is found, */ + /* frame format error. */ + ret = GPS_RCV_FRMSRCH_ERR_FORMAT; + /* After that, searching for start sequence. */ + } else { + /* Because the end sequence cannot be analyzed, */ + /* frame undetermined. */ + ret = GPS_RCV_FRMSRCH_NOT_FIXED; + + /* Set the size of unanalyzed data for the analysis completion size. */ + *ana_size = d_size; + } + } + } else if (start_seq_type == GPS__CWORD82__RET_START_SEQ_FULL) { + /* #GPF_60_024 */ + /* Is there one frame of data for full customization information? */ + if (d_size >= GPS__CWORD82__FULLBINARY_LEN) { + /* Is there an end sequence? */ + if (adbuf_p->datbuf[( (start_offset + GPS__CWORD82__FULLBINARY_LEN) - 1)] == 0xDA) { + /* Ignore -> MISRA-C:2004 Rule 12.1 */ + /* If an end sequence is found, */ + /* end as frame fix. */ + ret = GPS_RCV_FRMSRCH_FIXED; + + /* Set the frame size for the analysis completion size. */ + *ana_size = GPS__CWORD82__FULLBINARY_LEN; + } else { + /* If it is not an end sequence, */ + /* frame format error. */ + ret = GPS_RCV_FRMSRCH_ERR_FORMAT; + /* Searching for Start Sequence */ + } + } else { + /* Because the end sequence cannot be analyzed, */ + /* frame undetermined. */ + ret = GPS_RCV_FRMSRCH_NOT_FIXED; + + /* Set the size of unanalyzed data for the analysis completion size. */ + *ana_size = d_size; + } + } else if (start_seq_type == GPS__CWORD82__RET_START_SEQ_BIN) { + /* Is there data for one standard binary frame? */ + if (d_size >= GPS__CWORD82__NORMALBINARY_LEN) { + /* Is there an end sequence? */ + if (adbuf_p->datbuf[((start_offset + GPS__CWORD82__NORMALBINARY_LEN) - 1)] == 0xDA) { + /* Ignore -> MISRA-C:2004 Rule 12.1 */ + /* If an end sequence is found, */ + /* end as frame fix. */ + ret = GPS_RCV_FRMSRCH_FIXED; + + /* Set the frame size for the analysis completion size. */ + *ana_size = GPS__CWORD82__NORMALBINARY_LEN; + } else { + /* If it is not an end sequence, */ + /* frame format error. */ + ret = GPS_RCV_FRMSRCH_ERR_FORMAT; + /* Searching for Start Sequence */ + } + } else { + /* Because the end sequence cannot be analyzed, */ + /* frame undetermined. */ + ret = GPS_RCV_FRMSRCH_NOT_FIXED; + + /* Set the size of unanalyzed data for the analysis completion size. */ + *ana_size = d_size; + } + } else if (start_seq_type == GPS_UBLOX_RET_START_SEQ_UBX) { + /* TODO Checksum calculation using data from start_offset to d_size */ + /* TODO Check that the checksum is correct. (See test code.) */ + /* If the if checksums match, */ + /* end as frame fix. */ + ret = GPS_RCV_FRMSRCH_FIXED; + + /* Set the frame size for the analysis completion size. */ + *ana_size = d_size; + } else { + } + /* -- #GPF_60_024 */ + } else { + /* It is not a start sequence, so it is regarded as a frame format error. */ + ret = GPS_RCV_FRMSRCH_ERR_FORMAT; + + /* After that, searching for Start Sequence. */ + } + + /* If the frame format is abnormal, search for the start sequence. */ + if (ret == GPS_RCV_FRMSRCH_ERR_FORMAT) { + POSITIONING_LOG("FORMAT ERROR [start_seq_type:%d]\n", start_seq_type); + + /* Assuming that the start sequence has not been found until the end, */ + /* the size of the analysis completed is set as the size of the unanalyzed data. */ + *ana_size = d_size; + + /* ++ GPS _CWORD82_ support (Search from the second byte for safety (at least the first byte is checked at the beginning of the function)))*/ + for (i = start_offset + 1; i < (u_int32)(start_offset + d_size); i++) { + /* for( i = (start_offset + 2); i < (u_int32)(start_offset + d_size); i++ ) */ + /* -- GPS _CWORD82_ support */ + /* Start Sequence? */ + /* ++ GPS _CWORD82_ support */ + if (DevGpsRecvJudgeStartSeqence(&(adbuf_p->datbuf[i]), d_size) != GPS__CWORD82__RET_START_SEQ_NONE) { + /* if( (adbuf_p->datbuf[(i-1)] == GPS_CODE_START_SQ_HI) && */ + /* (adbuf_p->datbuf[i] == GPS_CODE_START_SQ_LOW) ) */ + /* -- GPS _CWORD82_ support */ + /* In the case of a start sequence, the analysis is completed before that. #01 */ + *ana_size = (i - start_offset - 1); + + break; + } + } + } + } + + return ret; +} + +/******************************************************************************* + * MODULE : DEV_Gps_Recv_ReadRcvData + * ABSTRACT : Data reception processing + * FUNCTION : Read serial data + * ARGUMENT : TG_GPS_RECV_RcvData* pst_rcv_data : Receive data read buffer + * NOTE : - + * RETURN : T_ErrCodes Error code + * MCSUB_NML Normal + * MCCOM_ERR_SYSTEM Abnormality + ******************************************************************************/ +int32 DevGpsRecvReadRcvData(TG_GPS_RECV_RcvData* pst_rcv_data) { + int32 ret = GPS_RCV_RET_ERR; + INT32 gps_ret = 0; + +#if FTEN_DRIVER + static int msg_kind = GPS_RCV_NMEA_GGA; + + // Serial data capture + GpsMngApiDat gps_mng_data; + + memset(&gps_mng_data, 0, sizeof(gps_mng_data)); +#ifdef TEST_HAL2 + if (msg_kind > GPS_RCV_NMEA_RMC) { + msg_kind = GPS_RCV_NMEA_GGA; + } +#else + if (msg_kind == GPS_RCV_NMEA_GGA) { + msg_kind = GPS_RCV_NMEA_RMC; + } else if (msg_kind == GPS_RCV_NMEA_RMC) { + msg_kind = GPS_RCV_NMEA_GGA; + } else { + msg_kind = GPS_RCV_NMEA_GGA; + } +#endif + + gps_ret = GpsMngApiGetRcvMsg(&g_gps_mng_obj, msg_kind, &gps_mng_data); + +#ifdef TEST_HAL2 + msg_kind++; +#endif + + pst_rcv_data->us_read_size = (u_int16)gps_mng_data.dataLength; + + if (pst_rcv_data->us_read_size >= GPS_RCV_MAXREADSIZE) { + return GPS_RCV_RET_ERR_SYSTEM; + } + + memcpy(pst_rcv_data->uc_read_data, gps_mng_data.data, pst_rcv_data->us_read_size); + + // Add '\0' + pst_rcv_data->uc_read_data[pst_rcv_data->us_read_size] = '\0'; + + if (GPS_CTL_RET_SUCCESS != gps_ret) { + ret = GPS_RCV_RET_ERR_SYSTEM; + } else { + ret = GPS_RCV_RET_NML; + } +#endif + + return ret; +} + +/******************************************************************************* + * MODULE : DEV_Gps_Recv_RcvNormal + * ABSTRACT : Receive (normal) Receive processing + * FUNCTION : Receive (normal) Processing at event reception + * ARGUMENT : - + * NOTE : + * RETURN : - + ******************************************************************************/ +void DevGpsRecvRcvNormal(void) { + TG_GPS_RECV_RcvData* pst_rcv_data = NULL; /* Buffer for reading serial data */ + TG_GPS_RECV_RcvBuf* pst_rcv_buf = NULL; /* Receive data storage buffer */ + TG_GPS_RECV_AnaDataBuf* pst_ana_data_buf = NULL; /* Analysis data storage buffer */ + TG_GPS_RECV_RcvFrameBuf* pst_rcv_frame_buf = NULL; /* Receive frame buffer */ + int32 i_ret = 0; /* Frame Detection Result */ + u_int16 ana_size = 0; /* Data analysis size storage */ + + /* Initializing process */ + pst_ana_data_buf = &g_gps_ana_databuf; + + /* Fast _CWORD71_ processing(memset fix) */ + /* Initializes the offset that needs initialization in the receive data storage buffer. */ + pst_ana_data_buf->offset = 0; + + /* Serial continuous reception data analysis processing */ + pst_rcv_data = &g_gps_rcvdata; + pst_rcv_buf = &g_gps_rcvbuf; + memcpy(&(pst_ana_data_buf->datbuf[0]), &(pst_rcv_buf->r_buf[0]), pst_rcv_buf->r_size); + memcpy(&(pst_ana_data_buf->datbuf[pst_rcv_buf->r_size]), + &(pst_rcv_data->uc_read_data[0]), + pst_rcv_data->us_read_size); + + pst_ana_data_buf->datsize = pst_rcv_buf->r_size + pst_rcv_data->us_read_size; + + /* Serial receive data analysis process */ + do { + /* Frame detection processing */ + i_ret = DevGpsRecvSearchFrameData(pst_ana_data_buf, &ana_size); + + /* For frame determination */ + if (i_ret == GPS_RCV_FRMSRCH_FIXED) { + /* Frames are stored in the receive frame buffer. */ + pst_rcv_frame_buf = &g_gps_rcv_framebuf; + memset(pst_rcv_frame_buf, 0, sizeof(TG_GPS_RECV_RcvFrameBuf)); /* Ignore -> No applicable rules for MISRA-C */ + /* Ignore -> No applicable rules for MISRA-C */ /* QAC 3200 */ + memcpy(& (pst_rcv_frame_buf->buf[0]), + &(pst_ana_data_buf->datbuf[pst_ana_data_buf->offset]), + ana_size); + pst_rcv_frame_buf->size = ana_size; + + /* Reception of the command */ + /* Send receipt notice (command) to main thread */ + /* Send Received Data to Main Thread */ + DevGpsRecvSndRcvData(pst_rcv_frame_buf); + } else if (i_ret == GPS_RCV_FRMSRCH_NOT_FIXED) { + /* Store unconfirmed data in the received data storage buffer, */ + /* and leave no unanalyzed data. */ + memset(pst_rcv_buf, 0, sizeof(TG_GPS_RECV_RcvBuf)); /* Ignore -> No applicable rules for MISRA-C */ + memcpy(& (pst_rcv_buf->r_buf[0]), + &(pst_ana_data_buf->datbuf[pst_ana_data_buf->offset]), + ana_size); + pst_rcv_buf->r_size = ana_size; + i_ret = GPS_RCV_FRMSRCH_NO_DATA; + } else if (i_ret == GPS_RCV_FRMSRCH_ERR_FORMAT) { + /* [Measures against resetting with 1S + _CWORD82_]From here */ + /* Clears the unanalyzed data stored in the receive data storage buffer, */ + /* and flag is set to "No Unparsed Data" so that data can be parsed from the beginning of the next. */ + memset(&(pst_rcv_buf->r_buf[0]), 0, pst_rcv_buf->r_size); + pst_rcv_buf->r_size = 0; + i_ret = GPS_RCV_FRMSRCH_NO_DATA; + /* [Measures against resetting with 1S + _CWORD82_]Up to this point */ + /* Since this section discards garbage data, */ + /* not subject to diagnosis registration. */ + /* Diagnosis registration check */ + } else if (i_ret == GPS_RCV_FRMSRCH_NO_DATA) { + /* Ignore -> No applicable rules for MISRA-C */ + memset(pst_rcv_buf, 0, sizeof(TG_GPS_RECV_RcvBuf)); + /* Termination */ + } else { + /* No unanalyzed data is assumed because it is impossible. */ + i_ret = GPS_RCV_FRMSRCH_NO_DATA; + + /* Ignore -> No applicable rules for MISRA-C */ + memset(pst_rcv_buf, 0, sizeof(TG_GPS_RECV_RcvBuf)); + } + + /* Update analysis data offset */ + pst_ana_data_buf->offset += ana_size; + + /* Repeat until no unanalyzed data is found. */ + } while (i_ret != GPS_RCV_FRMSRCH_NO_DATA); +} + +/******************************************************************************** + * MODULE : DEV_RcvDATA + * ABSTRACT : Acknowledgement + * FUNCTION : Send message notifications to the communication management thread + * ARGUMENT : TG_GPS_RCV_DATA *ptg_rcv_data....I/F information between the receiving thread + * and the communication management thread + * NOTE : + * RETURN : RET_API :RET_NORMAL:Normal completion + * :RET_ERROR:ABENDs + ********************************************************************************/ +RET_API DevRcvData(const TG_GPS_RCV_DATA* ptg_rcv_data) { + RET_API ret = RET_NORMAL; /* Return value */ + u_int16 w_size = 0; /* Data length setting */ + u_int16 w_all_len = 0; /* Sent message length */ + u_int16 w_mode = 0; /* Mode information */ + RID req_id = 0; /* Resources ID */ + T_APIMSG_MSGBUF_HEADER tg_header; /* Message header */ + + // Initialzie struct + memset(&tg_header, 0, sizeof(tg_header)); + + /* Transmitting buffer */ + u_int8 by_snd_buf[(sizeof(T_APIMSG_MSGBUF_HEADER) + sizeof(TG_GPS_RCV_DATA))]; + + /* Calculation of transmission data length */ + w_size = ptg_rcv_data->bydata_len + sizeof(ptg_rcv_data->dwret_status) + sizeof(ptg_rcv_data->bydata_len); + + /* <>>> */ + tg_header.signo = 0; /* Signal information setting */ + tg_header.hdr.sndpno = PNO_NAVI_GPS_RCV; /* Source thread No. setting */ + tg_header.hdr.respno = 0; /* Destination process No. */ + tg_header.hdr.cid = CID_GPS_RECVDATA; /* Command ID setting */ + tg_header.hdr.msgbodysize = w_size; /* Message data length setting */ + tg_header.hdr.rid = req_id; /* Resource ID Setting */ + tg_header.hdr.reserve = 0; /* Reserved area clear */ + + memcpy(&by_snd_buf[ 0 ], &tg_header, sizeof(T_APIMSG_MSGBUF_HEADER)); + /* <> */ + /* Copy data to send buffer */ + memcpy(&by_snd_buf[ sizeof(T_APIMSG_MSGBUF_HEADER)], ptg_rcv_data, w_size); + + /* <> */ + /* Calculation of Sent Message Length */ + w_all_len = w_size + sizeof(T_APIMSG_MSGBUF_HEADER); + + /* Mode information(Reserved) */ + w_mode = 0; + + /* Message transmission request */ + ret = _pb_SndMsg(PNO_NAVI_GPS_MAIN, w_all_len, reinterpret_cast(by_snd_buf), w_mode); + + /* End of the process */ + return ret; +} + +/******************************************************************************** + * MODULE : DEV_Gps_Recv_SndRcvData + * ABSTRACT : Receive data transmission processing + * FUNCTION : Transmit received data + * ARGUMENT : TG_GPS_RECV_RcvFrameBuf *frame_baf Receive frame buffer pointer + * NOTE : Fetches a command from the receive frame buffer and + * issue received data notifications to the main thread + * RETURN : None + ********************************************************************************/ +void DevGpsRecvSndRcvData(const TG_GPS_RECV_RcvFrameBuf* p_frame_buf) { + TG_GPS_RCV_DATA tg_rcv_data; + u_int16 w_cmd_len = 0; + + // Initialzie struct + memset(&tg_rcv_data, 0, sizeof(tg_rcv_data)); + + if (p_frame_buf != NULL) { + w_cmd_len = p_frame_buf->size; + + if (w_cmd_len <= GPS_READ_LEN) { + /* Ignore -> No applicable rules for MISRA-C */ + memset(&tg_rcv_data, 0, sizeof(TG_GPS_RCV_DATA)); + + /* Status Settings */ + tg_rcv_data.dwret_status = GPS_RECVOK; + + /* Command length setting */ + tg_rcv_data.bydata_len = w_cmd_len; + + /* Receive data setting */ + /* Sending from the Beginning of the Sirf Binary #03 */ + memcpy(&tg_rcv_data.bygps_data[0], &p_frame_buf->buf[0], w_cmd_len); + + /* Issuance of reception notice */ + DevRcvData(&tg_rcv_data); /* Ignore -> No applicable rules for MISRA-C */ + } + } +} + +/** + * @brief + * Pos_Gps_Recv Thread Stop Processing + */ +void DevGpsRecvThreadStopProcess(void) { +#if FTEN_DRIVER + GpsMngApiClose(&g_gps_mng_obj); +#endif + PosTeardownThread(ETID_POS_GPS_RECV); + return; +} + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/positioning_hal/src/GpsCommon/MDev_GpsRollOver.cpp b/positioning_hal/src/GpsCommon/MDev_GpsRollOver.cpp new file mode 100644 index 00000000..62227c26 --- /dev/null +++ b/positioning_hal/src/GpsCommon/MDev_GpsRollOver.cpp @@ -0,0 +1,275 @@ +/* + * @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 MDev_GpsRollOver.cpp +*/ + +#include "MDev_GpsRollOver.h" + +#define TIM_ROLOVR_CALCORCNT_DAYS (1024 * 7) /*1024 weeks * 7 days */ + +#define TMT_OK (1) /* Definition indicating normal status */ +#define TMT_NG (0) /* Definitions indicating abnormal status */ +#define TMT_TRUE (1) /* Definitions indicating the status for which the condition is true */ +#define TMT_FALSE (0) /* Definitions indicating a status in which the condition is false */ + +#define TIM_ROLOVR_LEAPYEARDAYS (366) /*Number of days in the leap year */ +#define TIM_ROLOVR_NOTLEAPYEARDAYS (365) /*Year days in non-leap years */ + +static const WORD kMonth[2][12] = { + {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}, /* Number of days per month(For non-leap year) */ + {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31} /* Number of days per month(For leap years) */ +}; +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * FUNCTION : ChkLeapYear + * Function : Leap year determination processing + * Feature Overview : Determine whether it is a leap year + * Input : u_int16 year + * Output : None + * Return value : u_int32 + TMT_TRUE Leap year + TMT_FALSE Non-leap year + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +static u_int32 ChkLeapYear(u_int16 year) { + int32 leap_year; + + /* Leap year determination processing (select subscripts in the [LEAP YEAR] table) *The processing that matches the time correction processing after time difference value addition/subtraction is used. */ + leap_year = TMT_FALSE; + if ((year % 4) == 0) { + leap_year = TMT_TRUE; + } + if ((year % 100) == 0) { + leap_year = TMT_FALSE; + } + + if ((year % 400) == 0) { + leap_year = TMT_TRUE; + } + + return (leap_year); +} + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * FUNCTION : RollOverFormerLaterDays + * Function : Calculation of the number of days before and after + * Feature Overview : The number of days from the beginning of the month to the previous day of the specified date, + and calculates the number of days after a given date, up to the end of the month + * Input : TG_TIM_ROLOVR_YMD* base_ymd Reference date + * Output : u_int32* former_days Number of days before (number of days before the specified date from the beginning of the month) + u_int32* later_days Number of days after(Number of days following the specified date until the end of the month) + * Return value : None + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +static void RollOverFormerLaterDays(TG_TIM_ROLOVR_YMD* base_ymd, u_int32* former_days, u_int32* later_days) { + int32 leap_year; + + leap_year = ChkLeapYear(base_ymd->year); + + *former_days = base_ymd->day - 1; + + *later_days = kMonth[leap_year][base_ymd->month - 1] - base_ymd->day; +} + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * FUNCTION : RollOverCalDaysWithinBY + * Function : Calculation of the difference in the number of days in the base year + * Feature Overview : Calculate the difference in the number of days between the base date and the conversion target date. + [Restriction]If the base date > conversion target date, the number of days difference is 0. + * Input : TG_TIM_ROLOVR_YMD* base_ymd Reference date + TG_TIM_ROLOVR_YMD* conv_ymd Conversion Date + * Output : None + * Return value : u_int32 Difference in the number of days(positive only) + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +static u_int32 RollOverCalDaysWithinBY(TG_TIM_ROLOVR_YMD* base_ymd, TG_TIM_ROLOVR_YMD* conv_ymd) { + int32 leap_year; /* Subscripts in the [LEAP YEAR] table */ + u_int32 days; /* Day Difference Calculation Result */ + u_int32 l_days; /* Number of days after the reference month */ + u_int32 f_days; /* Number of days before the conversion target month */ + u_int32 dmy_days; /* Dummy days(For storing unnecessary number of days in this function) */ + u_int32 m_days; /* Sum of the number of days in the month between the base month and the month to be converted */ + int32 cnt; /* A counter that sums the number of days in a month between the base month and the month to be converted */ + u_int16 diff_month; /* Difference between the conversion target month and the base month */ + u_int16 chk_month; /* Number of days of month to be acquired */ + + days = 0; + + if (base_ymd->month == conv_ymd->month) { + if (base_ymd->day <= conv_ymd->day) { + days = conv_ymd->day - base_ymd->day; + } else { + days = 0; /* Set the difference to 0(Negative difference in number of days is not calculated.) */ + } + } else if (base_ymd->month < conv_ymd->month) { + RollOverFormerLaterDays(base_ymd, &dmy_days, &l_days); + + m_days = 0; + diff_month = conv_ymd->month - base_ymd->month; + + leap_year = ChkLeapYear(base_ymd->year); + + /* Calculate the sum of the number of days in the month between the base month and the conversion target month B */ + chk_month = base_ymd->month + 1; + for (cnt = 0; cnt < (diff_month - 1); cnt++) { + m_days += kMonth[leap_year][chk_month - 1]; + chk_month++; + } + + RollOverFormerLaterDays(conv_ymd, &f_days, &dmy_days); + + days = l_days + m_days + f_days + 1; /* Calculate the difference in number of days as A+B+C+1 */ + + } else { + days = 0; /* Set the difference to 0(Negative difference in number of days is not calculated.) */ + } + + return days; +} + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * FUNCTION : RollOverGetAYearDays + * Function : Process of obtaining the number of days per year + * Feature Overview : Get the number of days in a given year + * Input : u_int16 base_year Year + * Output : None + * Return value : u_int32 Number of days + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +static u_int32 RollOverGetAYearDays(u_int16 base_year) { + int32 leap_year; /* Subscripts in the [LEAP YEAR] table */ + u_int32 days; + + leap_year = ChkLeapYear(base_year); /* Leap year determination processing */ + + if (leap_year == TMT_TRUE) { + days = TIM_ROLOVR_LEAPYEARDAYS; + } else { + days = TIM_ROLOVR_NOTLEAPYEARDAYS; + } + + return days; +} + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * FUNCTION : RollOverConvYMDWithoutBY + * Function : Date conversion processing outside the base year, month, and day + * Feature Overview : Calculate the date shifted by the specified number of days from the specified date. + *If less than (1024 weeks x 7 days) is specified as the number of days difference, the calculation result is + returns the specified date(No correction) + * Input : TG_TIM_ROLOVR_YMD* base_ymd Reference date + u_int32 days Difference in the number of days + u_int32 days_by Number of days after + number of days after month + * Output : TG_TIM_ROLOVR_YMD* conv_ymd Date of the conversion result + * Return value : None + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +static void RollOverConvYMDWithoutBY(TG_TIM_ROLOVR_YMD* base_ymd, u_int32 days, + u_int32 days_by, TG_TIM_ROLOVR_YMD* conv_ymd) { + int32 leap_year; /* Subscripts in the [LEAP YEAR] table */ + u_int32 rest_days; /* Remaining number of days */ + u_int16 cal_year; /* For year calculation */ + u_int16 cal_month; /* For monthly calculation */ + u_int32 cal_days; /* To calculate the number of days */ + + rest_days = days - days_by; /* Remaining number of days is different from the number of initialization days.-(Number of days after + Number of days after month) */ + cal_year = base_ymd->year + 1; /* The year is set to the year following the year of the initialization reference date. */ /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ + + cal_days = RollOverGetAYearDays(cal_year); /* Process of obtaining the number of days per year */ + + while (rest_days > cal_days) { + rest_days -= cal_days; /* Remaining Days = Remaining Days-Updated as Days of Year */ + + cal_year++; /* Increment Year */ + + cal_days = RollOverGetAYearDays(cal_year); /* Process of obtaining the number of days per year */ + } + + /* Year Finalization */ + conv_ymd->year = cal_year; /* Year Calculated */ + + cal_month = 1; /* Initialize Month January */ + + leap_year = ChkLeapYear(conv_ymd->year); /* Leap year determination processing */ + + cal_days = kMonth[leap_year][cal_month - 1]; /* Acquisition processing of the number of days/month */ /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ + + while (rest_days > cal_days) { + rest_days -= cal_days; /* Remaining Days = Remaining Days-Updated as Days of Month */ + + cal_month++; /* Increment month */ + + cal_days = kMonth[leap_year][cal_month - 1]; /* Acquisition processing of the number of days/month */ /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ + } + + /* Fix month */ + conv_ymd->month = cal_month; /* Month Calculated */ + + /* Date set */ + conv_ymd->day = (u_int16)rest_days; /* Day calculated Remaining number of days */ +} + + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * FUNCTION : RollOverConvYMD + * Function : Date conversion processing + * Feature Overview : Calculate the date shifted by the specified number of days from the specified date. + *If the base date is shifted by the number of days but the year is the same as the base date, the result returns the base date + * Input : TG_TIM_ROLOVR_YMD* base_ymd Reference date + u_int32 days Difference in the number of days + * Output : TG_TIM_ROLOVR_YMD* conv_ymd Date of the conversion result + * Return value : None + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +static void RollOverConvYMD(TG_TIM_ROLOVR_YMD* base_ymd, u_int32 days, TG_TIM_ROLOVR_YMD* conv_ymd) { + u_int32 days_by; /* Number of days after + number of days after month(=difference in number of days from the base date to the end of the year) */ + TG_TIM_ROLOVR_YMD tmp_ymd; + + + /* (number of days after + number of days after)Set calculation date( 12/31 ) */ + tmp_ymd.year = base_ymd->year; + tmp_ymd.month = 12; + tmp_ymd.day = 31; + + days_by = RollOverCalDaysWithinBY(base_ymd, &tmp_ymd); /* Calculation of the difference in the number of days in the base year */ + + if (days_by >= days) { + memcpy(conv_ymd, base_ymd, sizeof(TG_TIM_ROLOVR_YMD)); /* Returns the base date regardless of the number of days difference */ + + } else { + RollOverConvYMDWithoutBY(base_ymd, days, days_by, conv_ymd); + } +} + + + +/** + * @brief + * Conversion to a time that takes the GPS week correction counter into account + * + * Converting the GPS Week Correction Counter to a Considered Time + * + * @param[in] TG_TIM_ROLOVR_YMD* base_ymd Reference date + * @param[in] u_int8 gpsweekcorcnt GPS weekly correction counter + * @param[out] TG_TIM_ROLOVR_YMD* conv_ymd Correction Date + * @return none + * @retval none + */ +void GPSRollOverConvTime(TG_TIM_ROLOVR_YMD* base_ymd, TG_TIM_ROLOVR_YMD* conv_ymd, u_int8 gpsweekcorcnt) { + u_int32 days; /* Difference in the number of days */ + + days = TIM_ROLOVR_CALCORCNT_DAYS * gpsweekcorcnt; /* (1024 weeks x 7 days) x GPS week correction counter to calculate the difference in the number of days */ + + RollOverConvYMD(base_ymd, days, conv_ymd); /* Date conversion processing */ +} diff --git a/positioning_hal/src/GpsCommon/MDev_Gps_API.cpp b/positioning_hal/src/GpsCommon/MDev_Gps_API.cpp new file mode 100644 index 00000000..35f52016 --- /dev/null +++ b/positioning_hal/src/GpsCommon/MDev_Gps_API.cpp @@ -0,0 +1,491 @@ +/* + * @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 MDev_Gps_API.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "MDev_Gps_API.h" +#include "positioning_hal.h" +#include "positioning_def.h" +#include "MDev_Gps_Nmea.h" + +/*---------------------------------------------------------------------------*/ +// Functions + +/****************************************************************************** +@brief SendNmeaGps
+ NMEA transmission process +@outline Send NMEA to VehicleSensor Thread +@param[in] TG_GPS_NMEA* : pstNmeaData ... NMEA data +@param[out] none +@return RET_API +@retval RET_NORMAL : Normal completion +@retval RET_ERROR : ABENDs +*******************************************************************************/ +RET_API SendNmeaGps(const MDEV_GPS_NMEA* p_nmea_data) { + MDEV_GPS_RAWDATA_NMEA_MSG s_send_msg; + SENSOR_MSG_GPSDATA s_msg_buf; + RET_API ret = RET_NORMAL; + PNO u_pno = PNO_VEHICLE_SENSOR; + + /* Create GPS Data Notification Message */ + (void)memset(&s_send_msg, 0x00, sizeof(s_send_msg)); + + /* Message header */ + s_send_msg.h_dr.hdr.sndpno = PNO_NAVI_GPS_MAIN; + s_send_msg.h_dr.hdr.respno = 0x0000; + s_send_msg.h_dr.hdr.cid = CID_GPS_DATA; + s_send_msg.h_dr.hdr.msgbodysize = sizeof(MDEV_GPS_RAWDATA_NMEA); + s_send_msg.h_dr.hdr.rid = 0x00; + + /* Message data */ + s_send_msg.st_data.e_kind = MDEV_GPS_DATA_RAWDATA_NMEA; + (void)memcpy(&(s_send_msg.st_data.st_nmea_data), p_nmea_data, sizeof(s_send_msg.st_data.st_nmea_data)); + + /* Create message buffer */ + (void)memset(&s_msg_buf, 0, sizeof(s_msg_buf)); + (void)memcpy(&(s_msg_buf.st_head.hdr), &(s_send_msg.h_dr.hdr), sizeof(T_APIMSG_HEADER)); + + s_msg_buf.st_para.ul_did = POS_DID_GPS_NMEA; + s_msg_buf.st_para.us_size = GPS_NMEA_SZ; + + (void)memcpy(&(s_msg_buf.st_para.uc_data), &(s_send_msg.st_data.st_nmea_data), s_msg_buf.st_para.us_size); + + ret = _pb_SndMsg(u_pno, sizeof(s_msg_buf), &s_msg_buf, 0); + + if (ret != RET_NORMAL) { + POSITIONING_LOG("_pb_SndMsg ERROR!! [ret=%d]\n", ret); + ret = RET_ERROR; + } + + return ret; +} + +/** + * @brief + * GPS processing data transmission process + * + * @param[in] pstGpsTime SENSOR_MSG_GPSTIME - GPS time information + * @param[in] p_lonlat SENSORLOCATION_LONLATINFO - Latitude and longitude information + * @param[in] p_altitude SENSOR_LOCATION_ALTITUDEINFO - Altitude information + * @param[in] p_heading SENSORMOTION_HEADINGINFO_DAT - Bearing information + * + * @return RET_NORMAL Normal completion + * RET_ERROR ABENDs + */ +RET_API SendCustomGps(const SENSOR_MSG_GPSTIME* p_gps_time, + const SENSORLOCATION_LONLATINFO_DAT* p_lonlat, + const SENSORLOCATION_ALTITUDEINFO_DAT* p_altitude, + const SENSORMOTION_HEADINGINFO_DAT* p_heading, + const NAVIINFO_DIAG_GPS* p_diag_data) { + SENSOR_MSG_GPSDATA s_send_msg = {0}; + MDEV_GPS_CUSTOMDATA* p_custom_data = NULL; + RET_API ret = RET_NORMAL; + PNO u_pno = PNO_VEHICLE_SENSOR; + + /** Create GPS Data Notification Message */ + /* Fast _CWORD71_ processing(memset fix) */ + /* Initializes an area whose value is undefined in the message buffer. */ + (void)memset(&s_send_msg.st_head, 0x00, sizeof(s_send_msg.st_head)); + + /** Message header */ + s_send_msg.st_head.hdr.sndpno = PNO_NAVI_GPS_MAIN; + s_send_msg.st_head.hdr.respno = 0x0000; + s_send_msg.st_head.hdr.cid = CID_GPS_DATA; + s_send_msg.st_head.hdr.msgbodysize = sizeof(MDEV_GPS_CUSTOMDATA); + s_send_msg.st_head.hdr.rid = 0x00; + + /** Message data */ + s_send_msg.st_para.ul_did = VEHICLE_DID_GPS_CUSTOMDATA; + s_send_msg.st_para.us_size = sizeof(MDEV_GPS_CUSTOMDATA); + p_custom_data = reinterpret_cast(&(s_send_msg.st_para.uc_data)); + + (void)memcpy(&(p_custom_data->st_lonlat), p_lonlat, sizeof(SENSORLOCATION_LONLATINFO_DAT)); + (void)memcpy(&(p_custom_data->st_altitude), p_altitude, sizeof(SENSORLOCATION_ALTITUDEINFO_DAT)); + (void)memcpy(&(p_custom_data->st_heading), p_heading, sizeof(SENSORMOTION_HEADINGINFO_DAT)); + (void)memcpy(&(p_custom_data->st_diag_gps), p_diag_data, sizeof(NAVIINFO_DIAG_GPS)); + (void)memcpy(&(p_custom_data->st_gps_time), p_gps_time, sizeof(SENSOR_MSG_GPSTIME)); + /* Messaging */ + ret = _pb_SndMsg(u_pno, sizeof(s_send_msg), reinterpret_cast(&s_send_msg), 0); + + if (ret != RET_NORMAL) { + ret = RET_ERROR; + } + + return ret; +} + +/****************************************************************************** +@brief SendSpeedGps
+ Rate information transmission processing +@outline Sending speed information to vehicle sensor thread +@param[in] SENSORMOTION_SPEEDINFO_DAT* : p_seed ... Velocity information +@param[in] u_int16 : us_peed ... Vehicle speed(km/h) +@param[out] none +@return RET_API +@retval RET_NORMAL : Normal completion +@retval RET_ERROR : ABENDs +*******************************************************************************/ +RET_API SendSpeedGps(const SENSORMOTION_SPEEDINFO_DAT* p_seed, u_int16 us_peed) { + MDEV_GPS_NAVISPEED_MSG s_send_msg; + RET_API ret = RET_NORMAL; + PNO u_pno = PNO_VEHICLE_SENSOR; + + /** Create GPS Data Notification Message */ + (void)memset(&s_send_msg, 0x00, sizeof(s_send_msg)); + + /** Message header */ + s_send_msg.h_dr.hdr.sndpno = PNO_NAVI_GPS_MAIN; + s_send_msg.h_dr.hdr.respno = 0x0000; + s_send_msg.h_dr.hdr.cid = CID_GPS_DATA; + s_send_msg.h_dr.hdr.msgbodysize = sizeof(MDEV_GPS_NAVISPEED); + s_send_msg.h_dr.hdr.rid = 0x00; + + /** Message data */ + s_send_msg.st_data.e_kind = MDEV_GPS_DATA_NAVISPEED; + s_send_msg.st_data.us_speed_kmph = us_peed; + + (void)memcpy( &s_send_msg.st_data.st_speed, p_seed, sizeof(s_send_msg.st_data.st_speed) ); + + /* Messaging */ + ret = _pb_SndMsg(u_pno, (sizeof(T_APIMSG_MSGBUF_HEADER) + sizeof(MDEV_GPS_NAVISPEED)), &s_send_msg, 0); + + if (ret != RET_NORMAL) { + ret = RET_ERROR; + } + + return ret; +} + +/* ++ #GPF_60_103 */ +/****************************************************************************** +@brief SendTimeGps
+ Time information transmission processing +@outline Send GPS time information to vehicle sensor thread +@param[in] MDEV_GPS_RTC* : p_rtc ... GPS time information +@param[out] none +@return RET_API +@retval RET_NORMAL : Normal completion +@retval RET_ERROR : ABENDs +*******************************************************************************/ +RET_API SendTimeGps(const MDEV_GPS_RTC* p_rtc) { + MDEV_GPS_GPSTIME_MGS s_send_msg; + RET_API ret = RET_NORMAL; + PNO u_pno = PNO_VEHICLE_SENSOR; + + /** Create GPS data notification message */ + (void)memset(&s_send_msg, 0x00, sizeof(s_send_msg)); + + /** Message header */ + s_send_msg.h_dr.hdr.sndpno = PNO_NAVI_GPS_MAIN; + s_send_msg.h_dr.hdr.respno = 0x0000; + s_send_msg.h_dr.hdr.cid = CID_GPS_DATA; + s_send_msg.h_dr.hdr.msgbodysize = sizeof(MDEV_GPS_GPSTIME); + s_send_msg.h_dr.hdr.rid = 0x00; + + /** Message data */ + s_send_msg.st_data.e_kind = MDEV_GPS_DATA_GPSTIME; + + (void)memcpy(&s_send_msg.st_data.st_rtc_data, p_rtc, sizeof(s_send_msg.st_data.st_rtc_data)); + + /* Messaging */ + ret = _pb_SndMsg(u_pno, (sizeof(T_APIMSG_MSGBUF_HEADER) + sizeof(MDEV_GPS_GPSTIME)), &s_send_msg, 0); + + if (ret != RET_NORMAL) { + ret = RET_ERROR; + } + + return ret; +} + +/** + * @brief + * GPS clock drift transmit process + * + * @param[in] drift Clock drift + * + * @return RET_NORMAL Normal completion + * @return RET_ERROR ABENDs + */ +RET_API SendClockDriftGps(int32_t drift) { + SENSOR_MSG_GPSDATA s_send_msg; + RET_API ret = RET_NORMAL; + PNO u_pno = PNO_VEHICLE_SENSOR; + + /** Create GPS Data Notification Message */ + (void)memset(&s_send_msg, 0x00, sizeof(s_send_msg)); + + /** Message header */ + s_send_msg.st_head.hdr.sndpno = PNO_NAVI_GPS_MAIN; + s_send_msg.st_head.hdr.respno = 0x0000; + s_send_msg.st_head.hdr.cid = CID_GPS_DATA; + s_send_msg.st_head.hdr.msgbodysize = sizeof(SENSOR_MSG_GPSDATA_DAT); + s_send_msg.st_head.hdr.rid = 0x00; + + /** Message data */ + s_send_msg.st_para.ul_did = VEHICLE_DID_GPS_CLOCK_DRIFT; + s_send_msg.st_para.us_size = sizeof(int32_t); + + (void)memcpy(&(s_send_msg.st_para.uc_data), &drift, s_send_msg.st_para.us_size); + + /* Messaging */ + ret = _pb_SndMsg(u_pno, sizeof(s_send_msg), &s_send_msg, 0); + + if (ret != RET_NORMAL) { + ret = RET_ERROR; + } + + return ret; +} + +/** + * @brief + * GPS clock-frequency transmit process + * + * @param[in] Freq Clocking frequencies + * + * @return RET_NORMAL Normal completion + * @return RET_ERROR ABENDs + */ +RET_API SendClockFrequencyGps(uint32_t freq) { + SENSOR_MSG_GPSDATA s_send_msg; + RET_API ret = RET_NORMAL; + PNO u_pno = PNO_VEHICLE_SENSOR; + + /** Create GPS Data Notification Message */ + (void)memset(&s_send_msg, 0x00, sizeof(s_send_msg)); + + /** Message header */ + s_send_msg.st_head.hdr.sndpno = PNO_NAVI_GPS_MAIN; + s_send_msg.st_head.hdr.respno = 0x0000; + s_send_msg.st_head.hdr.cid = CID_GPS_DATA; + s_send_msg.st_head.hdr.msgbodysize = sizeof(SENSOR_MSG_GPSDATA_DAT); + s_send_msg.st_head.hdr.rid = 0x00; + + /** Message data */ + s_send_msg.st_para.ul_did = VEHICLE_DID_GPS_CLOCK_FREQ; + s_send_msg.st_para.us_size = sizeof(uint32_t); + + (void)memcpy(&(s_send_msg.st_para.uc_data), &freq, s_send_msg.st_para.us_size); + + /* Messaging */ + ret = _pb_SndMsg(u_pno, sizeof(s_send_msg), &s_send_msg, 0); + + if (ret != RET_NORMAL) { + ret = RET_ERROR; + } + + return(ret); +} + +/** + * @brief + * GPS rollover standard week number transmission processing + * + * @param[in] *p_week_rollover GPS rollover base week number + * + * @return RET_NORMAL Normal completion + * @return RET_ERROR ABENDs + */ +RET_API DevGpsSndWknRollover(const SensorWknRollOverHal* p_week_rollover) { + SENSOR_MSG_GPSDATA s_send_msg; + RET_API ret = RET_NORMAL; + PNO u_pno = PNO_VEHICLE_SENSOR; + + /** Create GPS Data Notification Message */ + (void)memset( &s_send_msg, 0x00, sizeof(s_send_msg) ); + + /** Message header */ + s_send_msg.st_head.hdr.sndpno = PNO_NAVI_GPS_MAIN; + s_send_msg.st_head.hdr.respno = 0x0000; + s_send_msg.st_head.hdr.cid = CID_GPS_DATA; + s_send_msg.st_head.hdr.msgbodysize = sizeof(SensorWknRollOverHal); + s_send_msg.st_head.hdr.rid = 0x00; + + /** Message data */ + s_send_msg.st_para.ul_did = VEHICLE_DID_GPS_WKNROLLOVER; + s_send_msg.st_para.us_size = sizeof(SensorWknRollOverHal); + + (void)memcpy(&(s_send_msg.st_para.uc_data), p_week_rollover, s_send_msg.st_para.us_size); + + /* Messaging */ + ret = _pb_SndMsg(u_pno, sizeof(s_send_msg), &s_send_msg, 0); + + if (ret != RET_NORMAL) { + ret = RET_ERROR; + } + + return ret; +} + +/****************************************************************************** +@brief DevGpsRstAnsSend
+ Reset response issuance processing +@outline Issue a reset response +@param[in] PNO : u_pno ... Notify-To Process No. +@param[in] RID : uc_rid ... Response resource ID +@param[in] u_int32 : ul_rst_sts ... Response result +@param[out] none +@return int32 +@retval RET_NORMAL : Normal +@retval RET_ERROR : Abnormality +*******************************************************************************/ +int32 DevGpsRstAnsSend(PNO u_pno, RID uc_rid, u_int32 ul_rst_sts) { + TG_GPS_RET_RESET_MSG s_send_msg; + RET_API ret = RET_NORMAL; + PCSTR l_thread_name; + + if (u_pno != PNO_NONE) { + /** Create GPS Reset Notification Message */ + (void)memset(&s_send_msg, 0x00, sizeof(s_send_msg)); + + /** Message data */ + s_send_msg.data.ret_rst_status = ul_rst_sts; + + /* Messaging */ + l_thread_name = _pb_CnvPno2Name(u_pno); + /* External Process Transmission and Reception Messages */ + ret = _pb_SndMsg_Ext(l_thread_name, + CID_POSIF_REQ_GPS_RESET, + sizeof(s_send_msg.data), + reinterpret_cast(&(s_send_msg.data)), + 0); + if (ret != RET_NORMAL) { + POSITIONING_LOG("GpsCommCtl # DevGpsRstAnsSend SndMsg Error ret[%d] pno[%03X]\n", ret, u_pno); + ret = RET_ERROR; + } + } + + return ret; +} + + +/** + * @brief Time setting response issuance processing + * + * @param[in] us_pno Notify-To Process No. + * @param[in] uc_rid Response resource ID + * @param[in] ul_rst_sts Response result + * + * @return Processing result + * @retval RET_NORMAL : Normal + * @retval RET_ERROR : Abnormality + */ +int32 DevGpsTimesetAnsSend(PNO us_pno, RID uc_rid, u_int32 ul_rst_sts) { + TG_GPS_RET_TIMESET_MSG st_snd_msg; + RET_API ret = RET_NORMAL; + + /** Create GPS Reset Notification Message */ + memset( &st_snd_msg, 0x00, sizeof(st_snd_msg) ); /* QAC 3200 */ + /** Message header */ + st_snd_msg.header.hdr.sndpno = PNO_NAVI_GPS_MAIN; + st_snd_msg.header.hdr.respno = 0x0000; + st_snd_msg.header.hdr.cid = CID_GPS_RETTIMESETTING; + st_snd_msg.header.hdr.msgbodysize = sizeof(st_snd_msg.status); + st_snd_msg.header.hdr.rid = uc_rid; + /** Message data */ + st_snd_msg.status = ul_rst_sts; + + /* Messaging */ + if (us_pno != PNO_NONE) { + ret = _pb_SndMsg(us_pno, sizeof(st_snd_msg), &st_snd_msg, 0); + + if (ret != RET_NORMAL) { + POSITIONING_LOG("DevGpsTimesetAnsSend SndMsg Error ret[%d] pno[%03X] \r\n", ret, us_pno); + + ret = RET_ERROR; + } + } + + return(ret); +} + + +/** + * @brief + * GPS clock drift transmit process + * + * @param[in] drift Clock drift + * + * @return RET_NORMAL Normal completion + * @return RET_ERROR ABENDs + * + */ +RET_API DevSendGpsConnectError(BOOL is_err) { + SENSOR_MSG_GPSDATA s_send_msg; + RET_API ret = RET_NORMAL; + PNO u_pno = PNO_VEHICLE_SENSOR; + + /** Create GPS Data Notification Message */ + (void)memset(&s_send_msg, 0x00, sizeof(s_send_msg)); + + /** Message header */ + s_send_msg.st_head.hdr.sndpno = PNO_NAVI_GPS_MAIN; + s_send_msg.st_head.hdr.respno = 0x0000; + s_send_msg.st_head.hdr.cid = CID_GPS_DATA; + s_send_msg.st_head.hdr.msgbodysize = sizeof(SENSOR_MSG_GPSDATA_DAT); + s_send_msg.st_head.hdr.rid = 0x00; + + /** Message data */ + s_send_msg.st_para.ul_did = POSHAL_DID_GPS_CONNECT_ERROR; + s_send_msg.st_para.us_size = sizeof(uint32_t); + + (void)memcpy(&(s_send_msg.st_para.uc_data), &is_err, s_send_msg.st_para.us_size); + + /* Messaging */ + ret = _pb_SndMsg(u_pno, sizeof(s_send_msg), &s_send_msg, 0); + + if (ret != RET_NORMAL) { + ret = RET_ERROR; + } + + return ret; +} + + +RET_API SndGpsTimeRaw(const SENSOR_GPSTIME_RAW* ps_gpstime_raw) { + SENSOR_MSG_GPSDATA st_snd_msg; + RET_API ret; + PNO _us_pno = PNO_VEHICLE_SENSOR; + + /** Create GPS Data Notification Message */ + (void)memset( &st_snd_msg, 0x00, sizeof(st_snd_msg) ); /* QAC 3200 */ + /** Message header */ + st_snd_msg.st_head.hdr.sndpno = PNO_NAVI_GPS_MAIN; + st_snd_msg.st_head.hdr.respno = 0x0000; + st_snd_msg.st_head.hdr.cid = CID_GPS_DATA; + st_snd_msg.st_head.hdr.msgbodysize = sizeof(SENSOR_MSG_GPSDATA_DAT); + st_snd_msg.st_head.hdr.rid = 0x00; + /** Message data */ + st_snd_msg.st_para.ul_did = VEHICLE_DID_GPS_TIME_RAW; + st_snd_msg.st_para.us_size = sizeof(SENSOR_GPSTIME_RAW); + (void)memcpy(&(st_snd_msg.st_para.uc_data), ps_gpstime_raw, st_snd_msg.st_para.us_size); /* QAC 3200 */ + + /* Messaging */ + ret = _pb_SndMsg( _us_pno, sizeof(st_snd_msg), &st_snd_msg, 0 ); + if (ret != RET_NORMAL) { + POSITIONING_LOG("_pb_SndMsg ERROR!! [ret=%d]\n", ret); + ret = RET_ERROR; + } + + return(ret); +} + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/positioning_hal/src/GpsCommon/MDev_Gps_Common.cpp b/positioning_hal/src/GpsCommon/MDev_Gps_Common.cpp new file mode 100644 index 00000000..c8b5c9a6 --- /dev/null +++ b/positioning_hal/src/GpsCommon/MDev_Gps_Common.cpp @@ -0,0 +1,2105 @@ +/* + * @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 MDev_Gps_Common.h +*/ + +/*---------------------------------------------------------------------------*/ + +#include "MDev_Gps_Common.h" + +#include +#include +#include "positioning_common.h" +#include "MDev_Gps_API.h" +#include "MDev_Gps_TimerCtrl.h" +#include "MDev_Gps_Nmea.h" +#include "MDev_GpsRecv.h" + +/*---------------------------------------------------------------------------*/ +// Value define + +#define BK_ID_POS_GPS_TD_STS_SZ 1 /* Datetime Status-Size */ +#define BK_GPSFIXCNT_OFFSET_CHK 12 /* Fix Count-Offset-Check SRAM */ +#define BK_GPSFIXCNT_RAM_OK 0 /* Fix Count RAM Status OK */ +#define BK_GPSFIXCNT_RAM_FILE_NG 1 /* Fix Count RAM Status FILE NG */ +#define BK_GPSFIXCNT_RAM_NG 2 /* Fix Count RAM Status NG */ + +#define JULIAN_DAY (-32044.0f) /* Be based on March 1, 4800 BC */ +#define MODIFIED_JULIAN_DAY_OFFSET (2400000.5f) +#define GPS_WS_MAX (7 * 24 * 60 * 60) /* GPS time(Weekly seconds)*/ +#define GET_METHOD_GPS (1) //!< \~english GPS + +/* Presence information definitions DR */ +#define SENSORLOCATION_EXISTDR_NODR (0) /* Without DR */ +#define SENSORLOCATION_EXISTDR_DR (1) /* There DR */ +/* DR state definition */ +#define SENSORLOCATION_DRSTATUS_INVALID (0) /* Invalid */ +#define SENSORLOCATION_DRSTATUS_GPS_NODR (1) /* Information use GPS, not yet implemented DR */ +#define SENSORLOCATION_DRSTATUS_NOGPS_DR (2) /* No information available GPS, DR implementation */ +#define SENSORLOCATION_DRSTATUS_GPS_DR (3) /* Information use GPS, DR implementation */ + +#define SENSORLOCATION_STATUS_DISABLE (0) //!< \~english Not available +#define SENSORLOCATION_STATUS_ENABLE (1) //!< \~english Available +/*---------------------------------------------------------------------------*/ +// Global values + +uint8_t g_gps_reset_cmd_sending = FALSE; /* Reset command transmission in progress judgment flag */ +uint8_t g_is_gpstime_sts = FALSE; /* Date/Time Status Fixed Value Setting Indication Flag */ +uint8_t g_gpstime_raw_tdsts = 0; +extern u_int8 g_gps_week_cor_cnt; +/*---------------------------------------------------------------------------*/ +// Functions +void WriteGpsTimeToBackup(uint8_t flag, POS_DATETIME* pst_datetime) { + int32_t ret; + ST_GPS_SET_TIME buf; + + memset(reinterpret_cast(&buf), 0, (size_t)sizeof(buf)); + + buf.flag = flag; + buf.year = pst_datetime->year; + buf.month = pst_datetime->month; + buf.date = pst_datetime->date; + buf.hour = pst_datetime->hour; + buf.minute = pst_datetime->minute; + buf.second = pst_datetime->second; + + ret = Backup_DataWt(D_BK_ID_POS_GPS_TIME_SET_INFO, &buf, 0, sizeof(ST_GPS_SET_TIME)); + if (ret != BKUP_RET_NORMAL) { + POSITIONING_LOG("Backup_DataWt ERROR!! [ret=%d]", ret); + } + + return; +} + + + +/******************************************************************************** + * MODULE : ChangeStatusGpsCommon + * ABSTRACT : State transition processing + * FUNCTION : Changes the state of the GPS communication management process to the specified state + * ARGUMENT : u_int32 sts : State of the transition destination + * NOTE : + * RETURN : None + ********************************************************************************/ +void ChangeStatusGpsCommon(u_int32 sts ) { + /* Set the transition destination state in the management information state */ + g_gps_mngr.sts = sts; +} + +/******************************************************************************** + * MODULE : RtyResetGpsCommon + * ABSTRACT : Reset retry counter(Periodic reception) + * FUNCTION : Reset the retry counter + * ARGUMENT : None + * NOTE : + * RETURN : None + ********************************************************************************/ +void RtyResetGpsCommon(void) { + g_gps_mngr.hrsetcnt = 0; /* Initialization of the number of tries until a hard reset */ + g_gps_mngr.sndngcnt = 0; /* Initialization of the number of tries until serial reset */ +} + +/****************************************************************************** +@brief SendRtyResetGpsCommon
+ Reset retry counter(ACK response) +@outline Reset the retry counter +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void SendRtyResetGpsCommon(void) { + g_gps_mngr.sndcnt = 0; /* Initialization of the number of transmission retries */ +} + +uint8_t HardResetChkGpsCommon(void) { + uint8_t ret = RETRY_OFF; /* Return value */ + static uint8_t uc_hardreset_cnt = 0; /* Number of hard resets */ + + g_gps_mngr.hrsetcnt++; /* Add number of tries until hard reset */ + + if (g_gps_mngr.hrsetcnt >= HRSET_MAX) { + // Todo For test don't have hardReset Method. + // DEV_Gps_HardReset(); + + g_gps_mngr.hrsetcnt = 0; /* Initialization of the number of tries until a hard reset */ + /* Discard all pending commands */ + DeleteAllSaveCmdGpsCommon(); + /* Determination of the number of hard reset executions */ + uc_hardreset_cnt++; + if (uc_hardreset_cnt >= 3) { + ret = RETRY_OFF; /* Set the return value (No retry: Specified number of hard resets completed) */ + uc_hardreset_cnt = 0; /* Clear the number of hard resets */ + } else { + ret = RETRY_OUT; /* Set the return value (retry out: hardware reset) */ + } + } else { /* When the number of tries until the hard reset is less than the maximum value */ + ret = RETRY_ON; /* Set return value (with retry) */ + } + + return (ret); /* End of the process */ +} + + +/****************************************************************************** +@brief SendReqGpsCommon
+ Common-Transmit Request Reception Matrix Function +@outline Common processing when receiving a transmission request +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void SendReqGpsCommon(void) { + RET_API ret = RET_NORMAL; + TG_GPS_SND_DATA* p_rcv_data = NULL; + TG_GPS_SAVECMD s_send_data; + TG_GPS_OUTPUT_FORMAT s_output_format; + + p_rcv_data = reinterpret_cast(&(g_gps_msg_rcvr.msgdat[0])); /* Incoming message structure */ + + /** Transmit Data Format Check */ + ret = CheckSendCmdGpsCommon(p_rcv_data->ub_data, p_rcv_data->us_size, &s_output_format); + + /** Normal format */ + if (ret == RET_NORMAL) { + memset( &s_send_data, 0x00, sizeof(s_send_data) ); /* Ignore -> MISRA-C:2004 Rule 16.10 */ + + s_send_data.us_pno = 0; /* Result notification destination process number */ + s_send_data.uc_rid = 0; /* Result Notification Resource ID */ + s_send_data.uc_rst = GPS_CMD_NOTRST; /* Reset flag */ + s_send_data.e_cmd_info = s_output_format; /* Command information */ + memcpy( &s_send_data.sndcmd, p_rcv_data, sizeof(TG_GPS_SND_DATA) ); /* Sending commands */ + + /* Command is suspended and terminated. */ + ret = DevGpsSaveCmd(&s_send_data); /* #GPF_60_040 */ + + if (ret != RET_NORMAL) { + POSITIONING_LOG("GPS Command Reserve bufferFull ! \r\n"); + } + } else { + POSITIONING_LOG("# GpsCommCtl # GPS Command Format Error!! \r\n"); + } + + return; +} + +/****************************************************************************** +@brief GPSResetReqGpsCommon
+ Common-GPS reset request reception matrix function +@outline Common processing at GPS reset reception +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void GPSResetReqGpsCommon(void) { + RET_API ret = RET_NORMAL; + POS_RESETINFO st_rcv_msg; + + memset(&st_rcv_msg, 0x00, sizeof(st_rcv_msg)); /* 2015/03/31 Coverity CID: 21530 support */ + + if (g_gps_reset_cmd_sending == FALSE) { + memcpy(&st_rcv_msg, reinterpret_cast(&(g_gps_msg_rcvr.msgdat)), + sizeof(POS_RESETINFO)); + ret = DevGpsResetReq(st_rcv_msg.respno, 0); + if (ret == RET_NORMAL) { + /* Set the reset command transmission judgment flag to transmission in progress */ + g_gps_reset_cmd_sending = TRUE; + /* Send reset command(Normal response transmission)*/ + GpsSetPosBaseEvent(st_rcv_msg.sndpno, POS_RET_NORMAL); + } else if (ret == RET_EV_NONE) { + /* nop */ + } else { + /* Send reset command(Internal Processing Error Response Transmission)*/ + GpsSetPosBaseEvent(st_rcv_msg.sndpno, POS_RET_ERROR_INNER); + } + } else { + memcpy(&st_rcv_msg, reinterpret_cast(&(g_gps_msg_rcvr.msgdat)), sizeof(POS_RESETINFO)); + GpsSetPosBaseEvent(st_rcv_msg.sndpno, POS_RET_ERROR_BUSY); + } + return; +} + +/****************************************************************************** +@brief CyclDataTimeOutGpsCommon
+ Common-Receive data monitoring timeout matrix function +@outline Common processing at reception cycle data monitoring timeout +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void CyclDataTimeOutGpsCommon(void) { + g_wcnct_err++; /* Count number of connection errors */ + + /* Stop all timers */ + DevGpsTimeStop(GPS_STARTUP_TIMER); /* Start confirmation monitoring timer */ /* Ignore -> MISRA-C:2004 Rule 16.10 */ + DevGpsTimeStop(GPS_RECV_ACK_TIMER); /* ACK reception monitoring timer */ /* Ignore -> MISRA-C:2004 Rule 16.10 */ + + /* Clear cyclic receive data up to now */ + DevGpsCycleDataClear(); + + /* Start confirmation monitoring timer setting */ + DevGpsTimeSet(GPS_STARTUP_TIMER); /* Ignore -> MISRA-C:2004 Rule 16.10 */ + + /* State-transition(Start Confirmation Monitor) */ + ChangeStatusGpsCommon(GPS_STS_STARTUP); + + return; +} + +/******************************************************************************** + * MODULE : CheckFrontStringPartGpsCommon + * ABSTRACT : Message start character string determination processing + * FUNCTION : Check that the message starts with the specified string + * ARGUMENT : pubTarget : Message + * : pubStart : Character string + * NOTE : When target is equal to start, it is also judged to be TRUE. + * : "*" in the start is treated as a single-character wildcard. + * RETURN : RET_NORMAL : Decision success + * : RET_ERROR : Decision failure + ********************************************************************************/ +RET_API CheckFrontStringPartGpsCommon(const u_char *p_tartget, const u_char *p_start) { + RET_API ret = RET_ERROR; + + while (((*p_tartget == *p_start) + || ('*' == *p_start)) /* #GPF_60_024 */ /* Ignore -> No applicable rules for MISRA-C */ + && (*p_tartget != '\0') + && (*p_start != '\0')) { + p_tartget++; /* Ignore -> MISRA-C:2004 Rule 17.4 */ + p_start++; /* Ignore -> MISRA-C:2004 Rule 17.4 */ + } + + if (*p_start == '\0') { + ret = RET_NORMAL; + } else { + ret = RET_ERROR; + } + + return ret; +} + +/****************************************************************************** +@brief JudgeFormatGpsCommon
+ Format determination processing +@outline Determine the format of the received GPS data +@param[in] pubSndData : Message to be judged +@param[in] ul_length : Message length +@param[out] peFormat : Where to Return the Format +@input none +@return int32 +@retval GPSRET_SNDCMD : Commands to be notified +@retval GPSRET_NOPCMD : User unset command +@retval GPSRET_CMDERR : No applicable command +*******************************************************************************/ +int32 JudgeFormatGpsCommon(u_char *p_send_data, u_int32 ul_length, TG_GPS_OUTPUT_FORMAT *p_format) { + int32 ret = GPSRET_NOPCMD; /* Return value */ + u_int32 cnt = 0; /* Ignore -> MISRA-C:2004 Rule 5.6 */ + + for (cnt = 0; cnt < (u_int32)GPSCMDANATBL_MAX; cnt++) { + /** End-of-table decision */ + if (CheckFrontStringPartGpsCommon((const u_char*)ENDMARK, kGpsCmdAnaTbl[cnt].c_sentence) == RET_NORMAL) { + /** When there is no matching sentence, */ + /** set return value. */ + ret = GPSRET_NOPCMD; /* Ignore -> MISRA-C:2004 Rule 3.1 */ + break; + } + + /* ++ #GPF_60_040 */ + /* Sentence determination of received command */ + + if (kGpsCmdAnaTbl[cnt].e_rcv_format == g_gps_mngr.rcv_cmd) { + /* Format set */ + (*p_format) = g_gps_mngr.rcv_cmd; + + /* Notification judgment */ + if (kGpsCmdAnaTbl[cnt].b_snd_cmd_flg == TRUE) { + /* Checksum implementation */ + if (CheckSumGpsCommon(reinterpret_cast(p_send_data), + ul_length, + kGpsCmdAnaTbl[cnt].e_rcv_format) != RET_NORMAL) { + /* Receive data anomaly */ + POSITIONING_LOG("# GpsCommCtl # GPS Data SUM ERROR! \r\n"); + + g_wrecv_err++; + + /* Set data error in return value */ + ret = GPSRET_CMDERR; + } else { + /* Notification to vehicle sensor */ + ret = GPSRET_SNDCMD; + g_wrecv_err = 0; + } + } + + break; + } + } + + return ret; +} + + +/****************************************************************************** +@brief CheckSumGpsCommon
+ Checksum processing +@outline Checking the integrity of GPS commands +@param[in] none +@param[out] none +@input u_int8 : p_uc_data[] ... Receive data pointer +@input u_int32 : ul_length ... Data length +@input u_int32 : e_cmd_info ... Command information
+ -FORMAT_NMEA NMEA data
+ -FORMAT_BIN Standard binary
+ -FORMAT_FULLBIN FULL binaries +@return RET_API +@retval RET_NORMAL : Normal completion +@retval RET_ERROR : ABENDs +*******************************************************************************/ +RET_API CheckSumGpsCommon(const u_int8 p_uc_data[], u_int32 ul_length, TG_GPS_OUTPUT_FORMAT e_cmd_info) { + RET_API l_ret = RET_ERROR; + u_int32 i = 0; + u_int8 uc_char = 0; + u_int8 uc_sum = 0; + u_int8 uc_cmp = 0; + int32_t ret = 0; + + static u_int8 uc_work[UBX_CMD_SIZE_MAX]; + + if ((GPS_FORMAT_MON_VER == e_cmd_info) + || (GPS_FORMAT_AID_INI == e_cmd_info) + || (GPS_FORMAT_ACK_ACKNACK == e_cmd_info) + || (GPS_FORMAT_NAV_TIMEUTC == e_cmd_info) + || (GPS_FORMAT_NAV_CLOCK == e_cmd_info) + || (GPS_FORMAT_RXM_RTC5 == e_cmd_info) + || (GPS_FORMAT_NAV_SVINFO == e_cmd_info) + || (GPS_FORMAT_CFG_NAVX5 == e_cmd_info)) { + (void)memcpy(uc_work, p_uc_data, ul_length - 2); + DevGpsSetChkSum(uc_work, ul_length); + + ret = memcmp(uc_work, p_uc_data, ul_length); + + if (ret == 0) { + l_ret = RET_NORMAL; + } else { + } + } else { + /** XOR each character between '$' and '*' */ + for ( i = 1; i < ul_length; i++ ) { + if (p_uc_data[i] == (u_int8)'*') { + /** '*'Detection */ + l_ret = RET_NORMAL; /* #GPF_60_111 */ + break; + } else { + /** '*'Not detected */ + uc_char = p_uc_data[i]; + uc_sum ^= uc_char; + } + } + + /** When the position of '*' is within two characters following '*' (access check of the receive buffer range) */ + if ( (l_ret == RET_NORMAL) && ((GPS_READ_LEN - 2) > i) ) { + /** Convert two characters following '*' to two hexadecimal digits */ + uc_cmp = (AtoHGpsCommon(p_uc_data[i + 1]) * 16) + AtoHGpsCommon(p_uc_data[i + 2]); + + /** Checksum result determination */ + if ( uc_cmp != uc_sum ) { + /** Abnormality */ + l_ret = RET_ERROR; + } + } else { + /** '*' Not detected, data length invalid */ + /** Abnormality */ + l_ret = RET_ERROR; + } + } + + return l_ret; +} + +/****************************************************************************** +@brief AtoHGpsCommon
+ ASCII-> hexadecimal conversions +@outline Convert ASCII codes to hexadecimal +@param[in] none +@param[out] none +@input u_int8 : ascii ... ASCII code('0' ~ '9', 'a' ~ 'f', 'A' ~ 'F') +@return u_int8 +@retval Hexadecimal number +*******************************************************************************/ +u_int8 AtoHGpsCommon(u_int8 ascii) { + u_int8 hex = 0; + if ((ascii >= '0') && (ascii <= '9')) { + hex = ascii - '0'; + } else if ((ascii >= 'a') && (ascii <= 'f')) { + hex = (ascii - 'a') + 0xa; /* Ignore -> MISRA-C:2004 Rule 12.1 */ + } else if ( (ascii >= 'A') && (ascii <= 'F') ) { + hex = (ascii - 'A') + 0xa; /* Ignore -> MISRA-C:2004 Rule 12.1 */ + } else { + /* nop */ + } + + return hex; +} + +/****************************************************************************** +@brief DevGpsSaveCmd
+ Command pending processing +@outline Temporarily save commands to be sent to GPS +@param[in] TG_GPS_SND_DATA* pstSndMsg : Sending commands +@param[out] none +@return RET_API +@retval RET_NORMAL : Normal completion +@retval RET_ERROR : ABENDs(buffer full) +*******************************************************************************/ +RET_API DevGpsSaveCmd(TG_GPS_SAVECMD *p_send_data) { + RET_API ret = RET_NORMAL; + u_int32 savp = 0; /* Holding buffer location storage */ + + /* Pending buffer full */ + if ( g_gps_save_cmdr.bufsav >= SAV_MAX ) { + /* Return an abend */ + ret = RET_ERROR; + } else { + savp = g_gps_save_cmdr.saveno; /* Get pending buffer position */ + + /* Copy data to pending buffer */ + memset(&g_gps_save_cmdr.savebuf[savp], 0x00, sizeof(g_gps_save_cmdr.savebuf[savp])); + memcpy(&g_gps_save_cmdr.savebuf[savp], p_send_data, sizeof(TG_GPS_SAVECMD)); + + g_gps_save_cmdr.saveno++; /* Pending index addition */ + + if (g_gps_save_cmdr.saveno >= SAV_MAX) { + g_gps_save_cmdr.saveno = 0; /* Reset Pending Index */ + } + + g_gps_save_cmdr.bufsav++; /* Number of pending buffers added */ + } + + return ret; +} + +/****************************************************************************** +@brief SendSaveCmdGpsCommon
+ Pending command transmission processing +@outline Send Pending Commands to GPS +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void SendSaveCmdGpsCommon(void) { + u_int32 sndp = 0; /* Holding buffer location storage */ + // TG_GPS_SND_DATA* p_send_data = NULL; /* Pointer to the pending command */ + BOOL b_ret = FALSE; /* Command send return value */ + + /** Retrieves one pending command, if any, and sends it. */ + if (g_gps_save_cmdr.bufsav != 0) { + /** Send position acquisition */ + sndp = g_gps_save_cmdr.sendno; + // p_send_data = reinterpret_cast(&(g_gps_save_cmdr.savebuf[sndp])); + + // todo For test No Uart + // b_ret = DEV_Gps_CmdWrite( p_send_data->us_size, &(p_send_data->uc_data[0]) );/* #GPF_60_106 */ + if ( b_ret != TRUE ) { + POSITIONING_LOG("DEV_Gps_CmdWrite fail. ret=[%d]\n", b_ret); + // ucResult = SENSLOG_RES_FAIL; + } + + /* ++ #GPF_60_040 */ + /** Configure Monitored Commands */ + g_gps_mngr.resp_cmd = g_gps_save_cmdr.savebuf[ sndp ].e_cmd_info; + g_gps_mngr.resp_pno = g_gps_save_cmdr.savebuf[ sndp ].us_pno; + g_gps_mngr.resp_rid = g_gps_save_cmdr.savebuf[ sndp ].uc_rid; + g_gps_mngr.resp_rst_flag = g_gps_save_cmdr.savebuf[ sndp ].uc_rst; + + /** Perform response monitoring */ + DevGpsTimeSet(GPS_RECV_ACK_TIMER); /* Ignore -> MISRA-C:2004 Rule 16.10 */ + /* State transition processing(sending) */ + ChangeStatusGpsCommon(GPS_STS_SENT); + } else { + /** No monitored commands */ + g_gps_mngr.resp_cmd = GPS_FORMAT_MIN; + g_gps_mngr.resp_pno = 0x0000; + g_gps_mngr.resp_rid = 0x00; + g_gps_mngr.resp_rst_flag = GPS_CMD_NOTRST; + } + + return; +} + +/* ++ #GPF_60_040 */ +/****************************************************************************** +@brief DeleteSaveCmdGpsCommon
+ Pending command deletion processing +@outline Deleting a Pending Command +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DeleteSaveCmdGpsCommon(void) { + u_int32 sndp = 0; /* Holding buffer location storage */ + + /** Delete one pending command, if any. */ + if (g_gps_save_cmdr.bufsav != 0) { + /** Send position acquisition */ + sndp = g_gps_save_cmdr.sendno; + + /** Clear Stored Data */ + memset(&g_gps_save_cmdr.savebuf[sndp], 0x00, sizeof(g_gps_save_cmdr.savebuf[sndp])); + + /** Transmit index addition */ + g_gps_save_cmdr.sendno++; + if ( g_gps_save_cmdr.sendno >= SAV_MAX ) { + /** Transmit index MAX or higher */ + /** Initialize transmission index */ + g_gps_save_cmdr.sendno = 0; + } + + /** Subtract pending buffers */ + g_gps_save_cmdr.bufsav--; + } + + return; +} + +/****************************************************************************** +@brief DeleteAllSaveCmdGpsCommon
+ Hold command abort processing +@outline Discards all pending commands and returns a reset response +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DeleteAllSaveCmdGpsCommon(void) { + u_int32 sndp = 0; /* Holding buffer location storage */ + PNO us_pno = 0; /* Ignore -> MISRA-C:2004 Rule 5.6 */ + RID uc_rid = 0; /* Ignore -> MISRA-C:2004 Rule 5.6 */ + + while (g_gps_save_cmdr.bufsav != 0) { + /** Determine whether it is a pending command that requires a response. */ + sndp = g_gps_save_cmdr.sendno; + + if (g_gps_save_cmdr.savebuf[ sndp ].uc_rst == GPS_CMD_RESET) { + /** In the case of a reset request, a reset response (communication error) is notified. */ + us_pno = g_gps_save_cmdr.savebuf[ sndp ].us_pno; + uc_rid = g_gps_save_cmdr.savebuf[ sndp ].uc_rid; + DevGpsRstAnsSend(us_pno, uc_rid, GPS_SENDNG); /* Ignore -> MISRA-C:2004 Rule 16.10 */ + } + + /** Delete */ + DeleteSaveCmdGpsCommon(); + } + + return; +} +/****************************************************************************** +@brief RcvCyclCmdNmeaGpsCommon
+ Cyclic data (NMEA) notification process +@outline Notifying VehicleSensor of Cyclic Data (NMEA) Reception +@param[in] u_int8 : *p_uc_data ... Receive data pointer +@param[in] u_int32 : ul_len ... Received data length +@param[in] TG_GPS_OUTPUT_FORMAT : s_output_format ... Receive Format +@param[out] none +@return none +@retval none +*******************************************************************************/ +void RcvCyclCmdNmeaGpsCommon(u_int8 *p_uc_data, u_int32 ul_len, TG_GPS_OUTPUT_FORMAT s_output_format) { + NAVIINFO_ALL navilocinfo; /* Navigation information */ + BOOL bcycle_rcv_flag = FALSE; /* Cyclic reception judgment result */ + + if (JudgeFormatOrderGpsCommon(s_output_format, g_rcv_format) == 0) { + /** If a sentence is received before the last received sentence, */ + /** determine as the beginning of cyclic data. */ + bcycle_rcv_flag = TRUE; + } + + if (bcycle_rcv_flag == TRUE) { + DevGpsSndCycleDataNmea(); + + /* Reset navigation information */ + memset(&navilocinfo, 0x00, sizeof(NAVIINFO_ALL)); + + /* NMEA data analysis */ + DevGpsAnalyzeNmea(&navilocinfo); + + /** Clear Buffer */ + // DevGpsCycleDataClear(); + } + + DevGpsCycleDataSetNmea(p_uc_data, (u_int32)ul_len, GetNmeaIdxGpsCommon(s_output_format)); + + /** Update receive format */ + g_rcv_format = s_output_format; + + return; +} + + +/** + * @brief + * Cyclic data (UBX) notification processing + * + * Notify vehicle sensor of reception of cyclic data (UBX) + * + * @param[in] u_int8 : *p_uc_data ... Receive data pointer + * @param[in] u_int32 : ul_len ... Received data length + * @param[in] TG_GPS_OUTPUT_FORMAT : eFormat ... Receive Format + */ +void RcvCyclCmdExtGpsCommon(u_int8 *p_uc_data, u_int32 ul_len, TG_GPS_OUTPUT_FORMAT e_format) { + SENSOR_GPSTIME_RAW st_gpstime_raw; + TG_GPS_UBX_NAV_TIMEUTC_DATA *pst_navtime_utc; + BOOL b_validtow; /* Valid Time of Week */ + BOOL b_validwkn; /* Valid Week of Number */ + BOOL b_validutc; /* Valid UTC Time */ + + /* For NAV-TIMEUTC */ + if (e_format == GPS_FORMAT_NAV_TIMEUTC) { + memset(&st_gpstime_raw, 0x00, sizeof(st_gpstime_raw)); + + /* NAV-TIMEUTC analysis */ + pst_navtime_utc = reinterpret_cast(p_uc_data + UBX_CMD_OFFSET_PAYLOAD); + + st_gpstime_raw.utc.year = pst_navtime_utc->year; + st_gpstime_raw.utc.month = pst_navtime_utc->month; + st_gpstime_raw.utc.date = pst_navtime_utc->day; + st_gpstime_raw.utc.hour = pst_navtime_utc->hour; + st_gpstime_raw.utc.minute = pst_navtime_utc->min; + st_gpstime_raw.utc.second = pst_navtime_utc->sec; + b_validtow = (BOOL)(((pst_navtime_utc->valid) & UBX_CMD_MSK_NAV_TIMEUTC_VALID_TOW) + >> (uint8_t)(UBX_CMD_MSK_NAV_TIMEUTC_VALID_TOW / 2)); + b_validwkn = (BOOL)(((pst_navtime_utc->valid) & UBX_CMD_MSK_NAV_TIMEUTC_VALID_WKN) + >> (uint8_t)(UBX_CMD_MSK_NAV_TIMEUTC_VALID_WKN / 2)); + b_validutc = (BOOL)(((pst_navtime_utc->valid) & UBX_CMD_MSK_NAV_TIMEUTC_VALID_UTC) + >> (uint8_t)(UBX_CMD_MSK_NAV_TIMEUTC_VALID_UTC / 2)); + + if ((b_validtow == TRUE) && (b_validwkn == TRUE)) { + st_gpstime_raw.tdsts = 2; /* Calibrated */ + g_gpstime_raw_tdsts = 2; + } else { + st_gpstime_raw.tdsts = 0; /* Uncalibrated */ + g_gpstime_raw_tdsts = 0; + } + + b_validutc = b_validutc; + POSITIONING_LOG("year=%04d, month=%02d, date=%02d, hour=%02d, minute=%02d," + " second=%02d, validTow=%d, validWkn=%d, validUtc=%d", + st_gpstime_raw.utc.year, st_gpstime_raw.utc.month, st_gpstime_raw.utc.date, + st_gpstime_raw.utc.hour, st_gpstime_raw.utc.minute, st_gpstime_raw.utc.second, + b_validtow, b_validwkn, b_validutc); + /* Notify GPS time to vehicle sensor */ + SndGpsTimeRaw((const SENSOR_GPSTIME_RAW*)&st_gpstime_raw); + } else { + POSITIONING_LOG("Forbidden ERROR!![e_format=%d]", e_format); + } + + /** Update Receive Format */ + g_rcv_format = e_format; + + return; +} + +/****************************************************************************** +@brief CheckSendCmdGpsCommon
+ Send command check processing +@outline Check the format of the send command +@param[in] const u_char* pubRcvData : Receive command +@param[in] u_int32 ul_length : Length of the command +@param[out] TG_GPS_OUTPUT_FORMAT* peFormat : Command format information +@return int32 +@retval RET_NORMAL : Normal +@retval RET_ERROR : Abnormality +*******************************************************************************/ +int32 CheckSendCmdGpsCommon(const u_char *p_rcv_data, u_int32 ul_length, TG_GPS_OUTPUT_FORMAT *p_format) { + u_int32 ret = RET_NORMAL; + u_int32 cnt = 0; /* Ignore -> MISRA-C:2004 Rule 5.6 */ + + /** Analysis of received commands */ + for (cnt = 0; cnt < (u_int32)GPSCMDANATBL_MAX; cnt++) { + /** End-of-table decision */ + if (CheckFrontStringPartGpsCommon((const u_char*)ENDMARK, kGpsCmdAnaTbl[cnt].c_sentence) == RET_NORMAL) { + /** When there is no matching sentence */ + + /** Return value setting */ + ret = RET_ERROR; /* Ignore -> MISRA-C:2004 Rule 3.1 */ + break; + } + + /** Sentence determination of received command */ + if (CheckFrontStringPartGpsCommon(p_rcv_data, kGpsCmdAnaTbl[cnt].c_sentence) == RET_NORMAL) { + if (ul_length == kGpsCmdAnaTbl[cnt].ul_length) { + /** When the sentences match */ + + /** Reception type determination */ + if ( kGpsCmdAnaTbl[cnt].ul_rcv_kind == RCV_RESP ) { + /** Response monitor format setting */ + *p_format = kGpsCmdAnaTbl[cnt].e_rcv_format; + } else { + /** Return value setting */ + ret = RET_ERROR; /* Ignore -> MISRA-C:2004 Rule 3.1 */ + } + + break; /* Ignore -> MISRA-C:2004 Rule 14.6 */ + } + } + } + + return ret; +} + + +/** + * @brief + * Get a string of fields from a NMEA sentence + * + * Operation is not guaranteed if a pointer other than a pointer to the NMEA sentence is specified. + * + * @param[in] field_number Field No. + * @param[in] p_src Pointers to NMEA sentences + * @param[out] p_dst Pointer to the output area + * @param[in] size Maximum output size + * + * @return Size + */ +int32_t GetStringFromNmeaGpsCommon(u_int8 field_number, u_int8* p_src, char* p_dst, size_t size ) { + uint8_t* ptr = NULL; + /* int32_t numeric; */ + uint32_t i = 0; + int32_t cnt = 0; + int8_t buf[GPS_NMEA_FIELD_LEN_MAX] = {0}; + + /* Null check */ + if ((p_src == NULL) || (p_dst == NULL)) { + POSITIONING_LOG("Argument ERROR [p_src=%p, p_dst=%p]", p_src, p_dst); + return 0; + } + + ptr = p_src; + + for (i = 0; i <= GPS_NMEA_MAX_SZ; i++) { + if (cnt == field_number) { + break; + } + + if (*ptr == NMEA_DATA_SEPARATOR) { + cnt++; + } + + ptr++; + } + + i = 0; + if (*ptr != NMEA_DATA_SEPARATOR) { + for (i = 0; i < (GPS_NMEA_FIELD_LEN_MAX - 1); i++) { + buf[i] = *ptr; + ptr++; + + if ((*ptr == NMEA_DATA_SEPARATOR) || (*ptr == NMEA_CHECKSUM_CHAR)) { + i++; + break; + } + } + } + + buf[i] = '\0'; + strncpy(p_dst, (const char *)buf, size); + + return i; +} + +/** + * @brief + * Get the number (real type) of a field from a NMEA sentence + * + * Operation is not guaranteed if a pointer other than a pointer to the NMEA sentence is specified. + * + * @param[in] field_number Field No. + * @param[in] p_src Pointers to NMEA sentences + * @param[out] p_valid Validity of numerical data + * + * @return Real-number - double + */ +double_t GetNumericFromNmeaGpsCommon(u_int8 field_number, u_int8* p_src, BOOL* p_valid) { + double_t numeric = 0; + char buf[GPS_NMEA_FIELD_LEN_MAX] = {0}; + int32_t ret = 0; + + /* Null check */ + if (p_src == NULL) { + POSITIONING_LOG("Argument ERROR [p_src=%p]", p_src); + } + + ret = GetStringFromNmeaGpsCommon(field_number, p_src, buf, GPS_NMEA_FIELD_LEN_MAX); + + if (ret > 0) { + numeric = atof((const char *)buf); + *p_valid = TRUE; + } else { + *p_valid = FALSE; + } + + return numeric; +} + +/** + * @brief + * Get the integer value (integer type) of a field from a NMEA sentence + * + * Operation is not guaranteed if a pointer other than a pointer to the NMEA sentence is specified. + * + * @param[in] field_number Field No. + * @param[in] p_src Pointers to NMEA sentences + * + * @return Integer - int32 + */ +int32 GetIntegerFromNmeaGpsCommon(u_int8 field_number, u_int8* p_src) { + int32_t integer = 0; + char buf[GPS_NMEA_FIELD_LEN_MAX] = {0}; + + /* Null check */ + if (p_src == NULL) { + POSITIONING_LOG("Argument ERROR [p_src=%p]", p_src); + } + + GetStringFromNmeaGpsCommon(field_number, p_src, buf, GPS_NMEA_FIELD_LEN_MAX); + integer = atoi((const char *)buf); + + return integer; +} + +/** + * @brief + * Get the latitude/longitude of the given fi eld from the NMEA sentence + * + * Operation is not guaranteed if a pointer other than a pointer to the NMEA sentence is specified. + * + * @param[in] field_number Field No. + * @param[in] p_src Pointers to NMEA sentences + * @param[out] p_valid Validity of the data + * + * @return Latitude and longitude [1/256 sec] + */ +int32_t GetLonLatFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src, BOOL *p_valid) { + int32_t result = 0; + double_t value = 0.0; + int32_t deg = 0; + double_t min = 0.0; + + /* Null check */ + if (p_src == NULL) { + POSITIONING_LOG("Argument ERROR [p_src=%p]", p_src); + } + + value = GetNumericFromNmeaGpsCommon(field_number, p_src, p_valid); + + if ((value < 0.0) || (value > 0.0)) { + deg = (int32_t)(value / 100.0f); + min = (double_t)(value - (deg * 100.0f)); + result = (int32_t)(((60.0f * 60.0f * 256.0f) * deg) + ((60.0f * 256.0f) * min)); + } + + return result; +} + +/** + * @brief + * Get the orientation of any fields from a NMEA sentence + * + * Operation is not guaranteed if a pointer other than a pointer to the NMEA sentence is specified. + * + * @param[in] field_number Field No. + * @param[in] p_src Pointers to NMEA sentences + * @param[out] p_valid Validity of the data + * + * @return Orientation[0.01 degree] + */ +u_int16 GetHeadingFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src, BOOL *p_valid) { + u_int16 result = 0; + double_t value = 0.0; + + /* Null check */ + if (p_src == NULL) { + POSITIONING_LOG("Argument ERROR [p_src=%p]", p_src); + } + + value = GetNumericFromNmeaGpsCommon(field_number, p_src, p_valid); + + result = (u_int16)((value + 0.005f) * 100.0f); + + return result; +} + + +/** + * @brief + * Get the orientation of any fields from a NMEA sentence + * + * Operation is not guaranteed if a pointer other than a pointer to the NMEA sentence is specified. + * + * @param[in] field_number Field No. + * @param[in] p_src Pointers to NMEA sentences + * @param[out] p_valid Validity of the data + * + * @return speed[0.01m/s] + */ +u_int16 GetSpeedFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src, BOOL *p_valid) { + u_int16 result = 0; + double_t value = 0.0; + + /* Null check */ + if (p_src == NULL) { + POSITIONING_LOG("Argument ERROR [p_src=%p]", p_src); + } + + value = GetNumericFromNmeaGpsCommon(field_number, p_src, p_valid); + + // speed[0.01m/s] + result = static_cast(value * 1.852 * 100 / 3.6); + + return result; +} + +/** + * @brief + * Get the altitude of certain fields from a NMEA sentence + * + * Operation is not guaranteed if a pointer other than a pointer to the NMEA sentence is specified. + * + * @param[in] field_number Field No. + * @param[in] p_src Pointers to NMEA sentences + * @param[out] p_valid Validity of the data + * + * @return Altitude [0.01m] + */ +int32 GetAltitudeFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src, BOOL *p_valid) { + int32 result = 0; + double_t value = 0.0; + + /* Null check */ + if (p_src == NULL) { + POSITIONING_LOG("Argument ERROR [p_src=%p]", p_src); + } + + value = GetNumericFromNmeaGpsCommon(field_number, p_src, p_valid); + + result = static_cast((value + 0.005f) * 100.0f); + + return result; +} + +/** + * @brief + * GPS Control Thread Stop Processing + */ +void StopThreadGpsCommon(void) { + /** Stop the start confirmation monitoring timer */ + (void)DevGpsTimeStop(GPS_STARTUP_TIMER); /* Start confirmation monitoring timer */ + + /** Stop the periodic reception data monitoring timer */ + (void)DevGpsTimeStop(GPS_CYCL_TIMER); /* Periodic reception data monitoring timer */ + + /** Stop the ACK reception monitoring timer */ + (void)DevGpsTimeStop(GPS_RECV_ACK_TIMER); /* ACK reception monitoring timer */ + + /** Stop the Navigation Providing Monitor Timer */ + (void)DevGpsTimeStop(GPS_NAVIFST_TIMER); /* Initial Navigation Monitoring Timer */ + + /** Stop the Navigation Providing Monitor Timer */ + (void)DevGpsTimeStop(GPS_NAVICYCLE_TIMER); /* Navi monitoring timer */ + + /** Stop the Navigation Providing Monitor Timer */ + (void)DevGpsTimeStop(GPS_NAVIDISRPT_TIMER); /* Navigation Monitoring Disruption Log Output Timer */ + + /** Stop Time Offering Monitor Timer */ + /* (void)DevGpsTimeStop(GPS_DIAGCLK_GUARDTIMER); */ /* Time Offering Monitoring Timer */ + + PosTeardownThread(ETID_POS_GPS); + + /* don't arrive here */ + return; +} + +/** + * @brief + * GPS format order determination process + * + * @param[in] s_format_1 GPS format1 + * @param[in] s_format_2 GPS format2 + * @return 0 Format 1 first
+ * 1 Format 2 first
+ * 2 Same as
+ * 3 Unable to determine + */ +uint8_t JudgeFormatOrderGpsCommon(TG_GPS_OUTPUT_FORMAT s_format_1, TG_GPS_OUTPUT_FORMAT s_format_2) { + uint8_t ret = 3; + u_int32 cnt = 0; + + if (s_format_1 == s_format_2) { + ret = 2; + } else if ((s_format_1 == GPS_FORMAT_MAX) || (s_format_2 == GPS_FORMAT_MIN)) { + ret = 1; + } else { + for (cnt = 0; cnt < static_cast(GPSCMDANATBL_MAX); cnt++) { + /** End-of-table decision */ + if (CheckFrontStringPartGpsCommon((const u_char*)ENDMARK, kGpsCmdAnaTbl[cnt].c_sentence) == RET_NORMAL) { + /** There is no matching sentence */ + ret = 3; + break; + } + + if (kGpsCmdAnaTbl[cnt].e_rcv_format == s_format_1) { + ret = 0; + break; + } else if (kGpsCmdAnaTbl[cnt].e_rcv_format == s_format_2) { + ret = 1; + break; + } + } + } + + return ret; +} + +/** + * @brief + * Get NMEA index process + * + * @param[in] iFmt GPS formats + * @return NMEA indexes + */ +ENUM_GPS_NMEA_INDEX GetNmeaIdxGpsCommon(TG_GPS_OUTPUT_FORMAT s_output_format) { + ENUM_GPS_NMEA_INDEX ret = GPS_NMEA_INDEX_MAX; + + switch (s_output_format) { + case GPS_FORMAT_GGA: + /* GGA */ + ret = GPS_NMEA_INDEX_GGA; + break; + case GPS_FORMAT_DGGA: + /* DGGA */ + ret = GPS_NMEA_INDEX_DGGA; + break; + case GPS_FORMAT_VTG: + /* VTG */ + ret = GPS_NMEA_INDEX_VTG; + break; + case GPS_FORMAT_RMC: + /* RMC */ + ret = GPS_NMEA_INDEX_RMC; + break; + case GPS_FORMAT_DRMC: + /* DRMC */ + ret = GPS_NMEA_INDEX_DRMC; + break; + case GPS_FORMAT_GLL: + /* GLL */ + ret = GPS_NMEA_INDEX_GLL; + break; + case GPS_FORMAT_DGLL: + /* DGLL */ + ret = GPS_NMEA_INDEX_DGLL; + break; + case GPS_FORMAT_GSA: + /* GSA */ + ret = GPS_NMEA_INDEX_GSA; + break; + case GPS_FORMAT_GSV1: + /* GSV(1) */ + ret = GPS_NMEA_INDEX_GSV1; + break; + case GPS_FORMAT_GSV2: + /* GSV(2) */ + ret = GPS_NMEA_INDEX_GSV2; + break; + case GPS_FORMAT_GSV3: + /* GSV(3) */ + ret = GPS_NMEA_INDEX_GSV3; + break; + case GPS_FORMAT_GSV4: + /* GSV(4) */ + ret = GPS_NMEA_INDEX_GSV4; + break; + case GPS_FORMAT_GSV5: + /* GSV(5) */ + ret = GPS_NMEA_INDEX_GSV5; + break; + case GPS_FORMAT_GST: + /* GST */ + ret = GPS_NMEA_INDEX_GST; + break; + case GPS_FORMAT__CWORD44__GP3: + /* _CWORD44_,GP,3 */ + ret = GPS_NMEA_INDEX__CWORD44__GP_3; + break; + case GPS_FORMAT__CWORD44__GP4: + /* _CWORD44_,GP,4 */ + ret = GPS_NMEA_INDEX__CWORD44__GP_4; + break; + case GPS_FORMAT_P_CWORD82_F_GP0: + /* P_CWORD82_F,GP,0 */ + ret = GPS_NMEA_INDEX_P_CWORD82_F_GP_0; + break; + case GPS_FORMAT_P_CWORD82_J_GP1: + /* P_CWORD82_J,GP,1 */ + ret = GPS_NMEA_INDEX_P_CWORD82_J_GP_1; + break; + default: + /* nop */ + break; + } + return ret; +} + +/** + * @brief + * GPS reset request command transmission processing + */ +void SendResetReqGpsCommon(void) { + T_APIMSG_MSGBUF_HEADER s_msg_header; /* Message header */ + POS_RESETINFO s_send_msg; /* Reset information */ + u_int8 uc_send_buf[(sizeof(T_APIMSG_MSGBUF_HEADER) + sizeof(POS_RESETINFO))]; /* Transmitting buffer */ + RET_API ret = RET_NORMAL; + + /* Send reset request */ + POSITIONING_LOG("GPS Reset Request"); + + /* <>>> */ + s_msg_header.signo = 0; /* Signal information setting */ + s_msg_header.hdr.sndpno = PNO_NAVI_GPS_MAIN; /* Source thread No. setting */ + s_msg_header.hdr.respno = PNO_NAVI_GPS_MAIN; /* Destination process No. */ + s_msg_header.hdr.cid = CID_GPS_REQRESET; /* Command ID setting */ + s_msg_header.hdr.msgbodysize = sizeof(POS_RESETINFO); /* Message data length setting */ + s_msg_header.hdr.rid = 0; /* Resource ID Setting */ + s_msg_header.hdr.reserve = 0; /* Reserved area clear */ + + (void)memcpy(&uc_send_buf[ 0 ], &s_msg_header, sizeof(T_APIMSG_MSGBUF_HEADER)); + + /* <> */ + s_send_msg.mode = GPS_RST_COLDSTART; /* Reset mode */ + s_send_msg.sndpno = PNO_NAVI_GPS_MAIN; /* Caller PNo. */ + s_send_msg.respno = PNO_NAVI_GPS_MAIN; /* Destination PNo. */ + s_send_msg.reserve[0] = 0; + s_send_msg.reserve[1] = 0; + s_send_msg.reserve[2] = 0; + + /* Copy data to send buffer */ + (void)memcpy( &uc_send_buf[ sizeof( T_APIMSG_MSGBUF_HEADER ) ], &s_send_msg, sizeof(POS_RESETINFO) ); + + /* <> */ + ret = _pb_SndMsg(PNO_NAVI_GPS_MAIN, sizeof(uc_send_buf), reinterpret_cast(uc_send_buf), 0); + + if (ret != RET_NORMAL) { + POSITIONING_LOG("GPS Reset Request Error[ret = %d]", ret); + } +} + + + +void DevGpsCommSettingTime(void) { + TG_GPS_SND_DATA *pst_rcv_data; + RET_API ret = RET_NORMAL; + + pst_rcv_data = reinterpret_cast(&(g_gps_msg_rcvr.msgdat[0])); + + ret = DevGpsResetReq(PNO_NONE, 0); + if (RET_NORMAL != ret) { + POSITIONING_LOG("DevGpsResetReq failed."); + } else { + /* Time setting(u-blox) */ + DevGpsSettingTime(reinterpret_cast(pst_rcv_data->ub_data)); + } + return; +} + +void DevGpsReadGpsTime(POS_DATETIME* pst_datetime) { + int32_t ret; + ST_GPS_SET_TIME buf; + POS_DATETIME st_datetime; + memset(&st_datetime, 0, sizeof(st_datetime)); + + // if (isGpsTimeStatusProof == TRUE) { + /* Clear the flag if the date and time status has already been calibrated. */ + // WriteGpsTimeToBackup(FALSE, &st_datetime); + // POSITIONING_LOG("D_BK_ID_POS_GPS_TIME_SET_INFO:W:Clear"); + // } else { + /* Read from backup RAM */ + ret = Backup_DataRd(D_BK_ID_POS_GPS_TIME_SET_INFO, 0, &buf, sizeof(ST_GPS_SET_TIME)); + if (ret == BKUP_RET_NORMAL) { + pst_datetime->year = buf.year; + pst_datetime->month = buf.month; + pst_datetime->date = buf.date; + pst_datetime->hour = buf.hour; + pst_datetime->minute = buf.minute; + pst_datetime->second = buf.second; + + /* Update of the date/time status fixed value setting indication flag */ + if (buf.flag == TRUE) { + g_is_gpstime_sts = TRUE; + POSITIONING_LOG("g_is_gpstime_sts = TRUE"); + POSITIONING_LOG("%d/%d/%d %d:%d:%d", buf.year, + buf.month, + buf.date, + buf.hour, + buf.minute, + buf.second); + } else { + g_is_gpstime_sts = FALSE; + POSITIONING_LOG("g_is_gpstime_sts = FALSE"); + } + } else { + POSITIONING_LOG("Backup_DataRd ERROR!! [ret=%d]", ret); + } + // } + return; +} + +RET_API GpsSetPosBaseEvent(uint16_t ul_snd_pno, int32_t event_val) { + RET_API ret = RET_ERROR; /* Return value */ + EventID ul_eventid = 0; /* Event ID */ + char event_name[32]; /* Event name character string buffer */ + + memset(reinterpret_cast(event_name), 0, sizeof(event_name)); + snprintf(event_name, sizeof(event_name), "VEHICLE_%X", ul_snd_pno); + /* Event Generation */ + ul_eventid = _pb_CreateEvent(FALSE, 0, event_name); + + // initialize EventID + if (0 != ul_eventid) { + ret = _pb_SetEvent(ul_eventid, SAPI_EVSET_ABSOLUTE, VEHICLE_EVENT_VAL_INIT); + if (RET_NORMAL != ret) { + /* Delete event and return event generation failed */ + ret = _pb_DeleteEvent(ul_eventid); + ul_eventid = 0; + } + } + + if (0 != ul_eventid) { + /* Event publishing(Event wait release) */ + ret = _pb_SetEvent(ul_eventid, SAPI_EVSET_ABSOLUTE, event_val); + if (RET_NORMAL != ret) { + /* Event issuance failure */ + POSITIONING_LOG("set Event failed."); + } + /* Event deletion */ + _pb_DeleteEvent(ul_eventid); + } else { + /* Event generation failure */ + POSITIONING_LOG("create Event failed."); + } + return ret; +} + +/** + * @brief + * GPS reset response command reception processing + * + * @param[in] p_data Pointer to the received GPS command + */ +BOOL DevGpsRcvProcGpsResetResp(TG_GPS_RCV_DATA* p_data) { + RET_API ret; + u_int32 ul_reset_sts = GPS_SENDNG; + BOOL b_snd_flag = TRUE; + uint8_t *p_gps_data; + char date[6]; /* ddmmyy */ + char work[8]; /* Work buffer for converting String data */ + + if (p_data == NULL) { + POSITIONING_LOG("Argument ERROR!! [p_data = %p]", p_data); + } else { + p_gps_data = p_data->bygps_data; + + /* Checksum determination */ + ret = CheckSumGpsCommon(p_gps_data, p_data->bydata_len, GPS_FORMAT_RMC); + if (ret == RET_NORMAL) { + /* Get Date information */ + GetStringFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_DATE, p_gps_data, date, 6); + + strncpy(work, &date[4], 2); + + if (atoi(work) == 0) { + ul_reset_sts = GPS_SENDOK; /* Reset Successful */ + /* Reissuing commands to a GPS device from here */ + /* Send NAV-TIMEUTC sentence addition requests */ + DevGpsNavTimeUTCAddReq(); + } + POSITIONING_LOG("reset flag = %d, year after reset = %s", ul_reset_sts, work); + } else { + b_snd_flag = FALSE; /* If the checksum is NG, wait for the next reply message without sending a callback. */ + POSITIONING_LOG("wrong checksum [ret = %d]", ret); + } + + if (b_snd_flag == TRUE) { + /** Drop the time status to an abnormal time */ + // Todo Suntec MACRO __CWORD71_ Disable, This Functions do nothing actrually. + // DEV_Gps_SetTimeStatus_NG(); + + /** Send Reset Response(Successful transmission) */ + DevGpsRstAnsSend(g_gps_mngr.resp_pno, g_gps_mngr.resp_rid, ul_reset_sts); + + POSITIONING_LOG("DEV_Gps_RstAndSend CALL!! [ul_reset_sts = %d]", ul_reset_sts); + + /** Clear reset command sending judgment flag */ + g_gps_reset_cmd_sending = FALSE; + } + } + return b_snd_flag; +} + +/** + * @brief + * GPS time setting response command reception processing + * + * @param[in] p_data Pointer to the received GPS command + */ +void DevGpsRcvProcTimeSetResp(TG_GPS_RCV_DATA* p_data) { + u_int32 ul_reset_sts = GPS_SENDNG; + BOOL b_snd_flag = TRUE; + POS_DATETIME st_datetime; /* Setting time */ + uint16_t set_gpsw; + uint32_t set_gpsws; + uint16_t rcv_gpsw; + uint32_t rcv_gpsws; + BOOL ret = FALSE; + int32_t status; + + if (p_data == NULL) { + POSITIONING_LOG("Argument ERROR!! [p_data = %p]", p_data); + } else { + /* Read from backup RAM */ + status = Backup_DataRd(D_BK_ID_POS_GPS_TIME_SET_INFO, 0, &st_datetime, sizeof(ST_GPS_SET_TIME)); + if (status == BKUP_RET_NORMAL) { + /* Convert to GSP week number, GPS week second */ + DevGpsYMD2GPSTIME(static_cast(st_datetime.year), + static_cast(st_datetime.month), + static_cast(st_datetime.date), static_cast(st_datetime.hour), + static_cast(st_datetime.minute), + static_cast(st_datetime.second), &set_gpsw, &set_gpsws); + + memcpy(&rcv_gpsw, &(p_data->bygps_data[24]), sizeof(uint16_t)); + memcpy(&rcv_gpsws, &(p_data->bygps_data[26]), sizeof(uint32_t)); + rcv_gpsws /= 1000; /* Convert week (ms) to week (s) */ + POSITIONING_LOG("recived gpsw[%d] gpsws[%d]\n", rcv_gpsw, rcv_gpsws); + + /* Check if GPS week number and GPS week second are within valid range */ + ret = DevGpsCheckGpsTime(set_gpsw, set_gpsws, rcv_gpsw, rcv_gpsws, GPS_SETTIME_RANGE); + if (ret == TRUE) { + /* Date/Time Status Fixed Value Setting Indication Flag ON */ + g_is_gpstime_sts = TRUE; + POSITIONING_LOG("g_is_gpstime_sts = TRUE"); + + /* Time Setting Successful : Update backup RAM time */ + ul_reset_sts = GPS_SENDOK; + WriteGpsTimeToBackup(TRUE, &st_datetime); + POSITIONING_LOG("SetGpsTime Success"); + POSITIONING_LOG("D_BK_ID_POS_GPS_TIME_SET_INFO:W:%d/%d/%d %d:%d:%d", + st_datetime.year, + st_datetime.month, + st_datetime.date, + st_datetime.hour, + st_datetime.minute, + st_datetime.second); + } else { + /* Time setting error (Difference in set time): Time deletion of backup RAM */ + memset(&st_datetime, 0, sizeof(st_datetime)); + WriteGpsTimeToBackup(FALSE, &st_datetime); + POSITIONING_LOG("SetGpsTime failed\n"); + POSITIONING_LOG("D_BK_ID_POS_GPS_TIME_SET_INFO:W:Clear\n"); + } + } else { + /* Backup RAM read failed */ + b_snd_flag = FALSE; + POSITIONING_LOG("Backup_DataRd ERROR!! [status=%d", status); + } + + if (b_snd_flag == TRUE) { + /** Send Time Set Response */ + DevGpsTimesetAnsSend(g_gps_mngr.resp_pno, g_gps_mngr.resp_rid, ul_reset_sts); + + POSITIONING_LOG("DEV_Gps_TimesetAndSend CALL!! [ul_reset_sts = %d]", ul_reset_sts); + } + } + return; +} + +/** + * @brief + * GPS rollover standard week number acquisition response reception processing + * + * @param[in] p_data Pointer to the received GPS command + */ +void DevGpsRcvProcWknRolloverGetResp(TG_GPS_RCV_DATA* p_data) { + RET_API ret; + SENSOR_WKN_ROLLOVER_HAL st_wkn_rollover; + + if (p_data == NULL) { + POSITIONING_LOG("Argument ERROR!! [p_data = %p]", p_data); + } else { + /* Checksum determination */ + ret = CheckSumGpsCommon(p_data->bygps_data, p_data->bydata_len, GPS_FORMAT_CFG_NAVX5); + if (ret == RET_NORMAL) { + /* Version information extraction */ + (void)memcpy(&(st_wkn_rollover.us_wkn), + &(p_data->bygps_data[UBX_CMD_OFFSET_PAYLOAD + UBX_CMD_OFFSET_WKNROLLOVER]), + sizeof(st_wkn_rollover.us_wkn)); + POSITIONING_LOG("wknRollover=%d", st_wkn_rollover.us_wkn); + + // gwknRollover = st_wkn_rollover.us_wkn; + + /* Send reference week number to vehicle sensor */ + ret = DevGpsSndWknRollover(&st_wkn_rollover); + if (ret != RET_NORMAL) { + POSITIONING_LOG("VehicleUtility_pb_SndMsg ERROR!! [ret=%d]", ret); + } + } else { + POSITIONING_LOG("CheckSumGpsCommon ERROR!! [ret = %d]", ret); + } + + if (ret != RET_NORMAL) { + /* GPS rollover standard week number acquisition request retransmission */ + ret = DevGpsWknRolloverGetReq(); + if (ret != RET_NORMAL) { + POSITIONING_LOG("DevGpsWknRolloverGetReq ERROR!! [ret=%d]", ret); + } + } + } + + return; +} + +/** + * @brief + * Receiving additional responses to NAV-SVINFO commands + * + * @param[in] p_data Pointer to the received GPS command + */ +void DevGpsRcvProcNavSvinfoAddResp(TG_GPS_RCV_DATA* p_data) { + RET_API ret; + /* Common Response Command Reception Processing */ + ret = DevGpsRcvProcCmnResp(p_data, GPS_CMD_SENTENCEADD_NAVSVINFO); + if (ret != RET_NORMAL) { + POSITIONING_LOG("DEV_Gps_RcvProcCmnResp(ADD_NAVSVINFO) ERROR!!"); + /* Retransmission of requests to add NAV-SVINFO commands */ + (void)DevGpsNavTimeUTCAddReq(); + } + return; +} + + +/** + * @brief + * Common processing when a GPS receiver error is detected + */ +void GpsReceiverErrGpsCommon(void) { + BOOL *pb_rcv_err = &(g_gps_mngr.rcv_err_flag); /* GSP receiver error detection status */ + + /* Update GPS receiver anomaly detection flag */ + *pb_rcv_err = TRUE; + + DevSendGpsConnectError(TRUE); + + return; +} + + +/** + * @brief + * GPS device setting response reception processing + * + * @param[in] p_data Pointer to the received GPS command + * @param[in] cmd Command in the configuration request + * @return RET_NORMAL Normal completion
+ * RET_ERROR ABENDs + */ +RET_API DevGpsRcvProcCmnResp(TG_GPS_RCV_DATA* p_data, uint8_t cmd) { + RET_API ret = RET_ERROR; + uint8_t is_ack; /* 1=ACK, 0=NACK */ + TG_GPS_UBX_ACK_DATA st_ack; + uint8_t *puc_msg_class = &(st_ack.uc_msg_class); + uint8_t *puc_msg_id = &(st_ack.uc_msg_id); + + if (p_data == NULL) { + POSITIONING_LOG("Argument ERROR!![p_data=%p]", p_data); + } else { + /* Checksum determination */ + ret = CheckSumGpsCommon(p_data->bygps_data, p_data->bydata_len, GPS_FORMAT_ACK_ACKNACK); + if (ret == RET_NORMAL) { + is_ack = p_data->bygps_data[UBX_CMD_OFFSET_ACKNAK]; + if (is_ack == 1) { + /* Response command information extraction */ + (void)memcpy(&st_ack, &(p_data->bygps_data[UBX_CMD_OFFSET_PAYLOAD]), sizeof(st_ack)); + POSITIONING_LOG("msgClass=0x%02x, msgID=0x%02x", *puc_msg_class, *puc_msg_id); + + /* When the response wait command does not match the reception command */ + if (((cmd == GPS_CMD_SENTENCEADD_NMEAGST) && ((*puc_msg_class != 0x06) || (*puc_msg_id != 0x01))) || + ((cmd == GPS_CMD_SENTENCEADD_NAVTIMEUTC) && ((*puc_msg_class != 0x06) || (*puc_msg_id != 0x01))) || + ((cmd == GPS_CMD_SENTENCEADD_NAVCLOCK) && ((*puc_msg_class != 0x06) || (*puc_msg_id != 0x01))) || + ((cmd == GPS_CMD_SENTENCEADD_RXMRTC5) && ((*puc_msg_class != 0x06) || (*puc_msg_id != 0x01))) || + ((cmd == GPS_CMD_SENTENCEADD_NAVSVINFO) && ((*puc_msg_class != 0x06) || (*puc_msg_id != 0x01))) || + ((cmd == GPS_CMD_AUTOMOTIVEMODE) && ((*puc_msg_class != 0x06) || (*puc_msg_id != 0x24)))) { + POSITIONING_LOG("Invalid ACK/NACK!! [cmd=%d, msgClass=0x%02x, msgID=0x%02x]\n", + cmd, *puc_msg_class, *puc_msg_id); + ret = RET_ERROR; + } + } else { + ret = RET_ERROR; + } + } else { + POSITIONING_LOG("DEV_Gps_ChkSum ERROR!![ret=%d]", ret); + } + } + return ret; +} + + +RET_API DevGpsResetReq(PNO us_pno, RID uc_rid) { + RET_API ret; + TG_GPS_SAVECMD st_save_cmd; + TG_GPS_SND_DATA* pst_snd_data; + uint8_t* p_ublox_data; + + /* SYNC_CHAR_1_2 CLASS ID length(L/U) */ + static const u_int8 csnd_cmd[] = { 0xB5, 0x62, 0x06, 0x04, 0x04, 0x00, + /* navBbrMask(L/U) resetMode/res CK_A CK_B */ + 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00 }; + + pst_snd_data = &(st_save_cmd.sndcmd); + p_ublox_data = pst_snd_data->ub_data; + + memset(&st_save_cmd, 0x00, sizeof(st_save_cmd)); + memcpy(p_ublox_data, csnd_cmd, UBX_CMD_SIZE_CFG_RST); + + /* Checksum assignment */ + DevGpsSetChkSum(p_ublox_data, UBX_CMD_SIZE_CFG_RST); + + pst_snd_data->us_size = UBX_CMD_SIZE_CFG_RST; + st_save_cmd.us_pno = us_pno; /* Result notification destination process number */ + st_save_cmd.uc_rid = uc_rid; /* Result Notification Resource ID */ + st_save_cmd.uc_rst = GPS_CMD_RESET; /* Reset flag */ + st_save_cmd.e_cmd_info = GPS_FORMAT_RMC; /* Command information */ + + /* Command is suspended and terminated. */ + ret = DevGpsSaveCmd(&st_save_cmd); + if (ret != RET_NORMAL) { + POSITIONING_LOG("# GpsCommCtl # GPS Command Reserve bufferFull !"); + + /** When the pending buffer is full, a reset response (communication error) is returned. */ + DevGpsRstAnsSend(st_save_cmd.us_pno, st_save_cmd.uc_rid, GPS_SENDNG); + } + + return ret; +} + +void DevGpsSettingTime(POS_DATETIME* pst_datetime) { + RET_API ret; + TG_GPS_SAVECMD st_save_cmd; + TG_GPS_SND_DATA* pst_snd_data; + uint8_t* p_ublox_data; + TG_GPS_UBX_AID_INI_POLLED* pst_ublox_data; + + uint16_t gpsw; + uint32_t gpsws; + + if (pst_datetime == NULL) { + POSITIONING_LOG("Argument ERROR!! [pstDataTime = %p]", pst_datetime); + } else { + pst_snd_data = &(st_save_cmd.sndcmd); + p_ublox_data = pst_snd_data->ub_data; + + (void)memset(&st_save_cmd, 0x00, sizeof(st_save_cmd)); + + /* Convert to GSP week number, GPS week second */ + (void)DevGpsYMD2GPSTIME(static_cast(pst_datetime->year), + static_cast(pst_datetime->month), + static_cast(pst_datetime->date), + static_cast(pst_datetime->hour), + static_cast(pst_datetime->minute), + static_cast(pst_datetime->second), &gpsw, &gpsws); + + + /* Send command sequence generation */ + pst_ublox_data = reinterpret_cast(reinterpret_cast(p_ublox_data)); + pst_ublox_data->uc_sync_char1 = 0xB5; + pst_ublox_data->uc_sync_char2 = 0x62; + pst_ublox_data->uc_class = 0x0B; + pst_ublox_data->uc_id = 0x01; + pst_ublox_data->uc_length[0] = 0x30; + pst_ublox_data->uc_length[1] = 0x00; + pst_ublox_data->uc_flags[0] = 0x02; /* Time setting: The second bit from the low order is 1. */ + + (void)memcpy(pst_ublox_data->wn, &gpsw, sizeof(uint16_t)); + + gpsws *= 1000; + (void)memcpy(pst_ublox_data->tow, &gpsws, sizeof(uint32_t)); + + /* Checksum assignment */ + DevGpsSetChkSum(reinterpret_cast(reinterpret_cast(pst_ublox_data)), + UBX_CMD_SIZE_AID_INI); + + /* Concatenate Poll Request Command */ + ret = DevGpsCatPollReq(p_ublox_data, UBX_POLL_CMD_KIND_AID_INI); + if (ret != RET_NORMAL) { + POSITIONING_LOG("DEV_Gps_CatPollReqUblox ERROR!! [ret = %d]", ret); + } else { + pst_snd_data->us_size = UBX_CMD_SIZE_AID_INI_PLUS_POLL; + st_save_cmd.us_pno = PNO_VEHICLE_SENSOR; /* Result notification destination process number */ + st_save_cmd.uc_rid = 0; /* Result Notification Resource ID */ + st_save_cmd.uc_rst = GPS_CMD_TIMESET; /* Response flag */ + st_save_cmd.e_cmd_info = GPS_FORMAT_AID_INI;/* Command information */ + + /* Command is suspended and terminated. */ + ret = DevGpsSaveCmd(&st_save_cmd); + if (ret != RET_NORMAL) { + POSITIONING_LOG("DevGpsSaveCmd ERROR!! [ret = %d]", ret); + /* Todo: retransmission by timer */ + } + /* Update non-volatile of time setting */ + WriteGpsTimeToBackup(FALSE, pst_datetime); + /* Waiting for a GPS device to respond */ + g_is_gpstime_sts = FALSE; + POSITIONING_LOG("Start SetGpsTime\n"); + POSITIONING_LOG("D_BK_ID_POS_GPS_TIME_SET_INFO:W:%d/%d/%d %d:%d:%d\n", + pst_datetime->year, + pst_datetime->month, + pst_datetime->date, + pst_datetime->hour, + pst_datetime->minute, + pst_datetime->second); + } + } + return; +} + +/** + * @brief + * Converting from the Gregorian calendar to the Julian Day of Modification + * + * @param[in] y year + * @param[in] m month + * @param[in] d day + * + * @return Modified Julian Date + */ +int DevGpsYMD2JD(int y, int m, int d) { + double jd = JULIAN_DAY; + double tmp; + int mjd = 0; + + if (m <= 2) { /* In January, February in December and 14th of the previous year */ + y--; + m += 12; + } + y += 4800; + m -= 3; + d -= 1; + + tmp = static_cast(y / 400); + y %= 400; + jd += tmp * 146097; + + tmp = static_cast(y / 100); + y %= 100; + jd += tmp * 36524; + + tmp = static_cast(y / 4); + y %= 4; + jd += tmp * 1461; + jd += static_cast(y * 365); + + tmp = static_cast(m / 5); + m %= 5; + jd += tmp * 153; + + tmp = static_cast(m / 2); + m %= 2; + jd += tmp * 61; + jd += static_cast(m * 31); + jd += static_cast(d); + + mjd = static_cast(jd - MODIFIED_JULIAN_DAY_OFFSET); /* Julian Date-> Modified Julian Date */ + + return mjd; +} + +/** + * @brief + * Converting from year, month, day, hour, minute and second (GPS hour) to GPS week and GPS week seconds + * + * Converting y:year, m:month, d:day, hh:hour, mm:minute, ss:second (GPS hour) to GPS week/GPS week second + * + * @param[in] y year + * @param[in] m month + * @param[in] d day + * @param[in] hh hour + * @param[in] mm munites + * @param[in] ss sec + * @param[out] gpsw + * @param[out] gpsws + * + * @return TRUE / FALSE + */ +BOOL DevGpsYMD2GPSTIME(const int32 y, const int32 m, const int32 d, const int32 hh, + const int32 mm, const int32 ss, u_int16* gpsw, u_int32* gpsws) { + /* + MJD :Y year M month D date modified Julian date (Convert date to modified Julian date) + GPSW : GPS week + GPSWS: GPS week second + */ + int mjd = DevGpsYMD2JD(y, m, d); + + *gpsw = (uint16_t)((mjd - 44244) / 7); + *gpsws = (uint32_t)(((mjd - 44244 - (*gpsw * 7)) * 86400) + + (hh * 3600) + (mm * 60) + ss + 16); /* Add leap seconds ( 16 seconds) at 2015/1/1 */ + + if (*gpsws >= GPS_WS_MAX) { + (*gpsw)++; + *gpsws -= GPS_WS_MAX; + } + + POSITIONING_LOG("*gpsw = %d, *gpsws = %d", *gpsw, *gpsws); + + return TRUE; +} + +/** + * @brief + * Concatenating Poll Request commands to UBX commands + * + * @param[in/out] *p_ublox_data Pointer to the concatenated UBX command + * @param[in] kind Additional command types + * + * @return RET_NORAML / RET_ERROR + */ +RET_API DevGpsCatPollReq(uint8_t *p_ublox_data, uint16_t kind) { + RET_API ret_api = RET_NORMAL; + TG_GPS_UBX_PACKET_HEADER *pst_ubx_pkg_hdr; + TG_GPS_UBX_COMMAND_NO_DATA *pst_ubx_poll_req; + uint32_t length; + + /* Header ID length Checksum */ + static uint8_t c_aidini_poll_cmd[] = {0xB5, 0x62, 0x0B, 0x01, 0x00, 0x00, 0x00, 0x00}; + static uint8_t c_cfgnav5_poll_cmd[] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x00, 0x00, 0x00}; + + uint8_t* p_poll_cmd; + uint16_t poll_cmd_length; + + if (p_ublox_data == NULL) { + POSITIONING_LOG("Argument ERROR!! [p_ublox_data=%p]", p_ublox_data); + ret_api = RET_ERROR; + } else { + if (kind == UBX_POLL_CMD_KIND_AID_INI) { + p_poll_cmd = c_aidini_poll_cmd; + poll_cmd_length = UBX_CMD_SIZE_AID_INI_POLL; + } else if (kind == UBX_POLL_CMD_KIND_CFG_NAV5) { + p_poll_cmd = c_cfgnav5_poll_cmd; + poll_cmd_length = UBX_CMD_SIZE_CFG_NAV5_POLL; + } else { + POSITIONING_LOG("Argument ERROR!! [kind=%d]", kind); + ret_api = RET_ERROR; + } + + if (ret_api != RET_ERROR) { + pst_ubx_pkg_hdr = reinterpret_cast(reinterpret_cast(p_ublox_data)); + length = pst_ubx_pkg_hdr->us_length; + + if (length == 0) { + POSITIONING_LOG("pst_ubx_pkg_hdr->us_length=0 ERROR!!"); + ret_api = RET_ERROR; + } else { + /* Add Poll Request Command */ + pst_ubx_poll_req = reinterpret_cast(reinterpret_cast(p_ublox_data + + length + + sizeof(TG_GPS_UBX_COMMAND_NO_DATA))); + + memcpy(reinterpret_cast(pst_ubx_poll_req), reinterpret_cast(p_poll_cmd), poll_cmd_length); + + DevGpsSetChkSum(reinterpret_cast(reinterpret_cast(pst_ubx_poll_req)), poll_cmd_length); + } + } + } + + return ret_api; +} + +void DevGpsSetGpsweekcorcnt(void) { + CLOCKGPS_GPSWEEKCOR_CNT_MSG gpsweekcor_cnt_msg; + memcpy(&gpsweekcor_cnt_msg, &g_gps_msg_rcvr, sizeof(CLOCKGPS_GPSWEEKCOR_CNT_MSG)); + g_gps_week_cor_cnt = gpsweekcor_cnt_msg.gpsweekcorcnt; + return; +} + +RET_API DevGpsNavTimeUTCAddReq(void) { + RET_API ret; + TG_GPS_SAVECMD st_save_cmd; + TG_GPS_SND_DATA* pst_snd_data; + uint8_t* p_ublox_data; + + /* SYNC_CHAR_1_2 CLASS ID length(L/U) Payload CK_A CK_B */ + static const u_int8 csnd_cmd[] = { 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x21, 0x01, 0x00, 0x00 }; + + pst_snd_data = &(st_save_cmd.sndcmd); + p_ublox_data = pst_snd_data->ub_data; + + memset(&st_save_cmd, 0x00, sizeof(st_save_cmd)); + + /* Send command generation */ + memcpy(p_ublox_data, csnd_cmd, UBX_CMD_SIZE_CFG_MSG); + + /* Checksum assignment */ + DevGpsSetChkSum(p_ublox_data, UBX_CMD_SIZE_CFG_MSG); + + pst_snd_data->us_size = UBX_CMD_SIZE_CFG_MSG; + st_save_cmd.us_pno = PNO_VEHICLE_SENSOR; /* Result notification destination process number */ + st_save_cmd.uc_rid = 0; /* Result notification resource ID */ + st_save_cmd.uc_rst = GPS_CMD_SENTENCEADD_NAVTIMEUTC; /* Response flag */ + st_save_cmd.e_cmd_info = GPS_FORMAT_ACK_ACKNACK; /* Command information */ + + /* Command is suspended and terminated. */ + ret = DevGpsSaveCmd(&st_save_cmd); + if (ret != RET_NORMAL) { + POSITIONING_LOG("# GpsCommCtl # GPS Command Reserve bufferFull !"); + } + + return ret; +} + +RET_API DevGpsWknRolloverGetReq(void) { + RET_API ret; + TG_GPS_SAVECMD st_save_cmd; + TG_GPS_SND_DATA* pst_snd_data; + uint8_t* p_ublox_data; + + /* SYNC_CHAR_1_2 CLASS ID length(L/U) CK_A CK_B */ + static const u_int8 csnd_cmd[] = { 0xB5, 0x62, 0x06, 0x23, 0x00, 0x00, 0x00, 0x00 }; + + pst_snd_data = &(st_save_cmd.sndcmd); + p_ublox_data = pst_snd_data->ub_data; + + (void)memset(&st_save_cmd, 0x00, sizeof(st_save_cmd)); /* QAC 3200 */ + + /* Send command generation */ + (void)memcpy(p_ublox_data, csnd_cmd, UBX_CMD_SIZE_CFG_NAVX5_POLL); /* QAC 3200 */ + + /* Checksum assignment */ + DevGpsSetChkSum(p_ublox_data, UBX_CMD_SIZE_CFG_NAVX5_POLL); + + pst_snd_data->us_size = UBX_CMD_SIZE_CFG_NAVX5_POLL; + st_save_cmd.us_pno = PNO_VEHICLE_SENSOR; /* Result notification destination process number */ + st_save_cmd.uc_rid = 0; /* Result notification resource ID */ + st_save_cmd.uc_rst = GPS_CMD_WKNROLLOVER; /* Response flag */ + st_save_cmd.e_cmd_info = GPS_FORMAT_CFG_NAVX5; /* Command information */ + + /* Command is suspended and terminated. */ + ret = DevGpsSaveCmd(&st_save_cmd); + if (ret != RET_NORMAL) { + POSITIONING_LOG("# GpsCommCtl # GPS Command Reserve bufferFull !"); + /* If an error occurs at this point that is not expected due to a command queue full error, retransmission control is required. */ + } + return ret; +} + +/** + * @brief + * GPS time setting validity check process + * + * @param[in] set_gpsw Set GPS week + * @param[in] set_gpsws Set GPS Week + * @param[in] rcv_gpsw Answer GPS Week + * @param[in] rcv_gpsws Answer GPS Weeksec + * @param[in] offset_range Tolerance time difference[sec] + * @return BOOL TRUE:Effective time / FALSE:Invalid time + */ +BOOL DevGpsCheckGpsTime(uint16_t set_gpsw, uint32_t set_gpsws, + uint16_t rcv_gpsw, uint32_t rcv_gpsws, int32_t offset_range) { + BOOL ret = FALSE; + int16_t gap_gpsw = 0; + int32_t gap_gpsws = 0; + + /* Check if GPS week number and GPS week second are within valid range */ + /* NG when Set Time > Receive Time */ + gap_gpsw = (int16_t)(rcv_gpsw - set_gpsw); /* QAC 3758 */ + if (gap_gpsw == 0) { + /* There is no difference between the set GPS week and the received GPS week */ + gap_gpsws = (int32_t)(rcv_gpsws - set_gpsws); /* QAC 3771 */ + if ((0 <= gap_gpsws) && (gap_gpsws <= offset_range)) { + /* Setting of GPS time succeeded if the difference between GPS week seconds is within the range. */ + ret = TRUE; + } + } else if (gap_gpsw == 1) { + /* Difference between the set GPS week and the received GPS week is 1 (over 7 weeks) */ + gap_gpsws = (int32_t)((rcv_gpsws + GPS_WS_MAX) - set_gpsws); /* QAC 3771 */ + if ((0 <= gap_gpsws) && (gap_gpsws <= offset_range)) { + /* Setting of GPS time succeeded if the difference between GPS week seconds is within the range. */ + ret = TRUE; + } + } else { + /* Set GPS time failed except above */ + } + + return ret; +} + + +/**************************************************************************** +@brief DevGpsCnvLonLatNavi
+ Longitude/latitude information conversion process(Navigation-provided data) +@outline Converts the latitude/longitude information provided by the navigation system into the format provided to the vehicle sensor. +@param[in] u_int8 : fix_sts ... Current positioning status +@param[in] int32 : lon ... Longitude information +@param[in] int32 : lat ... Latitude-information +@param[out] SENSORLOCATION_LONLATINFO_DAT* : p_lonlat ... Latitude and longitude information after conversion +@return none +@retval none +*******************************************************************************/ +void DevGpsCnvLonLatNavi(SENSORLOCATION_LONLATINFO_DAT* p_lonlat, u_int8 fix_sts, int32 lon, int32 lat) { + int32 longtitude; + int32 latitude; + int64_t tmp; + BOOL b_ret_lon = TRUE; + BOOL b_ret_lat = TRUE; + BOOL b_fix_sts = TRUE; + + /** Provides latitude/longitude info for positioning status (2D/3D) */ + if ((NAVIINFO_DIAG_GPS_FIX_STS_2D == fix_sts ) || + (NAVIINFO_DIAG_GPS_FIX_STS_3D == fix_sts)) { + /** Get longitude information */ + tmp = (int64_t)lon * 10000000; + longtitude = static_cast(tmp / (int64_t)(256 * 60 * 60)); + if (longtitude == -1800000000) { + /** West longitude 180 degrees -> East longitude 180 degrees */ + longtitude = +1800000000; + } + if ((longtitude > +1800000000) || + (longtitude < -1800000000)) { + /** Longitude range error */ + b_ret_lon = FALSE; + } + /** Get latitude information */ + tmp = (int64_t)lat * 10000000; + + latitude = static_cast(tmp / (int64_t)(256 * 60 * 60)); + + if ((latitude > +900000000) || + (latitude < -900000000)) { + /** Latitude range error */ + b_ret_lat = FALSE; + } + } else { + /** Does not provide latitude/longitude information in non-positioning state. */ + b_fix_sts = FALSE; + } + + /** Provided data setting */ + p_lonlat->getMethod = GET_METHOD_GPS; + p_lonlat->SyncCnt = 0x00; /* Synchronization counter value(fixed to 0) */ /* Ignore -> No applicable rules for MISRA-C */ + /** Available/Unusable Decision */ + if ((b_fix_sts == TRUE) && (b_ret_lon == TRUE) && (b_ret_lat == TRUE)) { + p_lonlat->isEnable = SENSORLOCATION_STATUS_ENABLE; /* Available status: Available */ + p_lonlat->Longitude = longtitude; /* Longitude */ /* Ignore -> MISRA-C:2004 Rule 9.1 */ + p_lonlat->Latitude = latitude; /* Latitude */ /* Ignore -> MISRA-C:2004 Rule 9.1 */ + } else { + p_lonlat->isEnable = SENSORLOCATION_STATUS_DISABLE; /* Available status: Not available */ + p_lonlat->Longitude = 0; /* Longitude 0 degree fixed */ + p_lonlat->Latitude = 0; /* Latitude fixed to 0 degrees */ + } + return; +} + + +/**************************************************************************** +@brief DevGpsCnvAltitudeNavi
+ Altitude information conversion process(Navigation-provided data) +@outline Converts the altitude information provided by the navigation system into the format provided to the vehicle sensor. +@param[in] u_int8 : fix_sts ... Current positioning status +@param[in] int32 : alt ... Altitude information +@param[out] SENSORLOCATION_ALTITUDEINFO_DAT* : p_altitude ... Converted altitude information +@return none +@retval none +*******************************************************************************/ +void DevGpsCnvAltitudeNavi(SENSORLOCATION_ALTITUDEINFO_DAT* p_altitude, u_int8 fix_sts, int32 alt) { + int32 altitude; + BOOL b_fix_sts = TRUE; + + /** Provides altitude information when in Fix Status (2D/3D) */ + if (((NAVIINFO_DIAG_GPS_FIX_STS_2D == fix_sts) || + (NAVIINFO_DIAG_GPS_FIX_STS_3D == fix_sts)) && + (alt != GPS_ALTITUDE_INVALID_VAL)) { + /* The unit of alt is [0.01m], so conversion is not necessary. */ + altitude = alt; + } else { + /** Does not provide altitude information in the non-positioning state. */ + b_fix_sts = FALSE; + } + /** Provided data setting */ + p_altitude->getMethod = GET_METHOD_GPS; + p_altitude->SyncCnt = 0x00; /* Synchronization counter value(fixed to 0) */ /* Ignore -> No applicable rules for MISRA-C */ + /** Available/Unusable Decision */ + if (b_fix_sts == TRUE) { + p_altitude->isEnable = SENSORLOCATION_STATUS_ENABLE; /* Available status: Available */ + p_altitude->Altitude = altitude; /* Altitude */ /* Ignore -> MISRA-C:2004 Rule 9.1 */ + } else { + p_altitude->isEnable = SENSORLOCATION_STATUS_DISABLE; /* Available status: Not available */ + p_altitude->Altitude = 0; /* Altitude 0m fixed */ + } + return; +} + +/**************************************************************************** +@brief DevGpsCnvHeadingNavi
+ Orientation information conversion processing(Navigation-provided data) +@outline Converts the orientation information provided by the navigation system into the format provided to the vehicle sensor. +@param[in] u_int8 : fix_sts ... Current positioning status +@param[in] u_int16 : head ... Bearing information +@param[out] SENSORMOTION_HEADINGINFO_DAT* : p_heading ... Converted backward direction information +@return none +@retval none +*******************************************************************************/ +void DevGpsCnvHeadingNavi(SENSORMOTION_HEADINGINFO_DAT* p_heading, u_int8 fix_sts, u_int16 head) { + u_int16 us_heading; + BOOL b_fix_sts = TRUE; + + /** Provides orientation when in Fix Status (2D/3D) */ + if (((NAVIINFO_DIAG_GPS_FIX_STS_2D == fix_sts ) || + (NAVIINFO_DIAG_GPS_FIX_STS_3D == fix_sts)) && + (head != GPS_HEADING_INVALID_VAL)) { + /** Orientation information conversion */ + us_heading = head; + /* Units of head are [0.01 degree] so no conversion is necessary. */ + us_heading -= (u_int16)((us_heading / 36000) * 36000); + } else { + /** Azimuth information is not provided in the non-positioning state. */ + b_fix_sts = FALSE; + } + + /** Provided data setting */ + p_heading->getMethod = GET_METHOD_GPS; + p_heading->SyncCnt = 0x00; /* Synchronization counter value(fixed to 0) */ /* Ignore -> No applicable rules for MISRA-C */ + /** Available/Unusable Decision */ + if (b_fix_sts == TRUE) { + p_heading->isEnable = SENSORLOCATION_STATUS_ENABLE; /* Available status: Available */ + p_heading->Heading = us_heading; /* Orientation */ /* Ignore -> MISRA-C:2004 Rule 9.1 */ + } else { + p_heading->isEnable = SENSORLOCATION_STATUS_DISABLE; /* Available status: Not available */ + p_heading->Heading = 0; /* Azimuth 0 degree fixed */ + } + + return; +} +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/positioning_hal/src/GpsCommon/MDev_Gps_Main.cpp b/positioning_hal/src/GpsCommon/MDev_Gps_Main.cpp new file mode 100644 index 00000000..0b9a4be6 --- /dev/null +++ b/positioning_hal/src/GpsCommon/MDev_Gps_Main.cpp @@ -0,0 +1,362 @@ +/* + * @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 MDev_Gps_Main.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "MDev_Gps_Main.h" +#include "positioning_common.h" +#include "MDev_Gps_Mtrx.h" +#include "MDev_Gps_TimerCtrl.h" +#include "MDev_Gps_Common.h" +#include "MDev_Gps_Nmea.h" +#include "MDev_GpsRecv.h" +#include "LineSensDrv_Thread.h" + +/*---------------------------------------------------------------------------*/ +// Global values + +BOOL g_gps_rcv_thread_stop = FALSE; + +/******************************************************************************** + * Matrix event translation table(Message input) + ********************************************************************************/ +const TG_GPS_MSGEVTCHNG kGpsMsgchkC[MDEV_GPSMSGCHKC_MAX] = { + /* ++ #GPF_60_024 */ + /* Matrix Event Translation Table for _CWORD82_ NMEA */ + {CID_GPS__CWORD82__REQUEST, GPS_EVT_SENDREQ}, /* NMEA transmission request */ + {CID_INI_INTERNAL_ACCOFF_START, GPS_EVT_ACC_OFF}, /* In-function ACC-OFF instructions */ + {CID_GPS_REQRESET, GPS_EVT_RESETREQ}, /* GPS reset request */ + {CID_NAVIINFO_DELIVER, GPS_EVT_NAVI_LOCATIONINFO}, /* Providing navigation information */ + {CID_NAVIINFO_SPEED_DELIVER, GPS_EVT_NAVI_SPEEDINFO}, /* Navigation speed information service */ + {CID_GPS_TIMESETTING, GPS_EVT_TIMESETTING}, /* GPS time setting instruction */ + {CID_GPS_BACKUPDATA_LOAD, GPS_EVT_BACKUPDATA_LOAD}, /* Backup memory read request */ + {CID_THREAD_STOP_REQ, GPS_EVT_STOPREQ}, /* Thread stop request */ + {CID_GPSWEEKCOR_CNT_NOTIFICATION, GPS_EVT_WEEKCOR_CNT_NOTIFICATIO}, /* GPS Week Adjustment Counter Notification */ + {DAT_END, DAT_END} /* Table end */ + /* -- #GPF_60_024 */ +}; + +/******************************************************************************** + * Matrix event translation table(Input Timeout [ Timer ID]) + ********************************************************************************/ +const TG_GPS_TIMEVTCHNG kGpsTimchkC[MDEV_PSTIMCHKC_MAX] = { + {GPS_STARTUP_TIMER, 0, GPS_EVT_TIMEOUT_CYCLDAT}, /* Start confirmation monitoring timer */ + {GPS_CYCL_TIMER, 0, GPS_EVT_TIMEOUT_CYCLDAT}, /* Periodic reception data monitoring timer */ + {GPS_RECV_ACK_TIMER, 0, GPS_EVT_TIMEOUT_RSPDAT}, /* ACK reception monitoring timer */ + {GPS_NAVIFST_TIMER, 0, GPS_EVT_TIMEOUT_NAVI}, /* Initial NAVI monitoring timer */ + {GPS_NAVICYCLE_TIMER, 0, GPS_EVT_TIMEOUT_NAVI}, /* NAVIGATION monitoring timer */ + {GPS_NAVIDISRPT_TIMER, 0, GPS_EVT_TIMEOUT_NAVI}, /* NAVIGATION Disruption Log Output Timer */ + {GPS_DIAGCLK_GUARDTIMER, 0, GPS_EVT_TIMEOUT_DIAGCLKGUARD}, /* Diagnosis provision time guard monitoring timer */ + {GPS_NMEADATA_GUARDTIMER, 0, GPS_EVT_TIMEOUT_NMEADATAGUARD}, /* NMEA data-providing guard monitoring timer */ + {GPS_RECOVERY_TIMER, 0, GPS_EVT_TIMEOUT_RECOVERY}, /* GPS recovery timer */ + {GPS_RECEIVERERR_TIMER, 0, GPS_EVT_TIMEOUT_RECEIVERERR}, /* GPS receiver anomaly detection timer */ + {DAT_END, 0, DAT_END} /* Table end */ +}; + +/******************************************************************************** + * Processing Branch Table + ********************************************************************************/ +/******************************************************************************** + * TAG : TG_GPS_JMP_TBL + * ABSTRACT : GPS jump table + * NOTE : GPS Communication Management Matrix Table + ********************************************************************************/ +const TG_GPS_JMP_TBL kGpsMatxC[GPS_STS_NUM][GPS_EVT_NUM] = { + /* - - - - - Confirming Start - - - - - */ + { + {&DevGpsInitStartSendReq}, /* Transmission request */ + {&DevGpsInitStartGPSResetReq}, /* GPS reset request */ + {&DevGpsInitStartRcvCyclCmd}, /* Cyclic reception command reception */ + {&DevGpsNop}, /* Receive Response Command */ + {&DevGpsNop}, /* Response monitoring timeout */ + {&DevGpsInitStartCyclDataTimeOut}, /* Periodic reception data monitoring timeout */ + {&DevGpsInitStartNaviDataTimeOut}, /* Navigation providing data monitoring timeout */ + {&DevGpsInitStartDiagClkGuardTimeOut}, /* Diagnosis provision time guard monitoring timeout */ + {&DevGpsInitStartAccOffStart}, /* In-function ACC-OFF instructions */ + {&DevGpsInitStartNaviInfoDeliver}, /* Providing navigation information */ + {&DevGpsInitStartNaviSpeedDeliver}, /* Navigation speed information service */ + {&DevGpsInitStartSettingTime}, /* GPS time setting instruction */ + {&DevGpsInitStartNmeaDataGuardTimeOut}, /* NMEA data-providing guard monitoring timeout */ + {&DevGpsInitStartBackupDataLoadReq}, /* Backup data read request */ + {&DevGpsInitStartStopReq}, /* Thread stop request */ + {&DevGpsInitStartGpsweekcorcntNtf}, /* GPS Week Adjustment Counter Notification */ + {&DevGpsInitStartRecoveryTimeOut}, /* GPS error monitoring timer out */ + {&DevGpsInitStartGpsReceiverErrTimeOut} /* GPS receiver anomaly detection timeout */ + }, + /* - - - - - - In operation - - - - - - */ + { + {&DevGpsNormalSendReq}, /* Transmission request */ + {&DevGpsNormalGPSResetReq}, /* GPS reset request */ + {&DevGpsNormalRcvCyclCmd}, /* Cyclic reception command reception */ + {&DevGpsNop}, /* Receive Response Command */ + {&DevGpsNop}, /* Response monitoring timeout */ + {&DevGpsNormalCyclDataTimeOut}, /* Periodic reception data monitoring timeout */ + {&DevGpsNormalNaviDataTimeOut}, /* Navigation providing data monitoring timeout */ + {&DevGpsNormalDiagClkGuardTimeOut}, /* Diagnosis provision time guard monitoring timeout */ + {&DevGpsNormalAccOffStart}, /* In-function ACC-OFF instructions */ + {&DevGpsNormalNaviInfoDeliver}, /* Providing navigation information */ + {&DevGpsNormalNaviSpeedDeliver}, /* Navigation speed information service */ + {&DevGpsNormalSettingTime}, /* GPS time setting instruction */ + {&DevGpsNormalNmeaDataGuardTimeOut}, /* NMEA data-providing guard monitoring timeout */ + {&DevGpsNormalBackupDataLoadReq}, /* Backup data read request */ + {&DevGpsNormalStopReq}, /* Thread stop request */ + {&DevGpsNormalGpsweekcorcntNtf}, /* GPS Week Adjustment Counter Notification */ + {&DevGpsNormalRecoveryTimeOut}, /* GPS error monitoring timer out */ + {&DevGpsNormalGpsReceiverErrTimeOut} /* GPS receiver anomaly detection timeout */ + }, + /* - - - - -- Sending - - - - - - */ + { + {&DevGpsSendSendReq}, /* Transmission request */ + {&DevGpsSendGPSResetReq}, /* GPS reset request */ + {&DevGpsSendRcvCyclCmd}, /* Cyclic reception command reception */ + {&DevGpsSendRcvRspCmd}, /* Receive Response Command */ + {&DevGpsSendRspDataTimeOut}, /* Response monitoring timeout */ + {&DevGpsSendCyclDataTimeOut}, /* Periodic reception data monitoring timeout */ + {&DevGpsSendNaviDataTimeOut}, /* Navigation providing data monitoring timeout */ + {&DevGpsSendDiagClkGuardTimeOut}, /* Diagnosis provision time guard monitoring timeout */ + {&DevGpsSendAccOffStart}, /* In-function ACC-OFF instructions */ + {&DevGpsSendNaviInfoDeliver}, /* Providing navigation information */ + {&DevGpsSendNaviSpeedDeliver}, /* Navigation speed information service */ + {&DevGpsSendSettingTime}, /* GPS time setting instruction */ + {&DevGpsSendNmeaDataGuardTimeOut}, /* NMEA data-providing guard monitoring timeout */ + {&DevGpsSendBackupDataLoadReq}, /* Backup data read request */ + {&DevGpsSendStopReq}, /* Thread stop request */ + {&DevGpsSendGpsweekcorcntNtf}, /* GPS Week Adjustment Counter Notification */ + {&DevGpsSendRecoveryTimeOut}, /* GPS error monitoring timer out */ + {&DevGpsSendGpsReceiverErrTimeOut} /* GPS receiver anomaly detection timeout */ + } +}; + +// Receive command analysis table(NMEA format) +const TG_GPS_CMD_ANA_TBL* kGpsCmdAnaTbl = kGpsCmdAnaTblUblox; + +//GPS process pending buffer management table +TG_GPS_MSGRCV g_gps_msg_rcvr; /* Incoming message */ +TG_GPS_MNG g_gps_mngr; /* GPS process management information */ + +// Global variable for GPS communication management +u_int16 g_wsend_err; /* Number of transmission errors */ +u_int16 g_wcnct_err; /* Number of connection errors */ +TG_GPS_OUTPUT_FORMAT g_rcv_format; /* Receive Format */ + +// Global variable for the receive thread +TG_GPS_RECV_RcvData g_gps_rcvdata; /* Serial receive buffer */ +TG_GPS_RECV_RcvBuf g_gps_rcvbuf; /* Receive data storage buffer */ +TG_GPS_RECV_AnaDataBuf g_gps_ana_databuf; /* Analysis data buffer */ +TG_GPS_RECV_RcvFrameBuf g_gps_rcv_framebuf; /* Receive frame buffer */ +TG_GPS_SAVEBUF g_gps_save_cmdr; /* Command pending buffer */ +u_int16 g_wrecv_err; /* Number of reception errors */ + +/*---------------------------------------------------------------------------*/ +// Functions + +/******************************************************************************** + * MODULE : DEV_Gps_MainThread + * ABSTRACT : Communication Management Thread Domain Processing + * FUNCTION : Receive a request from the host application and send a command to the serial port. + * ARGUMENT : PVOID pv....Thread Creation Arguments + * NOTE : + * RETURN : TRUE :Normal completion + * FALSE:ABENDs + ********************************************************************************/ +EFrameworkunifiedStatus DevGpsMainThread(HANDLE h_app) { + BOOL ret = FALSE; + + PosSetupThread(h_app, ETID_POS_GPS); + + ret = DevGpsInit(); + + if (TRUE == ret) { + g_gps_rcv_thread_stop = FALSE; + + /* Start receiving message */ + DevGpsRcvMsg(); + } else { + /* Initialization failed */ + POSITIONING_LOG("DevGpsMainThread Init Fail \n"); + } + + return eFrameworkunifiedStatusFail; +} + + +/******************************************************************************** + * MODULE : DEV_Gps_Init + * ABSTRACT : Communication management thread initialization processing + * FUNCTION : Initialize internal tables and serial ports + * ARGUMENT : None + * NOTE : + * RETURN : TRUE :Normal completion + * FALSE:ABENDs + ********************************************************************************/ +BOOL DevGpsInit(void) { + BOOL ret = FALSE; + + /* Global variable initialization(GPS Communication Management Thread Internal Use Variable) */ + /* Clear process management information */ + memset(&g_gps_mngr, 0x00, sizeof(g_gps_mngr)); /* #05 */ + + /* Clear the message receive buffer */ + memset(&g_gps_msg_rcvr, 0x00, sizeof(g_gps_msg_rcvr)); /* #05 */ + + /* Clear the pending buffer */ + memset(&g_gps_save_cmdr, 0x00, sizeof(g_gps_save_cmdr)); /* #05 */ + + /* Clear RTC backup-related data */ + g_gps_mngr.rcv_cmd = GPS_FORMAT_MIN; /* #GPF_60_040 */ + g_gps_mngr.resp_cmd = GPS_FORMAT_MIN; /* #GPF_60_040 */ + g_gps_mngr.rcv_err_flag = FALSE; + + g_wsend_err = 0; /* Initialization of the number of transmission errors */ + g_wcnct_err = 0; /* Connection error count initialization */ + g_rcv_format = GPS_FORMAT_MIN; /* Initialize Receive Format #GPF_60_024*/ + + /* Initialize the timer function initialization processing management table */ + DevGpsTimInit(); + + /* Start confirmation monitoring timer setting(5Sec) */ + ret = DevGpsTimeSet(GPS_STARTUP_TIMER); + + /* GPS receiver anomaly detection timer setting(600Sec) */ + ret = DevGpsTimeSet(GPS_RECEIVERERR_TIMER); + + // Set command table + kGpsCmdAnaTbl = kGpsCmdAnaTblUblox; + + /* State transition processing(Start Confirmation Monitor) */ + ChangeStatusGpsCommon(GPS_STS_STARTUP); + + return(ret); +} + +/****************************************************************************** +@brief DEV_Gps_RcvMsg
+ Message reception processing +@outline Receive a message and distribute processing into a matrix +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsRcvMsg(void) { + RET_API ret = RET_NORMAL; + void* p_rcv_msg = &g_gps_msg_rcvr; + + while (1) { + p_rcv_msg = &g_gps_msg_rcvr; + + // Receive meaasge + ret = _pb_RcvMsg(PNO_NAVI_GPS_MAIN, sizeof(g_gps_msg_rcvr), &p_rcv_msg, RM_WAIT); + + if (RET_RCVMSG == ret) { + memcpy(&g_gps_msg_rcvr, p_rcv_msg, sizeof(g_gps_msg_rcvr)); + + /* Message Event Check Processing */ + DevGpsMsgEventCheck(); + + if ((u_int32)NG != g_gps_mngr.event) { + /* If the event check is successful */ + /* Start Processing by Matrix */ + (kGpsMatxC[g_gps_mngr.sts][g_gps_mngr.event].func)(); + } + } else { + /** MSG reception error */ + POSITIONING_LOG("DevGpsRcvMsg error ret[%d] \n", ret); + } + } +} + +static inline void SetStopFlag(void) { + g_gps_rcv_thread_stop = TRUE; +} + +/******************************************************************************** + * MODULE : DEV_Gps_MsgEventCheck + * ABSTRACT : Message Event Check Processing + * FUNCTION : Check received messages and convert them to events + * ARGUMENT : None + * NOTE : + * RETURN : None + ********************************************************************************/ +void DevGpsMsgEventCheck(void) { + u_int32 ul_cnt = 0; + + /* Set Error to Event No. */ + g_gps_mngr.event = (u_int32)NG; + + /* For timeout notification */ + if (g_gps_msg_rcvr.header.hdr.cid == CID_TIMER_TOUT) { + /* Get event number from sequence number */ + DevGpsTimEventCheck(); + } else if (g_gps_msg_rcvr.header.hdr.cid == CID_GPS_RECVDATA) { + DevGpsCmdEventCheckNmea(); + } else { + while (kGpsMsgchkC[ul_cnt].cid != DAT_END) { + if (g_gps_msg_rcvr.header.hdr.cid == kGpsMsgchkC[ul_cnt].cid) { + POSITIONING_LOG("DevGpsMsgEventCheck: cid = %d", g_gps_msg_rcvr.header.hdr.cid); + + /* Get event number */ + g_gps_mngr.event = kGpsMsgchkC[ul_cnt].event; + + if (GPS_EVT_STOPREQ == g_gps_mngr.event) { + SetStopFlag(); + LineSensDrvThreadStopProcess(); + } + + break; + } + + ul_cnt++; + } + } +} + +/******************************************************************************** + * MODULE : DEV_Gps_TimEventCheck + * ABSTRACT : Timeout-Event check processing + * FUNCTION : Check received timeout messages and convert them to events + * ARGUMENT : None + * NOTE : + * RETURN : None + ********************************************************************************/ +void DevGpsTimEventCheck(void) { + u_int8 uc_time_kind = 0; + TimerToutMsg st_rcv_msg; + BOOL ret = FALSE; + + // Set Error to Event No. + g_gps_mngr.event = (u_int32)NG; + + // Get message + memcpy(&st_rcv_msg, &g_gps_msg_rcvr, sizeof(TimerToutMsg)); + + // Time judge kind + ret = DevGpsTimeJdgKind((u_int16)st_rcv_msg.TimerSeq); + + if (TRUE == ret) { + /* If the sequence numbers match, */ + /* Set the conversion code to the event number */ + uc_time_kind = (u_int8)((st_rcv_msg.TimerSeq >> 8) & 0x00FF); + g_gps_mngr.event = kGpsTimchkC[uc_time_kind].event; + } +} + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/positioning_hal/src/GpsCommon/MDev_Gps_Mtrx.cpp b/positioning_hal/src/GpsCommon/MDev_Gps_Mtrx.cpp new file mode 100644 index 00000000..b36a617c --- /dev/null +++ b/positioning_hal/src/GpsCommon/MDev_Gps_Mtrx.cpp @@ -0,0 +1,866 @@ +/* + * @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 MDev_Gps_Mtrx.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files +#include "MDev_Gps_Mtrx.h" +#include "positioning_def.h" + +#include "MDev_Gps_Main.h" +#include "MDev_Gps_API.h" +#include "MDev_GpsRecv.h" +#include "MDev_Gps_Common.h" +#include "MDev_Gps_Nmea.h" +#include "MDev_Gps_TimerCtrl.h" + +extern uint8_t g_gps_reset_cmd_sending; +/*---------------------------------------------------------------------------*/ +// Functions + +/******************************************************************************** + * MODULE : DEV_Gps_Nop + * ABSTRACT : No processing + * FUNCTION : NOP processing of the matrix + * ARGUMENT : None + * NOTE : + * RETURN : None + ********************************************************************************/ +void DevGpsNop(void) { + return; +} + +/******************************************************************************** + * MODULE : DEV_Gps_InitStart_SendReq + * ABSTRACT : Startup confirmation - Transmit request reception matrix function + * FUNCTION : + * ARGUMENT : + * NOTE : + * RETURN : + ********************************************************************************/ +void DevGpsInitStartSendReq(void) { + /** Send Request Receive Common Processing Call */ + SendReqGpsCommon(); + /* -- #GPF_60_040 */ + return; +} + +/******************************************************************************** + * MODULE : DEV_Gps_InitStart_GPSResetReq + * ABSTRACT : Startup confirmation - GPS reset request reception matrix function + * FUNCTION : + * ARGUMENT : + * NOTE : + * RETURN : + ********************************************************************************/ +void DevGpsInitStartGPSResetReq(void) { + /** Reset request reception common processing call */ + GPSResetReqGpsCommon(); + return; +} + +/******************************************************************************** + * MODULE : DEV_Gps_InitStart_RcvCyclCmd + * ABSTRACT : Startup confirmation - Cyclic receive command receive matrix function + * FUNCTION : + * ARGUMENT : + * NOTE : + * RETURN : + ********************************************************************************/ +void DevGpsInitStartRcvCyclCmd(void) { + BOOL* pb_rcverr = &(g_gps_mngr.rcv_err_flag); /* GSP receiver error detection status */ + + /* Stop start confirmation monitoring timer */ + (void)DevGpsTimeStop(GPS_STARTUP_TIMER); /* Ignore -> MISRA-C:2004 Rule 16.10 */ /* QAC 3200 */ + + /* GPS receiver anomaly detection timer stopped */ + (void)DevGpsTimeStop(GPS_RECEIVERERR_TIMER); + + /* State transition processing(in operation) */ + ChangeStatusGpsCommon(GPS_STS_NORMAL); + + // Send NAV-TIMEUTC sentence addition requests + DevGpsNavTimeUTCAddReq(); + + DevGpsWknRolloverGetReq(); + + /* Receive command analysis(u-blox) */ + if (g_gps_reset_cmd_sending == FALSE) { + DevGpsRcvCyclCmd(); + } + + /* If a GPS receiver error is detected, the diagnosis code (current) is deleted. */ + if (*pb_rcverr == TRUE) { + *pb_rcverr = FALSE; + DevSendGpsConnectError(FALSE); + } + // } + /* Reset retry counter */ + RtyResetGpsCommon(); + + /* Sending pending commands */ + SendSaveCmdGpsCommon(); /* State transition if pending commands exist( -> Sending) */ + + /* Cyclic reception data monitoring timer setting */ + (void)DevGpsTimeSet(GPS_CYCL_TIMER); /* Ignore -> MISRA-C:2004 Rule 16.10 */ /* QAC 3200 */ + + /* GPS receiver anomaly detection timer setting */ + (void)DevGpsTimeSet(GPS_RECEIVERERR_TIMER); + + /* -- #GPF_60_024 */ + return; +} + +/******************************************************************************** + * MODULE : DEV_Gps_InitStart_CyclDataTimeOut + * ABSTRACT : Startup confirmation - Reception cycle data monitoring timeout matrix function + * FUNCTION : + * ARGUMENT : + * NOTE : + * RETURN : + ********************************************************************************/ +void DevGpsInitStartCyclDataTimeOut(void) { + uint8_t ret = RETRY_OFF; + + POSITIONING_LOG("DEV_Gps_InitStart_CyclDataTimeOut"); + + g_wcnct_err++; /* Count number of connection errors */ + + /* Stop all timers */ + DevGpsTimeStop(GPS_STARTUP_TIMER); + + DevGpsTimeStop(GPS_CYCL_TIMER); + + DevGpsTimeStop(GPS_RECV_ACK_TIMER); + + /* Hard reset judgment processing */ + ret = HardResetChkGpsCommon(); + if (RETRY_ON == ret) { + DevGpsResetReq(PNO_NONE, 0); + + SendSaveCmdGpsCommon(); /* State transition if pending commands exist( -> Sending) */ + } else if (RETRY_OFF == ret) { + /* Fixed number of hard resets completed */ + } else { + /* Retrying(with hard reset) */ + } + /* Clear cyclic receive data up to now */ + DevGpsCycleDataClear(); + /* Start confirmation monitoring timer setting */ + DevGpsTimeSet(GPS_STARTUP_TIMER); + return; +} + +/****************************************************************************** +@brief DEV_Gps_InitStart_NaviDataTimeOut
+ Startup confirmation - Navigation providing data monitoring timeout matrix function +@outline Navigation providing data monitoring timeout matrix processing during startup confirmation +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsInitStartNaviDataTimeOut(void) { +} + +/****************************************************************************** +@brief DEV_Gps_InitStart_DiagClkGuardTimeOut
+ Startup confirmation - Time guard monitoring timeout matrix functions provided by diagnosis +@outline Diagnosis provision time guard monitoring timeout matrix process during startup checking +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsInitStartDiagClkGuardTimeOut(void) { +} + +/******************************************************************************** + * MODULE : DEV_Gps_InitStart_AccOffStart + * ABSTRACT : Startup confirmation - Feature ACC-OFF instruction matrix functions + * FUNCTION : + * ARGUMENT : + * NOTE : + * RETURN : + ********************************************************************************/ +void DevGpsInitStartAccOffStart(void) { +} + +/****************************************************************************** +@brief DEV_Gps_InitStart_NaviInfoDeliver
+ Startup confirmation - Navigation information provision matrix processing +@outline Navigation information provision matrix processing during startup confirmation +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsInitStartNaviInfoDeliver(void) { +} + +/****************************************************************************** +@brief DEV_Gps_InitStart_NaviSpeedDeliver
+ Startup confirmation - Navigation vehicle speed information provision matrix processing +@outline Navigation speed information provision matrix processing during startup confirmation +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsInitStartNaviSpeedDeliver(void) { +} + +/****************************************************************************** +@brief DEV_Gps_InitStart_SettingTime
+ Startup confirmation - GPS time setting matrix processing +@outline GPS time setting matrix process during startup confirmation +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsInitStartSettingTime(void) { + POSITIONING_LOG("DEV_Gps_InitStart_SettingTime\n"); + DevGpsCommSettingTime(); + return; +} + +/* start #GPF_60_109 */ +/****************************************************************************** +@brief DEV_Gps_InitStart_NmeaDataGuardTimeOut
+ Startup confirmation - NMEA monitoring timeout matrix processing +@outline NMEA data-providing guard monitoring timeout process during startup checking +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsInitStartNmeaDataGuardTimeOut(void) { +} + +/** + * @brief + * Startup confirmation - Backup memory read request matrix processing + */ +void DevGpsInitStartBackupDataLoadReq(void) { + POS_DATETIME st_date_time; + memset(&st_date_time, 0x00, sizeof(st_date_time)); + /* Import GPS date and time settings */ + DevGpsReadGpsTime(&st_date_time); + // Todo For Test don't need gstGpsFixCnt?? + // DEV_Gps_ReadGpsFixCnt(); /* Read GPS fix count */ + if (g_is_gpstime_sts == TRUE) { + DevGpsSettingTime(&st_date_time); /* Time setting */ + } + return; +} + +/** + * @brief + * Startup confirmation - Thread stop request matrix processing + */ +void DevGpsInitStartStopReq(void) { + /* Thread stop processing */ + StopThreadGpsCommon(); + + return; +} + +/** + * @brief + * Startup confirmation - GPS week adjustment counter notification process + */ +void DevGpsInitStartGpsweekcorcntNtf(void) { + DevGpsSetGpsweekcorcnt(); + return; +} + +/** + * @brief + * Startup confirmation - GPS recovery timeout process + */ +void DevGpsInitStartRecoveryTimeOut(void) { + return; +} + +/** + * @brief + * Startup confirmation - GPS receiver anomaly detection timeout processing + */ +void DevGpsInitStartGpsReceiverErrTimeOut(void) { + BOOL* pb_rcverr = &(g_gps_mngr.rcv_err_flag); /* GSP receiver error detection status */ + + *pb_rcverr = TRUE; + DevSendGpsConnectError(TRUE); + return; +} + +/******************************************************************************** + * MODULE : DEV_Gps_Normal_SendReq + * ABSTRACT : In operation - Transmit request reception matrix function + * FUNCTION : + * ARGUMENT : + * NOTE : + * RETURN : + ********************************************************************************/ +void DevGpsNormalSendReq(void) { + /** Store in a pending buffer #GPF_60_040 */ + SendReqGpsCommon(); + + /** Send the command #GPF_60_040 */ + SendSaveCmdGpsCommon(); + return; +} + +/******************************************************************************** + * MODULE : DEV_Gps_Normal_GPSResetReq + * ABSTRACT : In operation - GPS reset request reception matrix function + * FUNCTION : + * ARGUMENT : + * NOTE : + * RETURN : + ********************************************************************************/ +void DevGpsNormalGPSResetReq(void) { + /** Store in a pending buffer #GPF_60_040 */ + GPSResetReqGpsCommon(); + + /** Send the command #GPF_60_040 */ + SendSaveCmdGpsCommon(); + return; +} + +/******************************************************************************** + * MODULE : DEV_Gps_Normal_RcvCyclCmd_Nmea + * ABSTRACT : In operation - Cyclic receive command receive matrix function + * FUNCTION : + * ARGUMENT : + * NOTE : + * RETURN : + ********************************************************************************/ +void DevGpsNormalRcvCyclCmd(void) { + /* Cyclic reception data monitoring timer stopped */ + (void)DevGpsTimeStop(GPS_CYCL_TIMER); + /* GPS receiver anomaly detection timer stopped */ + (void)DevGpsTimeStop(GPS_RECEIVERERR_TIMER); + + /* Notify Vehicle Sensor */ + if (g_gps_reset_cmd_sending == FALSE) { + DevGpsRcvCyclCmd(); + } + + /* Reset retry counter */ + RtyResetGpsCommon(); + + /* Cyclic reception data monitoring timer setting */ + (void)DevGpsTimeSet(GPS_CYCL_TIMER); /* Ignore -> MISRA-C:2004 Rule 16.10 */ /* QAC 3200 */ + + /* GPS receiver anomaly detection timer setting */ + (void)DevGpsTimeSet(GPS_RECEIVERERR_TIMER); + + return; +} + +/******************************************************************************** + * MODULE : DEV_Gps_Normal_CyclDataTimeOut + * ABSTRACT : In operation - Reception cycle data monitoring timeout matrix function + * FUNCTION : + * ARGUMENT : + * NOTE : + * RETURN : + ********************************************************************************/ +void DevGpsNormalCyclDataTimeOut(void) { + /* Cyclic reception timeout common processing call */ + CyclDataTimeOutGpsCommon(); +} + +/****************************************************************************** +@brief DEV_Gps_Normal_NaviDataTimeOut
+ In operation - Navigation providing data monitoring timeout matrix function +@outline Navigation providing data monitoring timeout matrix processing during operation +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsNormalNaviDataTimeOut(void) { +} + +/****************************************************************************** +@brief DEV_Gps_Normal_DiagClkGuardTimeOut
+ In operation - Time guard monitoring timeout matrix functions provided by diagnosis +@outline Diagnosis provision time guard monitoring timeout matrix process during operation +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsNormalDiagClkGuardTimeOut(void) { +} + +/******************************************************************************** + * MODULE : DEV_Gps_Normal_AccOffStart + * ABSTRACT : Startup confirmation - Feature ACC-OFF instruction matrix functions + * FUNCTION : + * ARGUMENT : + * NOTE : + * RETURN : + ********************************************************************************/ +void DevGpsNormalAccOffStart(void) { +} + +/****************************************************************************** +@brief DEV_Gps_Normal_NaviInfoDeliver
+ In operation - Navigation information provision matrix processing +@outline Navigation information provision matrix processing during operation +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsNormalNaviInfoDeliver(void) { +} + +/****************************************************************************** +@brief DEV_Gps_Normal_NaviSpeedDeliver
+ In operation - Navigation vehicle speed information provision matrix processing +@outline Navigation vehicle speed information provision matrix processing during operation +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsNormalNaviSpeedDeliver(void) { +} + +/****************************************************************************** +@brief DEV_Gps_Normal_SettingTime
+ In operation - GPS time setting matrix processing +@outline GPS time setting matrix processing during operation +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsNormalSettingTime(void) { + POSITIONING_LOG("DEV_Gps_Normal_SettingTime"); + DevGpsCommSettingTime(); + /** Send the command */ + SendSaveCmdGpsCommon(); + + return; +} + +/* start #GPF_60_109 */ +/****************************************************************************** +@brief DEV_Gps_Normal_NmeaDataGuardTimeOut
+ In operation - NMEA monitoring timeout matrix processing +@outline NMEA data-providing guard monitoring timeout process during operation +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsNormalNmeaDataGuardTimeOut(void) { +} + +/** + * @brief + * In operation - Backup memory read request matrix processing + */ +void DevGpsNormalBackupDataLoadReq(void) { + POS_DATETIME st_date_time; + memset(&st_date_time, 0x00, sizeof(st_date_time)); + /* Import GPS date and time settings */ + DevGpsReadGpsTime(&st_date_time); + // Todo For test don't need gstGpsFixCnt?? + // DEV_Gps_ReadGpsFixCnt(); /* Read GPS fix count */ + if (g_is_gpstime_sts == TRUE) { + DevGpsSettingTime(&st_date_time); /* Time setting */ + } + return; +} + +/** + * @brief + * In operation - Thread stop request matrix processing + */ +void DevGpsNormalStopReq(void) { + /* Thread stop processing */ + StopThreadGpsCommon(); + + return; +} + +/** + * @brief + * In operation - GPS week adjustment counter notification process + */ +void DevGpsNormalGpsweekcorcntNtf(void) { + DevGpsSetGpsweekcorcnt(); + return; +} + +/** + * @brief + * In operation - GPS recovery timeout processing + */ +void DevGpsNormalRecoveryTimeOut(void) { +} + +/** + * @brief + * In operation - GPS receiver anomaly detection timeout processing + */ +void DevGpsNormalGpsReceiverErrTimeOut(void) { + BOOL* pb_rcverr = &(g_gps_mngr.rcv_err_flag); /* GSP receiver error detection status */ + + *pb_rcverr = TRUE; + DevSendGpsConnectError(TRUE); + return; +} + +/* ++ #GPF_60_040 */ +/****************************************************************************** +@brief DEV_Gps_Send_SendReq
+ Sending - Transmit request reception matrix function +@outline Processing for receiving a transmission request during transmission +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsSendSendReq(void) { + SendReqGpsCommon(); + return; +} + +/****************************************************************************** +@brief DEV_Gps_Send_GPSResetReq
+ Sending - GPS reset request reception matrix function +@outline Processing when receiving a GPS reset request during transmission +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsSendGPSResetReq(void) { + GPSResetReqGpsCommon(); + return; +} + +/****************************************************************************** +@brief DEV_Gps_Send_RcvCyclCmd
+ Sending - Cyclic receive command receive matrix function +@outline Processing at the reception of cyclic reception command during transmission +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsSendRcvCyclCmd(void) { + /* Cyclic reception data monitoring timer stopped */ + DevGpsTimeStop(GPS_CYCL_TIMER); + + /* GPS receiver anomaly detection timer stopped */ + DevGpsTimeStop(GPS_RECEIVERERR_TIMER); + + /* Notify Vehicle Sensor */ + if (g_gps_reset_cmd_sending == FALSE) { + DevGpsRcvCyclCmd(); + } + + /* Reset retry counter */ + RtyResetGpsCommon(); + + /* Cyclic reception data monitoring timer setting */ + (void)DevGpsTimeSet(GPS_CYCL_TIMER); + + (void)DevGpsTimeSet(GPS_RECEIVERERR_TIMER); + + return; +} + +/****************************************************************************** +@brief DEV_Gps_Send_RcvRspCmd
+ Sending - Response command reception matrix function +@outline Processing when receiving a response command during transmission +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsSendRcvRspCmd(void) { + TG_GPS_OUTPUT_FORMAT rcv_cmd = g_gps_mngr.rcv_cmd; + BOOL b_snd_flag = TRUE; + + /** Check whether the command matches the command being monitored. */ + if (rcv_cmd == g_gps_mngr.resp_cmd) { + /** When the command being monitored is a cold start request */ + if (g_gps_mngr.resp_rst_flag == GPS_CMD_RESET) { + POSITIONING_LOG("GPS_CMD_RESET\n"); + b_snd_flag = DevGpsRcvProcGpsResetResp(reinterpret_cast + (reinterpret_cast(g_gps_msg_rcvr.msgdat))); + } else if (g_gps_mngr.resp_rst_flag == GPS_CMD_TIMESET) { + POSITIONING_LOG("GPS_CMD_TIMESET\n"); + DevGpsRcvProcTimeSetResp(reinterpret_cast + (reinterpret_cast(g_gps_msg_rcvr.msgdat))); + } else if (g_gps_mngr.resp_rst_flag == GPS_CMD_SENTENCEADD_NAVTIMEUTC) { + POSITIONING_LOG("GPS_CMD_SENTENCEADD_NAVTIMEUTC\n"); + DevGpsRcvProcNavSvinfoAddResp(reinterpret_cast + (reinterpret_cast(g_gps_msg_rcvr.msgdat))); + } else if ( g_gps_mngr.resp_rst_flag == GPS_CMD_WKNROLLOVER ) { + POSITIONING_LOG("GPS_CMD_WKNROLLOVER\n"); + DevGpsRcvProcWknRolloverGetResp(reinterpret_cast + (reinterpret_cast(g_gps_msg_rcvr.msgdat))); + } else { + POSITIONING_LOG("g_gps_mngr.resp_rst_flag INVALID!! [resp_rst_flag = %d]", g_gps_mngr.resp_rst_flag); + } + + if (b_snd_flag == TRUE) { + /** Stop ACK monitoring timer */ + DevGpsTimeStop(GPS_RECV_ACK_TIMER); + /* State transition processing(in operation) */ + ChangeStatusGpsCommon(GPS_STS_NORMAL); + /** Reset retry counter */ + SendRtyResetGpsCommon(); + /** Delete command from pending buffer */ + DeleteSaveCmdGpsCommon(); + /* Sending pending commands */ + SendSaveCmdGpsCommon(); + } + } else { + } + + return; +} + +/****************************************************************************** +@brief DEV_Gps_Send_RspDataTimeOut
+ Sending - Response monitoring timeout reception matrix function +@outline Processing at reception of response monitoring timeout during transmission +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsSendRspDataTimeOut(void) { + DevGpsTimeStop(GPS_RECV_ACK_TIMER); + /** Send retry count */ + g_gps_mngr.sndcnt++; + + /** Retry judgment */ + if (g_gps_mngr.sndcnt == SNDNG_MAX) { + /** At the time of retry */ + POSITIONING_LOG("GPS Hw Reset : sndcnt[%d]", g_gps_mngr.sndcnt); + + /** When the command being monitored is a cold start request */ + if (g_gps_mngr.resp_rst_flag == GPS_CMD_RESET) { + POSITIONING_LOG("g_gps_mngr.sndcnt == SNDNG_MAX (GPS_CMD_RESET)"); + /** Send Reset ResponseCommunication error */ + DevGpsRstAnsSend(g_gps_mngr.resp_pno, g_gps_mngr.resp_rid, GPS_SENDNG); + g_gps_reset_cmd_sending = FALSE; + + } else if (g_gps_mngr.resp_rst_flag == GPS_CMD_TIMESET) { + POSITIONING_LOG("g_gps_mngr.sndcnt == SNDNG_MAX (GPS_CMD_TIMESET)"); + DevGpsTimesetAnsSend(g_gps_mngr.resp_pno, g_gps_mngr.resp_rid, GPS_SENDNG); + } else if (g_gps_mngr.resp_rst_flag == GPS_CMD_SENTENCEADD_NAVTIMEUTC) { + POSITIONING_LOG("g_gps_mngr.sndcnt == SNDNG_MAX (GPS_CMD_SENTENCEADD_NAVTIMEUTC)"); + DevGpsNavTimeUTCAddReq(); /* Retransmission of requests to add NAV-SVINFO commands */ + } else if (g_gps_mngr.resp_rst_flag == GPS_CMD_WKNROLLOVER) { /* GPS rollover reference date acquisition response */ + POSITIONING_LOG("g_gps_mngr.sndcnt == SNDNG_MAX (GPS_CMD_WKNROLLOVER)"); + DevGpsWknRolloverGetReq(); /* GPS rollover standard week number acquisition request retransmission */ + } else { + POSITIONING_LOG("g_gps_mngr.resp_rst_flag INVALID!! [resp_rst_flag = %d]", g_gps_mngr.resp_rst_flag); + } + + DeleteSaveCmdGpsCommon(); + + ChangeStatusGpsCommon(GPS_STS_NORMAL); + + SendSaveCmdGpsCommon(); + /** Reset retry counter */ + SendRtyResetGpsCommon(); + } else { + POSITIONING_LOG("GPS Send Retry : sndcnt[%d]", g_gps_mngr.sndcnt); + /** Retransmission of pending command */ + SendSaveCmdGpsCommon(); + } + + return; +} + +/****************************************************************************** +@brief DEV_Gps_Send_CyclDataTimeOut
+ Sending - Reception cycle data monitoring timeout matrix function +@outline Processing when receiving periodic reception data monitoring timeout during transmission +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsSendCyclDataTimeOut(void) { + CyclDataTimeOutGpsCommon(); +} + +/****************************************************************************** +@brief DEV_Gps_Send_NaviDataTimeOut
+ Sending - Navigation providing data monitoring timeout matrix function +@outline Navigation providing data monitoring timeout matrix processing during transmission +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsSendNaviDataTimeOut(void) { +} + +/****************************************************************************** +@brief DEV_Gps_Send_DiagClkGuardTimeOut
+ Sending - Time guard monitoring timeout matrix functions provided by diagnosis +@outline Diagnosis providing time guard monitoring timeout matrix process during sending +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsSendDiagClkGuardTimeOut(void) { +} + +/****************************************************************************** +@brief DEV_Gps_Send_AccOffStart
+ Sending - Feature ACC-OFF instruction reception matrix functions +@outline Processing when receiving ACC-OFF instruction in function being transmitted +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsSendAccOffStart(void) { +} + +/****************************************************************************** +@brief DEV_Gps_Send_NaviInfoDeliver
+ Sending - Navigation information provision matrix processing +@outline Navigation information provision matrix processing during transmission +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsSendNaviInfoDeliver(void) { +} + +/****************************************************************************** +@brief DEV_Gps_Send_NaviSpeedDeliver
+ Sending - Navigation vehicle speed information provision matrix processing +@outline Navigation vehicle speed information provision matrix processing during transmission +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsSendNaviSpeedDeliver(void) { +} + +/****************************************************************************** +@brief DEV_Gps_Send_SettingTime
+ Sending - GPS time setting matrix processing +@outline GPS time setting matrix processing during transmission +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsSendSettingTime(void) { + DevGpsCommSettingTime(); + return; +} + +/* start #GPF_60_109 */ +/****************************************************************************** +@brief DEV_Gps_Send_NmeaDataGuardTimeOut
+ Sending - NMEA monitoring timeout matrix processing +@outline Sending NMEA monitoring timeouts +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsSendNmeaDataGuardTimeOut(void) { +} + +/** + * @brief + * Sending - Backup memory read request matrix processing + */ +void DevGpsSendBackupDataLoadReq(void) { + POS_DATETIME st_date_time; + + memset(&st_date_time, 0x00, sizeof(st_date_time)); + /* Import GPS date and time settings */ + DevGpsReadGpsTime(&st_date_time); + // Todo For test don't need gstGpsFixCnt?? + // DEV_Gps_ReadGpsFixCnt(); /* Read GPS fix count */ + if (g_is_gpstime_sts == TRUE) { + DevGpsSettingTime(&st_date_time); /* Time setting */ + } + + return; +} + +/** + * @brief + * Sending - Thread stop request matrix processing + */ +void DevGpsSendStopReq(void) { + /* Thread stop processing */ + StopThreadGpsCommon(); + return; +} + +/** + * @brief + * Sending - GPS week adjustment counter notification process + */ +void DevGpsSendGpsweekcorcntNtf(void) { + DevGpsSetGpsweekcorcnt(); + return; +} + +/** + * @brief + * Sending - GPS recovery timeout processing + */ +void DevGpsSendRecoveryTimeOut(void) { + return; +} + +/** + * @brief + * Sending - GPS receiver anomaly detection timeout processing + */ +void DevGpsSendGpsReceiverErrTimeOut(void) { + BOOL* pb_rcverr = &(g_gps_mngr.rcv_err_flag); /* GSP receiver error detection status */ + + *pb_rcverr = TRUE; + DevSendGpsConnectError(TRUE); + return; +} + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/positioning_hal/src/GpsCommon/MDev_Gps_Nmea.cpp b/positioning_hal/src/GpsCommon/MDev_Gps_Nmea.cpp new file mode 100644 index 00000000..60fa1d8d --- /dev/null +++ b/positioning_hal/src/GpsCommon/MDev_Gps_Nmea.cpp @@ -0,0 +1,928 @@ +/* + * @copyright Copyright (c) 2017-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 MDev_Gps_Nmea.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "MDev_Gps_Nmea.h" +#include "positioning_common.h" +#include "MDev_Gps_API.h" +#include "MDev_Gps_Main.h" +#include "MDev_GpsRecv.h" +#include "MDev_Gps_Common.h" +#include "MDev_GpsRollOver.h" + +/*---------------------------------------------------------------------------*/ +// Value define +#define ROLOVR_GPSWEEKCOR_NG 0xFF /* WEEK COMP. CORRECT CORRECTOR INVALUE */ +#define TMT_TGET_INI_INTERVAL (100) /* GPS time compensation interval immediately after startup (value with 10[msec] as 1) */ +#define TMT_DISCONTINUITY_TIME_TOUT_SEQ (0x5051) /* Sequence number of GPS time acquisition timeout based on date/time discontinuity */ + +/*---------------------------------------------------------------------------*/ +// Global values + +static TG_GPS_RCVDATA g_st_gpscycle_data; +u_int8 g_gps_week_cor_cnt = ROLOVR_GPSWEEKCOR_NG; + +extern uint8_t g_gpstime_raw_tdsts; + +const TG_GPS_CMD_ANA_TBL kGpsCmdAnaTblUblox[MDEV_GPSCMDANATBLNMEA_MAX] = { + /* Sentences Maximum length Receiving type Presence of notification Receive Format */ + /* NMEA */ + {{GPS_CMD_NMEA_RMC}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_RMC }, /* RMC information */ + {{GPS_CMD_NMEA_VTG}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_VTG }, /* VTG information */ + {{GPS_CMD_NMEA_GGA}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GGA }, /* GGA information */ + {{GPS_CMD_NMEA_GSA}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GSA }, /* GSA information */ + {{GPS_CMD_NMEA_GSV_1}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GSV1 }, /* GSV information1 */ + {{GPS_CMD_NMEA_GSV_2}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GSV2 }, /* GSV information2 */ + {{GPS_CMD_NMEA_GSV_3}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GSV3 }, /* GSV information3 */ + {{GPS_CMD_NMEA_GSV_4}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GSV4 }, /* GSV information4 */ + {{GPS_CMD_NMEA_GSV_5}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GSV5 }, /* GSV information5 */ + {{GPS_CMD_NMEA_GLL}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GLL }, /* GLL information */ + {{GPS_CMD_NMEA_GST}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GST }, /* GST information */ + /* UBX */ + {{0xB5, 0x62, 0x0A, 0x04}, GPS_NMEA_MAX_SZ, RCV_RESP, TRUE, GPS_FORMAT_MON_VER }, /* MON-VER */ + {{0xB5, 0x62, 0x0B, 0x01}, GPS_NMEA_MAX_SZ, RCV_RESP, TRUE, GPS_FORMAT_AID_INI }, /* AID-INI */ + {{0xB5, 0x62, 0x05 }, GPS_NMEA_MAX_SZ, RCV_RESP, TRUE, GPS_FORMAT_ACK_ACKNACK }, /* ACK-ACKNACK */ + {{0xB5, 0x62, 0x01, 0x21}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_NAV_TIMEUTC }, /* NAV-TIMEUTC */ + {{0xB5, 0x62, 0x01, 0x22}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_NAV_CLOCK }, /* NAV-CLOCK */ + {{0xB5, 0x62, 0x02, 0x23}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_RXM_RTC5 }, /* RXM-RTC5 */ + {{0xB5, 0x62, 0x01, 0x30}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_NAV_SVINFO }, /* NAV-SVINFO */ + {{0xB5, 0x62, 0x06, 0x23}, GPS_NMEA_MAX_SZ, RCV_RESP, TRUE, GPS_FORMAT_CFG_NAVX5 }, /* CFG-NAVX5 */ + {{ENDMARK}, 0, 0, FALSE, GPS_FORMAT_MIN } /* Table termination */ +}; + +/*---------------------------------------------------------------------------*/ +// Functions +/** + * @brief + * Time Calculation Process Considering Rollover + * + * @param[in/out] + * + * @return none + * @retval none + */ +static BOOL DevCalcRollOverTime(TG_TIM_ROLOVR_YMD* base_ymd, TG_TIM_ROLOVR_YMD* conv_ymd) { + BOOL ret = TRUE; + static u_int16 year_old = 0; + static u_int16 confirm_cnt = 0; + u_int8 gps_week_corr = g_gps_week_cor_cnt; + + if (gps_week_corr == ROLOVR_GPSWEEKCOR_NG) { + gps_week_corr = 0; + ret = FALSE; + } else { + if (year_old > base_ymd->year) { + confirm_cnt++; + if (confirm_cnt >= 10) { + confirm_cnt = 0; + year_old = base_ymd->year; + g_gps_week_cor_cnt = ROLOVR_GPSWEEKCOR_NG; + /* Starting the GPS time acquisition timer (1 second timer) based on date/time discontinuity */ + _pb_ReqTimerStart(PNO_CLK_GPS, TMT_DISCONTINUITY_TIME_TOUT_SEQ, TIMER_TYPE_USN, TMT_TGET_INI_INTERVAL); + } + ret = FALSE; + } else { + confirm_cnt = 0; + year_old = base_ymd->year; + } + } + /* Calculate time for rollover */ + GPSRollOverConvTime(base_ymd, conv_ymd, gps_week_corr); + + return ret; +} +/******************************************************************************************************/ + + +/** + * @brief + * NMEA data notification + */ +void DevGpsSndCycleDataNmea(void) { + /* Notifying vehicle sensor of NMEA */ + + MDEV_GPS_NMEA st_nmea; + TG_GPS_NMEA_INFO st_gps_nmea_info; + RET_API l_ret = RET_NORMAL; + BOOL b_get = FALSE; + u_int8 uc_nmea_data[GPS_NMEA_MAX_SZ]; + u_int32 ul_strlen = 0; + u_int16 us_offset = sizeof(TG_GPS_NMEA_INFO); + + memset(&st_nmea, 0x00, sizeof(st_nmea) ); /* QAC 3200 */ + memset(&st_gps_nmea_info, 0x00, sizeof(st_gps_nmea_info) ); /* QAC 3200 */ + memset(uc_nmea_data, 0x00, sizeof(uc_nmea_data) ); /* QAC 3200 */ + + /* Get received NMEA data from storage area(GGA) */ + b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_GGA); + if (b_get == TRUE) { + /* Data present */ + st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GGA; /* Receive flag */ + ul_strlen = strlen(reinterpret_cast(uc_nmea_data)); /* QAC 310 */ + + if (ul_strlen > GPS_NMEA_MAX_SZ) { + ul_strlen = GPS_NMEA_MAX_SZ; + } + + if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) { + (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen); /* Data storage */ + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GGA].uc_size = (u_int8)ul_strlen; + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GGA].us_offset = us_offset; + us_offset += (u_int16)ul_strlen; + } + } + + /* Get received NMEA data from storage area(GLL) */ + b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_GLL); + if (b_get == TRUE) { + /* Data present */ + st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GLL; /* Receive flag */ + ul_strlen = strlen(reinterpret_cast(uc_nmea_data)); /* QAC 310 */ + + if (ul_strlen > GPS_NMEA_MAX_SZ) { + ul_strlen = GPS_NMEA_MAX_SZ; + } + + if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) { + (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen); /* Data storage */ + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GLL].uc_size = (u_int8)ul_strlen; + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GLL].us_offset = us_offset; + us_offset += (u_int16)ul_strlen; + } + } + + /* Get received NMEA data from storage area(GSA) */ + b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_GSA); + if (b_get == TRUE) { + /* Data present */ + st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GSA1; /* Receive flag */ + ul_strlen = strlen(reinterpret_cast(uc_nmea_data)); /* QAC 310 */ + if (ul_strlen > GPS_NMEA_MAX_SZ) { + ul_strlen = GPS_NMEA_MAX_SZ; + } + + if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) { + (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen); /* Data storage */ + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSA1].uc_size = (u_int8)ul_strlen; + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSA1].us_offset = us_offset; + us_offset += (u_int16)ul_strlen; + } + } + + /* Get received NMEA data from storage area(GST) */ + b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_GST); + if (b_get == TRUE) { + /* Data present */ + st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GST; /* Receive flag */ + ul_strlen = strlen(reinterpret_cast(uc_nmea_data)); /* QAC 310 */ + if (ul_strlen > GPS_NMEA_MAX_SZ) { + ul_strlen = GPS_NMEA_MAX_SZ; + } + + if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) { + (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen); /* Data storage */ + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GST].uc_size = (u_int8)ul_strlen; + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GST].us_offset = us_offset; + us_offset += (u_int16)ul_strlen; + } + } + + /* Get received NMEA data from storage area(RMC) */ + b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_RMC); + if (b_get == TRUE) { + /* Data present */ + st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_RMC; /* Receive flag */ + ul_strlen = strlen(reinterpret_cast(uc_nmea_data)); /* QAC 310 */ + if (ul_strlen > GPS_NMEA_MAX_SZ) { + ul_strlen = GPS_NMEA_MAX_SZ; + } + + if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) { + (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen); /* Data storage */ + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_RMC].uc_size = (u_int8)ul_strlen; + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_RMC].us_offset = us_offset; + us_offset += (u_int16)ul_strlen; + } + } + + /* Get received NMEA data from storage area(VTG) */ + b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_VTG); + if (b_get == TRUE) { + /* Data present */ + st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_VTG; /* Receive flag */ + ul_strlen = strlen(reinterpret_cast(uc_nmea_data)); /* QAC 310 */ + if (ul_strlen > GPS_NMEA_MAX_SZ) { + ul_strlen = GPS_NMEA_MAX_SZ; + } + + if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) { + (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen); /* Data storage */ + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_VTG].uc_size = (u_int8)ul_strlen; + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_VTG].us_offset = us_offset; + us_offset += (u_int16)ul_strlen; + } + } + + /* Get received NMEA data from storage area(GSV1) */ + b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_GSV1); + if (b_get == TRUE) { + /* Data present */ + st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GSV1; /* Receive flag */ + ul_strlen = strlen(reinterpret_cast(uc_nmea_data)); /* QAC 310 */ + if (ul_strlen > GPS_NMEA_MAX_SZ) { + ul_strlen = GPS_NMEA_MAX_SZ; + } + + if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) { + (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen); /* Data storage */ + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV1].uc_size = (u_int8)ul_strlen; + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV1].us_offset = us_offset; + us_offset += (u_int16)ul_strlen; + } + } + + /* Get received NMEA data from storage area(GSV2) */ + b_get = DevGpsCycleDataGetNmea( uc_nmea_data, sizeof(uc_nmea_data), GPS_NMEA_INDEX_GSV2 ); + if (b_get == TRUE) { + /* Data present */ + st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GSV2; /* Receive flag */ + ul_strlen = strlen(reinterpret_cast(uc_nmea_data)); /* QAC 310 */ + if (ul_strlen > GPS_NMEA_MAX_SZ) { + ul_strlen = GPS_NMEA_MAX_SZ; + } + + if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) { + (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen); /* Data storage */ + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV2].uc_size = (u_int8)ul_strlen; + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV2].us_offset = us_offset; + us_offset += (u_int16)ul_strlen; + } + } + + /* Get received NMEA data from storage area(GSV3) */ + b_get = DevGpsCycleDataGetNmea( uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_GSV3 ); + if (b_get == TRUE) { + /* Data present */ + st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GSV3; /* Receive flag */ + ul_strlen = strlen(reinterpret_cast(uc_nmea_data)); /* QAC 310 */ + if (ul_strlen > GPS_NMEA_MAX_SZ) { + ul_strlen = GPS_NMEA_MAX_SZ; + } + + if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) { + (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen); + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV3].uc_size = static_cast(ul_strlen); + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV3].us_offset = us_offset; + us_offset += (u_int16)ul_strlen; + } + } + + /* Get received NMEA data from storage area(GSV4) */ + b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data), GPS_NMEA_INDEX_GSV4); + if (b_get == TRUE) { + /* Data present */ + st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GSV4; /* Receive flag */ + ul_strlen = strlen(reinterpret_cast(uc_nmea_data)); /* QAC 310 */ + if (ul_strlen > GPS_NMEA_MAX_SZ) { + ul_strlen = GPS_NMEA_MAX_SZ; + } + + if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) { + (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen); + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV4].uc_size = static_cast(ul_strlen); + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV4].us_offset = us_offset; + us_offset += (u_int16)ul_strlen; + } + } + + /* Get received NMEA data from storage area(GSV5) */ + b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data), GPS_NMEA_INDEX_GSV5); + if (b_get == TRUE) { + /* Data present */ + st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GSV5; /* Receive flag */ + ul_strlen = strlen(reinterpret_cast(uc_nmea_data)); /* QAC 310 */ + if (ul_strlen > GPS_NMEA_MAX_SZ) { + ul_strlen = GPS_NMEA_MAX_SZ; + } + + if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) { + (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen); + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV5].uc_size = static_cast(ul_strlen); + st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV5].us_offset = us_offset; + us_offset += (u_int16)ul_strlen; + } + } + + if (0 != st_gps_nmea_info.ul_rcvsts) { + /* Receive flag */ + (void)memcpy(&(st_nmea.uc_nmea_data[0]), &st_gps_nmea_info, sizeof(st_gps_nmea_info)); + + /* Provided to vehicle sensor */ + l_ret = SendNmeaGps(&st_nmea); + + if (RET_NORMAL != l_ret) { + POSITIONING_LOG("SendNmeaGps SndMsg Error\n"); + } + } else { + /* Do not provide to vehicle sensor when data acquisition fails or no data */ + } + + return; +} + +/** + * @brief + * Analysis of the received command + */ +void DevGpsRcvCyclCmd(void) { + int32 i_ret = 0; + TG_GPS_OUTPUT_FORMAT e_format = GPS_FORMAT_MIN; + + TG_GPS_RCV_DATA st_rcv_data; /* Pointer to the received message */ + (void)memcpy(&st_rcv_data, &(g_gps_msg_rcvr.msgdat[0]), sizeof(st_rcv_data) ); /* QAC 3200 */ + + /* Analysis of received commands */ + i_ret = JudgeFormatGpsCommon(reinterpret_cast(&(st_rcv_data.bygps_data[0])), + static_cast(st_rcv_data.bydata_len), + &e_format); + + if (i_ret == GPSRET_SNDCMD) { + /* For NMEA formats */ + if ((e_format == GPS_FORMAT_RMC) || + (e_format == GPS_FORMAT_VTG) || + (e_format == GPS_FORMAT_GGA) || + (e_format == GPS_FORMAT_GSA) || + (e_format == GPS_FORMAT_GSV1) || + (e_format == GPS_FORMAT_GSV2) || + (e_format == GPS_FORMAT_GSV3) || + (e_format == GPS_FORMAT_GSV4) || + (e_format == GPS_FORMAT_GSV5) || + (e_format == GPS_FORMAT_GLL) || + (e_format == GPS_FORMAT_GST)) { + /* NMEA reception process */ + RcvCyclCmdNmeaGpsCommon(&(st_rcv_data.bygps_data[0]), (u_int32)st_rcv_data.bydata_len, e_format); + } else if ((e_format == GPS_FORMAT_MON_VER) || + (e_format == GPS_FORMAT_AID_INI) || + (e_format == GPS_FORMAT_ACK_ACKNACK) || + (e_format == GPS_FORMAT_NAV_TIMEUTC) || + (e_format == GPS_FORMAT_NAV_CLOCK) || + (e_format == GPS_FORMAT_RXM_RTC5) || + (e_format == GPS_FORMAT_NAV_SVINFO)) { + /* UBX reception process */ + RcvCyclCmdExtGpsCommon(&(st_rcv_data.bygps_data[0]), (u_int32)st_rcv_data.bydata_len, e_format); + } else { + POSITIONING_LOG("Forbidden ERROR!![e_format=%d]", (int)e_format); + } + } else if (i_ret == GPSRET_CMDERR) { + /* Receive command error */ + + /* Discard previously received data */ + DevGpsCycleDataClear(); + /* Initialize receive format */ + g_rcv_format = GPS_FORMAT_MIN; + } else { + } + + return; +} + +/** + * @brief + * Check of the received command + */ +void DevGpsCmdEventCheckNmea(void) { + u_int32 ul_cnt = 0; + TG_GPS_RCV_DATA st_rcv_data; + u_char* pub_rcv_data = NULL; + BOOL brk_flg = FALSE; + + memset(&st_rcv_data, 0, sizeof(TG_GPS_RCV_DATA)); + memcpy( &st_rcv_data, &(g_gps_msg_rcvr.msgdat[0]), sizeof(TG_GPS_RCV_DATA) ); + pub_rcv_data = reinterpret_cast(&(st_rcv_data.bygps_data[0])); + + /* Analysis of received commands */ + for (ul_cnt = 0; ul_cnt < (u_int32)GPSCMDANATBL_MAX; ul_cnt++) { + /* End-of-table decision */ + if (CheckFrontStringPartGpsCommon(reinterpret_cast(ENDMARK), + reinterpret_cast(kGpsCmdAnaTbl[ul_cnt].c_sentence)) == RET_NORMAL ) { + g_wrecv_err++; + + /* Data error is set to Event ID. */ + g_gps_mngr.event = (u_int32)NG; + + brk_flg = TRUE; + } else if (CheckFrontStringPartGpsCommon(pub_rcv_data, kGpsCmdAnaTbl[ul_cnt].c_sentence) == RET_NORMAL) { + /* Reception type determination */ + + /* Using $GPRMC in responses to resets */ + if ((g_gps_mngr.sts == GPS_STS_SENT) && + (g_gps_mngr.resp_cmd == GPS_FORMAT_RMC) && + (kGpsCmdAnaTbl[ul_cnt].e_rcv_format == GPS_FORMAT_RMC)) { + POSITIONING_LOG("Received response ($GPRMC) form GPS device.\n"); + + /** Response command */ + g_gps_mngr.event = GPS_EVT_RECVRSPDAT; + + /** Receive format setting */ + g_gps_mngr.rcv_cmd = kGpsCmdAnaTbl[ul_cnt].e_rcv_format; + } else if (kGpsCmdAnaTbl[ul_cnt].ul_rcv_kind == RCV_CYCLE) { + /* Cyclic receive command */ + g_gps_mngr.event = GPS_EVT_RECVCYCLDAT; + + /* Receive format setting */ + g_gps_mngr.rcv_cmd = kGpsCmdAnaTbl[ul_cnt].e_rcv_format; + } else if (kGpsCmdAnaTbl[ul_cnt].ul_rcv_kind == RCV_RESP) { + /** Response command */ + g_gps_mngr.event = GPS_EVT_RECVRSPDAT; + + /** Receive format setting */ + g_gps_mngr.rcv_cmd = kGpsCmdAnaTbl[ul_cnt].e_rcv_format; + } else { + /* Undefined value */ + /* Data error is set to Event ID. */ + g_gps_mngr.event = (u_int32)NG; + } + + brk_flg = TRUE; + } + + if (brk_flg == TRUE) { + break; + } + } + + return; +} + +/** + * @brief + * Get GPS reception status + * + * By analyzing the last received GSA-sentence and using the satellite-number as the Satellite number + * Determines the reception status based on whether or not notification has been made, and returns it. + * + * @param[in] no_sv Satellite number + * + * @return NAVIINFO_DIAG_GPS_RCV_STS_NOTUSEFIX - Positioning not used + * NAVIIFNO_DIAG_GPS_RCV_STS_USEFIX - Positioning use + */ +u_int8 DevGpsGetGpsRcvSts(u_int8 sv) { + u_int8 rcv_sts = NAVIINFO_DIAG_GPS_RCV_STS_TRACHING; /* Tracking in progress */ + u_int8 uc_nmea_data[GPS_NMEA_MAX_SZ]; + u_int8 uc_no = 0; + BOOL b_get = FALSE; + int32 i = 0; + + /* Get received NMEA data from storage area(GSA) */ + b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data), GPS_NMEA_INDEX_GSA); + + if (b_get == TRUE) { + for (i = 0; i < GPS_NMEA_NUM_GSA_SV; i++) { + /* Get satellite number */ + uc_no = (uint8_t)GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSA_SV + (u_int8)(1 * i), uc_nmea_data); + + if (uc_no == sv) { + rcv_sts = NAVIINFO_DIAG_GPS_RCV_STS_USEFIX; /* Positioning use */ + break; + } + } + } + + return rcv_sts; +} + +/** + * @brief + * GPS information analysis + * + * Analyzing received NMEA sentences + * @param[out] navilocinfo Navigation information + */ +void DevGpsAnalyzeNmea(NAVIINFO_ALL* navilocinfo) { + u_int8 uc_nmea_data[GPS_NMEA_MAX_SZ]; + int32 no_sv = 0; /* number of Satellites in View */ + int32 __offset = 0; + char utc_time[12]; /* hhmmss.sss */ + char _date[6]; /* ddmmyy */ + char _status = 0; /* 'V' or 'A' */ + char indicator; /* 'N' or 'S' or 'E' or 'W' */ + char work[8]; /* Work buffer for converting String data */ + BOOL b_get = FALSE; + uint8_t fixsts = NAVIINFO_DIAG_GPS_FIX_STS_NON; + uint8_t fixsts_gga; + BOOL bvalid_lon = FALSE; + BOOL bvalid_lat = FALSE; + BOOL bvalid = FALSE; + TG_TIM_ROLOVR_YMD base_ymd; + TG_TIM_ROLOVR_YMD conv_ymd; + BOOL roll_over_sts; + u_int8 _tdsts = g_gpstime_raw_tdsts; + + GpsSatelliteInfo st_visible_satellite_list[GPS_MAX_NUM_VISIBLE_SATELLITES]; /* Visible satellite list */ + GpsSatelliteInfo st_tmp_buf; + + int32 i = 0; + int32 j = 0; + int32 k = 0; + + SENSORLOCATION_LONLATINFO_DAT st_lonlat; + SENSORLOCATION_ALTITUDEINFO_DAT st_altitude; + SENSORMOTION_HEADINGINFO_DAT st_heading; + // MDEV_GPS_RTC st_rtc; + SENSOR_MSG_GPSTIME st_gps_time; + SENSORMOTION_SPEEDINFO_DAT st_speed; + + memset(&st_lonlat, 0x00, sizeof(SENSORLOCATION_LONLATINFO_DAT)); + memset(&st_altitude, 0x00, sizeof(SENSORLOCATION_ALTITUDEINFO_DAT)); + memset(&st_heading, 0x00, sizeof(SENSORMOTION_HEADINGINFO_DAT)); + // memset(&st_rtc, 0x00, sizeof(MDEV_GPS_RTC)); + memset(&st_gps_time, 0x00, sizeof(SENSOR_MSG_GPSTIME)); + memset(&st_speed, 0x00, sizeof(SENSORMOTION_SPEEDINFO_DAT)); + + /* Satellite signal strength list initialization */ + (void)memset(st_visible_satellite_list, 0x00, sizeof(GpsSatelliteInfo) * GPS_MAX_NUM_VISIBLE_SATELLITES); + + /* Get received NMEA data from storage area(GSA) */ + b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data), GPS_NMEA_INDEX_GSA); + + if (b_get == TRUE) { + fixsts = static_cast(GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSA_FS, uc_nmea_data)); + } + + /* Get received NMEA data from storage area(RMC) */ + b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_RMC); + + if (b_get == TRUE) { + navilocinfo->stDiagGps.stFix.stWgs84.lLat = + GetLonLatFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_LATITUDE, uc_nmea_data, &bvalid_lat); /* GPS location information and latitude */ + + GetStringFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_NS, uc_nmea_data, &indicator, sizeof(indicator)); + + if (indicator != GPS_NMEA_RMC_IND_NORTH) { + navilocinfo->stDiagGps.stFix.stWgs84.lLat *= -1; + } + + POSITIONING_LOG("lLat = %d", navilocinfo->stDiagGps.stFix.stWgs84.lLat); + + navilocinfo->stDiagGps.stFix.stWgs84.lLon = + GetLonLatFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_LONGITUDE, uc_nmea_data, &bvalid_lon); /* GPS position information and longitude */ + + GetStringFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_EW, uc_nmea_data, &indicator, sizeof(indicator)); + + if (indicator != GPS_NMEA_RMC_IND_EAST) { + navilocinfo->stDiagGps.stFix.stWgs84.lLon *= -1; + } + + st_lonlat.Longitude = navilocinfo->stDiagGps.stFix.stWgs84.lLon; + st_lonlat.Latitude = navilocinfo->stDiagGps.stFix.stWgs84.lLat; + + POSITIONING_LOG("lLon = %d", navilocinfo->stDiagGps.stFix.stWgs84.lLon); + + /* Get Date information */ + (void)GetStringFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_DATE, uc_nmea_data, _date, 6); + (void)memset(&base_ymd, 0, sizeof(base_ymd)); /* QAC 3200 */ + (void)memset(&conv_ymd, 0, sizeof(conv_ymd)); /* QAC 3200 */ + (void)memset(work, 0, sizeof(work)); /* QAC 3200 */ + + (void)strncpy(work, &_date[4], 2); /* QAC 3200 */ + base_ymd.year = (u_int16)(2000 + atoi(work)); /* YEAR */ + + st_gps_time.utc.year = (uint8_t)atoi(work); + + (void)strncpy(work, &_date[2], 2); /* QAC 3200 */ + base_ymd.month = (u_int16)(atoi(work)); /* MONTH */ + + st_gps_time.utc.month = (uint8_t)atoi(work); + + (void)strncpy(work, &_date[0], 2); /* QAC 3200 */ + base_ymd.day = (u_int16)(atoi(work)); /* DAY */ + + st_gps_time.utc.date = (uint8_t)atoi(work); + + POSITIONING_LOG("year = %d", base_ymd.year); + POSITIONING_LOG("month = %d", base_ymd.month); + POSITIONING_LOG("date = %d", base_ymd.day); + + /* UTC time information acquisition */ + (void)GetStringFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_UTC, uc_nmea_data, utc_time, 12); + + (void)strncpy(work, &utc_time[0], 2); /* QAC 3200 */ + navilocinfo->stNaviGps.utc.hour = (uint8_t)atoi(work); /* HOUR */ + + st_gps_time.utc.hour = navilocinfo->stNaviGps.utc.hour; + POSITIONING_LOG("hour = %d", navilocinfo->stNaviGps.utc.hour); + + (void)strncpy(work, &utc_time[2], 2); /* QAC 3200 */ + navilocinfo->stNaviGps.utc.minute = (uint8_t)atoi(work); /* MINUTE */ + + st_gps_time.utc.minute = navilocinfo->stNaviGps.utc.minute; + POSITIONING_LOG("minute = %d", navilocinfo->stNaviGps.utc.minute); + + (void)strncpy(work, &utc_time[4], 2); /* QAC 3200 */ + navilocinfo->stNaviGps.utc.second = (uint8_t)atoi(work); /* SECOND */ + + st_gps_time.utc.second = navilocinfo->stNaviGps.utc.second; + POSITIONING_LOG("second = %d", navilocinfo->stNaviGps.utc.second); + + /* Compass information acquisition */ + navilocinfo->stNaviGps.heading = + GetHeadingFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_COG, uc_nmea_data, &bvalid); + + st_heading.Heading = navilocinfo->stNaviGps.heading; + POSITIONING_LOG("heading = %u", navilocinfo->stNaviGps.heading); + + st_speed.Speed = GetSpeedFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_SPEED, uc_nmea_data, &bvalid); + + /* Fix Status/Time Status Calculation */ + (void)GetStringFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_STATUS, uc_nmea_data, &_status, sizeof(_status)); + + if ((_status == GPS_NMEA_RMC_STS_VALID) && (bvalid_lat == TRUE) && (bvalid_lon == TRUE)) { + /* Fix status information */ + switch (fixsts) { + case GPS_NMEA_GSA_FIX_STS_NON: + navilocinfo->stDiagGps.stFix.ucFixSts = NAVIINFO_DIAG_GPS_FIX_STS_NON; + break; + case GPS_NMEA_GSA_FIX_STS_2D: + navilocinfo->stDiagGps.stFix.ucFixSts = NAVIINFO_DIAG_GPS_FIX_STS_2D; + break; + case GPS_NMEA_GSA_FIX_STS_3D: + navilocinfo->stDiagGps.stFix.ucFixSts = NAVIINFO_DIAG_GPS_FIX_STS_3D; + break; + default: + POSITIONING_LOG("GSA Nav Mode Error [fixsts:%d]", fixsts); + break; + } + + if (_tdsts == POS_TIMESTS_OK) { + roll_over_sts = DevCalcRollOverTime(&base_ymd, &conv_ymd); + navilocinfo->stNaviGps.utc.year = conv_ymd.year; /* year (after conversion) */ + navilocinfo->stNaviGps.utc.month = (u_int8)(conv_ymd.month); /* month (after conversion) */ + navilocinfo->stNaviGps.utc.date = (u_int8)(conv_ymd.day); /* dat (after conversion) */ + if (roll_over_sts == FALSE) { + navilocinfo->stNaviGps.tdsts = POS_TIMESTS_NG; + /* Reserve[0] is time setting information: anomaly time, but can be calculated by rolling over. */ + navilocinfo->stNaviGps.reserve[0] = GPS_TIME_ROLOVR; + } else { + /* When the location information is normal, the time information is also judged to be normal. */ + navilocinfo->stNaviGps.tdsts = POS_TIMESTS_OK; /* Time calibration completed */ + /* Reserve[0] is time setting information: Use time status received from GPS device. */ + navilocinfo->stNaviGps.reserve[0] = GPS_TIME_RX; + } + } else { + navilocinfo->stNaviGps.tdsts = POS_TIMESTS_NG; /* Time uncalibrated */ + /* Reserve[0] is time setting information: Use time status received from GPS device. */ + navilocinfo->stNaviGps.reserve[0] = GPS_TIME_RX; + navilocinfo->stNaviGps.utc.year = base_ymd.year; /* year(after conversion) */ + navilocinfo->stNaviGps.utc.month = (u_int8)(base_ymd.month); /* month (after conversion) */ + navilocinfo->stNaviGps.utc.date = (u_int8)(base_ymd.day); /* day (after conversion) */ + } + + if (bvalid != TRUE) { + /* Invalid value if measurement orientation is invalid. */ + navilocinfo->stNaviGps.heading = GPS_HEADING_INVALID_VAL; + // POSITIONING_LOG("RMC Heading[cog] Invalid"); + } + } else { + /* Fix status information: Non-position fix is set regardless of FS of GSA. */ + navilocinfo->stDiagGps.stFix.ucFixSts = NAVIINFO_DIAG_GPS_FIX_STS_NON; + /* If the location information is invalid, the time information is also judged to be invalid. */ + /* Time not calibrated after receiver reset (time entry or master reset or CSF activation) */ + navilocinfo->stNaviGps.tdsts = POS_TIMESTS_NG; + /* Reserve[0] is time setting information: Use time status received from GPS device. */ + navilocinfo->stNaviGps.reserve[0] = GPS_TIME_RX; + navilocinfo->stNaviGps.utc.year = base_ymd.year; /* year (after conversion) */ + navilocinfo->stNaviGps.utc.month = (u_int8)(base_ymd.month); /* month (after conversion) */ + navilocinfo->stNaviGps.utc.date = (u_int8)(base_ymd.day); /* day (after conversion) */ + // POSITIONING_LOG("RMC Invalid[status:%d, bvalidLat:%d, bvalidLon:%d]", _status, bvalid_lat, bvalid_lon); + } + + // POSITIONING_LOG("year(Fix) = %d", navilocinfo->stNaviGps.utc.year); + // POSITIONING_LOG("month(Fix) = %d", navilocinfo->stNaviGps.utc.month); + // POSITIONING_LOG("date(Fix) = %d", navilocinfo->stNaviGps.utc.date); + // POSITIONING_LOG("tdsts = %d", navilocinfo->stNaviGps.tdsts); + } + + /* Get received NMEA data from storage area(GGA) */ + b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data), GPS_NMEA_INDEX_GGA); + + if (b_get == TRUE) { + /* Data status acquisition */ + fixsts_gga = (uint8_t)GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GGA_FS, uc_nmea_data); + + /* Altitude information acquisition */ + if (((fixsts == GPS_NMEA_GSA_FIX_STS_2D) || + (fixsts == GPS_NMEA_GSA_FIX_STS_3D)) && + ((fixsts_gga == GPS_NMEA_GGA_FIX_STATUS_GPS) || + (fixsts_gga == GPS_NMEA_GGA_FIX_STATUS_DGPS) || + (fixsts_gga == GPS_NMEA_GGA_FIX_STATUS_DR))) { + navilocinfo->stNaviGps.altitude = + GetAltitudeFromNmeaGpsCommon(GPS_NMEA_FNO_GGA_MSL, uc_nmea_data, &bvalid); + + if (bvalid != TRUE) { + /* If the location information is invalid, set an invalid value. */ + navilocinfo->stNaviGps.altitude = GPS_ALTITUDE_INVALID_VAL; + // POSITIONING_LOG("GGA Altitude[msl] Invalid"); + } + } else { + /* If the location information is invalid, set an invalid value. */ + navilocinfo->stNaviGps.altitude = GPS_ALTITUDE_INVALID_VAL; + // POSITIONING_LOG("GGA Invalid[fixsts:%d, fixstsGGA:%d]", fixsts, fixsts_gga); + } + + st_altitude.Altitude = navilocinfo->stNaviGps.altitude; + // POSITIONING_LOG("altitude = %d", navilocinfo->stNaviGps.altitude); + } + + DevGpsCnvLonLatNavi(&st_lonlat, navilocinfo->stDiagGps.stFix.ucFixSts, navilocinfo->stDiagGps.stFix.stWgs84.lLon, + navilocinfo->stDiagGps.stFix.stWgs84.lLat); + + DevGpsCnvAltitudeNavi(&st_altitude, navilocinfo->stDiagGps.stFix.ucFixSts, navilocinfo->stNaviGps.altitude); + + DevGpsCnvHeadingNavi(&st_heading, navilocinfo->stDiagGps.stFix.ucFixSts, navilocinfo->stNaviGps.heading); + + SendCustomGps(&st_gps_time, &st_lonlat, &st_altitude, &st_heading, &navilocinfo->stDiagGps); + // For test todo don't needed + // SendTimeGps(&st_rtc); + // SendSpeedGps(&st_speed, 0); + + /* Create visual satellite information list from GSV1~GSV5 and GSV */ + for (i = 0; i < 5; i++) { + /* Get received NMEA data from storage area */ + b_get = DevGpsCycleDataGetNmea( uc_nmea_data, sizeof(uc_nmea_data), + (ENUM_GPS_NMEA_INDEX)(GPS_NMEA_INDEX_GSV1 + i)); + + if (b_get == TRUE) { + /* Get number of Satellites in View */ + no_sv = GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSV_NOSV, uc_nmea_data); + + for (j = 0; j < GPS_NMEA_NUM_GSV_SINFO; j++) { + if (__offset >= no_sv) { + break; + } + + st_visible_satellite_list[__offset].sv = static_cast( + GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSV_SV + (u_int8)(4 * j), uc_nmea_data)); /* Satellite information_No */ + st_visible_satellite_list[__offset].elv = static_cast( + GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSV_ELV + (u_int8)(4 * j), uc_nmea_data)); /* Satellite information_elevation */ + st_visible_satellite_list[__offset].az = static_cast( + GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSV_AZ + (u_int8)(4 * j), uc_nmea_data)); /* Satellite Information_Azimuth */ + st_visible_satellite_list[__offset].cno = static_cast( + GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSV_CNO + (u_int8)(4 * j), uc_nmea_data)); /* Satellite information_level */ + st_visible_satellite_list[__offset].sts = DevGpsGetGpsRcvSts(st_visible_satellite_list[__offset].sv); + + /* Sort in ascending order of status (priority to use fix) and received signal strength */ + for (k = __offset; k > 0; k--) { + if (((st_visible_satellite_list[k].sts == NAVIINFO_DIAG_GPS_RCV_STS_USEFIX) && + (st_visible_satellite_list[k - 1].sts == NAVIINFO_DIAG_GPS_RCV_STS_TRACHING)) || + ((st_visible_satellite_list[k - 1].sts == st_visible_satellite_list[k].sts) && + (st_visible_satellite_list[k].cno > st_visible_satellite_list[k - 1].cno))) { + (void)memcpy(&st_tmp_buf, &st_visible_satellite_list[k], sizeof(GpsSatelliteInfo)); + (void)memcpy(&st_visible_satellite_list[k], &st_visible_satellite_list[k - 1], sizeof(GpsSatelliteInfo)); + (void)memcpy(&st_visible_satellite_list[k - 1], &st_tmp_buf, sizeof(GpsSatelliteInfo)); + } else { + break; + } + } + + __offset++; + } + } + } + + return; +} + +/**************************************************************************** +@brief DevGpsCycleDataClear
+ Cyclic data storage area clear processing +@outline Clear the cyclic data storage area +@param[in] none +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsCycleDataClear(void) { + int32 i = 0; + + /* Sensor counter, reception flag initialization */ + g_st_gpscycle_data.uc_sns_cnt = 0; + + for (i = 0; i < GPS_NMEA_INDEX_MAX; i++) { + g_st_gpscycle_data.st_nmea_data.uc_rcv_flag[i] = GPS_CYCLECMD_NOTRCV; + } + + g_st_gpscycle_data.st_binary_data.uc_rcv_flag = GPS_CYCLECMD_NOTRCV; + g_st_gpscycle_data.st_fullbin_data.uc_rcv_flag = GPS_CYCLECMD_NOTRCV; +} + +/****************************************************************************** +@brief DEV_Gps_CycleData_SetNmea
+ NMEA data setting process +@outline Set NMEA data in cyclic data storage area +@param[in] u_int8* : p_data ... NMEA data to be set +@param[in] u_int32 : ul_length ... Data length +@param[in] ENUM_GPS_NMEA_INDEX: e_format ... Sentence identification +@param[out] none +@return none +@retval none +*******************************************************************************/ +void DevGpsCycleDataSetNmea(const u_int8* p_data, u_int32 ul_length, ENUM_GPS_NMEA_INDEX e_format) { + u_int32 ul_copy_sz = 0; + + /** Anomaly detection */ + if (e_format >= GPS_NMEA_INDEX_MAX) { + POSITIONING_LOG("# GpsCommCtl_API # Set NMEA Sentence ERROR ! \r\n"); + } else { + /** Storage size determination */ + if (GPS_NMEA_MAX_SZ < ul_length) { + ul_copy_sz = GPS_NMEA_MAX_SZ; + POSITIONING_LOG("# GpsCommCtl_API # Set NMEA Cmd Size ERROR ! \r\n"); + } else { + ul_copy_sz = ul_length; + } + + /** Storing */ + g_st_gpscycle_data.st_nmea_data.uc_rcv_flag[e_format] = GPS_CYCLECMD_RCV; + memset(&(g_st_gpscycle_data.st_nmea_data.st_nmea[e_format].uc_data[0]), 0x00, GPS_NMEA_MAX_SZ); + memcpy(&(g_st_gpscycle_data.st_nmea_data.st_nmea[e_format].uc_data[0]), p_data, ul_copy_sz); + } + + return; +} + +/****************************************************************************** +@brief DevGpsCycleDataGetNmea
+ NMEA data setting process +@outline Set NMEA data in cyclic data storage area +@param[in] u_int32 : ul_buf_size ... Storage destination buffer size +@param[in] ENUM_GPS_NMEA_INDEX: e_format ... Sentence identification +@param[out] u_int8* : p_data ... Storage destination buffer pointer +@return BOOL +@retval TRUE : Data present +@retval FALSE : No data +*******************************************************************************/ +BOOL DevGpsCycleDataGetNmea(u_int8 *p_data, u_int32 ul_buf_size, ENUM_GPS_NMEA_INDEX e_format) { + BOOL ret = TRUE; + + /** Determining whether data exists in the cyclic data area */ + if (GPS_CYCLECMD_RCV == g_st_gpscycle_data.st_nmea_data.uc_rcv_flag[e_format]) { + if (GPS_NMEA_MAX_SZ <= ul_buf_size) { + /** Copy to storage destination buffer */ + memcpy(p_data, &(g_st_gpscycle_data.st_nmea_data.st_nmea[e_format].uc_data[0]), GPS_NMEA_MAX_SZ); + } else { + /** Storage destination buffer size is small */ + ret = FALSE; + } + } else { + /** Not received */ + ret = FALSE; + } + + return ret; +} + +/** + * @brief + * Setting of the check sum + * + * @param[in] buffer Pointer of data + * @param[in] length length of data + */ +void DevGpsSetChkSum(u_int8* buffer, u_int32 length) { + u_int16 i = 0; + u_int8 ck_a = 0; + u_int8 ck_b = 0; + + if (buffer != NULL) { + for (i = 2; i < (length - 2); i++) { + ck_a = ck_a + buffer[i]; + ck_b = ck_b + ck_a; + } + + /* Checksum_Set */ + buffer[length - 2] = ck_a; + buffer[length - 1] = ck_b; + } else { + } +} + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/positioning_hal/src/GpsCommon/MDev_Gps_TimerCtrl.cpp b/positioning_hal/src/GpsCommon/MDev_Gps_TimerCtrl.cpp new file mode 100644 index 00000000..8dbf8dc7 --- /dev/null +++ b/positioning_hal/src/GpsCommon/MDev_Gps_TimerCtrl.cpp @@ -0,0 +1,293 @@ +/* + * @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 MDev_Gps_TimerCtrl.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "MDev_Gps_TimerCtrl.h" + +// #include "MDev_Gps_Main.h" +// #include "MDev_GpsRecv.h" + +/*---------------------------------------------------------------------------*/ +// Global values + +static GPS_TIM_MNG g_st_tim_mng; + +/** Timer setting information table */ +static const GPS_TIM_INFO kTimInfo[TIM_NUM] = { + /* GSP related */ + {TIMVAL_GPS_STARTUP, PNO_NAVI_GPS_MAIN}, /* Start confirmation monitoring timer */ + {TIMVAL_GPS_RCVCYCLDAT, PNO_NAVI_GPS_MAIN}, /* Periodic reception data monitoring timer */ + {TIMVAL_GPS_RCVACK, PNO_NAVI_GPS_MAIN}, /* ACK reception monitoring timer */ + {TIMVAL_GPS_NAVIFST, PNO_NAVI_GPS_MAIN}, /* Initial Navigation Monitoring Timer */ + {TIMVAL_GPS_NAVICYCLE, PNO_NAVI_GPS_MAIN}, /* Navi monitoring timer */ + {TIMVAL_GPS_NAVIDISRPT, PNO_NAVI_GPS_MAIN}, /* Navigation Monitoring Disruption Log Output Timer */ + {TIMVAL_GPS_DIAGCLKGUARD, PNO_NAVI_GPS_MAIN}, /* Diagnosis provision time guard monitoring timer */ + {TIMVAL_GPS_NMEADATAGUARD, PNO_NAVI_GPS_MAIN}, /* NMEA data providing guard monitoring timer */ + {TIMVAL_GPS_RECOVERY, PNO_NAVI_GPS_MAIN}, /* GPS recovery timer */ + {TIMVAL_GPS_RECEIVERERR, PNO_NAVI_GPS_MAIN}, /* GPS receiver anomaly detection timer */ + /* Sensor related */ + {TIMVAL_SNS_RCVFSTDAT, PNO_VEHICLE_SENSOR}, /* Initial cyclic sensor data reception monitoring timer */ + {TIMVAL_SNS_RCVCYCLDAT, PNO_VEHICLE_SENSOR}, /* Cyclic sensor data reception monitoring timer */ + {TIMVAL_SNS_RCVDISRPT, PNO_VEHICLE_SENSOR}, /* Cyclic sensor data interruption log output timer */ +}; + +/*---------------------------------------------------------------------------*/ +// Functions + +static uint16_t TimeMakSeqNo(GPS_TIM_KIND tim_kind) { + GPS_TIM_MNG* pst_tim_mng = NULL; + u_int16 seq_no = 0; /* Timer sequence number */ + + pst_tim_mng = &g_st_tim_mng; + + /*------------------------------------------------------------------------*/ + /* Definition of Sequence Number */ + /* |------------------- Sequence number(2Byte) -----------------------| */ + /* 15 8 7 0 */ + /* +-------------------------------+-----------------------------------+ */ + /* | Timer type(1Byte) | Counter(1Byte)(0x01 ? 0xFF) | */ + /* +-------------------------------+-----------------------------------+ */ + /* The timer type is 0x00. ? (Number of timers-1) */ + /* counters is 0x01 ? 0xFF(Do not use 0x00) */ + /* (Counters are counted up each time a timer is started. */ + /* If the counter counts up when it is 0xFF, */ + /* be counted up from the 0x01.) */ + /*------------------------------------------------------------------------*/ + seq_no = ((u_int16)tim_kind << 8) | (pst_tim_mng->sts[tim_kind].cnt); + + return seq_no; +} + +static BOOL VehicleUtilitySetTimer(GPS_TIM_KIND tim_kind) { + GPS_TIM_MNG* pst_tim_mng = NULL; + const uint32_t * p_time_val; + const PNO* p_pno; + RET_API api_ret = RET_NORMAL; + u_int16 seq_no = 0; + BOOL ret = TRUE; + + // Initialize + pst_tim_mng = &g_st_tim_mng; + p_time_val = &(kTimInfo[tim_kind].timer_val); /* Timer set value */ + p_pno = &(kTimInfo[tim_kind].pno); /* Notify party PNO */ + + if (pst_tim_mng->sts[tim_kind].flag == TIMER_ON) { + /*-----------------------------------------------------------------------*/ + /* When the same timer has already started, */ + /* terminate without starting the timer because the timer is set multiple times. */ + /*-----------------------------------------------------------------------*/ + ret = FALSE; + } else { + /*-----------------------------------------------------------------------*/ + /* Count up the timer counter of the corresponding timer by 1. */ + /*-----------------------------------------------------------------------*/ + if (pst_tim_mng->sts[tim_kind].cnt >= TIM_CNTMAX) { + /*-----------------------------------------------------------------------*/ + /* When the count reaches the maximum number, it counts again from 1. */ + /*-----------------------------------------------------------------------*/ + pst_tim_mng->sts[tim_kind].cnt = TIM_CNTMIN; + } else { + /*-----------------------------------------------------------------------*/ + /* If the count has not reached the maximum, it is counted up. */ + /*-----------------------------------------------------------------------*/ + pst_tim_mng->sts[tim_kind].cnt++; + } + + /*-----------------------------------------------------------------------*/ + /* Creating timer sequence numbers */ + /*-----------------------------------------------------------------------*/ + seq_no = TimeMakSeqNo(tim_kind); + + /*-----------------------------------------------------------------------*/ + /* Start the timer */ + /*-----------------------------------------------------------------------*/ + api_ret = _pb_ReqTimerStart(*p_pno, seq_no, TIMER_TYPE_USN, static_cast(*p_time_val)); + if (api_ret != RET_NORMAL) { + ret = FALSE; + } else { + /*-----------------------------------------------------------------------*/ + /* If successful timer start, */ + /* set the start/stop flag of the corresponding timer to start (MCSUB_ON). */ + /*-----------------------------------------------------------------------*/ + pst_tim_mng->sts[tim_kind].flag = TIMER_ON; + } + } + + return ret; +} + +static BOOL VehicleUtilityStopTimer(GPS_TIM_KIND tim_kind) { + GPS_TIM_MNG* pst_tim_mng = NULL; + const PNO* p_pno; + BOOL ret = TRUE; + RET_API api_ret = RET_NORMAL; + u_int16 seq_no = 0; + + // Initialize + pst_tim_mng = &g_st_tim_mng; + p_pno = &(kTimInfo[tim_kind].pno); /* Notify party PNO */ + + /* Check timer start/stop flag */ + if (pst_tim_mng->sts[tim_kind].flag == TIMER_OFF) { + /* If it is already stopped, do nothing. */ + ret = FALSE; + } else { + /*-----------------------------------------------------------------------*/ + /* Creating timer sequence numbers */ + /*-----------------------------------------------------------------------*/ + seq_no = TimeMakSeqNo(tim_kind); + + /*-----------------------------------------------------------------------*/ + /* Set the corresponding timer to stop */ + /*-----------------------------------------------------------------------*/ + api_ret = _pb_TimerStop(*p_pno, seq_no, TIMER_TYPE_USN); + + if (api_ret != RET_NORMAL) { + ret = FALSE; + } + + /*-----------------------------------------------------------------------*/ + /* Set the start/stop flag of the corresponding timer to stop (MCSUB_OFF) */ + /* Set the ID of the corresponding timer to invalid (DEV_TED_INVALID) */ + /*-----------------------------------------------------------------------*/ + pst_tim_mng->sts[tim_kind].flag = TIMER_OFF; + } + + return ret; +} + +static BOOL VehicleUtilityTimeJdgKnd(uint16_t seqno) { + GPS_TIM_MNG* pst_tim_mng = NULL; + BOOL ret = FALSE; + u_int8 timekind = 0; + u_int8 count = 0; + + // Initialize + pst_tim_mng = &g_st_tim_mng; + + timekind = (u_int8)((seqno & 0xff00) >> 8); + count = (u_int8)(seqno & 0x00ff); + + /* Timer type is unexpected */ + if (timekind >= TIM_NUM) { + ret = FALSE; + } else { + if ((pst_tim_mng->sts[timekind].cnt == count) && (pst_tim_mng->sts[timekind].flag == TIMER_ON)) { + /* The counter matches and the counter start/stop flag is "Start". */ + ret = TRUE; + } else { + /* Not applicable due to differences */ + ret = FALSE; + } + } + + return ret; +} + +/******************************************************************************* + * MODULE : DEV_Gps_Tim_Init + * ABSTRACT : Timer function initialization processing + * FUNCTION : Initialize the timer function + * ARGUMENT : None + * NOTE : 1.Initialize timer management table + * RETURN : None + ******************************************************************************/ +void DevGpsTimInit(void) { + GPS_TIM_MNG* pst_tim_mng = NULL; + u_int32 i = 0; + + // Initialie + pst_tim_mng = &g_st_tim_mng; + + /* Initialize timer management table */ + memset(pst_tim_mng, 0x00, sizeof(GPS_TIM_MNG)); + + for (i = 0; i < TIM_NUM; i++) { + pst_tim_mng->sts[i].flag = TIMER_OFF; + pst_tim_mng->sts[i].cnt = 0; + } + + return; +} + +/******************************************************************************* + * MODULE : DevGpsTimeSet + * ABSTRACT : Timer start processing + * FUNCTION : Starts a timer of the specified type + * ARGUMENT : GPS_TIM_KIND tim_kind Timer type + * NOTE : 1.Increment total number of timer start + * 2.Timer Sequence Number Creation + * 3.Get timeout value + * 4.Timer start + * RETURN : TRUE : Normal completion + * : FALSE : ABENDs + ******************************************************************************/ +BOOL DevGpsTimeSet(GPS_TIM_KIND tim_kind) { + BOOL ret = TRUE; + + /* Binding of unused timer */ + if ((tim_kind != GPS_RECV_ACK_TIMER) + && (tim_kind != GPS_STARTUP_TIMER) + && (tim_kind != GPS_CYCL_TIMER) + && (tim_kind != GPS_NAVIFST_TIMER) + && (tim_kind != GPS_NAVICYCLE_TIMER) + && (tim_kind != GPS_NAVIDISRPT_TIMER) + && (tim_kind != GPS_RECOVERY_TIMER) + && (tim_kind != GPS_RECEIVERERR_TIMER)) { + return ret; + } + ret = VehicleUtilitySetTimer(tim_kind); + return ret; +} + +/******************************************************************************* + * MODULE : DevGpsTimeStop + * ABSTRACT : Timer stop processing + * FUNCTION : Stops a timer of the specified type + * ARGUMENT : GPS_TIM_KIND tim_kind Timer type + * NOTE : 1.Get the sequence number of the specified type + * 2.Timer stop + * RETURN : TRUE : Normal completion + * : FALSE : ABENDs + ******************************************************************************/ +BOOL DevGpsTimeStop(GPS_TIM_KIND tim_kind) { + BOOL ret = TRUE; + ret = VehicleUtilityStopTimer(tim_kind); + return ret; +} + +/******************************************************************************** + * MODULE : DevGpsTimeJdgKind + * ABSTRACT : Timer Sequence Number Determination + * FUNCTION : Determine whether the timer sequence number corresponds to the one being managed + * ARGUMENT : Timer Sequence Number + * NOTE : + * RETURN : TRUE : Normal completion(No problem) + * : FALSE : ABENDs(Unusual number) + ********************************************************************************/ +BOOL DevGpsTimeJdgKind(u_int16 seqno) { + BOOL ret; + ret = VehicleUtilityTimeJdgKnd(seqno); + return ret; +} + +/*---------------------------------------------------------------------------*/ +/*EOF*/ 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(&(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(&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(&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(ul_work); + d_work = d_work / static_cast(i); + d_work = d_work * static_cast(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(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(&g_speed_kmph_data), static_cast(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
+ * 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 +#include +#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*/ -- cgit 1.2.3-korg