diff options
Diffstat (limited to 'video_in_hal/positioning_hal/src')
13 files changed, 0 insertions, 7248 deletions
diff --git a/video_in_hal/positioning_hal/src/Common/positioning_common.cpp b/video_in_hal/positioning_hal/src/Common/positioning_common.cpp deleted file mode 100755 index 00b3a8b..0000000 --- a/video_in_hal/positioning_hal/src/Common/positioning_common.cpp +++ /dev/null @@ -1,388 +0,0 @@ -/* - * @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<u_int32 *>(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<void**>(&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<const void*>(&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<const void*>(&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/video_in_hal/positioning_hal/src/Common/positioning_hal.cpp b/video_in_hal/positioning_hal/src/Common/positioning_hal.cpp deleted file mode 100755 index 94f79c4..0000000 --- a/video_in_hal/positioning_hal/src/Common/positioning_hal.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* - * @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 <stdio.h> - -#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/video_in_hal/positioning_hal/src/GpsCommon/MDev_GpsRecv.cpp b/video_in_hal/positioning_hal/src/GpsCommon/MDev_GpsRecv.cpp deleted file mode 100755 index 13159b4..0000000 --- a/video_in_hal/positioning_hal/src/GpsCommon/MDev_GpsRecv.cpp +++ /dev/null @@ -1,608 +0,0 @@ -/* - * @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 <stub/gps_mng_api.h> -//} - -/*---------------------------------------------------------------------------*/ -// 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); - - /* <<Creation of message header section>>>> */ - 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)); - /* <<Creation of data section>> */ - /* Copy data to send buffer */ - memcpy(&by_snd_buf[ sizeof(T_APIMSG_MSGBUF_HEADER)], ptg_rcv_data, w_size); - - /* <<Messaging>> */ - /* 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<void*>(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/video_in_hal/positioning_hal/src/GpsCommon/MDev_GpsRollOver.cpp b/video_in_hal/positioning_hal/src/GpsCommon/MDev_GpsRollOver.cpp deleted file mode 100755 index 62227c2..0000000 --- a/video_in_hal/positioning_hal/src/GpsCommon/MDev_GpsRollOver.cpp +++ /dev/null @@ -1,275 +0,0 @@ -/* - * @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/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_API.cpp b/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_API.cpp deleted file mode 100755 index 35f5201..0000000 --- a/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_API.cpp +++ /dev/null @@ -1,491 +0,0 @@ -/* - * @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<BR> - 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<MDEV_GPS_CUSTOMDATA *>(&(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<void*>(&s_send_msg), 0); - - if (ret != RET_NORMAL) { - ret = RET_ERROR; - } - - return ret; -} - -/****************************************************************************** -@brief SendSpeedGps<BR> - 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<BR> - 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<BR> - 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<void *>(&(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/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_Common.cpp b/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_Common.cpp deleted file mode 100755 index c8b5c9a..0000000 --- a/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_Common.cpp +++ /dev/null @@ -1,2105 +0,0 @@ -/* - * @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 <native_service/ns_backup.h> -#include <native_service/ns_backup_id.h> -#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<void *>(&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<BR> - 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<BR> - 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<TG_GPS_SND_DATA*>(&(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<BR> - 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<POS_RESETINFO *>(&(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<POS_RESETINFO*>(&(g_gps_msg_rcvr.msgdat)), sizeof(POS_RESETINFO)); - GpsSetPosBaseEvent(st_rcv_msg.sndpno, POS_RET_ERROR_BUSY); - } - return; -} - -/****************************************************************************** -@brief CyclDataTimeOutGpsCommon<BR> - 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<BR> - 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<u_int8*>(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<BR> - 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<BR> - -FORMAT_NMEA NMEA data<BR> - -FORMAT_BIN Standard binary<BR> - -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<BR> - 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<BR> - 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<BR> - 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<TG_GPS_SND_DATA*>(&(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<BR> - 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<BR> - 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<BR> - 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<TG_GPS_UBX_NAV_TIMEUTC_DATA*>(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<BR> - 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<u_int16>(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<int32>((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<br> - * 1 Format 2 first<br> - * 2 Same as<br> - * 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<u_int32>(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"); - - /* <<Creation of message header section>>>> */ - 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)); - - /* <<Creation of data section>> */ - 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) ); - - /* <<Messaging>> */ - ret = _pb_SndMsg(PNO_NAVI_GPS_MAIN, sizeof(uc_send_buf), reinterpret_cast<void *>(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<TG_GPS_SND_DATA*>(&(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<POS_DATETIME *>(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<void *>(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<int32>(st_datetime.year), - static_cast<int32>(st_datetime.month), - static_cast<int32>(st_datetime.date), static_cast<int32>(st_datetime.hour), - static_cast<int32>(st_datetime.minute), - static_cast<int32>(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<br> - * 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<int32>(pst_datetime->year), - static_cast<int32>(pst_datetime->month), - static_cast<int32>(pst_datetime->date), - static_cast<int32>(pst_datetime->hour), - static_cast<int32>(pst_datetime->minute), - static_cast<int32>(pst_datetime->second), &gpsw, &gpsws); - - - /* Send command sequence generation */ - pst_ublox_data = reinterpret_cast<TG_GPS_UBX_AID_INI_POLLED*>(reinterpret_cast<void *>(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<u_int8 *>(reinterpret_cast<void*>(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<double>(y / 400); - y %= 400; - jd += tmp * 146097; - - tmp = static_cast<double>(y / 100); - y %= 100; - jd += tmp * 36524; - - tmp = static_cast<double>(y / 4); - y %= 4; - jd += tmp * 1461; - jd += static_cast<double>(y * 365); - - tmp = static_cast<double>(m / 5); - m %= 5; - jd += tmp * 153; - - tmp = static_cast<double>(m / 2); - m %= 2; - jd += tmp * 61; - jd += static_cast<double>(m * 31); - jd += static_cast<double>(d); - - mjd = static_cast<int>(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<TG_GPS_UBX_PACKET_HEADER*>(reinterpret_cast<void*>(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<TG_GPS_UBX_COMMAND_NO_DATA*>(reinterpret_cast<void*>(p_ublox_data + - length + - sizeof(TG_GPS_UBX_COMMAND_NO_DATA))); - - memcpy(reinterpret_cast<void*>(pst_ubx_poll_req), reinterpret_cast<void*>(p_poll_cmd), poll_cmd_length); - - DevGpsSetChkSum(reinterpret_cast<u_int8 *>(reinterpret_cast<void*>(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<BR> - 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<int32>(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<int32>(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<BR> - 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<BR> - 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/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_Main.cpp b/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_Main.cpp deleted file mode 100755 index 0b9a4be..0000000 --- a/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_Main.cpp +++ /dev/null @@ -1,362 +0,0 @@ -/* - * @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<BR> - 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/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_Mtrx.cpp b/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_Mtrx.cpp deleted file mode 100755 index b36a617..0000000 --- a/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_Mtrx.cpp +++ /dev/null @@ -1,866 +0,0 @@ -/* - * @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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<TG_GPS_RCV_DATA*> - (reinterpret_cast<void*>(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<TG_GPS_RCV_DATA*> - (reinterpret_cast<void*>(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<TG_GPS_RCV_DATA*> - (reinterpret_cast<void*>(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<TG_GPS_RCV_DATA*> - (reinterpret_cast<void*>(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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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<BR> - 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/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_Nmea.cpp b/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_Nmea.cpp deleted file mode 100755 index 60fa1d8..0000000 --- a/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_Nmea.cpp +++ /dev/null @@ -1,928 +0,0 @@ -/* - * @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<const char *>(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<const char *>(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<const char *>(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<const char *>(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<const char *>(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<const char *>(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<const char *>(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<const char *>(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<const char *>(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<u_int8>(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<const char *>(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<u_int8>(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<const char *>(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<u_int8>(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<u_char*>(&(st_rcv_data.bygps_data[0])), - static_cast<u_int32>(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<u_char*>(&(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<const u_char*>(ENDMARK), - reinterpret_cast<const u_char*>(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<uint8_t>(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<uint8_t>( - GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSV_SV + (u_int8)(4 * j), uc_nmea_data)); /* Satellite information_No */ - st_visible_satellite_list[__offset].elv = static_cast<uint8_t>( - GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSV_ELV + (u_int8)(4 * j), uc_nmea_data)); /* Satellite information_elevation */ - st_visible_satellite_list[__offset].az = static_cast<uint16_t>( - GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSV_AZ + (u_int8)(4 * j), uc_nmea_data)); /* Satellite Information_Azimuth */ - st_visible_satellite_list[__offset].cno = static_cast<uint8_t>( - 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<BR> - 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<BR> - 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<BR> - 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/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_TimerCtrl.cpp b/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_TimerCtrl.cpp deleted file mode 100755 index 8dbf8dc..0000000 --- a/video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_TimerCtrl.cpp +++ /dev/null @@ -1,293 +0,0 @@ -/* - * @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<u_int32>(*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/video_in_hal/positioning_hal/src/LineSensDrv/LineSensDrv_Api.cpp b/video_in_hal/positioning_hal/src/LineSensDrv/LineSensDrv_Api.cpp deleted file mode 100755 index d363a4b..0000000 --- a/video_in_hal/positioning_hal/src/LineSensDrv/LineSensDrv_Api.cpp +++ /dev/null @@ -1,136 +0,0 @@ -/* - * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -/** -* @file LineSensDrv_Api.cpp -*/ - -/*---------------------------------------------------------------------------*/ -// Include files - -#include "LineSensDrv_Api.h" -#include "positioning_def.h" - -/*---------------------------------------------------------------------------*/ -// Macro definitions - -#define LSDRV_GPS_DATA_TOP_TH 5 // Threshold for determining the beginning of GPS data -#define LSDRV_SENS_DATA_RCV_WAIT_TIME 400 // Sensor data wait time of GPS reception flag ON(Total) -#define LSDRV_SENS_DATA_RCV_WAIT_TERM 50 // Sensor data wait time of GPS reception flag ON(1 time) - -#define LSDRV_EST_GPS_CNT_ARRAY_NUM 3U // Number of arrays for holding estimated GPS counter -#define LSDRV_EST_GPS_CNT_ENABLE_TH 5U // Estimated GPS counter enable/disable determination threshold -#define LSDRV_EST_GPS_CNT_ADD_VALUE 10U // Estimated GPS counter update addition value - -// GPS data reception cycle:1sec sensor counters: 100ms - -// for debug -#define LINE_SENS_DRV_API_DEBUG_SWITCH 0 // 0:OFF 1:ON - -/*---------------------------------------------------------------------------*/ -// Global variable - -static HANDLE g_gps_irq_mutex = NULL; // GPS received flag Mutex handles -static BOOL g_rcv_pps_int = FALSE; // PPS interrupt reception flag (GPS->_CWORD102_) -static BOOL g_rcv_gps_irq = FALSE; // GPS reception flag (GPS->_CWORD56_) -static u_int8 g_rcv_gps_sens_cnt_tmp = 0; // Sensor counter when GPS reception flag is ON(for retention) -static u_int8 g_rcv_gps_sens_cnt = 0; // Sensor counter when GPS reception flag is ON -static u_int8 g_gps_sens_cnt_top = 0; // Sensor counter when first GPS data is received -static u_int8 g_est_gps_cnt[LSDRV_EST_GPS_CNT_ARRAY_NUM]; // Array for storing estimated GPS counter values -static BOOL g_est_gps_cnt_available = FALSE; // Estimated GPS counter value enable/disable judgment - -typedef struct VehicleSensDataMaster { - DID ul_did; // Data ID - u_int16 us_size; // Size of the data - u_int8 uc_rcv_flag; // Receive flag - u_int8 uc_sns_cnt; // Sensor counter - u_int8 uc_data[132]; // Vehicle sensor data -} VEHICLESENS_DATA_MASTER; - - -/******************************************************************************* - * MODULE : DeliveryLineSensorDataPositioning - * ABSTRACT : LineSensor vehicle signaling notification messages sending process - * FUNCTION : Send LineSensor vehicle signalling notification messages - * ARGUMENT : *pstSendBuf:Transmitted data - * : uc_data_num :Number of sent data - * NOTE : - * RETURN : None - ******************************************************************************/ -void DeliveryLineSensorDataPositioning(LSDRV_MSG_LSDATA_G* pst_send_buf, u_int8 uc_data_num) { - if (pst_send_buf != NULL) { - /* Initializing sent messages */ - memset(reinterpret_cast<void *>(&(pst_send_buf->st_head)), 0, sizeof(T_APIMSG_MSGBUF_HEADER)); - - /* Message Header Settings */ - pst_send_buf->st_head.hdr.sndpno = PNO_LINE_SENS_DRV; /* Source process number */ - pst_send_buf->st_head.hdr.cid = CID_LINESENS_VEHICLE_DATA_G; /* Command ID */ - pst_send_buf->st_head.hdr.msgbodysize = sizeof(LSDRV_MSG_LSDATA_DAT_G); /* Message data body length */ - - /* Message data is already set */ - pst_send_buf->st_para.uc_data_num = uc_data_num; - - /* Send messages */ - (void)_pb_ZcSndMsg(PNO_VEHICLE_SENSOR, sizeof( LSDRV_MSG_LSDATA_G ), 0); - } -} - -/******************************************************************************* - * MODULE : LineSensDrvApi_Initialize - * ABSTRACT : LineSensDrvApi initialization process - * FUNCTION : LineSensDrvApi initialization process - * ARGUMENT : - - * NOTE : - * RETURN : - - ******************************************************************************/ -BOOL LineSensDrvApiInitialize(void) { - BOOL ret = TRUE; - - g_gps_irq_mutex = _pb_CreateMutex(NULL, FALSE, MUTEX_GPS_IRQ_FLG); - - if (g_gps_irq_mutex == 0) { - ret = FALSE; - } else { - g_rcv_gps_irq = FALSE; - g_rcv_gps_sens_cnt_tmp = 0; - LineSensDrvApiInitEstGpsCnt(); - ret = TRUE; - } - - return (ret); -} - -/******************************************************************************* - * MODULE : LineSensDrvApi_InitEstGpsCnt - * ABSTRACT : Estimated GPS counter related parameter initialization processing - * FUNCTION : Estimated GPS counter related parameter initialization processing - * ARGUMENT : - - * NOTE : - * RETURN : - - ******************************************************************************/ -void LineSensDrvApiInitEstGpsCnt(void) { - /* Initializing process */ - g_rcv_pps_int = FALSE; - g_rcv_gps_sens_cnt = 0; - g_gps_sens_cnt_top = 0; - - (void)memset(reinterpret_cast<void *>(&g_est_gps_cnt[0]), 0, sizeof(g_est_gps_cnt)); - g_est_gps_cnt_available = FALSE; - - return; -} - -/*---------------------------------------------------------------------------*/ -/*EOF*/ diff --git a/video_in_hal/positioning_hal/src/LineSensDrv/LineSensDrv_Sensor.cpp b/video_in_hal/positioning_hal/src/LineSensDrv/LineSensDrv_Sensor.cpp deleted file mode 100755 index 78ae488..0000000 --- a/video_in_hal/positioning_hal/src/LineSensDrv/LineSensDrv_Sensor.cpp +++ /dev/null @@ -1,622 +0,0 @@ -/* - * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -/** -* @file LineSensDrv_Snesor.cpp -*/ - -/*---------------------------------------------------------------------------*/ -// Include files - -#include "LineSensDrv_Sensor.h" -#include "LineSensDrv_Api.h" - -/*---------------------------------------------------------------------------*/ -// Value Define - -#define LSDRV_MASK_WORD_L 0x00FF -#define LSDRV_MASK_WORD_U 0xFF00 - -#define LINE_SENS_DRV_SENSOR_DEBUG_FACTORY 0 -#define LINE_SENS_DRV_SENSOR_DEBUG_DIAG 0 - -#define VEHICLE_SNS_INFO_PULSE_NUM 32 - -/*---------------------------------------------------------------------------*/ -// Global variable - -static LSDRV_SPEEDKMPH_DAT g_speed_kmph_data; -static uint8_t g_rcv_data_len; /* Receive SYS data length */ -extern uint8_t g_uc_vehicle_reverse; -/*---------------------------------------------------------------------------*/ -// Functions - -/******************************************************************************* - * MODULE : LineSensDrv_Sensor - * ABSTRACT : Sensor data reception processing - * FUNCTION : Convert sensor data to delivery format - * ARGUMENT : *ucRcvdata : Data pointer - * NOTE : - * RETURN : None - ******************************************************************************/ -void LineSensDrvSensor(u_int8* uc_rcvdata) { - u_int8 uc_sens_cnt = 0; - u_int16 us_sp_kmph = 0; /* Vehicle speed(km/h) */ - u_int16 us_sp_pls1 = 0; /* Total vehicle speed pulse(Latest) */ - u_int16 us_sp_pls2 = 0; /* Total vehicle speed pulse(Last time) */ - u_int8 us_sp_ret = 0; /* Last vehicle speed information acquisition result */ - u_int8 uc_size = 0; /* Size of the data */ - u_int16 us_cnt = 0; /* Data counter */ - u_int16 us_cnt2 = 0; /* Data counter */ - u_int8* uc_data_pos = NULL; /* Data storage location */ - LSDRV_MSG_LSDATA_G* p_snd_buf = NULL; - LSDRV_LSDATA_G* p_snd_data_base = NULL; - LSDRV_LSDATA_G* p_snd_data = NULL; - RET_API ret = RET_NORMAL; - - /* Receive sensor data top address acquisition */ - uc_data_pos = (uc_rcvdata); - - /* Create send buffer/delivery all received data */ - ret = _pb_GetZcSndBuf(PNO_VEHICLE_SENSOR, reinterpret_cast<void**>(&p_snd_buf)); - if ((ret == RET_NORMAL) && (p_snd_buf != NULL)) { - p_snd_data_base = p_snd_buf->st_para.st_data; - (void)memset(p_snd_data_base, 0, LSDRV_KINDS_MAX * sizeof( LSDRV_LSDATA_G )); - - /* Sensor counter */ - p_snd_data = p_snd_data_base + LSDRV_SNS_COUNTER; - uc_sens_cnt = (u_int8)*(uc_data_pos); - p_snd_data->ul_did = VEHICLE_DID_SNS_COUNTER; /* Data ID */ - p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_1; /* Size of the data */ - p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ - p_snd_data->uc_data[0] = uc_sens_cnt; /* Data content */ - uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_1); - - /* Gyro output */ - p_snd_data = p_snd_data_base + LSDRV_GYRO_EXT; - p_snd_data->ul_did = VEHICLE_DID_GYRO_EXT; /* Data ID */ - p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */ - p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ - for (us_cnt = 0; us_cnt < LSDRV_SNDMSG_DTSIZE_20; us_cnt++) { - /* Since [0] is older and [9] is newer, the order of received data is switched. */ - /* Be in reverse order for endian conversion */ - p_snd_data->uc_data[LSDRV_SNDMSG_DTSIZE_20 - (us_cnt + 1)] = (u_int8)*(uc_data_pos + us_cnt); - } - uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20); - - p_snd_data = p_snd_data_base + LSDRV_GYRO_X; - p_snd_data->ul_did = VEHICLE_DID_GYRO; /* Data ID */ - p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */ - p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ - for (us_cnt = 0; us_cnt < LSDRV_SNDMSG_DTSIZE_20; us_cnt++) { - /* Since [0] is older and [9] is newer, the order of received data is switched. */ - /* Be in reverse order for endian conversion */ - p_snd_data->uc_data[LSDRV_SNDMSG_DTSIZE_20 - (us_cnt + 1)] = (u_int8)*(uc_data_pos + us_cnt); - } - uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20); - - /* Reverse flag */ - p_snd_data = p_snd_data_base + LSDRV_REV; - p_snd_data->ul_did = VEHICLE_DID_REV; /* Data ID */ - p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_1; /* Size of the data */ - p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor Counter */ - p_snd_data->uc_data[0] = g_uc_vehicle_reverse; - /* Gyro Temperature */ - p_snd_data = p_snd_data_base + LSDRV_GYRO_TEMP; - p_snd_data->ul_did = VEHICLE_DID_GYRO_TEMP; /* Data ID */ - p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_2; /* Size of the data */ - p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ - p_snd_data->uc_data[1] = (u_int8)(*(uc_data_pos ) & (u_int8)(LSDRV_TEMP_MASK >> 8)); - p_snd_data->uc_data[0] = (u_int8)(*(uc_data_pos + 1) & (u_int8)(LSDRV_TEMP_MASK)); - uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_2); - - /* Vehicle speed pulse */ - p_snd_data = p_snd_data_base + LSDRV_SPEED_PULSE; - p_snd_data->ul_did = VEHICLE_DID_SPEED_PULSE; /* Data ID */ - p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */ - p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ - for (us_cnt = 0; us_cnt < LSDRV_SNDMSG_DTSIZE_20; us_cnt++) { - /* Since [0] is older and [9] is newer, the order of received data is switched. */ - /* Be in reverse order for endian conversion */ - p_snd_data->uc_data[LSDRV_SNDMSG_DTSIZE_20 - (us_cnt + 1)] = (u_int8)*(uc_data_pos + us_cnt); - } - uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20); - - /* Vehicle speed(km/h) */ - p_snd_data = p_snd_data_base + LSDRV_SPEED_PULSE; - us_sp_kmph = 0; - us_sp_pls1 = (u_int16)p_snd_data->uc_data[1]; - us_sp_pls1 = (u_int16)((us_sp_pls1 << 8) | p_snd_data->uc_data[0]); - us_sp_ret = LineSensDrvGetLastSpeedPulse(&us_sp_pls2, us_sp_pls1, uc_sens_cnt); - - LineSensDrvSetLastSpeedPulse(us_sp_pls1, uc_sens_cnt); - - p_snd_data = p_snd_data_base + LSDRV_SPEED_KMPH; - if (us_sp_ret != LSDRV_SPKMPH_INVALID) { - /* Vehicle speed pulse before 100 ms is valid */ - LineSensDrvSpeedPulseSave(us_sp_pls1, us_sp_pls2, uc_sens_cnt); - us_sp_kmph = LineSensDrvSpeedCalc(uc_sens_cnt); - /* The size can be set only when the vehicle speed [km/h] is calculated. "0" is notified to the vehicle sensor when the size cannot be set. */ - p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_2; /* Size of the data */ - } - p_snd_data->ul_did = VEHICLE_DID_SPEED_KMPH; /* Data ID */ - p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ - p_snd_data->uc_data[0] = (u_int8)(us_sp_kmph & 0x00FF); - p_snd_data->uc_data[1] = (u_int8)(us_sp_kmph >> 8); - - POSITIONING_LOG("[LOG, %d(cnt), %d(km/h), %d(pls1), %d(pls2)] \r\n", - uc_sens_cnt, - us_sp_kmph, - us_sp_pls1, - us_sp_pls2); - - /* G-Sensor X-axes */ - p_snd_data = p_snd_data_base + LSDRV_GSENSOR_X; - p_snd_data->ul_did = VEHICLE_DID_GSNS_X; /* Data ID */ - p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */ - p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ - us_cnt2 = p_snd_data->uc_size - 1; - for (us_cnt = 0; us_cnt < 10; us_cnt++) { - /* Since [0] is older and [9] is newer, the order of received data is switched. */ - p_snd_data->uc_data[us_cnt2 ] = (u_int8)*( uc_data_pos + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt) ); - p_snd_data->uc_data[us_cnt2 - 1] = (u_int8)*( uc_data_pos + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt) + 1); - us_cnt2 = us_cnt2 - 2; - } - - /* G-Sensor Y-axes */ - p_snd_data = p_snd_data_base + LSDRV_GSENSOR_Y; - p_snd_data->ul_did = VEHICLE_DID_GSNS_Y; /* Data ID */ - p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */ - p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ - us_cnt2 = p_snd_data->uc_size - 1; - for (us_cnt = 0; us_cnt < 10; us_cnt++) { - /* Since [0] is older and [9] is newer, the order of received data is switched. */ - p_snd_data->uc_data[us_cnt2] = (u_int8)*( (uc_data_pos + sizeof(u_int16)) \ - + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt)); - - p_snd_data->uc_data[us_cnt2-1] = (u_int8)*( (uc_data_pos + sizeof(u_int16)) \ - + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt) + 1); - us_cnt2 = us_cnt2 - 2; - } - uc_data_pos = ( uc_data_pos + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * 10) ); - - /* Inter-Pulse time */ - p_snd_data = p_snd_data_base + LSDRV_PULSE_TIME; - p_snd_data->ul_did = VEHICLE_DID_PULSE_TIME; /* Data ID */ - p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_132; /* Size of the data */ - p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ - - /* Clear the buffer for variable length */ - (void)memset(&p_snd_data->uc_data[0], 0x00, sizeof(p_snd_data->uc_data)); - - if (g_rcv_data_len == LSDRV_PLSTIME_LEN) { - /* Inter-Pulse time (number of items + time). The number of items is stored at the beginning. */ - uc_size = (u_int8)*uc_data_pos; - if (uc_size > VEHICLE_SNS_INFO_PULSE_NUM) { - uc_size = VEHICLE_SNS_INFO_PULSE_NUM; - } - p_snd_data->uc_data[0] = uc_size; - p_snd_data->uc_data[1] = 0x00; - p_snd_data->uc_data[2] = 0x00; - p_snd_data->uc_data[3] = 0x00; - uc_data_pos = ( uc_data_pos + sizeof(u_int8) ); - - /* Since [0] is old and [31] is new in the received time, the order is changed. */ - /* Be in reverse order for endian conversion */ - for (us_cnt = 0; us_cnt < (uc_size * sizeof(u_int32)); us_cnt++) { - p_snd_data->uc_data[(uc_size * sizeof(u_int32)) - (us_cnt + 1) + 4] = (u_int8)*(uc_data_pos + us_cnt); - } - } - - /* Messaging */ - DeliveryLineSensorDataPositioning(p_snd_buf, LSDRV_KINDS_MAX); - } - - return; -} - -/******************************************************************************* - * MODULE : LineSensDrv_SpeedCalc - * ABSTRACT : Vehicle speed calculation processing for sensor data - * FUNCTION : Calculate vehicle speed for sensor data - * ARGUMENT : uc_sens_cnt : Sensor counter - * NOTE : - * RETURN : Vehicle speed(0.01km/h) - ******************************************************************************/ -u_int16 LineSensDrvSpeedCalc(u_int8 uc_sens_cnt) { - u_int32 ul_sp_caluc = 0; /* Vehicle speed(2^-8km/h) */ - u_int16 us_speed = 0; /* Vehicle speed(km/h) */ - u_int8 uc_ptr = 0; /* Data storage pointer for sensor data */ /* #010 */ - u_int8 uc_ptr2 = 0; /* Data storage pointer for sensor data */ /* #010 */ - u_int32 ul_work = 0; /* Variables for unsigned 32-bit type calculations */ /* #010 */ - double d_work = 0.0; /* Variables for calculating double types */ /* #010 */ /* Ignore -> MISRA-C++:2008 Rule 3-9-2 */ - u_int16 us_sens_cnt_search = 0; /* Sensor counter to be searched */ /* #010 */ - u_int16 us_sens_cnt_ref = 0; /* Sensor counters to be compared */ /* #010 */ - int32 i = 0; /* Generic counters */ /* #010 */ - int32 j = 0; /* Generic counters */ /* #010 */ - u_int16 us_offset = 0; /* Offset value */ /* #010 */ - - /* #016 start */ - /* Is the number of data that can be calculated at the vehicle speed already received? */ - if (LSDRV_PLSDATA_NRCV == g_speed_kmph_data.uc_calc_start) { - /* Do not compute if there is not enough data. */ - } else { - if (0 == g_speed_kmph_data.uc_ptr) { - uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1; - uc_ptr2 = LSDRV_SPKMPH_TBL_NUM - 1; - } else { - uc_ptr = g_speed_kmph_data.uc_ptr - 1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ - uc_ptr2 = g_speed_kmph_data.uc_ptr - 1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ - } - - ul_work = 0; - if (LSDRV_SPKMPH_AVE_TIME > uc_sens_cnt) { - us_offset = LSDRV_SENSCNT_BRW_ADD; - } else { - us_offset = 0; - } - - us_sens_cnt_search = (u_int16)uc_sens_cnt + us_offset - LSDRV_SPKMPH_AVE_TIME; - - for (i = 0; i < LSDRV_SPKMPH_TBL_NUM; i++) { - /* Invalid data is detected, and the search is completed. */ - if (LSDRV_SPKMPH_DATA_DIS == g_speed_kmph_data.st_data[uc_ptr].uc_flag) { - break; - } - - /* When the sensor counter is 29 or less, the borrow is considered. */ - if (LSDRV_SPKMPH_AVE_TIME > g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt) { - us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt + us_offset; - } else { - us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt; - } - - /* Checking the sensor counter to finish search */ - if (us_sens_cnt_search >= us_sens_cnt_ref) { - break; - } - - /* Add to calculate average value */ - ul_work += (u_int32)g_speed_kmph_data.st_data[uc_ptr].us_speed_pulse; - - if (0 == uc_ptr) { - uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1; /* To the end of the data table */ - } else { - uc_ptr--; /* To the previous data */ - } - } - - /* Averaging computation */ - if (0 == i) { - d_work = 0; - } else { - if (ul_work == 1) { - for (j = 0; j < LSDRV_SPKMPH_TBL_NUM; j++) { - if ((g_speed_kmph_data.st_data[uc_ptr2].us_speed_pulse == 1) - && (g_speed_kmph_data.st_data[uc_ptr2].uc_noise_flag == 1)) { - ul_work = 0; - break; - } else if (g_speed_kmph_data.st_data[uc_ptr2].us_speed_pulse == 1 && - g_speed_kmph_data.st_data[uc_ptr2].uc_noise_flag == 0) { - ul_work = 1; - break; - } else { - /* nop */ - } - - /* Borrow actions for pointer values */ - if (0 == uc_ptr2) { - uc_ptr2 = LSDRV_SPKMPH_TBL_NUM - 1; /* To the end of the data table */ - } else { - uc_ptr2--; /* To the previous data */ - } - } - } - - d_work = static_cast<double>(ul_work); - d_work = d_work / static_cast<double>(i); - d_work = d_work * static_cast<double>(LSDRV_SENS_COEFFICIENT); - d_work = d_work * 100; /* [1km/h] -> [0.01km/h] */ - d_work = d_work + 0.5; /* Preparation for rounding */ - } - - ul_sp_caluc = static_cast<int32>(d_work); - - /* When the vehicle speed calculation result overflows, the upper limit value is used for clipping. */ - if (LSDRV_PLSMAX - 1 >= ul_sp_caluc) { - us_speed = (u_int16)ul_sp_caluc; - } else { - us_speed = (u_int16)(LSDRV_PLSMAX - 1); - } - } - - return us_speed; -} - -/******************************************************************************* - * MODULE : LineSensDrv_SpeedKmphDataInit - * ABSTRACT : Data table initialization process for vehicle speed calculation - * FUNCTION : Initialize the data table for calculating the vehicle speed - * ARGUMENT : None - * NOTE : - * RETURN : None - ******************************************************************************/ -void LineSensDrvSpeedKmphDataInit(void) { - int32 i = 0; - - memset(reinterpret_cast<void*>(&g_speed_kmph_data), static_cast<int32>(0), (size_t)sizeof(g_speed_kmph_data)); - - /* Disable all data storage flags */ - for (i = 0; i < LSDRV_SPKMPH_TBL_NUM; i++) { - g_speed_kmph_data.st_data[i].uc_flag = LSDRV_SPKMPH_DATA_DIS; - } - - return; -} - -/******************************************************************************* - * MODULE : LineSensDrv_SpeedPulseSave - * ABSTRACT : Sensor data vehicle speed pulse save processing - * FUNCTION : Saving the vehicle speed pulse of the sensor data to the data table for calculating the vehicle speed - * ARGUMENT : us_sp1 : Vehicle speed pulse of the latest information - * : us_sp2 : Vehicle speed pulse of the previous information - * : uc_sens_cnt : Sensor counter - * NOTE : - * RETURN : None - ******************************************************************************/ -void LineSensDrvSpeedPulseSave(u_int16 us_sp1, u_int16 us_sp2, u_int8 uc_sens_cnt) { - u_int16 us_sp_diff = 0; /* Vehicle speed pulse difference */ - u_int8 uc_ptr = 0; /* Data storage pointer for sensor data */ /* #010 */ - u_int8 us_last_ptr = 0; /* Last pointer */ /* #010 */ - int32 i = 0; /* Generic counters */ /* #010 */ - u_int8 uc_fstsns_cnt = 0; /* Initial sensor data sensor counter value */ /* #016 */ - int32 l_data_num = 0; /* Number of registered data */ /* #016 */ - u_int8 uc_noise_flag = 0; /* Noise flag */ /* #017 */ - - /* Calculate the vehicle speed pulse difference between the latest and last 100ms information */ - if (us_sp1 >= us_sp2) { - /* The cumulative pulse number of the latest information is larger. */ - us_sp_diff = us_sp1 - us_sp2; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ - } else { - /* The cumulative pulse number of the latest information is smaller (the accumulated pulse overflows) */ - us_sp_diff = (LSDRV_PLSMAX - us_sp2) + us_sp1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ - } - - /* Call noise check only if us_sp_diff is 1 */ - if (us_sp_diff == 1) { - uc_noise_flag = LineSensDrvCheckNoise(uc_sens_cnt); - } - - /* Saving sensor data vehicle speed pulse in data table for vehicle speed calculation */ - if (LSDRV_PLSDATA_NRCV == g_speed_kmph_data.uc_sns_rcv) { - if (LSDRV_PLSDATA_RCV == g_speed_kmph_data.uc_fstsns_rcv) { - /* If the sensor data has already been received for the first time, set the temporary sensor counter value when the sensor data has been saved for the first time. */ - uc_fstsns_cnt = uc_sens_cnt; - for (i = (LSDRV_SPKMPH_TBL_NUM - 1); i >= 0 ; i--) { - if (LSDRV_SPKMPH_DATA_EN == g_speed_kmph_data.st_data[i].uc_flag) { - /* Data storage flag is valid */ - if (0 != uc_fstsns_cnt) { - uc_fstsns_cnt--; - } else { - uc_fstsns_cnt = LSDRV_SENSCNT_MAX; - } - - g_speed_kmph_data.st_data[i].uc_sens_cnt = uc_fstsns_cnt; - } - } - } - - /* Sensor data reception status <- "Received" */ - g_speed_kmph_data.uc_sns_rcv = LSDRV_PLSDATA_RCV; - } - - uc_ptr = g_speed_kmph_data.uc_ptr; - - /* If the sensor counter is the same as the previous one, overwrite update */ - if (0 == uc_ptr) { - us_last_ptr = LSDRV_SPKMPH_TBL_NUM - 1; - } else { - us_last_ptr = uc_ptr - 1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ - } - - if (g_speed_kmph_data.st_data[us_last_ptr].uc_sens_cnt == uc_sens_cnt) { - /* Next update of the data storage location */ - if (0 == g_speed_kmph_data.uc_ptr) { - g_speed_kmph_data.uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1; - } else { - g_speed_kmph_data.uc_ptr--; - } - - uc_ptr = g_speed_kmph_data.uc_ptr; - } - - /* Input into data table for calculation of vehicle speed */ - g_speed_kmph_data.st_data[uc_ptr].uc_flag = LSDRV_SPKMPH_DATA_EN; /* Data validity */ - g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt = uc_sens_cnt; /* Sensor counter input */ - g_speed_kmph_data.st_data[uc_ptr].us_speed_pulse = us_sp_diff; /* Vehicle speed pulse difference */ - g_speed_kmph_data.st_data[uc_ptr].uc_noise_flag = uc_noise_flag; /* Noise flag */ /* #017 */ - - /* Next update of the data storage location */ - if ((LSDRV_SPKMPH_TBL_NUM - 1) <= g_speed_kmph_data.uc_ptr) { - g_speed_kmph_data.uc_ptr = 0; - } else { - g_speed_kmph_data.uc_ptr++; - } - - /* Determine whether the vehicle speed can be calculated. */ - if (g_speed_kmph_data.uc_calc_start == LSDRV_PLSDATA_NRCV) { - /* Data count detection */ - l_data_num = 0; - for (i = 0; i < LSDRV_SPKMPH_TBL_NUM; i++) { - if (LSDRV_SPKMPH_DATA_EN == g_speed_kmph_data.st_data[i].uc_flag) { - l_data_num++; - } - } - - if (LSDRV_SPKMPH_MIN_DATA_N <= l_data_num) { - /* Vehicle Speed Calculation Required Data Number Received */ - g_speed_kmph_data.uc_calc_start = LSDRV_PLSDATA_RCV; - } - } - - return; -} - -/******************************************************************************* - * MODULE : LineSensDrv_CheckNoise - * ABSTRACT : Sensor data noise check processing - * FUNCTION : The vehicle speed pulse is saved in the data table for the vehicle speed calculation. - * ARGUMENT : uc_sens_cnt : Sensor counter of the latest data - * NOTE : - * RETURN : Noise flag - ******************************************************************************/ -u_int8 LineSensDrvCheckNoise(u_int8 uc_sens_cnt) { - int32 i = 0; /* Generic counters */ - u_int16 us_sens_cnt_search = 0; /* Sensor counter to be searched*/ - u_int8 uc_ptr = 0; /* Data storage pointer */ - u_int16 us_offset = 0; /* Offset value */ - u_int8 noise_flag = 0; /* Noise flag */ - u_int16 us_sens_cnt_ref = 0; /* Sensor counters to be compared */ - - /* If there is no point where the difference in vehicle speed pulse is 1 or more - between -1 and -14 of sensor counter of the latest data, - set the noise flag of the latest data to 1. */ - /* Set the noise flag to 1 */ - noise_flag = 1; - - /* The Target is the one before the storage location of the latest data. */ - if (0 == g_speed_kmph_data.uc_ptr) { - uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1; - } else { - uc_ptr = g_speed_kmph_data.uc_ptr - 1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ - } - - if (LSDRV_SPKMPH_NOISE_TIME > uc_sens_cnt) { - us_offset = LSDRV_SENSCNT_BRW_ADD; - } else { - us_offset = 0; - } - - us_sens_cnt_search = (u_int16)uc_sens_cnt + us_offset - LSDRV_SPKMPH_NOISE_TIME; - - for (i = 0; i < LSDRV_SPKMPH_NOISE_TIME; i++) { - /* When the sensor counter is 15 or less, the borrow is considered. */ - if (LSDRV_SPKMPH_NOISE_TIME > g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt) { - us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt + us_offset; - } else { - us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ - } - - /* Checking the sensor Counter to Finish Search */ - if (us_sens_cnt_ref <= us_sens_cnt_search) { - noise_flag = 1; - break; - } else { - if (g_speed_kmph_data.st_data[uc_ptr].us_speed_pulse >= 1) { - noise_flag = 0; - break; - } - } - - /* Borrow actions for pointer values */ - if (0 == uc_ptr) { - uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1; /* To the end of the data table */ - } else { - uc_ptr--; /* To the previous data */ - } - } - - return noise_flag; -} - -/** - * @brief - * Return the last (100ms ago) vehicle speed pulse - * - * @param[out] us_sp2 Last vehicle speed pulse - * @param[in] us_sp1 The latest vehicle speed pulse - * @param[in] uc_sens_cnt Latest sensor counter - * - * @return LSDRV_SPKMPH_INVALID Vehicle speed pulse information valid<br> - * LSDRV_SPKMPH_VALID Vehicle speed pulse information invalid - */ -u_int8 LineSensDrvGetLastSpeedPulse(u_int16* us_sp2, u_int16 us_sp1, u_int8 uc_sens_cnt) { - u_int8 ret = LSDRV_SPKMPH_INVALID; /* Return value */ - u_int16 sp_pls_diff = 0; /* Vehicle speed pulse difference */ - u_int16 sp_pls = 0; /* Vehicle speed pulse every 100 ms */ - u_int8 cnt_diff = 0; /* Sensor counter difference */ - - /* Check if the last vehicle speed pulse has been set */ - if (g_speed_kmph_data.st_last_data.uc_flag == LSDRV_SPKMPH_DATA_EN) { - /* Differential calculation of sensor counter */ - if (uc_sens_cnt >= g_speed_kmph_data.st_last_data.uc_sens_cnt) { - /* Latest sensor counter is larger */ - cnt_diff = uc_sens_cnt - g_speed_kmph_data.st_last_data.uc_sens_cnt; - } else { - /* Last sensor counter is larger(sensor counter overflows) */ - cnt_diff = (LSDRV_SENSCNT_MAX - g_speed_kmph_data.st_last_data.uc_sens_cnt) + uc_sens_cnt + 1; - } - - /* Check if sensor counter is continuous */ - if (cnt_diff <= 1) { - /* Continuous or same as the previous one, so the previous (100ms previous) vehicle speed pulse is set as it is */ - *us_sp2 = g_speed_kmph_data.st_last_data.us_speed_pulse; - } else { - /* Determine the vehicle speed pulse 100ms ago from the average considering the skipped portion because it is not consecutive. */ - if (us_sp1 >= g_speed_kmph_data.st_last_data.us_speed_pulse) { - /* Larger latest cumulative vehicle speed pulse */ - sp_pls_diff = us_sp1 - g_speed_kmph_data.st_last_data.us_speed_pulse; - } else { - /* Last cumulative vehicle speed pulse is larger(Cumulative vehicle speed pulse overflows) */ - sp_pls_diff = (LSDRV_PLSMAX - g_speed_kmph_data.st_last_data.us_speed_pulse) + us_sp1; - } - - /* Calculate average vehicle speed pulse including skip period */ - sp_pls = (u_int16)(sp_pls_diff / cnt_diff); - - /* Calculate the vehicle speed pulse 100ms ahead from the vehicle speed average */ - if (us_sp1 >= sp_pls) { - /* Does not overflow even if the 100ms vehicle speed pulse is pulled from the latest one. */ - *us_sp2 = us_sp1 - sp_pls; - } else { - /* Subtracting a 100ms vehicle speed pulse from the latest one overflows */ - *us_sp2 = LSDRV_PLSMAX - (sp_pls - us_sp1); - } - } - - ret = LSDRV_SPKMPH_VALID; - } - - return ret; -} - -/** - * @brief - * Return the last (100ms ago) vehicle speed pulse - * - * @param[in] us_sp Vehicle speed pulse - * @param[in] uc_sens_cnt Sensor counter - */ -void LineSensDrvSetLastSpeedPulse(u_int16 us_sp, u_int8 uc_sens_cnt) { - /* Vehicle speed pulse information valid setting */ - g_speed_kmph_data.st_last_data.uc_flag = LSDRV_SPKMPH_DATA_EN; - /* Sensor counter setting */ - g_speed_kmph_data.st_last_data.uc_sens_cnt = uc_sens_cnt; - /* Vehicle speed pulse setting */ - g_speed_kmph_data.st_last_data.us_speed_pulse = us_sp; -} - -/*---------------------------------------------------------------------------*/ -/*EOF*/ diff --git a/video_in_hal/positioning_hal/src/LineSensDrv/LineSensDrv_Thread.cpp b/video_in_hal/positioning_hal/src/LineSensDrv/LineSensDrv_Thread.cpp deleted file mode 100755 index 9fb1c29..0000000 --- a/video_in_hal/positioning_hal/src/LineSensDrv/LineSensDrv_Thread.cpp +++ /dev/null @@ -1,125 +0,0 @@ -/* - * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -/** -* @file LineSensDrv_Thread.cpp -*/ - -/*---------------------------------------------------------------------------*/ -// Include files - -#include "LineSensDrv_Thread.h" -#include <native_service/frameworkunified_types.h> -#include <native_service/frameworkunified_framework_if.h> -#include "positioning_def.h" -#include "positioning_common.h" -/*---------------------------------------------------------------------------*/ -// Global variable - -static u_int8 g_sys_recv_flg = 0; // For debugging -char g_threadname[] = "POS_Sens"; -uint8_t g_uc_vehicle_reverse = 0; -uint8_t g_uc_reverse_state = 0; -static BOOL g_line_sens_thread_stop = FALSE; - -#define POS_REV_STATE_PROCESSING 0 -#define POS_REV_STATE_IDLE 1 - -/******************************************************************************* - * MODULE : LineSensDrv_Thread - * ABSTRACT : LineSensor driver thread main process - * FUNCTION : Main processing - * ARGUMENT : lpvPara : - * NOTE : - * RETURN : - ******************************************************************************/ -EFrameworkunifiedStatus LineSensDrvThread(HANDLE h_app) { - int32 l_ret = RET_LSDRV_SUCCESS; - EFrameworkunifiedStatus l_status = eFrameworkunifiedStatusOK; - - - (void)PosSetupThread(h_app, ETID_POS_SENS); - - /* Execute the initialization processes */ - l_ret = LineSensDrvMainThreadInit(h_app); - if (RET_LSDRV_SUCCESS != l_ret) { - l_status = eFrameworkunifiedStatusFail; - } - - return l_status; -} - -/******************************************************************************* - * MODULE : LineSensDrv_MainThread_Init - * ABSTRACT : Thread initialization process - * FUNCTION : Initialize thread - * ARGUMENT : None - * NOTE : - * RETURN : RET_LSDRV_SUCCESS:Success in initialization - * RET_LSDRV_ERROR :Initialization failed - ******************************************************************************/ -int32 LineSensDrvMainThreadInit(HANDLE h_app) { - int32 l_ret = RET_LSDRV_SUCCESS; - BOOL b_ret = TRUE; - - /****** Global variable initialization **********/ - LineSensDrvParamInit(); - - /****** LineSensDrvApi initialization **********/ - b_ret = LineSensDrvApiInitialize(); - - if (TRUE != b_ret) { - l_ret = RET_LSDRV_ERROR; - } - - return l_ret; -} - -/******************************************************************************* - * MODULE : LineSensDrv_Param_Init - * ABSTRACT : Global variable initialization processing - * FUNCTION : Initialize global variables - * ARGUMENT : None - * NOTE : - * RETURN : None - ******************************************************************************/ -void LineSensDrvParamInit(void) { - LineSensDrvSpeedKmphDataInit(); // Data table initialization process for vehicle speed calculation -} - -/** - * @brief - * Pos_Sens thread stop processing - */ -void LineSensDrvThreadStopProcess(void) { - g_line_sens_thread_stop = TRUE; - - if (POS_REV_STATE_IDLE == g_uc_reverse_state) { - PosTeardownThread(ETID_POS_SENS); - } - - return; -} - -/** - * @brief - * Get initial sensor data reception flag - */ -u_int8 LineSensDrvGetSysRecvFlag(void) { - return g_sys_recv_flg; -} - -/*---------------------------------------------------------------------------*/ -/*EOF*/ |