diff options
| author |   takeshi_hoshina <takeshi_hoshina@mail.toyota.co.jp> | 2020-10-21 13:54:45 +0900 |
|---|---|---|
| committer | takeshi_hoshina <takeshi_hoshina@mail.toyota.co.jp> | 2020-10-21 13:54:45 +0900 |
| commit | 21238f116bfa1a7eda4524c4e7923d9df31d0cdc (patch) | |
| tree | ea555ce10cdef78fca66428511a3b13422cad7e8 | |
| parent | 706ad73eb02caf8532deaf5d38995bd258725cb8 (diff) | |
positioning_hal branchsandbox/ToshikazuOhiwa/positioning_hal
32 files changed, 11298 insertions, 0 deletions
diff --git a/LICENSE b/LICENSE new file mode 100644 index 00000000..f433b1a5 --- /dev/null +++ b/LICENSE @@ -0,0 +1,177 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS diff --git a/Makefile b/Makefile new file mode 100644 index 00000000..0b5a74da --- /dev/null +++ b/Makefile @@ -0,0 +1,91 @@ +# +# @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. +# + +######### installed program/file ################ +TARGET := libpositioning_hal.so + +HEADER := hal_api/gps_hal.h hal_api/positioning_hal.h + +######### include paths/files ################### +CXXFLAGS += -I. +CXXFLAGS += -I./inc/Common +CXXFLAGS += -I./inc/GpsCommon +CXXFLAGS += -I./inc/LineSensDrv +CXXFLAGS += -I./hal_api/ +CXXFLAGS += -I$(SDKTARGETSYSROOT)/usr/agl/include/ +CXXFLAGS += -include $(SDKTARGETSYSROOT)/usr/agl/include/agl_types_obsoluted.h + + +######### compile options ####################### +CXXFLAGS += -Wall -fPIC +CXXFLAGS += -DLINUX -D__CWORD71__VEHICLE_ -D__CWORD71_ + +######### link options ########################## +LDFLAGS += -shared +LDFLAGS += -Wl,--no-as-needed +LDFLAGS += -Wl,--no-undefined +LDFLAGS += -L$(SDKTARGETSYSROOT)/usr/agl/lib + +RPATH := /usr/lib + +######### linked library ######################## +LIBS += -Wl,-Bdynamic -lstdc++ +LIBS += -Wl,-Bdynamic -lNS_FrameworkUnified +#LIBS += -Wl,-Bdynamic -lgps_mng +LIBS += -Wl,-Bdynamic -lPOS_base_API +LIBS += -Wl,-Bdynamic -lns_backup + +######### source files ########################## +VPATH := src/Common +VPATH += src/GpsCommon +VPATH += src/LineSensDrv + +OBJS += positioning_common.o +OBJS += positioning_hal.o +OBJS += MDev_Gps_API.o +OBJS += MDev_Gps_Main.o +OBJS += MDev_GpsRecv.o +OBJS += MDev_Gps_Nmea.o +OBJS += MDev_Gps_Common.o +OBJS += MDev_Gps_Mtrx.o +OBJS += MDev_Gps_TimerCtrl.o +OBJS += LineSensDrv_Sensor.o +OBJS += LineSensDrv_Thread.o +OBJS += LineSensDrv_Api.o +OBJS += MDev_GpsRollOver.o + +######### make targets ########################## +all: $(TARGET) + @echo $@ done. + +$(TARGET): $(OBJS) + $(CC) $(LDFLAGS) -Wl,-rpath=:$(RPATH) $(OBJS) $(LIBS) -o $(TARGET) + @echo $@ done. + +######### make cleans ########################### +GCNO_FILES := $(filter %.gcno,$(OBJS:%.o=%.gcno)) + +clean: + rm -f $(OBJS) $(TARGET) $(GCNO_FILES) + @echo $@ done. + +######### make installs ######################### +install: + install -d -m 0755 $(DESTDIR)/usr/lib + install -m 0755 $(TARGET) $(DESTDIR)/usr/lib + @echo $@ done. + + diff --git a/README.md b/README.md new file mode 100644 index 00000000..2d46d425 --- /dev/null +++ b/README.md @@ -0,0 +1,3 @@ +positioning_hal library +================== +Positioning HAL implementation library for AGL Reference Board. diff --git a/hal_api/gps_hal.h b/hal_api/gps_hal.h new file mode 100644 index 00000000..3321275b --- /dev/null +++ b/hal_api/gps_hal.h @@ -0,0 +1,932 @@ +/* + * @copyright Copyright (c) 2018-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. + */ + +#ifndef HAL_API_GPS_HAL_H_ +#define HAL_API_GPS_HAL_H_ + +/** + * @file gps_hal.h + */ + +/** @addtogroup positioning + * @{ + */ +/** @addtogroup positioning_hal + * @ingroup positioning + * @{ + */ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include <native_service/frameworkunified_types.h> +#include <vehicle_service/pos_message_header.h> +#include <vehicle_service/std_types.h> + +/*---------------------------------------------------------------------------*/ +// Value define + +typedef int32 RET_API; +typedef uint32_t DID; + +/************************************************************************* + * RET_API define + *************************************************************************/ +/*--- for message ---*/ +#define POS_TIMESTS_NG 0x00 +//!< \~english time has not been adjusted after GPS receiver reset(time input or master reset or CSF start) +#define POS_TIMESTS_RTC 0x01 +//!< \~english time output from RTC Backup(have time adjustment) +#define POS_TIMESTS_OK 0x02 //!< \~english time adjustment completed + +/* command ID */ +#define CID_POSIF_REGISTER_LISTENER_GPS_TIME 0x0201 +//!< \~english GPS time delivery command ID + +#define POS_DID_GPS__CWORD82__SETINITIAL 0x80000033 +//!< \~english data ID of setting initial location and time to GPS + +#define POS_DID_GPS__CWORD82__SETRMODEEX 0x80000035 +//!< \~english data ID of setting GPS receiver mode(pull extension sentence) + +#define POS_DID_GPS__CWORD82__SELSENT 0x80000036 +//!< \~english data ID of setting command to GPS that output any sentence + +/* NAVIINFO_RET_API */ +#define NAVIINFO_RET_NORMAL (0) //!< \~english normal finish +#define NAVIINFO_RET_ERROR_PARAM (-1) //!< \~english parameter error +#define NAVIINFO_RET_ERROR_INNER (-2) //!< \~english internal error +#define NAVIINFO_RET_ERROR_BUFFULL (-3) //!< \~english buffer full +#define NAVIINFO_RET_ERROR_NOSUPPORT (-4) //!< \~english no support +#define NAVIINFO_RET_ERROR_RESOURCE (-5) //!< \~english lack of resource + +/* Positioning Level/GPS Information Positioning Status Information Definition */ +#define NAVIINFO_DIAG_GPS_FIX_STS_NON (0) //!< \~english position not fix +#define NAVIINFO_DIAG_GPS_FIX_STS_2D (1) //!< \~english 2D position fix +#define NAVIINFO_DIAG_GPS_FIX_STS_3D (2) //!< \~english 3D position fix + +/* Reception status definition */ +#define NAVIINFO_DIAG_GPS_RCV_STS_NOTUSE (0) //!< \~english not used +#define NAVIINFO_DIAG_GPS_RCV_STS_SEARCHING (1) //!< \~english searching +#define NAVIINFO_DIAG_GPS_RCV_STS_TRACHING (2) //!< \~english tracking +#define NAVIINFO_DIAG_GPS_RCV_STS_NOTUSEFIX (3) //!< \~english not used for position fix +#define NAVIINFO_DIAG_GPS_RCV_STS_USEFIX (4) //!< \~english used for position fix + +/* GPS Response Status Definition */ +#define GPS_SENDOK 0 //!< \~english send result OK +#define GPS_SENDNG 1 //!< \~english send result NG + +/* GPS reset request mode definition */ +#define GPS_RST_COLDSTART 0xff +//!< \~english GPS reset request(cold start) + +#define RET_NORMAL 0 //!< \~english normally end + +#define RET_RCVMSG 1 //!< \~english receive message only +#define RET_RCVSIG 2 //!< \~english receive signal only +#define RET_RCVBOTH 3 //!< \~english receive message and signal +#define RET_NOMSG 4 //!< \~english receive no message + +#define RET_QUE 5 //!< \~english insert signal +#define RET_NOTQUE 6 //!< \~english do not insert signal +#define RET_PROCDOWN 7 //!< \~english sub process down found +#define RET_SLEEP 100 //!< \~english sleep time + + +#define RET_OSERROR (-127) //!< \~english OS system call error + +#define RET_ERROR (-1) +//!< \~english abnormally end(do not know reason) +#define RET_ERRPARAM (-2) //!< \~english parameter error +#define RET_ERRNOTRDY (-3) +//!< \~english port or semphore ID is not created +#define RET_ERRPROC (-4) //!< \~english error occurs in API process +#define RET_ERRTIMEOUT (-5) +//!< \~english timeout occurs but process unfinished +#define RET_ERRMSGFULL (-6) //!< \~english message table full + + +#define RET_ERROVERFLW (-7) +//!< \~english message size exceeds receive buffer size + +#define RET_ERRINIT (-8) //!< \~english abnormally initialization + +#define RET_EV_NONE (-20) //!< \~english event does not exist +#define RET_EV_MAX (-21) //!< \~english event value exceeds max value +#define RET_EV_MIN (-22) +//!< \~english event value is lower than min value + +/* POS_RET_API */ +#define POS_RET_NORMAL 0 //!< \~english normal finish +#define POS_RET_ERROR (-1) //!< \~english error occured +#define POS_RET_ERROR_DID (-2) //!< \~english data ID error +#define POS_RET_ERROR_INNER (-3) //!< \~english internal error +#define POS_RET_ERROR_PARAM (-4) //!< \~english parameter error +#define POS_RET_ERROR_BUFFULL (-5) //!< \~english buffer full error +#define POS_RET_ERROR_CREATE_EVENT (-6) //!< \~english create event error +#define POS_RET_ERROR_OUTOF_MEMORY (-8) +//!< \~english share memory allocation size error + +#define POS_RET_ERROR_SIZE (-9) //!< \~english memory size error +#define POS_RET_ERROR_TIMEOUT (-10) //!< \~english timeout error +#define POS_RET_ERROR_NOSUPPORT (-11) //!< \~english no support +#define POS_RET_ERROR_BUSY (-12) //!< \~english busy +#define POS_RET_ERROR_RESOURCE (-13) //!< \~english lack of resources +#define POS_RET_ERROR_MIN (-13) //!< \~english min value of error + + +#define NUM_OF_100msData (10) +//!< \~english number of gyro data every 100ms + + +#define CID_GPS_SERIAL1 (CID)0x0101 +//!< \~english Defination of notification about Gps Data has been receiced + +#define CID_GPS_SERIAL2 (CID)0x0102 //!< \~english notification about 1PPS interupt + + +#define CID_GPS_SERIAL3 (CID)0x0103 +//!< \~english Defination of SIF2 Data + +#define CID_GPS_SERIAL4 (CID)0x0104 +//!< \~english Defination of _CWORD82_ command send + +#define CID_GPS_SERIAL5 (CID)0x0105 +//!< \~english Defination of _CWORD82_ command send + +#define CID_GPS_SERIAL6 (CID)0x0106 +//!< \~english Defination of u-blox request data + +#define CID_GPS_SERIAL7 (CID)0x0107 +//!< \~english Defination of u-blox ack data + +#define CID_GPS_RECVDATA (CID)(CID_GPS_BASE | CID_GPS_SERIAL1) +//!< \~english gps data receiced CID + +#define CID_GPS_1PPSINT (CID)(CID_GPS_BASE | CID_GPS_SERIAL2) +//!< \~english 1PPS interupt CID + +#define CID_GPS_SIF2DATA (CID)(CID_GPS_BASE | CID_GPS_SERIAL3) +//!< \~english SIF2 Data CID + +/** + * \~english @brief _CWORD82_ command send CID + * \~english @brief Message structure\n + * \~english @brief Message buffer header structure @ref T_APIMSG_MSGBUF_HEADER\n + * \~english @brief Message header structure @ref T_APIMSG_HEADER\n + * \~english @brief Message main structure @ref TgGpsSndData\n + * \~english @brief Message data(Vehicle sensor information setting message) structure @ref VEHICLE_MSG_SEND_DAT\n + */ +#define CID_GPS__CWORD82__REQUEST (CID)(CID_GPS_BASE | CID_GPS_SERIAL4) + +#define CID_GPS_CYCLEDATA_PROV (CID)(CID_GPS_BASE | CID_GPS_SERIAL5) +//!< \~english Gps cycle data CID + +#define CID_GPS_RCV_REQUESTDATA (CID)(CID_GPS_BASE | CID_GPS_SERIAL6) +//!< \~english u-blox request data CID + +#define CID_GPS_RCV_ACKDATA (CID)(CID_GPS_BASE | CID_GPS_SERIAL7) +//!< \~english u-blox ack data CID + +#define CID_GPS_SERIAL8 (CID)0x0108 +//!< \~english Defination of Time Setting request + +#define CID_GPS_SERIAL9 (CID)0x0109 +//!< \~english Defination of Time Setting ack + +#define CID_GPS_SERIAL10 (CID)0x010A +//!< \~english Defination of back up data read request + +#define CID_GPS_SERIAL11 (CID)0x010B +//!< \~english Defination gps of week count request + +/** + * \~english @brief Defination of Time Setting request + * \~english @brief Message structure\n + * \~english @brief Message header structure @ref T_APIMSG_MSGBUF_HEADER\n + * \~english @brief Message body structure @ref TgGpsSndData\n + * \~english @brief Positioning information structure @ref POS_MSGINFO\n + * */ +#define CID_GPS_TIMESETTING (CID)(CID_GPS_BASE | CID_GPS_SERIAL8) + +/** + * \~english @brief Defination of Time Setting ack + * \~english @brief Message structure\n + * \~english @brief Message body structure @ref TG_GPS_RET_TIMESET_MSG\n + */ +#define CID_GPS_RETTIMESETTING (CID)(CID_GPS_BASE | CID_GPS_SERIAL9) + +/** + * \~english @brief Read backup data request CID + * \~english @brief When this command is received, the backup data is read.\n + * \~english @brief Message-Related Structures\n + * \~english @brief Message buffer header structure @ref T_APIMSG_MSGBUF_HEADER + */ +#define CID_GPS_BACKUPDATA_LOAD (CID)(CID_GPS_BASE | CID_GPS_SERIAL10) + +/** + * \~english @brief Gps Week count request CID + * \~english @brief Message structure\n + * \~english @brief Message buffer header structure @ref T_APIMSG_MSGBUF_HEADER\n + * \~english @brief Message header structure @ref T_APIMSG_HEADER\n + * \~english @brief Message data(Gps Week correction count notify message) structure @ref GpsWeekCorCntMsg\n + */ +#define CID_GPSWEEKCOR_CNT_NOTIFICATION (CID)(CID_GPS_BASE | CID_GPS_SERIAL11) + + + +/** + * \~english @brief GPS reset result delivery command ID + * \~english @brief If you want to catch above envents, use NSFW like below. + * \~english @code + * l_eStatus = FrameworkunifiedAttachCallbackToDispatcher(hThread, TN_POSITIONING_GPS, CID_POSIF_REQ_GPS_RESET, OnRecivePosResetMsg); + * @endcode + */ +#define CID_POSIF_REQ_GPS_RESET 0x0784 + +#define POSHAL_DID_GPS_CONNECT_ERROR 0x800000B5U +//!< \~english GPS receive error DID + +#define GPS_READ_LEN 316 +//!< \~english Gps NMEA FULLBINARY max length + +#define GPS_CMD_NMEA_GGA_SZ (71U) //!< \~english GGA +#define GPS_CMD_NMEA_DGGA_SZ (75U) //!< \~english double precison GGA +#define GPS_CMD_NMEA_VTG_SZ (37U) //!< \~english VTG +#define GPS_CMD_NMEA_RMC_SZ (61U) //!< \~english RMC +#define GPS_CMD_NMEA_DRMC_SZ (67U) //!< \~english double precison RMC +#define GPS_CMD_NMEA_GLL_SZ (44U) //!< \~english GLL +#define GPS_CMD_NMEA_DGLL_SZ (50U) //!< \~english double precison GLL +#define GPS_CMD_NMEA_GSA_SZ (66U) //!< \~english GSA +#define GPS_CMD_NMEA_GSV_SZ (70U) //!< \~english GSV +#define GPS_CMD_NMEA__CWORD44__GP_3_SZ (78U) //!< \~english _CWORD44_,GP,3 +#define GPS_CMD_NMEA__CWORD44__GP_4_SZ (25U) //!< \~english _CWORD44_,GP,4 +#define GPS_CMD_NMEA_P_CWORD82_F_GP_0_SZ (50U) //!< \~english _CWORD44_,GP,0 +#define GPS_CMD_NMEA_P_CWORD82_J_GP_1_SZ (31U) //!< \~english _CWORD44_,GP,1 +#define GPS_CMD_NMEA_P_CWORD82_I_GP_SZ (71U) //!< \~english P_CWORD82_I,GP +#define GPS_CMD_NMEA_P_CWORD82_E_GP_0_SZ (50U) //!< \~english P_CWORD82_E,GP,0 +#define GPS_CMD_NMEA_P_CWORD82_J_GP_0_SZ (63U) //!< \~english P_CWORD82_J,GP,0 +#define GPS_CMD_NMEA_P_CWORD82_E_GP_2_SZ (21U) //!< \~english P_CWORD82_E,GP,2 +#define GPS_CMD_NMEA_P_CWORD82_G_GP_0_SZ (34U) //!< \~english P_CWORD82_G,GP,0 +#define GPS_CMD_NMEA_P_CWORD82_J_GP_7_SZ (37U) //!< \~english P_CWORD82_J,GP,7 +#define GPS_CMD_NMEA_P_CWORD82_J_GP_8_SZ (45U) //!< \~english P_CWORD82_J,GP,8 +#define GPS_CMD_BINARY_SZ (81U) //!< \~english standard binary + +#define GPS_CMD_FULLBIN_SZ (316U) +//!< \~english FULL binary(GPS10 format) + +#define GPS_CMD_FULLBIN_VINFO (30U + 160U + 25U + 8U) +//!< \~english FULL bianry version + +#define GPS_NMEA_MAX_SZ (255U) //!< \~english NMEA max size +#define GPS_DATASIZE_RTC (13U) //!< \~english RTC Data size +#define GPS_NMEARCVSTS_SZ (1U) //!< \~english received NMEA size + +#define GPS_GPSANT_SZ (1U) //!< \~english gps antenna status size +#define GPS_SNSCNT_SZ (1U) //!< \~english Sensor Count value size + + +#define GPS_CMD_NMEA_PROV_SZ (GPS_CMD_NMEA_DRMC_SZ + GPS_CMD_NMEA_GSA_SZ + \ + (GPS_CMD_NMEA_GSV_SZ*3) + GPS_CMD_NMEA__CWORD44__GP_3_SZ) +//!< \~english NMEA size + +#define GPS_NMEA_SZ (GPS_NMEARCVSTS_SZ + GPS_GPSANT_SZ + GPS_SNSCNT_SZ + GPS_CMD_NMEA_PROV_SZ) +//!< \~english NEMA size + +#define GPS_FULLBIN_SZ (GPS_GPSANT_SZ + GPS_SNSCNT_SZ + GPS_CMD_FULLBIN_SZ) +//!< \~english FULLBIN size + +#define GPS__CWORD44_GP4_SZ (GPS_GPSANT_SZ + GPS_SNSCNT_SZ + GPS_CMD_NMEA__CWORD44__GP_4_SZ) +//!< \~english _CWORD44_GP4 size + +#define GPS_MSGDATA_SZ_MAX (512U) //!< \~english message maximum data size +#define GPS_TLGRM_LEN 253 //!< \~english message data length +#define GPS_MSG_VSINFO_DSIZE 1904 //!< \~english message body size + +#define POS_MSG_INFO_DSIZE 1028 //!< \~english message body maximum size + +/** + * @enum MdevGpsDataKind + * \~english enumeration of gps data kind + */ +typedef enum MdevGpsDataKind { + MDEV_GPS_DATA_RAWDATA_NMEA = 0, //!< \~english raw NMEA data of GPS + MDEV_GPS_DATA_RAWDATA_FULLBIN, //!< \~english raw binary data of GPS + MDEV_GPS_DATA_RAWDATA__CWORD44_GP4, //!< \~english raw _CWORD44_ data of GPS + MDEV_GPS_DATA_NAVISPEED, //!< \~english speed data + MDEV_GPS_DATA_CUSTOMDATA, //!< \~english position information + MDEV_GPS_DATA_GPSTIME, //!< \~english GPS time + MDEV_GPS_DATA_GPSTIME_RAW, //!< \~english raw time of GPS + MDEV_GPS_DATA_INITIAL //!< \~english initialize data +} MDEV_GPS_DATA_KIND; + +/** + * @struct NAVIINFO_UTCTIME + * \~english UTC time information structure + */ +typedef struct { + uint16_t year; //!< \~english year + uint8_t month; //!< \~english month + uint8_t date; //!< \~english day + uint8_t hour; //!< \~english hour + uint8_t minute; //!< \~english minute + uint8_t second; //!< \~english second + uint8_t reserved; //!< \~english reserve +} NAVIINFO_UTCTIME; + +/** + * @struct SENSORLOCATION_LONLATINFO_DAT + * \~english longitude and latitude information data + * - Get method(getMethod) + * - SENSOR_GET_METHOD_GPS - longitude and latitude from GPS + * - SENSOR_GET_METHOD_NAVI - longitude and latitude from Navigation + * - Synchrony count(SyncCnt) + * - Count for position data synchronous \n + * When delivery altitude and heading data, position data can be synchronized by SyncCnt.\n + * But the data of different method can not be synchronized by SyncCnt.\n + * Example 1: [LonLat from GPS] and [Heading from GPS] can be synchronized by SyncCnt.\n + * Example 2: [LonLat from GPS] and [LonLat from Navi] can not be synchronized by SyncCnt.\n + * Caution: The sensor count in sensor data delivery is another data. + * - Enable or not(isEnable) \n + * To describe this delivery message is whether can be used or not + * - 0 - not avaliable + * - not 0 - avaliable + * - when GPS data is specified,longitude and latitude is invalid at following condition:\n + * so [not avaliable] provieded + * - After system start, GPS unit has not received current location data and GPS unit \n + * status is not positioning fixed. + * - If it is not initialized status, certainly provide [avaliable] when Navigation data specified + * - If the status is [not avaliable], data following can not be guaranteed. + * - Position status(posSts) + * - It is valid only when "Get method is Navigation" and "evironment is _CWORD80_".(otherwise it will be set as 0) + * - Bit0 : GPS data used result(1:used, 0:not used, definition of maskbit:POS_LOC_INFO_USE_GSP) + * - Bit1 : DGPS data used result(1:used, 0:not used, definition of maskbit:POS_LOC_INFO_USE_DGPS) + * - Bit3 : MapMatching data used result(1:used, 0:not used, definition of \n + * maskbit:POS_LOC_INFO_USE_MAPMATCHING) + * - Position accuracy(posAcc) + * - Detected accruray of current position:1LSB=1m \n + * - It is valid only when "Get method is Navigation" and "evironment is _CWORD80_".(otherwise it will be set as 0)\n + * 0000H:0m \n + * 0001H:1m \n + * :\n + * FFFDH:65533m \n + * FFFEH:65534m and larger than 65534m \n + * FFFFH:no data + * - Longitude : (WGS-84)(10^ -7degree as 1) \n + * East longitude is positive value and west longitude is negative value. + * - Latitude : (WGS-84)(10^ -7degree as 1) \n + * North latitude positive value and south latitude is negative value. + */ +typedef struct { + uint8_t getMethod; //!< \~english get method + uint8_t SyncCnt; //!< \~english Synchrony count + uint8_t isEnable; //!< \~english enable or not + uint8_t posSts; //!< \~english position status + uint16_t posAcc; //!< \~english position accuracy + int32_t Longitude; //!< \~english longitude(10^-7degree) + //!< \~english max: +180.0000000degree[east longitude] + //!< \~english min: -170.9999999degree[west longitude] + int32_t Latitude; //!< \~english latitude(10^-7degree) + //!< \~english max: +90.0000000degree[north Latitude] + //!< \~english min: -90.0000000degree[south Latitude] +} SENSORLOCATION_LONLATINFO_DAT; + + +/** + * @struct SENSORLOCATION_ALTITUDEINFO_DAT + * \~english altitude information data + * - Get method(getMethod) + * - SENSOR_GET_METHOD_GPS - altitude from GPS + * - SENSOR_GET_METHOD_NAVI - altitude from Navigation + * - Synchrony count(SyncCnt) + * - Count for position data synchronous \n + * When delivery altitude and heading data, position data can be synchronized by SyncCnt. \n + * But the data of different method can not be synchronized by SyncCnt.\n + * example 1: [longitude and latitude from GPS] and [heading from GPS] can be synchronized by SyncCnt.\n + * example 2: [longitude and latitude from GPS] and [longitude and latitude from Navi] can not be + * synchronized by SyncCnt. \n + * Caution: The sensor count in sensor data delivery is another data. + * - Enable or not(isEnable) \n + * To describe this delivery message is whether can be used or not. + * - 0 - not avaliable + * - not 0 - avaliable + * - when GPS data specified, Altitude is invalid at following condition(so [not avaliable] provieded): + * - Immediately after system start, GPS unit has not received current location data and GPS unit status \n + * is not positioning fix + * - If it is not initialization status, certainly provide [avaliable] when Navi data specified + * - If the status is [not avaliable], data following can not be guaranteed. + * - Altitude + * - altitude data(unit 0.01m) + */ +typedef struct { + uint8_t getMethod; //!< \~english get method + uint8_t SyncCnt; //!< \~english Synchrony count + uint8_t isEnable; //!< \~english enable or not + uint8_t Reserved[3]; //!< \~english reserve + int32_t Altitude; //!< \~english altitude(0.01m) +} SENSORLOCATION_ALTITUDEINFO_DAT; + + +/** + * @struct SENSORMOTION_HEADINGINFO_DAT + * \~english heading information data + * - Get method(getMethod) + * - SENSOR_GET_METHOD_GPS - Heading from GPS + * - SENSOR_GET_METHOD_NAVI - Heading from Navi + * - Synchrony count(SyncCnt) + * - Count for position data synchronous \n + * When delivery altitude and heading data, position data can be synchronized by this count. \n + * But the data of different method can not be synchronized by this count.\n + * example 1: [longitude and latitude from GPS] and [heading from GPS] can be synchronized by the count. \n + * example 2: [longitude and latitude from GPS] and [longitude and latitude from Navi] can not be \n + * synchronized by the count. \n + * Caution: The sensor count in sensor data delivery is another data. + * - Enable or not(isEnable) \n + * To describe this data is whether enable or not + * - 0 - not avaliable + * - not 0 - avaliable + * - Heading is invalid at following condition when GPS data specified, so [not avaliable] provieded \n + * - Immediately after system start, GPS unit has not received current location data and GPS unit status \n + * is not positioning fix + * - If it is not initialization status, certainly provide [avaliable] when Navi data specified + * - If the status is [not avaliable], data following can not be guaranteed. + * - Position status(posSts) + * - It is valid only when "Get method is Navi" and "evironment is _CWORD80_".(otherwise it will be set as 0) + * - Bit0 : GPS data used result(1:used, 0:not used, definition of maskbit:POS_LOC_INFO_USE_GSP) + * - Bit1 : DGPS data used result(1:used, 0:not used, definition of maskbit:POS_LOC_INFO_USE_DGPS) + * - Bit3 : MapMatching data used result(1:used, 0:not used, definition of \n + * maskbit:POS_LOC_INFO_USE_MAPMATCHING) + * - Heading + * - Heading data(0.01degree as 1, based on north and clockwise count)\n + * But in environment _CWORD95_/_CWORD101___CWORD84_,the heading from Navi is a approximate that separate 360 degree \n + * into 16 part. \n + * example: 0, 2300, 4500, ..., 31500, 33800 + */ +typedef struct { + uint8_t getMethod; //!< \~english get method + uint8_t SyncCnt; //!< \~english Synchrony count + uint8_t isEnable; //!< \~english enable or not + uint8_t posSts; //!< \~english position status + uint8_t Reserved1[2]; //!< \~english reserve + uint16_t Heading; //!< \~english heading(0.01degree) + //!< \~english max: 359.99 degree + //!< \~english min: 0.00 degree + uint8_t Reserved2[2]; //!< \~english reserve +} SENSORMOTION_HEADINGINFO_DAT; + +/** + * @struct NAVIINFO_DIAG_GPS_FIX_CNT + * \~english position fix count data structure + */ +typedef struct { + uint32_t ulCnt3d; //!< \~english position fix count:3D + uint32_t ulCnt2d; //!< \~english position fix count:2D + uint32_t ulCntElse; //!< \~english position fix count:not fix +} NAVIINFO_DIAG_GPS_FIX_CNT; + +/** + * @struct NAVIINFO_DIAG_GPS_FIX_XYZ + * \~english longitude and latitude data structure + */ +typedef struct { + int32_t lLat; //!< \~english GPS latitude + int32_t lLon; //!< \~english GPS longitude +} NAVIINFO_DIAG_GPS_FIX_XYZ; + +/** + * @struct NAVIINFO_DIAG_GPS_FIX + * \~english position fix information structure + */ +typedef struct { + uint8_t ucFixSts; //!< \~english fix status + uint8_t ucReserve[3]; //!< \~english reserve + NAVIINFO_DIAG_GPS_FIX_CNT stCnt; //!< \~english fix count data + NAVIINFO_DIAG_GPS_FIX_XYZ stWgs84; //!< \~english longitude and latitude data +} NAVIINFO_DIAG_GPS_FIX; + +/** + * @struct NAVIINFO_DIAG_GPS_PRN + * \~english satellite information structure + * - reception status(ucRcvSts) + * - NAVIINFO_DIAG_GPS_RCV_STS_NOTUSE : not used + * - NAVIINFO_DIAG_GPS_RCV_STS_SEARCHING : searching + * - NAVIINFO_DIAG_GPS_RCV_STS_TRACHING : tracking + * - NAVIINFO_DIAG_GPS_RCV_STS_NOTUSEFIX : not used for position fix + * - NAVIINFO_DIAG_GPS_RCV_STS_USEFIX : used for position fix + */ +typedef struct { + uint8_t ucRcvSts; //!< \~english reception status + uint8_t ucPrn; //!< \~english satellite No. + uint8_t ucelv; //!< \~english satellite dramatic angle + uint8_t ucLv; //!< \~english satellite signal level + uint16_t usAzm; //!< \~english satellite azimuth + uint8_t ucReserve[2]; //!< \~english reserve +} NAVIINFO_DIAG_GPS_PRN; + +/** + * @struct NAVIINFO_DIAG_GPS_SAT + * \~english all satellite information structure + */ +typedef struct { + NAVIINFO_DIAG_GPS_PRN stPrn[12]; //!< \~english all satellite information +} NAVIINFO_DIAG_GPS_SAT; + +/** + * @struct NAVIINFO_DIAG_GPS + * \~english position fixed and satellite information structure + */ +typedef struct { + NAVIINFO_DIAG_GPS_FIX stFix; //!< \~english position fixed information + NAVIINFO_DIAG_GPS_SAT stSat; //!< \~english all satellite information +} NAVIINFO_DIAG_GPS; + +/** + * @struct NAVIINFO_NAVI_GPS + * \~english other GPS related information structure + * - altitude(altitude) \n + * As the altitude is used as unit [0.01m] in internal, \n + * the altitude data range is (-21,474,839~21,474,839). + * - UTC(utc) + * - The time set after rollover. + * - date and time status(tdsts) + * - 0= time has not been adjusted after GPS receiver reset(time input or master reset or CSF start) + * - 1= time output from RTC Backup(have time adjustment result) + * - 2= time adjustment completed + */ +typedef struct { + int32_t altitude; //!< \~english altitude + uint16_t speed; //!< \~english speed + uint16_t heading; //!< \~english heading + NAVIINFO_UTCTIME utc; //!< \~english UTC time + uint8_t tdsts; //!< \~english date and time status + uint8_t reserve[3]; //!< \~english reserve +} NAVIINFO_NAVI_GPS; + +/** + * @struct NAVIINFO_ALL + * \~english Navi data structure + */ +typedef struct { + uint8_t ucSensorCnt; //!< \~english sensor count + uint8_t reserve[3]; //!< \~english reserve + NAVIINFO_DIAG_GPS stDiagGps; //!< \~english position fix related information + NAVIINFO_NAVI_GPS stNaviGps; //!< \~english other GPS related information +} NAVIINFO_ALL; + +/** + * @struct POS_MSGINFO + * \~english message delivery positioning sensor information + */ +typedef struct { + DID did; //!< \~english data ID + PNO pno; //!< \~english delivery destination PNO + uint16_t size; //!< \~english data body size + uint8_t rcv_flag; //!< \~english received flag + uint8_t reserve; //!< \~english reserve + uint8_t data[POS_MSG_INFO_DSIZE]; //!< \~english data body +} POS_MSGINFO; + +/** + * @struct SENSOR_GPSTIME + * \~english GPS time information + * - date amd time status(tdsts) + * - 0= time has not been adjusted after GPS receiver reset(time input or master reset or CSF start) + * - 1= time output from RTC Backup(have time adjustment result) + * - 2= time adjustment completed + */ +typedef struct { + NAVIINFO_UTCTIME utc; //!< \~english UTC time + uint8_t tdsts; //!< \~english time status + uint8_t reserve[3]; //!< \~english reserve +} SENSOR_GPSTIME; + +#define SENSOR_MSG_GPSTIME SENSOR_GPSTIME //!< \~english GPS time information +#define SENSOR_GPSTIME_RAW SENSOR_GPSTIME //!< \~english GPS time information + +/** + * @struct MdevGpsCustomData + * \~english Struct of Gps custom data + */ +typedef struct MdevGpsCustomData { + MDEV_GPS_DATA_KIND e_kind; //!< \~english kind + SENSORLOCATION_LONLATINFO_DAT st_lonlat; //!< \~english longtitude,latitude + SENSORLOCATION_ALTITUDEINFO_DAT st_altitude; //!< \~english altitude + SENSORMOTION_HEADINGINFO_DAT st_heading; //!< \~english headings + SENSOR_MSG_GPSTIME st_gps_time; //!< \~english Gps Time + NAVIINFO_DIAG_GPS st_diag_gps; //!< \~english Gps data from diag +} MDEV_GPS_CUSTOMDATA; + +/** + * @struct MdevGpsCustomDataMgs + * \~english Struct of Gps custom data message + */ +typedef struct MdevGpsCustomDataMgs { + T_APIMSG_MSGBUF_HEADER h_dr; //!< \~english header + MDEV_GPS_CUSTOMDATA st_data; //!< \~english Gps Custom Data +} MDEV_GPS_CUSTOMDATA_MGS; + +/** + * @struct MdevGpsRtc + * \~english Struct of Gps RTC data + */ +typedef struct MdevGpsRtc { + u_int32 ul_retsts; //!< \~english return status + u_int8 uc_datalen; //!< \~english data length + u_int8 uc_ctrlcode; //!< \~english control code + u_int8 uc_bcd_year; //!< \~english year + u_int8 uc_bcd_month; //!< \~english month + u_int8 uc_bcd_day; //!< \~english day + u_int8 uc_bcd_hour; //!< \~english hour + u_int8 uc_bcd_min; //!< \~english minute + u_int8 uc_bcd_sec; //!< \~english second + u_int8 uc_status; //!< \~english status + u_int8 u_reserve[3]; //!< \~english reserve +} MDEV_GPS_RTC; + +/** + * @struct MdevGpsGpsTime + * \~english Struct of Gps Time + */ +typedef struct MdevGpsGpsTime { + MDEV_GPS_DATA_KIND e_kind; //!< \~english kind + MDEV_GPS_RTC st_rtc_data; //!< \~english Gps Time +} MDEV_GPS_GPSTIME; + +/** + * @struct MdevGpsGpsTimeMgs + * \~english Struct of Gps Time message + */ +typedef struct MdevGpsGpsTimeMgs { + T_APIMSG_MSGBUF_HEADER h_dr; //!< \~english header + MDEV_GPS_GPSTIME st_data; //!< \~english Gps Time Data +} MDEV_GPS_GPSTIME_MGS; + +/** + * @struct DevGpsUtcTime + * \~english Struct of Gps UTC time + */ +typedef struct DevGpsUtcTime { + u_int16 us_year; //!< \~english Year + u_int8 uc_month; //!< \~english month + u_int8 uc_date; //!< \~english day + u_int8 uc_hour; //!< \~english hour + u_int8 uc_minute; //!< \~english minute + u_int8 uc_second; //!< \~english second + u_int8 u_reserved; //!< \~english reserve +} DEV_GPS_UTCTIME; + +/** + * @enum MdevGpsFixSts + * \~english enumeration of gps fix status + */ +typedef enum MdevGpsFixSts { + MDEV_GPS_NOTFIX = 0, //!< \~english not fix + MDEV_GPS_FIX //!< \~english fix +} MDEV_GPS_FIX_STS; + +/** + * @struct MdevGpsNmea + * \~english Struct of gps NMEA Data + */ +typedef struct MdevGpsNmea { + u_int8 uc_nmea_data[GPS_MSG_VSINFO_DSIZE]; //!< \~english NMEA Data +} MDEV_GPS_NMEA; + +/** + * @struct MdevGpsFullbin + * \~english Struct of Gps full bin data + */ +typedef struct MdevGpsFullbin { + u_int8 uc_fullbin_data[GPS_FULLBIN_SZ]; //!< \~english gps full bin data + u_int8 u_reserve[3]; //!< \~english reserve +} MDEV_GPS_FULLBIN; + +/** + * @struct MdevGps_CWORD44_gp4 + * \~english Struct of Gps _CWORD44_GP4 data + */ +typedef struct MdevGps_CWORD44_gp4 { + u_int8 uc__CWORD44_gp4_data[GPS__CWORD44_GP4_SZ]; //!< \~english gps _CWORD44_GP4 data + u_int8 u_reserve; //!< \~english reserve +} MDEV_GPS__CWORD44_GP4; + +/** + * @struct MdevGpsCycledata + * \~english Struct of Gps cycle data + */ +typedef struct MdevGpsCycledata { + MDEV_GPS_DATA_KIND e_kind; //!< \~english kind + u_int8 uc_data[GPS_MSGDATA_SZ_MAX - sizeof(MDEV_GPS_DATA_KIND)]; //!< \~english data +} MDEV_GPS_CYCLEDATA; + +/** + * @struct MdevGpsCycledataMsg + * \~english Struct of Gps cycle data message + */ +typedef struct MdevGpsCycledataMsg { + T_APIMSG_MSGBUF_HEADER h_dr; //!< \~english header + MDEV_GPS_CYCLEDATA st_data; //!< \~english data +} MDEV_GPS_CYCLEDATA_MSG; + +/** + * @struct MdevGpsRawdataNmea + * \~english Struct of Gps NMEA Raw Data + */ +typedef struct MdevGpsRawdataNmea { + MDEV_GPS_DATA_KIND e_kind; //!< \~english kind + MDEV_GPS_NMEA st_nmea_data; //!< \~english NMEA Data +} MDEV_GPS_RAWDATA_NMEA; + +/** + * @struct MdevGpsRawdataNmeaMsg + * \~english Struct of Gps NMEA Raw Data message + */ +typedef struct MdevGpsRawdataNmeaMsg { + T_APIMSG_MSGBUF_HEADER h_dr; //!< \~english header(see vs-positioning-base-library) + MDEV_GPS_RAWDATA_NMEA st_data; //!< \~english data +} MDEV_GPS_RAWDATA_NMEA_MSG; + +/** + * @struct MdevGpsRawdataFullbin + * \~english Struct of Gps Full Bin Raw Data + */ +typedef struct MdevGpsRawdataFullbin { + MDEV_GPS_DATA_KIND e_kind; //!< \~english kind + MDEV_GPS_FULLBIN st_fullbin_data; //!< \~english Full Binary Data +} MDEV_GPS_RAWDATA_FULLBIN; + +/** + * @struct MdevGpsRawdataFullbinMsg + * \~english Struct of Gps Full Bin Raw Data message + */ +typedef struct MdevGpsRawdataFullbinMsg { + T_APIMSG_MSGBUF_HEADER h_dr; //!< \~english header + MDEV_GPS_RAWDATA_FULLBIN st_data; //!< \~english Gps Full Bin Data +} MDEV_GPS_RAWDATA_FULLBIN_MSG; + +/** + * @struct MdevGpsRawdata_CWORD44_gp4 + * \~english Struct of Gps _CWORD44_GP4 Raw Data + */ +typedef struct MdevGpsRawdata_CWORD44_gp4 { + MDEV_GPS_DATA_KIND e_kind; //!< \~english kind + MDEV_GPS__CWORD44_GP4 st__CWORD44_gp4; //!< \~english Gps _CWORD44_GP4 Data +} MDEV_GPS_RAWDATA__CWORD44_GP4; + +/** + * @struct MdevGpsRawdata_CWORD44_gp4Msg + * \~english Struct of Gps _CWORD44_GP4 Raw Data message + */ +typedef struct MdevGpsRawdata_CWORD44_gp4Msg { + T_APIMSG_MSGBUF_HEADER h_dr; //!< \~english header + MDEV_GPS_RAWDATA__CWORD44_GP4 st_data; //!< \~english Gps _CWORD44_GP4 Raw Data +} MDEV_GPS_RAWDATA__CWORD44_GP4_MSG; + +/** + * @struct SENSORMOTION_SPEEDINFO_DAT + * \~english speed information data + * - Get method(getMethod) + * - SENSOR_GET_METHOD_POS - The speed calculated in positioning based on speed pulse will be deliveried. + * - SENSOR_GET_METHOD_NAVI - speed from Navi + * - Synchrony count(SyncCnt) + * - 0 (not change). + * - Enable or not(isEnable) \n + * To describe this data is whether enable or not + * - 0 - not avaliable + * - not 0 - avaliable + * - Speed is invalid at following condition when speed pulse specified, so [not avaliable] provieded + * - Immediately after system start, the sensor data have not been received from SYS micon + * - If it is not initialization status, certainly provide [avaliable] when Navi data specified + * - If the status is [not avaliable], data following can not be guaranteed. + * - Speed + * - speed data(unit 0.01m/sec) + */ +typedef struct { + uint8_t getMethod; //!< \~english get method + uint8_t SyncCnt; //!< \~english Synchrony count + uint8_t isEnable; //!< \~english enable or not + uint8_t Reserved1[3]; //!< \~english reserve + uint16_t Speed; //!< \~english speed(0.01m/sec) + uint8_t Reserved2[2]; //!< \~english reserve +} SENSORMOTION_SPEEDINFO_DAT; + +/** + * @struct MdevGpsNavispeed + * \~english Struct of Gps speed data + */ +typedef struct MdevGpsNavispeed { + MDEV_GPS_DATA_KIND e_kind; //!< \~english kind + u_int16 us_speed_kmph; //!< \~english Speed(km/h) + u_int8 u_reserve[2]; //!< \~english reserve + SENSORMOTION_SPEEDINFO_DAT st_speed; //!< \~english speed data +} MDEV_GPS_NAVISPEED; + +/** + * @struct MdevGpsNavispeedMsg + * \~english Struct of Gps speed data message + */ +typedef struct MdevGpsNavispeedMsg { + T_APIMSG_MSGBUF_HEADER h_dr; //!< \~english header + MDEV_GPS_NAVISPEED st_data; //!< \~english Gps Speed Data +} MDEV_GPS_NAVISPEED_MSG; + +/** + * @struct TgGpsSndData + * \~english Struct of _CWORD82_ Gps send data + */ +typedef struct TgGpsSndData { + uint8_t reserve[4]; //!< \~english reserve + u_int16 us_size; //!< \~english send data length + u_int8 ub_data[502]; //!< \~english send data +} TG_GPS_SND_DATA; + +/** + * @struct TgGpsRetPram + * \~english Gps Response data + */ +typedef struct TgGpsRetPram { + unsigned int ret_status; //!< \~english response flag + unsigned char data_len; //!< \~english data size + unsigned char ctrl_code; //!< \~english control code + unsigned char gps_data[GPS_TLGRM_LEN]; //!< \~english data contents +} TG_GPS_RET_PRAM; + +/** + * @struct SensorWknRollOverHal + * \~english Struct of GPS week number + */ +typedef struct SensorWknRollOverHal { + uint16_t us_wkn; //!< \~english GPS week number +} SENSOR_WKN_ROLLOVER_HAL; + +/** + * @struct POS_DATETIME + * \~english GPS time information structure + */ +typedef struct { + uint16_t year; //!< \~english year + uint8_t month; //!< \~english month + uint8_t date; //!< \~english day + uint8_t hour; //!< \~english hour + uint8_t minute; //!< \~english minute + uint8_t second; //!< \~english second + uint8_t reserved; //!< \~english reserve +} POS_DATETIME; + +/** + * @struct TG_GPS_RET_RESET + * \~english GPS reset status + */ +typedef struct { + unsigned long ret_rst_status; //!< \~english response GPS reset status +} TG_GPS_RET_RESET; + +/** + * @struct TG_GPS_RET_RESET_MSG + * \~english GPS reset information message + */ +typedef struct { + TG_GPS_RET_RESET data; //!< \~english GPS reset status data +} TG_GPS_RET_RESET_MSG; + +/** + * @struct VEHICLE_MSG_SEND_DAT + * \~english Vehicle information setting message structure + */ +typedef struct { + DID did; //!< \~english Data ID + u_int16 size; //!< \~english Data size + u_int8 data[502]; //!< \~english Data +} VEHICLE_MSG_SEND_DAT; + +/** + * @struct GpsWeekCorCntMsg + * \~english Gps Week correction count notify message structure + */ +typedef struct GpsWeekCorCntMsg { + T_APIMSG_MSGBUF_HEADER stHead; //!< \~english Message header + u_int8 gpsweekcorcnt; //!< \~english Gps Week correction count + int8 dummy[3]; //!< \~english Alignment adjustment +} CLOCKGPS_GPSWEEKCOR_CNT_MSG; + +/** @}*/ // end of positioning_hal +/** @}*/ // end of positioning + +/*---------------------------------------------------------------------------*/ +#endif // HAL_API_GPS_HAL_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/hal_api/positioning_hal.h b/hal_api/positioning_hal.h new file mode 100644 index 00000000..8a74b814 --- /dev/null +++ b/hal_api/positioning_hal.h @@ -0,0 +1,872 @@ +/* + * @copyright Copyright (c) 2018-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. + */ + +#ifndef HAL_API_POSITIONING_HAL_H_ +#define HAL_API_POSITIONING_HAL_H_ + +/** + * @file positioning_hal.h + */ + +/** @addtogroup positioning + * @{ + */ +/** @addtogroup positioning_hal + * @ingroup positioning + * @{ + */ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include <native_service/frameworkunified_types.h> +#include <vehicle_service/pos_message_header.h> +#include <vehicle_service/std_types.h> + +/*---------------------------------------------------------------------------*/ +// Value define + +#define POSHAL_THREAD_NAME_SENS "POS_Sens" +//!< \~english Thread name for receive sensor +#define POSHAL_THREAD_NAME_GPS_MAIN "POS_Gps" +//!< \~english Thread name for GPS NMEA analysis and delivery message +#define POSHAL_THREAD_NAME_GPS_RECV "POS_Gps_Recv" +//!< \~english Thread name for receive GPS +#define POSHAL_THREAD_NAME_GPS_ROLOVR "POS_Gps_Rolovr" +//!< \~english Thread name for monitoring GPS rollover + +#define POSHAL_DID_SNS_COUNTER 0x8000001A +//!< \~english Data ID of sensor counter +#define POSHAL_DID_SPEED_PULSE 0x80000012 +//!< \~english Data ID of speed pulse +#define POSHAL_DID_SPEED_PULSE_FLAG 0x80000076 +//!< \~english Data ID of speed pulse flag +#define POSHAL_DID_SPEED_PULSE_FST 0x80000028 +//!< \~english Data ID of first time speed pulse +#define POSHAL_DID_SPEED_PULSE_FLAG_FST 0x8000007D +//!< \~english Data ID of first time speed pulse flag +#define POSHAL_DID_SPEED_KMPH 0x80000013 +//!< \~english Data ID of KMPH speed +#define POSHAL_DID_PULSE_TIME 0x8000003A +//!< \~english Data ID of pulse time +#define POSHAL_DID_GYRO_X 0x80000014 +//!< \~english Data ID of X axis of gyro +#define POSHAL_DID_GYRO_Y 0x80000085 +//!< \~english Data ID of Y axis of gyro +#define POSHAL_DID_GYRO_Z 0x80000086 +//!< \~english Data ID of Z axis ofgyro +#define POSHAL_DID_GYRO POSHAL_DID_GYRO_X +//!< \~english Data ID of gyro +#define POSHAL_DID_GYRO_X_FST 0x80000029 +//!< \~english Data ID of first time X axis gyro +#define POSHAL_DID_GYRO_Y_FST 0x80000043 +//!< \~english Data ID of first time Y axis gyro +#define POSHAL_DID_GYRO_Z_FST 0x80000023 +//!< \~english Data ID of first time Z axis gyro +#define POSHAL_DID_GYRO_FST POSHAL_DID_GYRO_X_FST +//!< \~english Data ID of first time gyro +#define POSHAL_DID_GYRO_EXT 0x80000027 +//!< \~english Data ID of extend gyro +#define POSHAL_DID_GYRO_TEMP 0x80000090 +//!< \~english Data ID of gyro temperature +#define POSHAL_DID_GYRO_TEMP_FST 0x80000091 +//!< \~english Data ID of first time gyro temperature +#define POSHAL_DID_REV 0x80000017 +//!< \~english Data ID of reverse signal +#define POSHAL_DID_REV_FST 0x8000007E +//!< \~english Data ID of first time reverse signal +#define POSHAL_DID_GSNS_X 0x80000015 +//!< \~english Data ID of x axis gsensor +#define POSHAL_DID_GSNS_Y 0x80000016 +//!< \~english Data ID of Y axis gsensor +#define POSHAL_DID_GSNS_Z 0x80000026 +//!< \~english Data ID of Z axis gsensor +#define POSHAL_DID_GSNS_X_FST 0x80000087 +//!< \~english Data ID of first time x axis gsensor +#define POSHAL_DID_GSNS_Y_FST 0x80000088 +//!< \~english Data ID of first time Y axis gsensor +#define POSHAL_DID_GSNS_Z_FST 0x80000089 +//!< \~english Data ID of first time Z axis gsensor +#define POSHAL_DID_GPS_NMEA 0x8000009AU +//!< \~english Data ID of GPS NMEA sentence +#define POSHAL_DID_GPS_ANTENNA 0x80000019 +//!< \~english Data ID of GPS antenna status +#define POSHAL_DID_GPS_CUSTOMDATA 0x80000094U +//!< \~english Data ID of GPS custom data +#define POSHAL_DID_GPS_CUSTOMDATA_NAVI 0x800000A0 +//!< \~english Data ID of GPS custom data for navi +#define POSHAL_DID_GPS_TIME 0x80000098 +//!< \~english Data ID of GPS time +#define POSHAL_DID_GPS_TIME_RAW 0x800000B1U +//!< \~english Data ID of GPS raw time +#define POSHAL_DID_GPS_VERSION 0x8000001E +//!< \~english Data ID of GPS version +#define POSHAL_DID_GPS__CWORD82___CWORD44_GP4 0x80000031U +//!< \~english Data ID of _CWORD82_ GPS _CWORD44_GP4 data +#define POSHAL_DID_GPS__CWORD82__FULLBINARY 0x80000032U +//!< \~english Data ID of _CWORD82_ GPS full binary data +#define POSHAL_DID_GPS_WKNROLLOVER 0x800000B2U +//!< \~english Data ID of GPS week counter +#define POSHAL_DID_GPS_CLOCK_DRIFT 0x800000B3U +//!< \~english Data ID of GPS time drift data +#define POSHAL_DID_GPS_CLOCK_FREQ 0x800000B4U +//!< \~english Data ID of GPS time frequency data +#define POSHAL_DID_GPS_INTERRUPT_FLAG 0x80000077 +//!< \~english Data ID of GPS interrupt flag + +#define CONFIG_SENSOR_EXT_VALID 1 //!< \~english for first sensor + +#define CID_LINESENS_VEHICLE_DATA 0x0300 //!< \~english vehicle notify ID + +/** + * \~english @brief LineSensor Vehicle Signal Notification + * \~english @brief This command is sent when LineSensor vehicle signals are notified when sensor data is received.\n + * \~english @brief Message related structure @ref LsdrvMsgLsdataG\n + * \~english @brief Message buffer header structure @ref T_APIMSG_MSGBUF_HEADER\n + * \~english @brief Message header structure @ref T_APIMSG_HEADER\n + * \~english @brief Message body structure @ref LsdrvMsgLsdataDatG\n + * \~english @brief Message body (vehicle signal notification message information) structure @ref LsdrvLsdataG\n + */ +#define CID_LINESENS_VEHICLE_DATA_G 0x0303 + +/** + * \~english @brief Gyro Failure Status Notification + * \~english @brief This command is sent when giro failure status is notified.\n + * \~english @brief Message related structure @ref LsdrvMsgLsdataGyroTrouble\n + * \~english @brief Message buffer header structure @ref T_APIMSG_MSGBUF_HEADER\n + * \~english @brief Message header structure @ref T_APIMSG_HEADER\n + * \~english @brief Message body structure @ref LsdrvMsgLsdataDatGyroTrouble\n + */ +#define CID_LINESENS_VEHICLE_DATA_GYRO_TROUBLE 0x0304 + +/** + * \~english @brief Get SYS GPS Interrupt Signals + * \~english @brief This command is sent when SYS GPS interrupt signal is acquired.\n + * \~english @brief Message related structure @ref LsdrvMsgLsdataGpsInterruptSignal\n + * \~english @brief Message buffer header structure @ref T_APIMSG_MSGBUF_HEADER\n + * \~english @brief Message header structure @ref T_APIMSG_HEADER\n + * \~english @brief Message body structure @ref LsdrvMsgLsdataDatGpsInterruptSignal\n + */ +#define CID_LINESENS_VEHICLE_DATA_SYS_GPS_INTERRUPT_SIGNAL 0x0305 + + +/** + * \~english @brief Gyro Connection Status Notification + * \~english @brief This command is sent when giro connection status is notified.\n + * \~english @brief Message related structure @ref LsdrvMsgLsdataGyroConnectStatus\n + * \~english @brief Message buffer header structure @ref T_APIMSG_MSGBUF_HEADER\n + * \~english @brief Message header structure @ref T_APIMSG_HEADER\n + * \~english @brief Message body structure @ref LsdrvMsgLsdataDatGyroConnectStatus\n + */ +#define CID_LINESENS_VEHICLE_DATA_GYRO_CONNECT_STATUS 0x0306 + + +/** + * \~english @brief GPS Antenna Connection Status Notification + * \~english @brief This command is sent when the GPS antenna connection status is notified.\n + * \~english @brief Message related structure @ref LsdrvMsgLsdataGpsAntennaStatus\n + * \~english @brief Message buffer header structure @ref T_APIMSG_MSGBUF_HEADER\n + * \~english @brief Message header structure @ref T_APIMSG_HEADER\n + * \~english @brief Message body structure @ref LsdrvMsgLsdataDatGpsAntennaStatus\n + */ +#define CID_LINESENS_VEHICLE_DATA_GPS_ANTENNA_STATUS 0x0307 + + +/** + * \~english @brief LineSensor Vehicle Signal Notification(Initial Sensor) + * \~english @brief This command is sent when LineSensor vehicle signals (initial sensor) are notified.\n + * \~english @brief Message related structure @ref LsdrvMsgLsdataFst\n + * \~english @brief Message buffer header structure @ref T_APIMSG_MSGBUF_HEADER\n + * \~english @brief Message header structure @ref T_APIMSG_HEADER\n + * \~english @brief Message body structure @ref LsdrvMsgLsdataDatFst\n + * \~english @brief Gyro X-axis @ref LsdrvLsdataFstGyroX\n + * \~english @brief Gyro Y-axis @ref LsdrvLsdataFstGyroY\n + * \~english @brief Gyro Z-axis @ref LsdrvLsdataFstGyroZ\n + * \~english @brief Velocity information @ref LsdrvLsdataFstSpeed\n + * \~english @brief Vehicle speed pulse @ref LsdrvLsdataFstSpeedPulseFlag\n + * \~english @brief REV flag @ref LsdrvLsdataFstRev\n + * \~english @brief Gyro Temperature @ref LsdrvLsdataFstGyroTemp\n + * \~english @brief G-sensor X-axis @ref LsdrvLsdataFstGsensorX\n + * \~english @brief G-sensor Y-axis @ref LsdrvLsdataFstGsensorY\n + * \~english @brief G-sensor Z-axis @ref LsdrvLsdataFstGsensorZ\n + */ +#define CID_LINESENS_VEHICLE_DATA_FST 0x0302 + +/** + * \~english @brief GPS data notification + * \~english @brief This command is used to notify the data input from the GPS receiver device.\n + * \~english @brief Message related structure @ref SensorMsgGpsdata\n + * \~english @brief Message buffer header structure @ref T_APIMSG_MSGBUF_HEADER\n + * \~english @brief Message header structure @ref T_APIMSG_HEADER\n + * \~english @brief Message body structure @ref SensorMsgGpsdataDat\n + */ +#define CID_GPS_DATA 0x0301 + +/** + * \~english @brief Notification of effective ephemeris number at shutdown + * \~english @brief This command is sent to notify the effective ephemeris number at shutdown.\n + * \~english @brief Message related structure @ref SensorMsgGpsdata\n + * \~english @brief Message buffer header structure @ref T_APIMSG_MSGBUF_HEADER\n + * \~english @brief Message body structure @ref SensorMsgGpsdata\n + */ +#define CID_EPH_NUM_NOTIFICATION 0x0310 + + +#define LSDRV_FSTSNS_SENSOR_FIRST_SAVE_NUM 50 +//!< \~english number of save first sensor + +#define LSDRV_FSTSNS_SENSOR_FIRST_SEND_NUM 30 +//!< \~english number of send first sensor + +#define LSDRV_FSTSNS_DETAILED_DATA_GYRO_NUM 10 +//!< \~english number of gyro including detail data + +#define LSDRV_FSTSNS_DETAILED_DATA_GSENSOR_NUM 10 +//!< \~english number of Gsns includeing detail data + +#define LSDRV_FSTSNS_DSIZE_GYRO_X (LSDRV_FSTSNS_SENSOR_FIRST_SEND_NUM * LSDRV_FSTSNS_DETAILED_DATA_GYRO_NUM * 2) +//!< \~english data size of X axis of gyro +#define LSDRV_FSTSNS_DSIZE_GYRO_Y (LSDRV_FSTSNS_SENSOR_FIRST_SEND_NUM * LSDRV_FSTSNS_DETAILED_DATA_GYRO_NUM * 2) +//!< \~english data size of Y axis of gyro +#define LSDRV_FSTSNS_DSIZE_GYRO_Z (LSDRV_FSTSNS_SENSOR_FIRST_SEND_NUM * LSDRV_FSTSNS_DETAILED_DATA_GYRO_NUM * 2) +//!< \~english data size of Z axis of gyro +#define LSDRV_FSTSNS_DSIZE_SPEED (LSDRV_FSTSNS_SENSOR_FIRST_SEND_NUM * 2) +//!< \~english data size of speed +#define LSDRV_FSTSNS_DSIZE_SPEED_FLG LSDRV_FSTSNS_SENSOR_FIRST_SEND_NUM +//!< \~english data size of speed pulse +#define LSDRV_FSTSNS_DSIZE_REV LSDRV_FSTSNS_SENSOR_FIRST_SEND_NUM +//!< \~english data size of reverse +#define LSDRV_FSTSNS_DSIZE_GYRO_TEMP (LSDRV_FSTSNS_SENSOR_FIRST_SEND_NUM * 2) +//!< \~english data size of gyro temperature +#define LSDRV_FSTSNS_DSIZE_GSENSOR_X (LSDRV_FSTSNS_SENSOR_FIRST_SEND_NUM * LSDRV_FSTSNS_DETAILED_DATA_GSENSOR_NUM * 2) +//!< \~english data size of X axis of Gsns + +#define LSDRV_FSTSNS_DSIZE_GSENSOR_Y (LSDRV_FSTSNS_SENSOR_FIRST_SEND_NUM * LSDRV_FSTSNS_DETAILED_DATA_GSENSOR_NUM * 2) + //!< \~english data size of Y axis of Gsns +#define LSDRV_FSTSNS_DSIZE_GSENSOR_Z (LSDRV_FSTSNS_SENSOR_FIRST_SEND_NUM * LSDRV_FSTSNS_DETAILED_DATA_GSENSOR_NUM * 2) +//!< \~english data size of Z axis of Gsns + +#define SENSOR_MSG_VSINFO_DSIZE 1904 //!< \~english message body max size + + +// Thread control Command ID +#define CID_THREAD_CREATE_COMP (0x0001) //!< \~english Thread Start Complete Notify Command ID +#define CID_THREAD_STOP_REQ (0x0002) //!< \~english Thread Stop Request Command ID +#define CID_THREAD_STOP_COMP (0x0003) //!< \~english Thread Stop Complete Notify Command ID + +// A vehicle signal of data size +#define LSDRV_SNDMSG_DTSIZE_1 1 //!< \~english SndMSG data size 1Byte +#define LSDRV_SNDMSG_DTSIZE_2 2 //!< \~english SndMSG data size 2Byte +#define LSDRV_SNDMSG_DTSIZE_20 20 //!< \~english SndMSG data size 20Byte +#define LSDRV_SNDMSG_DTSIZE_132 132 //!< \~english SndMSG data size 132Byte + +// NMEA receive flag +#define POS_SNS_GPS_NMEA_GGA (0x00000001U) //!< \~english GGA sentence +#define POS_SNS_GPS_NMEA_GLL (0x00000002U) //!< \~english GLL sentence +#define POS_SNS_GPS_NMEA_GSA1 (0x00000004U) //!< \~english GSA1 sentence +#define POS_SNS_GPS_NMEA_GSA2 (0x00000008U) //!< \~english GSA2 sentence +#define POS_SNS_GPS_NMEA_GSA3 (0x00000010U) //!< \~english GSA3 sentence +#define POS_SNS_GPS_NMEA_GSA4 (0x00000020U) //!< \~english GSA4 sentence +#define POS_SNS_GPS_NMEA_GST (0x00000040U) //!< \~english GST sentence +#define POS_SNS_GPS_NMEA_RMC (0x00000080U) //!< \~english RMC sentence +#define POS_SNS_GPS_NMEA_VTG (0x00000100U) //!< \~english VTG sentence +#define POS_SNS_GPS_NMEA_GSV1 (0x00000200U) //!< \~english GSV1 sentence +#define POS_SNS_GPS_NMEA_GSV2 (0x00000400U) //!< \~english GSV2 sentence +#define POS_SNS_GPS_NMEA_GSV3 (0x00000800U) //!< \~english GSV3 sentence +#define POS_SNS_GPS_NMEA_GSV4 (0x00001000U) //!< \~english GSV4 sentence +#define POS_SNS_GPS_NMEA_GSV5 (0x00002000U) //!< \~english GSV5 sentence +#define POS_SNS_GPS_NMEA_GSV6 (0x00004000U) //!< \~english GSV6 sentence +#define POS_SNS_GPS_NMEA_GSV7 (0x00008000U) //!< \~english GSV7 sentence +#define POS_SNS_GPS_NMEA_GSV8 (0x00010000U) //!< \~english GSV8 sentence +#define POS_SNS_GPS_NMEA_GSV9 (0x00020000U) //!< \~english GSV9 sentence +#define POS_SNS_GPS_NMEA_GSV10 (0x00040000U) //!< \~english GSV10 sentence +#define POS_SNS_GPS_NMEA_GSV11 (0x00080000U) //!< \~english GSV11 sentence +#define POS_SNS_GPS_NMEA_GSV12 (0x00100000U) //!< \~english GSV12 sentence +#define POS_SNS_GPS_NMEA_GSV13 (0x00200000U) //!< \~english GSV13 sentence +#define POS_SNS_GPS_NMEA_RESERVE1 (0x00400000U) //!< \~english Reserved +#define POS_SNS_GPS_NMEA_RESERVE2 (0x00800000U) //!< \~english Reserved + + +/** + * @enum POS_SNS_GPS_NMEA_SNO + * \~english type of NMEA + */ +typedef enum { + POS_SNS_GPS_NMEA_SNO_GGA = 0, //!< \~english GGA sentence + POS_SNS_GPS_NMEA_SNO_GLL, //!< \~english GLL sentence + POS_SNS_GPS_NMEA_SNO_GSA1, //!< \~english GSA1 sentence + POS_SNS_GPS_NMEA_SNO_GSA2, //!< \~english GSA2 sentence + POS_SNS_GPS_NMEA_SNO_GSA3, //!< \~english GSA3 sentence + POS_SNS_GPS_NMEA_SNO_GSA4, //!< \~english GSA4 sentence + POS_SNS_GPS_NMEA_SNO_GST, //!< \~english GST sentence + POS_SNS_GPS_NMEA_SNO_RMC, //!< \~english RMC sentence + POS_SNS_GPS_NMEA_SNO_VTG, //!< \~english VTG sentence + POS_SNS_GPS_NMEA_SNO_GSV1, //!< \~english GSV1 sentence + POS_SNS_GPS_NMEA_SNO_GSV2, //!< \~english GSV2 sentence + POS_SNS_GPS_NMEA_SNO_GSV3, //!< \~english GSV3 sentence + POS_SNS_GPS_NMEA_SNO_GSV4, //!< \~english GSV4 sentence + POS_SNS_GPS_NMEA_SNO_GSV5, //!< \~english GSV5 sentence + POS_SNS_GPS_NMEA_SNO_GSV6, //!< \~english GSV6 sentence + POS_SNS_GPS_NMEA_SNO_GSV7, //!< \~english GSV7 sentence + POS_SNS_GPS_NMEA_SNO_GSV8, //!< \~english GSV8 sentence + POS_SNS_GPS_NMEA_SNO_GSV9, //!< \~english GSV9 sentence + POS_SNS_GPS_NMEA_SNO_GSV10, //!< \~english GSV10 sentence + POS_SNS_GPS_NMEA_SNO_GSV11, //!< \~english GSV11 sentence + POS_SNS_GPS_NMEA_SNO_GSV12, //!< \~english GSV12 sentence + POS_SNS_GPS_NMEA_SNO_GSV13, //!< \~english GSV13 sentence + POS_SNS_GPS_NMEA_SNO_RESERVE1, //!< \~english Reserved + POS_SNS_GPS_NMEA_SNO_RESERVE2, //!< \~english Reserved + + POS_SNS_GPS_NMEA_SNO_MAX //!< \~english MAX sentence(invalid) +} POS_SNS_GPS_NMEA_SNO; + +/** + * @enum LsDrvKind + * \~english type of Sensor + */ +typedef enum { + LSDRV_GYRO_X, //!< \~english Gyro X-Axis + LSDRV_GYRO_Y, //!< \~english Gyro Y-Axis + LSDRV_GYRO_Z, //!< \~english Gyro Z-Axis + LSDRV_SPEED_PULSE, //!< \~english Speed pulse + LSDRV_SPEED_PULSE_FLAG, //!< \~english Speed pulse Flag + LSDRV_SPEED_KMPH, //!< \~english Speed km/h + LSDRV_GYRO_EXT, //!< \~english extend Gyro + LSDRV_REV, //!< \~english Reverse + LSDRV_GYRO_TEMP, //!< \~english Gyro temperature + LSDRV_GSENSOR_X, //!< \~english G-sensor X-Axis + LSDRV_GSENSOR_Y, //!< \~english G-sensor Y-Axis + LSDRV_GSENSOR_Z, //!< \~english G-sensor Z-Axis + LSDRV_PULSE_TIME, //!< \~english Pulse time + LSDRV_SNS_COUNTER, //!< \~english Sensor counter + LSDRV_GPS_INTERRUPT_FLAG, //!< \~english GPS Interrupt Flag + LSDRV_KINDS_MAX //!< \~english MAX +} LsDrvKind; + + +/** + * @enum EnumTidPos + * \~english Thread id notified to positioning when starting or terminating each thread is completed. + */ +typedef enum EnumTidPos { + ETID_POS_MAIN = 0, + //!< \~english Thread for vehicle sensor(unused in hal) + ETID_POS_SENS, + //!< \~english Thread for receive sensor + ETID_POS_GPS, + //!< \~english Thread for GPS NMEA analysis and delivery message + ETID_POS_GPS_RECV, + //!< \~english Thread for receive GPS + ETID_POS_GPS_ROLLOVER, + //!< \~english Thread for monitoring GPS rollover + ETID_POS_MAX + //!< \~english Max thread id +} EnumTID_POS; + +/** + * @struct LsdrvLsdata + * \~english Struct of LineSensor message + */ +typedef struct LsdrvLsdata { + u_int32 ul_did; //!< \~english data ID + u_int8 uc_size; //!< \~english data size + u_int8 uc_rcvflag; //!< \~english receive flag + u_int8 uc_sns_cnt; //!< \~english sensor count + u_int8 u_reserve; //!< \~english reserve + u_int8 uc_data[132]; //!< \~english data contents +} LSDRV_LSDATA; + +/** + * @struct LsdrvLsdataG + * \~english Struct of LineSensorG message + */ +typedef struct LsdrvLsdataG { + u_int32 ul_did; //!< \~english data ID + u_int8 uc_size; //!< \~english data size + u_int8 uc_rcv_flag; //!< \~english receive flag + u_int8 uc_sns_cnt; //!< \~english sensor count + u_int8 reserve; //!< \~english reserve + u_int8 uc_data[132]; //!< \~english data content +} LSDRV_LSDATA_G; + +/** + * @struct LsdrvLsdataFstGyroX + * \~english Struct of X axis of of gyro data(first sensor message) + */ +typedef struct LsdrvLsdataFstGyroX { + u_int32 ul_did; //!< \~english data ID + u_int16 uc_size; //!< \~english data size + u_int8 uc_partition_max; //!< \~english total of partition + u_int8 uc_partition_num; //!< \~english data number + u_int8 uc_data[LSDRV_FSTSNS_DSIZE_GYRO_X]; //!< \~english data contents +} LSDRV_LSDATA_FST_GYRO_X; + +/** + * @struct LsdrvLsdataFstGyroY + * \~english Struct of Y axis of of gyro data(first sensor message) + */ +typedef struct LsdrvLsdataFstGyroY { + u_int32 ul_did; //!< \~english data ID + u_int16 uc_size; //!< \~english data size + u_int8 uc_partition_max; //!< \~english total of partition + u_int8 uc_partition_num; //!< \~english data number + u_int8 uc_data[LSDRV_FSTSNS_DSIZE_GYRO_Y]; //!< \~english data contents +} LSDRV_LSDATA_FST_GYRO_Y; + +/** + * @struct LsdrvLsdataFstGyroZ + * \~english Struct of Z axis of of gyro data(first sensor message) + */ +typedef struct LsdrvLsdataFstGyroZ { + u_int32 ul_did; //!< \~english data ID + u_int16 uc_size; //!< \~english data size + u_int8 uc_partition_max; //!< \~english total of partition + u_int8 uc_partition_num; //!< \~english data number + u_int8 uc_data[LSDRV_FSTSNS_DSIZE_GYRO_Z]; //!< \~english data contents +} LSDRV_LSDATA_FST_GYRO_Z; + +/** + * @struct LsdrvLsdataFstSpeed + * \~english Struct of speed data(first sensor message) + */ +typedef struct LsdrvLsdataFstSpeed { + u_int32 ul_did; //!< \~english data ID + u_int16 uc_size; //!< \~english data size + u_int8 uc_partition_max; //!< \~english total of partition + u_int8 uc_partition_num; //!< \~english data number + u_int8 uc_data[LSDRV_FSTSNS_DSIZE_SPEED]; //!< \~english data contents +} LSDRV_LSDATA_FST_SPEED; + +/** + * @struct LsdrvLsdataFstSpeedPulseFlag + * \~english Struct of speed pulse data(first sensor message) + */ +typedef struct LsdrvLsdataFstSpeedPulseFlag { + u_int32 ul_did; //!< \~english data ID + u_int16 uc_size; //!< \~english data size + u_int8 uc_partition_max; //!< \~english total of partition + u_int8 uc_partition_num; //!< \~english data number + u_int8 uc_data[LSDRV_FSTSNS_DSIZE_SPEED_FLG]; //!< \~english data contents +} LSDRV_LSDATA_FST_SPEED_PULSE_FLAG; + +/** + * @struct LsdrvLsdataFstRev + * \~english Struct of reverse data(first sensor message) + */ +typedef struct LsdrvLsdataFstRev { + u_int32 ul_did; //!< \~english data ID + u_int16 uc_size; //!< \~english data size + u_int8 uc_partition_max; //!< \~english total of partition + u_int8 uc_partition_num; //!< \~english data number + u_int8 uc_data[LSDRV_FSTSNS_DSIZE_REV]; //!< \~english data contents +} LSDRV_LSDATA_FST_REV; + +/** + * @struct LsdrvLsdataFstGyroTemp + * \~english Struct of gyro temperature data(first sensor message) + */ +typedef struct LsdrvLsdataFstGyroTemp { + u_int32 ul_did; //!< \~english data ID + u_int16 uc_size; //!< \~english data size + u_int8 uc_partition_max; //!< \~english total of partition + u_int8 uc_partition_num; //!< \~english data number + u_int8 uc_data[LSDRV_FSTSNS_DSIZE_GYRO_TEMP]; //!< \~english data contents +} LSDRV_LSDATA_FST_GYRO_TEMP; + +/** + * @struct LsdrvLsdataFstGsensorX + * \~english Struct of X axis of Gsns data(first sensor message) + */ +typedef struct LsdrvLsdataFstGsensorX { + u_int32 ul_did; //!< \~english data ID + u_int16 uc_size; //!< \~english data size + u_int8 uc_partition_max; //!< \~english total of partition + u_int8 uc_partition_num; //!< \~english data number + u_int8 uc_data[LSDRV_FSTSNS_DSIZE_GSENSOR_X]; //!< \~english data contents +} LSDRV_LSDATA_FST_GSENSOR_X; + +/** + * @struct LsdrvLsdataFstGsensorY + * \~english Struct of Y axis of Gsns data(first sensor message) + */ +typedef struct LsdrvLsdataFstGsensorY { + u_int32 ul_did; //!< \~english data ID + u_int16 uc_size; //!< \~english data size + u_int8 uc_partition_max; //!< \~english total of partition + u_int8 uc_partition_num; //!< \~english data number + u_int8 uc_data[LSDRV_FSTSNS_DSIZE_GSENSOR_Y]; //!< \~english data contents +} LSDRV_LSDATA_FST_GSENSOR_Y; + +/** + * @struct LsdrvLsdataFstGsensorZ + * \~english Struct of Z axis of Gsns data(first sensor message) + */ +typedef struct LsdrvLsdataFstGsensorZ { + u_int32 ul_did; //!< \~english data ID + u_int16 uc_size; //!< \~english data size + u_int8 uc_partition_max; //!< \~english total of partition + u_int8 uc_partition_num; //!< \~english data number + u_int8 uc_data[LSDRV_FSTSNS_DSIZE_GSENSOR_Z]; //!< \~english data contents +} LSDRV_LSDATA_FST_GSENSOR_Z; + +/** + * @struct LsdrvMsgLsdataDatFst + * \~english Struct of LineSensor data (first sensor message) + */ +typedef struct LsdrvMsgLsdataDatFst { + LSDRV_LSDATA_FST_GYRO_X st_gyro_x; //!< \~english data of X axis of gyro + LSDRV_LSDATA_FST_GYRO_Y st_gyro_y; //!< \~english data of Y axis of gyro + LSDRV_LSDATA_FST_GYRO_Z st_gyro_z; //!< \~english data of Z axis of gyro + LSDRV_LSDATA_FST_SPEED st_speed; //!< \~english data of speed + LSDRV_LSDATA_FST_SPEED_PULSE_FLAG st_spd_pulse_flg; //!< \~english data of speed pulse + LSDRV_LSDATA_FST_REV st_rev; //!< \~english data of reverse + LSDRV_LSDATA_FST_GYRO_TEMP st_gyro_temp; //!< \~english data of gyro temperature + LSDRV_LSDATA_FST_GSENSOR_X st_gsns_x; //!< \~english data of X axis of Gsns + LSDRV_LSDATA_FST_GSENSOR_Y st_gsns_y; //!< \~english data of Y axis of Gsns + LSDRV_LSDATA_FST_GSENSOR_Z st_gsns_z; //!< \~english data of Z axis of Gsns +} LSDRV_MSG_LSDATA_DAT_FST; + +/** + * @struct LsdrvMsgLsdataDat + * \~english Struct of LineSensor package data + */ +typedef struct LsdrvMsgLsdataDat { + u_int8 uc_data_num; //!< \~english data number + u_int8 reserve[3]; //!< \~english reserve + LSDRV_LSDATA st_data[12]; //!< \~english data +} LSDRV_MSG_LSDATA_DAT; + +/** + * @struct LsdrvMsgLsdata + * \~english Struct of LineSensor message data + */ +typedef struct LsdrvMsgLsdata { + T_APIMSG_MSGBUF_HEADER st_head; //!< \~english message header + LSDRV_MSG_LSDATA_DAT st_para; //!< \~english data of gyro +} LSDRV_MSG_LSDATA; + +/** + * @struct LsdrvMsgLsdataDatG + * \~english Struct of LineSensorG package data + */ +typedef struct LsdrvMsgLsdataDatG { + u_int8 uc_data_num; //!< \~english data number + u_int8 reserve[3]; //!< \~english reserve + LSDRV_LSDATA_G st_data[15]; //!< \~english data +} LSDRV_MSG_LSDATA_DAT_G; + +/** + * @struct LsdrvMsgLsdataG + * \~english Struct of LineSensorG message data + */ +typedef struct LsdrvMsgLsdataG { + T_APIMSG_MSGBUF_HEADER st_head; //!< \~english message header + LSDRV_MSG_LSDATA_DAT_G st_para; //!< \~english data of message +} LSDRV_MSG_LSDATA_G; + +/** + * @struct LsdrvMsgLsdataDatGyroTrouble + * \~english Struct of Gyro trouble + */ +typedef struct LsdrvMsgLsdataDatGyroTrouble { + u_int32 ul_did; //!< \~english data ID + u_int8 uc_size; //!< \~english data size + u_int8 reserve; //!< \~english reserve + u_int8 uc_data; //!< \~english data content + u_int8 reserve2; //!< \~english reserve +} LSDRV_MSG_LSDATA_DAT_GYRO_TROUBLE; + +/** + * @struct LsdrvMsgLsdataGyroTrouble + * \~english Struct of message about Gyro trouble + */ +typedef struct LsdrvMsgLsdataGyroTrouble { + T_APIMSG_MSGBUF_HEADER st_head; //!< \~english message header + LSDRV_MSG_LSDATA_DAT_GYRO_TROUBLE st_para; //!< \~english data of message +} LSDRV_MSG_LSDATA_GYRO_TROUBLE; + +/** + * @struct LsdrvMsgLsdataDatGpsInterruptSignal + * \~english Struct of Gps interrupt signal data + */ +typedef struct LsdrvMsgLsdataDatGpsInterruptSignal { + u_int32 ul_did; //!< \~english data ID + u_int8 uc_size; //!< \~english data size + u_int8 reserve; //!< \~english reserve + u_int8 uc_data; //!< \~english data content + u_int8 reserve2; //!< \~english reserve +} LSDRV_MSG_LSDATA_DAT_GPS_INTERRUPT_SIGNAL; + +/** + * @struct LsdrvMsgLsdataGpsInterruptSignal + * \~english Struct of message about Gps interrupt signal + */ +typedef struct LsdrvMsgLsdataGpsInterruptSignal { + T_APIMSG_MSGBUF_HEADER st_head; //!< \~english message header + LSDRV_MSG_LSDATA_DAT_GPS_INTERRUPT_SIGNAL st_para; //!< \~english data of message +} LSDRV_MSG_LSDATA_GPS_INTERRUPT_SIGNAL; + +/** + * @struct LsdrvMsgLsdataDatGyroConnectStatus + * \~english Struct of Gyro connect status + */ +typedef struct LsdrvMsgLsdataDatGyroConnectStatus { + u_int32 ul_did; //!< \~english data ID + u_int8 uc_size; //!< \~english data size + u_int8 reserve; //!< \~english reserve + u_int8 uc_data; //!< \~english data content + u_int8 reserve2; //!< \~english reserve +} LSDRV_MSG_LSDATA_DAT_GYRO_CONNECT_STATUS; + +/** + * @struct LsdrvMsgLsdataGyroConnectStatus + * \~english Struct of message about Gyro connect status + */ +typedef struct LsdrvMsgLsdataGyroConnectStatus { + T_APIMSG_MSGBUF_HEADER st_head; //!< \~english message header + LSDRV_MSG_LSDATA_DAT_GYRO_CONNECT_STATUS st_para; //!< \~english data of message +} LSDRV_MSG_LSDATA_GYRO_CONNECT_STATUS; + +/** + * @struct LsdrvMsgLsdataDatGpsAntennaStatus + * \~english Struct of Gps Antenna Status + */ +typedef struct LsdrvMsgLsdataDatGpsAntennaStatus { + u_int32 ul_did; //!< \~english data ID + u_int8 uc_size; //!< \~english data size + u_int8 uc_rcv_flag; //!< \~english receive flag + u_int8 uc_sns_cnt; //!< \~english sensor count + u_int8 uc_data; //!< \~english data content +} LSDRV_MSG_LSDATA_DAT_GPS_ANTENNA_STATUS; + +/** + * @struct LsdrvMsgLsdataGpsAntennaStatus + * \~english Struct of message about Gps Antenna Status + */ +typedef struct LsdrvMsgLsdataGpsAntennaStatus { + T_APIMSG_MSGBUF_HEADER st_head; //!< \~english message header + LSDRV_MSG_LSDATA_DAT_GPS_ANTENNA_STATUS st_para; //!< \~english data of message +} LSDRV_MSG_LSDATA_GPS_ANTENNA_STATUS; + +/** + * @struct LsdrvLsdataFst + * \~english Struct of sensor data (first sensor message) + */ +typedef struct LsdrvLsdataFst { + u_int32 ul_did; //!< \~english data ID + u_int16 uc_size; //!< \~english data size + u_int8 uc_partition_max; //!< \~english total of partition + u_int8 uc_partition_num; //!< \~english data number + u_int8 uc_data[60]; //!< \~english data contents +} LSDRV_LSDATA_FST; + +/** + * @struct LsdrvMsgLsdataFst + * \~english Struct of message about sensor data (first sensor message) + */ +typedef struct LsdrvMsgLsdataFst { + T_APIMSG_MSGBUF_HEADER st_head; //!< \~english message header + LSDRV_MSG_LSDATA_DAT_FST st_para; //!< \~english data of message +} LSDRV_MSG_LSDATA_FST; + +/** + * @struct SensorMsgGpsdataDat + * \~english Struct of Gps data + * \~english @par Detail + * This data structure can have 7 types of data that identified by data ID.\n + * The contents are as follows.\n + * | data ID | content | data format | + * |:----------------------------------|:------------------------------------------|:------------------------------| + * | @ref POSHAL_DID_GPS_NMEA | NMEA data | @ref MdevGpsNmea "MDEV_GPS_NMEA" | + * | @ref POSHAL_DID_GPS_CUSTOMDATA | GPS custom data | @ref MdevGpsCustomDataMgs "MDEV_GPS_CUSTOMDATA" | + * | @ref POSHAL_DID_GPS_CLOCK_DRIFT | clock drift | @ref int32_t | + * | @ref POSHAL_DID_GPS_CLOCK_FREQ | clock frequency | @ref int32_t | + * | @ref POSHAL_DID_GPS_WKNROLLOVER | GPS week number | @ref SensorWknRollOverHal | + * | @ref POSHAL_DID_GPS_CONNECT_ERROR | GPS signal error(TRUE:Error, FALSE:Normal)| @ref BOOL | + * | @ref POSHAL_DID_GPS_TIME (_RAW) | GPS time information | @ref SENSOR_GPSTIME | + */ +typedef struct SensorMsgGpsdataDat { + u_int32 ul_did; //!< \~english data ID + u_int16 us_size; //!< \~english data size + u_int8 uc_rcv_flag; //!< \~english receive flag + u_int8 uc_sns_cnt; //!< \~english sensor count + u_int8 uc_gps_cnt_flag; //!< \~english GPS count + u_int8 reserve[3]; //!< \~english reserve + u_int8 uc_data[SENSOR_MSG_VSINFO_DSIZE]; //!< \~english data contents +} SENSOR_MSG_GPSDATA_DAT; + +/** + * @struct SensorMsgGpsdata + * \~english Struct of message about GPS data + */ +typedef struct SensorMsgGpsdata { + T_APIMSG_MSGBUF_HEADER st_head; + //!< \~english message header(see vs-positioning-base-library) + SENSOR_MSG_GPSDATA_DAT st_para; + //!< \~english data of message +} SENSOR_MSG_GPSDATA; + +/** + * @struct TG_GPS_RET_TIMESET_MSG + * \~english Struct of message about GPS time set response + */ +typedef struct { + T_APIMSG_MSGBUF_HEADER header; //!< \~english message header + uint64_t status; //!< \~english response status +} TG_GPS_RET_TIMESET_MSG; + +/** + * @struct TG_GPS_NMEA_SENTENCE_INFO + * \~english Struct of GPS NMEA sentence info + */ +typedef struct { + uint8_t uc_size; //!< \~english sentence size + uint8_t reserve; //!< \~english reserve + uint16_t us_offset; //!< \~english sentence start offset +} TG_GPS_NMEA_SENTENCE_INFO; + +/** + * @struct TG_GPS_NMEA_INFO + * \~english Struct of GPS NMEA info + */ +typedef struct { + uint32_t ul_rcvsts; //!< \~english NMEA receive info + uint8_t reserve[4]; //!< \~english reserve + TG_GPS_NMEA_SENTENCE_INFO st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_MAX]; + //!< \~english sentence infomation +} TG_GPS_NMEA_INFO; + +///////////////////////////////////////////////////////////////////////////////////// +/// \ingroup positioning_hal +/// \~english @par Brief +/// - start thread of NMEA analysis and delivery message +/// \~english @param [in] h_app +/// HANDLE - handle +/// +/// \~english @retval eFrameworkunifiedStatusOK Success +/// \~english @retval eFrameworkunifiedStatusFail Failed +/// +/// \~english @par Prerequisite +/// - None +/// \~english @par Change of internal state +/// - This API can not change internal state +/// \~english @par Conditions of processing failure +/// - None +/// \~english @par Detail +/// - start thread of NMEA analysis and delivery message +/// - the API can only be used by 1 process +/// \~english @par Classification +/// Public +/// \~english @par Type +/// Sync +/// \~english @see +/// None +///////////////////////////////////////////////////////////////////////////////////// +EFrameworkunifiedStatus StartGpsMainThreadPositioning(HANDLE h_app); + +///////////////////////////////////////////////////////////////////////////////////// +/// \ingroup positioning_hal +/// \~english @par Brief +/// - start thread for receive GPS manager +/// \~english @param [in] h_app +/// HANDLE - handle +/// +/// \~english @retval eFrameworkunifiedStatusOK Success +/// \~english @retval eFrameworkunifiedStatusFail Failed +/// +/// \~english @par Prerequisite +/// - None +/// \~english @par Change of internal state +/// - This API can not change internal state. +/// \~english @par Conditions of processing failure +/// - None +/// \~english @par Detail +/// - start model for receive GPS manager +/// - the API can only be used by 1 process +/// \~english @par Classification +/// Public +/// \~english @par Type +/// Sync +/// \~english @see +/// None +///////////////////////////////////////////////////////////////////////////////////// +EFrameworkunifiedStatus StartGpsRecvThreadPositioning(HANDLE h_app); + +///////////////////////////////////////////////////////////////////////////////////// +/// \ingroup positioning_hal +/// \~english @par Brief +/// - start thread for receive sensor +/// \~english @param [in] h_app +/// HANDLE - handle +/// +/// \~english @retval eFrameworkunifiedStatusOK Success +/// \~english @retval eFrameworkunifiedStatusFail Failed +/// +/// \~english @par Prerequisite +/// - None +/// \~english @par Change of internal state +/// - This API can not change internal state. +/// \~english @par Conditions of processing failure +/// - None +/// \~english @par Detail +/// - start thread for receive sensor +/// - the API can only be used by 1 process +/// \~english @par Classification +/// Public +/// \~english @par Type +/// Sync +/// \~english @see +/// None +///////////////////////////////////////////////////////////////////////////////////// +EFrameworkunifiedStatus StartLineSensorThreadPositioning(HANDLE h_app); + +///////////////////////////////////////////////////////////////////////////////////// +/// \ingroup positioning_hal +/// \~english @par Brief +/// - start monitoring thread of GPS rollover +/// \~english @param [in] h_app +/// HANDLE - handle +/// +/// \~english @retval eFrameworkunifiedStatusOK Success +/// \~english @retval eFrameworkunifiedStatusFail Failed +/// +/// \~english @par Prerequisite +/// - None +/// \~english @par Change of internal state +/// - This API can not change internal state. +/// \~english @par Conditions of processing failure +/// - The parameter hApp is NULL [eFrameworkunifiedStatusFail] +/// - When fail to start thread. [return value of FrameworkunifiedStartChildThread] +/// \~english @par Detail +/// - start monitoring thread of GPS rollover +/// - the API can only be used by 1 process +/// \~english @par Classification +/// Public +/// \~english @par Type +/// Sync +/// \~english @see +/// FrameworkunifiedCreateChildThread, FrameworkunifiedStartChildThread +///////////////////////////////////////////////////////////////////////////////////// +EFrameworkunifiedStatus StartGpsRolloverThreadPositioning(HANDLE h_app); + +/** @}*/ // end of positioning_hal +/** @}*/ // end of positioning + +/*---------------------------------------------------------------------------*/ +#endif // HAL_API_POSITIONING_HAL_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/inc/Common/LineSensDrv_Api.h b/inc/Common/LineSensDrv_Api.h new file mode 100644 index 00000000..24c7f558 --- /dev/null +++ b/inc/Common/LineSensDrv_Api.h @@ -0,0 +1,44 @@ +/* + * @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.h +*/ + +#ifndef INC_COMMON_LINESENSDRV_API_H_ +#define INC_COMMON_LINESENSDRV_API_H_ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "positioning_def.h" + +/*---------------------------------------------------------------------------*/ +// Define + +#define MUTEX_GPS_IRQ_FLG "MUTEX_GPS_IRQ_FLG" + +/*---------------------------------------------------------------------------*/ +// Prototype + +void DeliveryLineSensorDataPositioning(LSDRV_MSG_LSDATA_G*, u_int8); +BOOL LineSensDrvApiInitialize(void); +void LineSensDrvApiInitEstGpsCnt(void); + +/*---------------------------------------------------------------------------*/ +#endif // INC_COMMON_LINESENSDRV_API_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/inc/Common/MDev_Gps_API.h b/inc/Common/MDev_Gps_API.h new file mode 100644 index 00000000..d165a1e6 --- /dev/null +++ b/inc/Common/MDev_Gps_API.h @@ -0,0 +1,51 @@ +/* + * @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.h +*/ + +#ifndef INC_COMMON_MDEV_GPS_API_H_ +#define INC_COMMON_MDEV_GPS_API_H_ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "gps_hal.h" +#include "positioning_def.h" + +/*---------------------------------------------------------------------------*/ +// Prototype + +RET_API SendNmeaGps(const MDEV_GPS_NMEA* p_nmea_data); +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); +RET_API SendSpeedGps(const SENSORMOTION_SPEEDINFO_DAT* p_seed, u_int16 us_peed); +RET_API SendTimeGps(const MDEV_GPS_RTC* p_rtc); +RET_API SendClockDriftGps(int32_t drift); +RET_API SendClockFrequencyGps(uint32_t freq); +RET_API DevGpsSndWknRollover(const SensorWknRollOverHal* p_week_rollover); +int32 DevGpsRstAnsSend(PNO u_pno, RID uc_rid, u_int32 ul_rst_sts); +int32 DevGpsTimesetAnsSend(PNO u_pno, RID uc_rid, u_int32 ul_rst_sts); +RET_API DevSendGpsConnectError(BOOL); +RET_API SndGpsTimeRaw(const SENSOR_GPSTIME_RAW* pst_gpstime_raw); +/*---------------------------------------------------------------------------*/ +#endif // INC_COMMON_MDEV_GPS_API_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/inc/Common/positioning_common.h b/inc/Common/positioning_common.h new file mode 100644 index 00000000..dad0272e --- /dev/null +++ b/inc/Common/positioning_common.h @@ -0,0 +1,63 @@ +/* + * @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.h +*/ + +#ifndef INC_COMMON_POSITIONING_COMMON_H_ +#define INC_COMMON_POSITIONING_COMMON_H_ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "positioning_def.h" + +/*---------------------------------------------------------------------------*/ +// Definition + +/*---------------------------------------------------------------------------*/ +// ENUMERATION + +/*---------------------------------------------------------------------------*/ +// STRUCTURE + +typedef struct PosResetinfo { + uint8_t mode; // Reset mode + uint8_t reserve[3]; // reserve + PNO sndpno; // Caller PNO + PNO respno; // Destination PNO +} POS_RESETINFO; + +/*! + @brief Thread activation information +*/ +typedef struct StThreadSetupInfo { + EnumSetupMode_POS mode; // Thread activation mode +} ST_THREAD_SETUP_INFO; + + +/*---------------------------------------------------------------------------*/ +// Prototype +EFrameworkunifiedStatus PosInitialize(HANDLE h_app); +EnumSetupMode_POS PosSetupThread(HANDLE h_app, EnumTID_POS e_tid); +void PosTeardownThread(EnumTID_POS e_tid); +EFrameworkunifiedStatus PosCreateThread(HANDLE h_app, int8_t index); + +/*---------------------------------------------------------------------------*/ +#endif // INC_COMMON_POSITIONING_COMMON_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/inc/Common/positioning_def.h b/inc/Common/positioning_def.h new file mode 100644 index 00000000..df95cb3a --- /dev/null +++ b/inc/Common/positioning_def.h @@ -0,0 +1,307 @@ +/* + * @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_def.h +*/ + +#ifndef INC_COMMON_POSITIONING_DEF_H_ +#define INC_COMMON_POSITIONING_DEF_H_ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include <string.h> +#include <stdlib.h> +#include <sys/ioctl.h> + +#include <native_service/frameworkunified_types.h> +#include <native_service/frameworkunified_application.h> +#include <native_service/frameworkunified_framework_if.h> +#include <native_service/frameworkunified_multithreading.h> +#include <native_service/frameworkunified_service_protocol.h> +#include <vehicle_service/std_types.h> +#include <vehicle_service/pos_message_header.h> +#include <vehicle_service/sys_timerapi.h> +#include <vehicle_service/WPF_STD.h> + +#include "gps_hal.h" +#include "positioning_hal.h" + +#include "positioning_log.h" + +/*---------------------------------------------------------------------------*/ +// Definition + +#define POS_THREAD_NAME "Positioning" + +/** + * @enum ENUM_GPS_NMEA_INDEX + * \~english NMEA index + */ +typedef enum { + GPS_NMEA_INDEX_GGA = 0, //!< \~english GGA + GPS_NMEA_INDEX_DGGA, //!< \~english DGGA + GPS_NMEA_INDEX_VTG, //!< \~english VTG + GPS_NMEA_INDEX_RMC, //!< \~english RMC + GPS_NMEA_INDEX_DRMC, //!< \~english DRMC + GPS_NMEA_INDEX_GLL, //!< \~english GLL + GPS_NMEA_INDEX_DGLL, //!< \~english DGLL + GPS_NMEA_INDEX_GSA, //!< \~english GSA + GPS_NMEA_INDEX_GSV1, //!< \~english GSV1 + GPS_NMEA_INDEX_GSV2, //!< \~english GSV2 + GPS_NMEA_INDEX_GSV3, //!< \~english GSV3 + GPS_NMEA_INDEX_GSV4, //!< \~english GSV4 + GPS_NMEA_INDEX_GSV5, //!< \~english GSV5 + GPS_NMEA_INDEX_GST, //!< \~english GST + GPS_NMEA_INDEX__CWORD44__GP_3, //!< \~english _CWORD44_,GP,3 + GPS_NMEA_INDEX__CWORD44__GP_4, //!< \~english _CWORD44_,GP,4 + GPS_NMEA_INDEX_P_CWORD82_F_GP_0, //!< \~english _CWORD44_,GP,0 + GPS_NMEA_INDEX_P_CWORD82_J_GP_1, //!< \~english _CWORD44_,GP,1 + GPS_NMEA_INDEX_MAX //!< \~english GGA +} ENUM_GPS_NMEA_INDEX; + +//!< \~english Gps cycle Data Event +#define GPS_EVT_CYCLEDATA_PROV ("GPS_EVT_CYCLEDATA_PROV") +//!< \~english Event value: Waiting. +#define GPS_EVT_PROV_WAIT (0x00000000) +//!< \~english Event value: Clear Waiting status. +#define GPS_EVT_PROV_RELEASE (0x00000001) + +//!< \~english Gps received +#define GPS_CYCLECMD_NOTRCV (0x00) +//!< \~english Gps no received +#define GPS_CYCLECMD_RCV (0x01) + +//!< \~english store position of Hour data +#define GPSCMD_DRMC_HOUR_POS (7) +//!< \~english store position of minute data +#define GPSCMD_DRMC_MIN_POS (9) +//!< \~english store position of second data +#define GPSCMD_DRMC_SEC_POS (11) +//!< \~english store position of status +#define GPSCMD_DRMC_STATUS_POS (14) +//!< \~english store position of day data +#define GPSCMD_DRMC_DAY_POS (54) +//!< \~english store position of month data +#define GPSCMD_DRMC_MONTH_POS (56) +//!< \~english store position of year data +#define GPSCMD_DRMC_YEAR_POS (58) + +//!< \~english store position of control code +#define GPSCMD_RTC_CTRLCODE_POS (0) +//!< \~english store position of year data (BCD) +#define GPSCMD_RTC_BCD_YEAR_POS (1) +//!< \~english store position of month data(BCD) +#define GPSCMD_RTC_BCD_MONTH_POS (2) +//!< \~english store position of day data(BCD) +#define GPSCMD_RTC_BCD_DAY_POS (3) +//!< \~english store position of hour data(BCD) +#define GPSCMD_RTC_BCD_HOUR_POS (4) +//!< \~english store position of minute data(BCD) +#define GPSCMD_RTC_BCD_MIN_POS (5) +//!< \~english store position of second data(BCD) +#define GPSCMD_RTC_BCD_SEC_POS (6) +//!< \~english store position of status +#define GPSCMD_RTC_STATUS_POS (7) + +#define GPSCMD_RTC_CTRLCODE (0xA4) //!< \~english control code (RTC) +#define GPSCMD_RTC_STATUS_UTCTIME (0x00) //!< \~english Normal +#define GPSCMD_RTC_STATUS_ERRTIME (0x03) //!< \~english AbNormal + +// Internal thread activation status determination +#define THREAD_STS_MSK_POS_MAIN (0x01) +#define THREAD_STS_MSK_POS_SENS (0x02) +#define THREAD_STS_MSK_POS_GPS (0x04) +#define THREAD_STS_MSK_POS_GPS_RECV (0x08) +#define THREAD_STS_MSK_POS_GPS_ROLLOVER (0x10) +#define POS_DID_GPS_NMEA 0x8000009AU /* QAC 1281 */ +#define POS_DID_GPS_CLOCK_DRIFT 0x800000B3U /* QAC 1281 */ +#define POS_DID_GPS_CLOCK_FREQ 0x800000B4U /* QAC 1281 */ +#define VEHICLE_DID_GPS_CUSTOMDATA 0x80000094U +#define VEHICLE_DID_GPS_CLOCK_DRIFT POS_DID_GPS_CLOCK_DRIFT +#define VEHICLE_DID_GPS_CLOCK_FREQ POS_DID_GPS_CLOCK_FREQ +#define CID_NAVIINFO_DELIVER (0x0205) /* Navigation information setting CID */ +#define CID_NAVIINFO_SPEED_DELIVER (0x0206) /* Vehicle speed setting CID */ +#define VEHICLE_DID_GPS_WKNROLLOVER 0x800000B2U +#define VEHICLE_DID_GPS_TIME_RAW 0x800000B1 +#define POS_DID_GYRO_TEMP 0x80000090 +#define VEHICLE_DID_SPEED_PULSE 0x80000012 +#define VEHICLE_DID_GYRO_X 0x80000014 +#define VEHICLE_DID_GYRO_Y 0x80000020 +#define VEHICLE_DID_GYRO_Z 0x80000021 +#define VEHICLE_DID_GYRO VEHICLE_DID_GYRO_X +#define VEHICLE_DID_REV 0x80000017 +#define VEHICLE_DID_SNS_COUNTER 0x8000001A +#define VEHICLE_DID_GPS_COUNTER 0x8000001B +#define VEHICLE_DID_GYRO_EXT 0x80000027 /* 3 ~ 14bit A/D value,0bit:REV */ +#define VEHICLE_DID_SPEED_PULSE_FST 0x80000028 /* Pulse(Number) */ +#define VEHICLE_DID_GYRO_FST_X 0x80000029 +#define VEHICLE_DID_GYRO_FST_Y 0x80000022 +#define VEHICLE_DID_GYRO_FST_Z 0x80000023 +#define VEHICLE_DID_GYRO_FST VEHICLE_DID_GYRO_FST +#define VEHICLE_DID_GYRO_TEMP POS_DID_GYRO_TEMP +#define VEHICLE_DID_SPEED_KMPH 0x80000013 +#define VEHICLE_DID_GSNS_X 0x80000015 +#define VEHICLE_DID_GSNS_Y 0x80000016 +#define VEHICLE_DID_GSNS_Z 0x80000024 +#define VEHICLE_DID_PULSE_TIME 0x8000003A +#define CID__CWORD83__CMD_RCV 0x0103 +#define CID__CWORD83__CMD_SND_STS 0x0104 +#define CID_GPS_SERIAL0 (CID)0x0100 +#define CID_GPS_REQRESET (CID)(CID_GPS_BASE | CID_GPS_SERIAL0) + +#define VEHICLE_CONFIG_GPS_UNITSET_NOGPS (0x00U) //!< \~english no Gps +#define VEHICLE_CONFIG_GPS_UNITSET__CWORD82_ (0x01U) //!< \~english _CWORD82_ Gps +#define VEHICLE_CONFIG_GPS_UNITSET_EXTBOX (0x02U) //!< \~english extern u-blox + +//!< \~english no Navi +#define VEHICLE_CONFIG_NAVI_FUNCTION_NONAVI (0x00U) +//!< \~english Navi has AW navi +#define VEHICLE_CONFIG_NAVI_FUNCTION_AWNAVI (0x01U) + +#define VEHICLE_GPS_SYSTIME_YEAR_MAX (2085U) //!< \~english Gps Time allowed max year +#define VEHICLE_GPS_SYSTIME_MONTH_MAX (12U) //!< \~english Gps Time allowed max month +#define VEHICLE_GPS_SYSTIME_DAY_MAX (31U) //!< \~english Gps Time allowed max day +#define VEHICLE_GPS_SYSTIME_HOUR_MAX (23U) //!< \~english Gps Time allowed max hour +#define VEHICLE_GPS_SYSTIME_MINUTE_MAX (59U) //!< \~english Gps Time allowed max minute +#define VEHICLE_GPS_SYSTIME_SECOND_MAX (59U) //!< \~english Gps Time allowed max second + +#define VEHICLE_GPS_SYSTIME_YEAR_MIN (1986U) //!< \~english Gps Time allowed min year +#define VEHICLE_GPS_SYSTIME_MONTH_MIN (1U) //!< \~english Gps Time allowed min month +#define VEHICLE_GPS_SYSTIME_DAY_MIN (1U) //!< \~english Gps Time allowed min day +#define VEHICLE_GPS_SYSTIME_HOUR_MIN (0U) //!< \~english Gps Time allowed min hour +#define VEHICLE_GPS_SYSTIME_MINUTE_MIN (0U) //!< \~english Gps Time allowed min minute +#define VEHICLE_GPS_SYSTIME_SECOND_MIN (0U) //!< \~english Gps Time allowed min second +#define VEHICLE_EVENT_VAL_INIT (-14) /* Event initial value */ +//!< \~english check OK (value not change) +#define MDEV_GPS_SRAM_CHK_OK (0) +//!< \~english check OK(vaule change) +#define MDEV_GPS_SRAM_CHK_CHG (1) +//!< \~english check NG +#define MDEV_GPS_SRAM_CHK_NG (-1) + +/*---------------------------------------------------------------------------*/ +// ENUMERATION + +/*! + @brief Positioning operating status definitions +*/ +typedef enum EnumExeStsPos { + EPOS_EXE_STS_STOP = 0, /* Stopped */ + EPOS_EXE_STS_RUNNING, /* Running (from FrameworkunifiedOnStart to FrameworkunifiedOnStop) */ + EPOS_EXE_STS_RUNNING_COLDSTART /* Running after cold start */ +} EnumExeSts_POS; + +/*! + @brief Positioning Thread Startup Modes +*/ +typedef enum EnumSetupModePos { + EPOS_SETUP_MODE_NORMAL = 0, /* Normal start */ + EPOS_SETUP_MODE_DATA_RESET /* Data reset start */ /* QAC 930 */ +} EnumSetupMode_POS; + +/** + * @struct TG_GPS_RCV_DATA + * \~english Struct of Gps receive data for VehicleSens + */ +typedef struct TgGpsRcvData { + uint32_t dwret_status; //!< \~english return status + uint16_t bydata_len; //!< \~english data length + uint8_t bygps_data[GPS_READ_LEN]; //!< \~english receive data + uint8_t u_reserve2[2]; //!< \~english reserve +} TG_GPS_RCV_DATA; + +/** + * @struct TG_GPS_NMEA_DAT + * \~english Struct of NMEA data + */ +typedef struct TgGpsNmeaDat { + u_int8 uc_data[GPS_NMEA_MAX_SZ]; //!< \~english NMEA data +} TG_GPS_NMEA_DAT; + +/** + * @struct TG_GPS_CYCLEDATA_NMEA + * \~english Struct of NMEA cycle data + */ +typedef struct TgGpsCycledataNmea { + //!< \~english receive flag + u_int8 uc_rcvflag[GPS_NMEA_INDEX_MAX]; + //!< \~english reserve + u_int8 u_reserve[3]; + //!< \~english structure of NMEA data + TG_GPS_NMEA_DAT st_nmea[GPS_NMEA_INDEX_MAX]; +} TG_GPS_CYCLEDATA_NMEA; + +/** + * @struct TG_GPS_CYCLEDATA_BINARY + * \~english Struct of binary cycle data + */ +typedef struct TgGpsCycledataBinary { + u_int8 uc_rcvflag; //!< \~english receive flag + u_int8 u_reserve[3]; //!< \~english reserve + u_int8 uc_data[GPS_CMD_BINARY_SZ]; //!< \~english binary data + u_int8 u_reserve2[3]; //!< \~english reserve flag +} TG_GPS_CYCLEDATA_BINARY; + +/** + * @struct TG_GPS_CYCLEDATA_FULLBIN + * \~english Struct of full bin cycle data + */ +typedef struct TgGpsCycledataFullbin { + u_int8 uc_rcvflag; //!< \~english receive flag + u_int8 u_reserve[3]; //!< \~english reserve + u_int8 uc_data[GPS_CMD_FULLBIN_SZ]; //!< \~english fullbin data + u_int8 u_reserve2; //!< \~english reserve +} TG_GPS_CYCLEDATA_FULLBIN; + +/** + * @struct TG_GPS_CYCLEDATA_RTC + * \~english Struct of RTC cycle data + */ +typedef struct TgGpsCycledataRtc { + u_int8 uc_rcvflag; //!< \~english receive flag + u_int8 u_reserve[3]; //!< \~english reserve + u_int8 uc_data[GPS_DATASIZE_RTC]; //!< \~english Rtc data +} TG_GPS_CYCLEDATA_RTC; + +/** + * @struct TG_GPS_CYCLEDATA + * \~english Struct of Gps cycle data + */ +typedef struct TgGpsCycledata { + u_int8 uc_sesncnt; //!< \~english count value + u_int8 u_reserve[3]; //!< \~english reserve + TG_GPS_CYCLEDATA_NMEA st_nmea_data; //!< \~english NMEA Data + TG_GPS_CYCLEDATA_BINARY st_binary_data; //!< \~english binary Data + TG_GPS_CYCLEDATA_FULLBIN st_fullbin_data; //!< \~english Full Bin Data + TG_GPS_CYCLEDATA_RTC st_rtc_data; //!< \~english RTC Data +} TG_GPS_CYCLEDATA; + +typedef struct { + u_int16 year; /* YEAR */ + u_int8 month; /* MONTH */ + u_int8 date; /* DAY */ + u_int8 hour; /* HOUR */ + u_int8 minute;/* MINUTE */ + u_int8 second;/* SECOND */ + u_int8 flag; /* Whether or not the time is set */ +} ST_GPS_SET_TIME; + +/*---------------------------------------------------------------------------*/ +#endif // INC_COMMON_POSITIONING_DEF_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/inc/Common/positioning_log.h b/inc/Common/positioning_log.h new file mode 100644 index 00000000..6ca4c96e --- /dev/null +++ b/inc/Common/positioning_log.h @@ -0,0 +1,43 @@ +/* + * @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_log.h +*/ + +#ifndef INC_COMMON_POSITIONING_LOG_H_ +#define INC_COMMON_POSITIONING_LOG_H_ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include <stdio.h> + +/*---------------------------------------------------------------------------*/ +// Definition + +// #define POSITIONING_DEBUG + +#ifdef POSITIONING_DEBUG +#define POSITIONING_LOG(...) printf(__VA_ARGS__) +#else +#define POSITIONING_LOG(...) +#endif + +/*---------------------------------------------------------------------------*/ +#endif // INC_COMMON_POSITIONING_LOG_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/inc/GpsCommon/MDev_GpsRecv.h b/inc/GpsCommon/MDev_GpsRecv.h new file mode 100644 index 00000000..6ea47877 --- /dev/null +++ b/inc/GpsCommon/MDev_GpsRecv.h @@ -0,0 +1,170 @@ +/* + * @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.h +*/ + +#ifndef INC_GPSCOMMON_MDEV_GPSRECV_H_ +#define INC_GPSCOMMON_MDEV_GPSRECV_H_ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "gps_hal.h" +#include "positioning_def.h" + +/*---------------------------------------------------------------------------*/ +// Define + +/* ++ GPS _CWORD82_ support */ +#define GPS__CWORD82__CMD_LEN_MAX 200 +/* -- GPS _CWORD82_ support */ + +/* ++ GPS _CWORD82_ support */ +#define GPS__CWORD82__RET_START_SEQ_NONE 0 /* _CWORD82_ Start Sequence Determination Result = None */ +#define GPS__CWORD82__RET_START_SEQ_NMEA 1 /* _CWORD82_ Start Sequence Determination Result = NMEA */ +#define GPS__CWORD82__RET_START_SEQ_FULL 2 /* _CWORD82_ Start Sequence Determination Result = Full customization */ +#define GPS__CWORD82__RET_START_SEQ_BIN 3 /* _CWORD82_ Start Sequence Determination Result = Standard binary #GPF_60_024 */ + +#define GPS_UBLOX_RET_START_SEQ_UBX 4 /* Ublox Start Sequence Decision Result = UBX Protocol (binary) */ +#define GPS__CWORD82__FULLBINARY_LEN GPS_CMD_FULLBIN_SZ /* _CWORD82_ Full Customization Instruction Length */ + +#define GPS__CWORD82__NORMALBINARY_LEN 81 /* _CWORD82_ standard binary instruction length #GPF_60_024 */ +/* -- GPS _CWORD82_ support */ + +/* Maximum size of serial data read */ +#define GPS_RCV_MAXREADSIZE (2048) /* Changed from 256 -> 2048 #01 */ + +/* Receive data storage buffer size */ +#define GPS_RCV_RCVBUFSIZE 512 /* Maximum size of the command to be received must be larger than the maximum size */ + +/* Max-size receive commands ID4:(188+8)byte */ +/* Analysis data buffer size */ +#define GPS_RCV_ANADATABUFSIZE (GPS_RCV_MAXREADSIZE + GPS_RCV_RCVBUFSIZE) + +/* Receive frame buffer size */ +#define GPS_RCV_RCVFRAMEBUFSIZE GPS_RCV_RCVBUFSIZE /* Maximum size of the command to be received must be larger than the maximum size */ + +/* Reception notification data buffer size */ +#define GPS_RCV_SNDCMD_SIZE_MAX + +/* Common return values */ +#define GPS_RCV_RET_ERR_EXT (-4) /* External cause error */ +#define GPS_RCV_RET_ERR_SYSTEM (-3) /* System error */ +#define GPS_RCV_RET_ERR (-1) /* Abnormality */ +#define GPS_RCV_RET_NML 0 /* Normal */ + +// Reception thread reception frame detection result +#define GPS_RCV_FRMSRCH_ERR_FORMAT (-1) /* Frame format error */ +#define GPS_RCV_FRMSRCH_FIXED 0 /* Frame determination */ +#define GPS_RCV_FRMSRCH_NOT_FIXED 1 /* Frame undetermined */ +#define GPS_RCV_FRMSRCH_NO_DATA 2 /* No analysis data */ + +// Result of receive thread receive frame analysis +#define GPS_RCV_FRMANA_ERR_PRM (-3) /* Parameter error */ +#define GPS_RCV_FRMANA_ERR_FORMAT (-2) /* Frame format error */ +#define GPS_RCV_FRMANA_ERR_CS (-1) /* Sumcheck error */ +#define GPS_RCV_FRMANA_OK 0 /* Normal data */ + +/* Serial reception event determination result */ +enum GpsRcvRet { + GPS_RCV_REVT_NML = 0, /* Receiving(Normal) */ + GPS_RCV_REVT_ERR, /* Receiving(Error) */ + GPS_RCV_REVT_NUM /* Number of events received */ +}; + +// Error logging +#define ERLG_SER_FRAMEERR "Serial Framing Error" +#define ERLG_SER_OVERRUNERR "Serial Overrun Error" +#define ERLG_SER_PARITYERR "Serial Parity Error" +#define ERLG_RCVEV_JDGERR "DEV_Gps_Recv_JdgSerEvt error" +#define ERLG_RCVDATA_SNDERR "DEV_Gps_Recv_SndRcvData error" +#define ERLG_COMEVNT_WAITERR "WaitCommEvent error" + +/*---------------------------------------------------------------------------*/ +// Struct + +// Serial receive buffer structure +typedef struct GPSRecvData { + u_int16 us_read_size; /* Serial data read data size */ + u_int8 uc_reserve[2]; /* Preliminary */ + u_int8 uc_read_data[GPS_RCV_MAXREADSIZE]; /* Serial read data */ +} TG_GPS_RECV_RcvData; + +// Receive data storage buffer structure +typedef struct GPSRecvBuf { + u_int16 r_size; /* Received size */ + u_int8 rsv[2]; /* Reserved */ + u_int8 r_buf[GPS_RCV_RCVBUFSIZE]; /* Receive command data */ +} TG_GPS_RECV_RcvBuf; + +// Data analysis buffer +typedef struct GPSRecvAnaDataBuf { + u_int16 datsize; /* Size of the data */ + u_int16 offset; /* Analysis start offset */ + u_int8 datbuf[GPS_RCV_ANADATABUFSIZE]; /* Analysis data */ +} TG_GPS_RECV_AnaDataBuf; + +// Receive frame buffer +typedef struct GPSRecvFrameBuf { + u_int16 size; /* Frame-size */ + u_int8 rsv[2]; /* Reserved */ + u_int8 buf[GPS_RCV_RCVFRAMEBUFSIZE]; /* Command data */ +} TG_GPS_RECV_RcvFrameBuf; + +/*---------------------------------------------------------------------------*/ +// External variable + +extern TG_GPS_RECV_RcvData g_gps_rcvdata; /* Serial receive buffer */ +extern TG_GPS_RECV_RcvBuf g_gps_rcvbuf; /* Receive data storage buffer */ +extern TG_GPS_RECV_AnaDataBuf g_gps_ana_databuf; /* Analysis data buffer */ +extern TG_GPS_RECV_RcvFrameBuf g_gps_rcv_framebuf; /* Receive frame buffer */ +extern u_int16 g_wrecv_err; /* Number of reception errors */ + +/*---------------------------------------------------------------------------*/ +// Prototype + +/* Initializing process */ +void DevGpsRecvInit(void); + +/* Buffer clear processing */ +void DevGpsRecvClrRcvBuf(void); + +/* Receive (normal) receive processing */ +void DevGpsRecvRcvNormal(void); + +/* Data reception processing */ +int32 DevGpsRecvReadRcvData(TG_GPS_RECV_RcvData* pst_rcv_data); + +/* Frame detection processing */ +int32 DevGpsRecvSearchFrameData(const TG_GPS_RECV_AnaDataBuf *adbuf_p, u_int16 *ana_size); + +/* Receive data transmission processing */ +void DevGpsRecvSndRcvData(const TG_GPS_RECV_RcvFrameBuf *p_frame_buf); + +/* Acknowledgement */ +RET_API DevRcvData(const TG_GPS_RCV_DATA* ptg_rcv_data); + +/* Pos_Gps_Recv Thread Stop Processing */ +void DevGpsRecvThreadStopProcess(void); + +EFrameworkunifiedStatus DevGpsRecvThread(HANDLE h_app); + +/*---------------------------------------------------------------------------*/ +#endif // INC_GPSCOMMON_MDEV_GPSRECV_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/inc/GpsCommon/MDev_GpsRollOver.h b/inc/GpsCommon/MDev_GpsRollOver.h new file mode 100644 index 00000000..22da3c9c --- /dev/null +++ b/inc/GpsCommon/MDev_GpsRollOver.h @@ -0,0 +1,34 @@ +/* + * @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.h +*/ +#ifndef INC_GPSCOMMON_MDEV_GPSROLLOVER_H_ +#define INC_GPSCOMMON_MDEV_GPSROLLOVER_H_ + +#include <vehicle_service/WPF_STD.h> + +typedef struct TimRolovrYMD { + u_int16 year; + u_int16 month; + u_int16 day; + u_int8 dummy[2]; +} TG_TIM_ROLOVR_YMD; + + +void GPSRollOverConvTime(TG_TIM_ROLOVR_YMD* base_ymd, TG_TIM_ROLOVR_YMD* conv_ymd, u_int8 gpsweekcorcnt); + +#endif // INC_GPSCOMMON_MDEV_GPSROLLOVER_H_ diff --git a/inc/GpsCommon/MDev_Gps_Common.h b/inc/GpsCommon/MDev_Gps_Common.h new file mode 100644 index 00000000..78128580 --- /dev/null +++ b/inc/GpsCommon/MDev_Gps_Common.h @@ -0,0 +1,187 @@ +/* + * @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 +*/ + +#ifndef INC_GPSCOMMON_MDEV_GPS_COMMON_H_ +#define INC_GPSCOMMON_MDEV_GPS_COMMON_H_ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "positioning_def.h" +#include "MDev_Gps_Main.h" + +/*---------------------------------------------------------------------------*/ +// Define + +#define NMEA_CHECKSUM_CHAR '*' /* *## (HEX) */ +#define NMEA_DATA_SEPARATOR ',' + +#define GPS_NMEA_FIELD_LEN_MAX (GPS_NMEA_MAX_SZ) + +// NMEA GGA Sentence +/* Fix Status */ +#define GPS_NMEA_GGA_FIX_STATUS_NON 0 /* invalid */ +#define GPS_NMEA_GGA_FIX_STATUS_GPS 1 /* GPS SPS Mode, Fix valid */ +#define GPS_NMEA_GGA_FIX_STATUS_DGPS 2 /* DGPS SPS Mode, Fix valid */ +#define GPS_NMEA_GGA_FIX_STATUS_DR 6 /* Estimated/Dead Reckoning */ +/* Field No */ +#define GPS_NMEA_FNO_GGA_FS 6 +#define GPS_NMEA_FNO_GGA_MSL 9 + +/* number of SV Fields */ +#define GPS_NMEA_NUM_GSA_SV 12 +/* Fix Status */ +#define GPS_NMEA_GSA_FIX_STS_NON 1 +#define GPS_NMEA_GSA_FIX_STS_2D 2 +#define GPS_NMEA_GSA_FIX_STS_3D 3 +/* Field No */ +#define GPS_NMEA_FNO_GSA_FS 2 +#define GPS_NMEA_FNO_GSA_SV 3 + +/* number of Satellite Infomation Fields (sv+elv+az+cno) */ +#define GPS_NMEA_NUM_GSV_SINFO 4 + /* Field No */ +#define GPS_NMEA_FNO_GSV_NOSV 3 +#define GPS_NMEA_FNO_GSV_SV 4 +#define GPS_NMEA_FNO_GSV_ELV 5 +#define GPS_NMEA_FNO_GSV_AZ 6 +#define GPS_NMEA_FNO_GSV_CNO 7 + +#define GPS_NMEA_RMC_STS_INVALID 'V' +#define GPS_NMEA_RMC_STS_VALID 'A' +#define GPS_NMEA_RMC_IND_NORTH 'N' +#define GPS_NMEA_RMC_IND_EAST 'E' + +/* Field No */ +#define GPS_NMEA_FNO_RMC_UTC 1 +#define GPS_NMEA_FNO_RMC_STATUS 2 +#define GPS_NMEA_FNO_RMC_LATITUDE 3 +#define GPS_NMEA_FNO_RMC_NS 4 +#define GPS_NMEA_FNO_RMC_LONGITUDE 5 +#define GPS_NMEA_FNO_RMC_EW 6 +#define GPS_NMEA_FNO_RMC_SPEED 7 +#define GPS_NMEA_FNO_RMC_COG 8 +#define GPS_NMEA_FNO_RMC_DATE 9 + +// Diag Fix Count +#define GPS_DIAG_FIX_COUNT_3D (0x01) +#define GPS_DIAG_FIX_COUNT_2D (0x02) +#define GPS_DIAG_FIX_COUNT_ELSE (0x04) + +/* GPS time setting related */ +#define GPS_SETTIME_RANGE 5 /* 5 seconds : Valid range of GPS time setting */ +#define GPS_TIME_RANGE 10 /* 10 seconds : GPS time change range */ + +/* NAV-SVINFO logging status */ +#define GPS_NAVSVINFO_STS_INIT 0 +#define GPS_NAVSVINFO_STS_READY 1 +#define GPS_NAVSVINFO_STS_DONE 2 +#define GPS_NAVSVINFO_STS_SETTING 3 + +/* Time setting information */ +#define GPS_TIME_RX 0 /* Time not set */ +#define GPS_TIME_ROLOVR 1 /* First GPS reception(for rollover) */ +#define GPS_TIME_SET 2 /* Time setting in progress */ + +/*! + @brief Satellite position information +*/ +typedef struct GpsSatelliteInfoStr { + uint8_t sv; /* Satellite number */ + uint8_t elv; /* Height 0-99 */ + uint16_t az; /* Orientation 0-359 */ + uint8_t cno; /* Signal strength */ + uint8_t sts; /* Reception status */ +} GpsSatelliteInfo; + +/*---------------------------------------------------------------------------*/ +// Global values + +extern BOOL g_gps_rcv_thread_stop; +extern uint8_t g_is_gpstime_sts; + +/*---------------------------------------------------------------------------*/ +// Prototype + +void ChangeStatusGpsCommon(u_int32 sts); +void RtyResetGpsCommon(void); +void SendRtyResetGpsCommon(void); +void SendReqGpsCommon(void); +void GPSResetReqGpsCommon(void); +void CyclDataTimeOutGpsCommon(void); +RET_API CheckFrontStringPartGpsCommon(const u_char *p_tartget, const u_char *p_start); +int32 JudgeFormatGpsCommon(u_char *p_send_data, u_int32 ul_length, TG_GPS_OUTPUT_FORMAT *p_format); +RET_API CheckSumGpsCommon(const u_int8 p_uc_data[], u_int32 ul_length, TG_GPS_OUTPUT_FORMAT e_cmd_info); +u_int8 AtoHGpsCommon(u_int8 ascii); +RET_API DevGpsSaveCmd(TG_GPS_SAVECMD *p_send_data); +void SendSaveCmdGpsCommon(void); +void DeleteSaveCmdGpsCommon(void); +void DeleteAllSaveCmdGpsCommon(void); +void RcvCyclCmdNmeaGpsCommon(u_int8 *p_uc_data, u_int32 ul_len, TG_GPS_OUTPUT_FORMAT s_output_format); +int32 CheckSendCmdGpsCommon(const u_char *p_rcv_data, u_int32 ul_length, TG_GPS_OUTPUT_FORMAT *p_format); +int32_t GetStringFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src, char* p_dst, size_t size); +double GetNumericFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src, BOOL *p_valid); +int32 GetIntegerFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src); +int32 GetLonLatFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src, BOOL *p_valid); +u_int16 GetHeadingFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src, BOOL *p_valid); +int32 GetAltitudeFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src, BOOL *p_valid); +u_int16 GetSpeedFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src, BOOL *p_valid); +void StopThreadGpsCommon(void); +uint8_t JudgeFormatOrderGpsCommon(TG_GPS_OUTPUT_FORMAT s_format_1, TG_GPS_OUTPUT_FORMAT s_format_2); +ENUM_GPS_NMEA_INDEX GetNmeaIdxGpsCommon(TG_GPS_OUTPUT_FORMAT s_output_format); +void SendResetReqGpsCommon(void); + + +void WriteGpsTimeToBackup(uint8_t flag, POS_DATETIME* pst_datetime); +void DevGpsCommSettingTime(void); +RET_API GpsSetPosBaseEvent(uint16_t snd_pno, int32_t event_val); +uint8_t HardResetChkGpsCommon(void); +void RcvCyclCmdExtGpsCommon(u_int8 *p_data, u_int32 len, TG_GPS_OUTPUT_FORMAT format); + +BOOL DevGpsRcvProcGpsResetResp(TG_GPS_RCV_DATA*); +void DevGpsRcvProcTimeSetResp(TG_GPS_RCV_DATA*); +void DevGpsRcvProcWknRolloverGetResp(TG_GPS_RCV_DATA* p_data); +void DevGpsRcvProcNavSvinfoAddResp(TG_GPS_RCV_DATA* p_data); +void DevGpsReadGpsTime(POS_DATETIME* p_st_datetime); +RET_API DevGpsRcvProcCmnResp(TG_GPS_RCV_DATA* p_data, uint8_t cmd); +RET_API DevGpsResetReq(PNO us_pno, RID uc_rid); +void DevGpsSettingTime(POS_DATETIME* p_st_datetime); +int DevGpsYMD2JD(int y, int m, int d); +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); +void DevGpsSetChkSum(u_int8* uc_buffer, u_int32 ul_length); +RET_API DevGpsCatPollReq(uint8_t *p_ublox_data, uint16_t kind); +void DevGpsSetGpsweekcorcnt(void); +RET_API DevGpsNavTimeUTCAddReq(void); +RET_API DevGpsWknRolloverGetReq(void); +BOOL DevGpsCheckGpsTime(uint16_t set_gpsw, uint32_t set_gpsws, + uint16_t rcv_gpsw, uint32_t rcv_gpsws, int32_t offset_range); + +void DevGpsCnvLonLatNavi(SENSORLOCATION_LONLATINFO_DAT* p_lonlat, + u_int8 fix_sts, int32 lon, int32 lat); +void DevGpsCnvAltitudeNavi(SENSORLOCATION_ALTITUDEINFO_DAT* p_altitude, + u_int8 fix_sts, int32 alt); +void DevGpsCnvHeadingNavi(SENSORMOTION_HEADINGINFO_DAT* p_heading, + u_int8 fix_sts, u_int16 heading); +/*---------------------------------------------------------------------------*/ +#endif // INC_GPSCOMMON_MDEV_GPS_COMMON_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/inc/GpsCommon/MDev_Gps_Main.h b/inc/GpsCommon/MDev_Gps_Main.h new file mode 100644 index 00000000..d40cb0ca --- /dev/null +++ b/inc/GpsCommon/MDev_Gps_Main.h @@ -0,0 +1,311 @@ +/* + * @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.h +*/ + +#ifndef INC_GPSCOMMON_MDEV_GPS_MAIN_H_ +#define INC_GPSCOMMON_MDEV_GPS_MAIN_H_ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "gps_hal.h" +#include "positioning_def.h" + +// #include "gps_hal.h" +// #include "MDev_Gps_Nmea.h" +// #include "LineSensDrv_Api.h" + +/*---------------------------------------------------------------------------*/ +// Value define + +#define OFF 0 /* OFF */ +#define ON 1 /* ON */ +#define OK 0 /* Normal */ +#define NG (-1L) /* Abnormality */ +#define DAT_END 0xff /* Exit code */ + +// State +typedef enum GpsMatrixSts { + GPS_STS_STARTUP = 0, /* Startup confirmation */ + GPS_STS_NORMAL, /* In operation */ + GPS_STS_SENT, /* Command sent status */ + GPS_STS_NUM +}TG_GPS_MATRIX_STS; + +// Event +typedef enum GpsMatrixEvt { + GPS_EVT_SENDREQ = 0, /* Transmission request */ + GPS_EVT_RESETREQ, /* GPS reset request */ + GPS_EVT_RECVCYCLDAT, /* Cyclic reception command reception */ + GPS_EVT_RECVRSPDAT, /* Receive response command */ + GPS_EVT_TIMEOUT_RSPDAT, /* Response monitoring timeout */ + GPS_EVT_TIMEOUT_CYCLDAT, /* Periodic reception data monitoring timeout */ + GPS_EVT_TIMEOUT_NAVI, /* Navigation providing data monitoring timeout */ + GPS_EVT_TIMEOUT_DIAGCLKGUARD, /* Diagnosis provision time guard monitoring timeout*/ + GPS_EVT_ACC_OFF, /* In-function ACC-OFF instructions */ + GPS_EVT_NAVI_LOCATIONINFO, /* Providing navigation information */ + GPS_EVT_NAVI_SPEEDINFO, /* Navigation speed information service */ + GPS_EVT_TIMESETTING, /* GPS time setting instruction */ + GPS_EVT_TIMEOUT_NMEADATAGUARD, /* NMEA data-providing guard monitoring timeout*/ + GPS_EVT_BACKUPDATA_LOAD, /* Backup data read request */ + GPS_EVT_STOPREQ, /* Thread stop request */ + GPS_EVT_WEEKCOR_CNT_NOTIFICATIO, /* GPS Week Adjustment Counter Notification */ + GPS_EVT_TIMEOUT_RECOVERY, /* GPS error monitoring reset timer */ + GPS_EVT_TIMEOUT_RECEIVERERR, /* GPS receiver anomaly detection timeout */ + GPS_EVT_NUM +} TG_GPS_MATRIX_EVT; + +typedef enum GpsOutputFormat { + GPS_FORMAT_MIN = -1, /* Minimum Receive Format(Initialization) */ + GPS_FORMAT_BINARY, /* Standard binary */ + GPS_FORMAT_FULLBIN, /* Full binary */ + GPS_FORMAT_GGA, /* GGA */ + GPS_FORMAT_DGGA, /* DGGA */ + GPS_FORMAT_VTG, /* VTG */ + GPS_FORMAT_RMC, /* RMC */ + GPS_FORMAT_DRMC, /* DRMC */ + GPS_FORMAT_GLL, /* GLL */ + GPS_FORMAT_DGLL, /* DGLL */ + GPS_FORMAT_GSA, /* GSA */ + GPS_FORMAT_GSV1, /* GSV(1) */ + GPS_FORMAT_GSV2, /* GSV(2) */ + GPS_FORMAT_GSV3, /* GSV(3) */ + GPS_FORMAT_GSV4, /* GSV(4) */ + GPS_FORMAT_GSV5, /* GSV(5) */ + GPS_FORMAT_GST, /* GST */ + GPS_FORMAT__CWORD44__GP3, /* _CWORD44_,GP,3 */ + GPS_FORMAT__CWORD44__GP4, /* _CWORD44_,GP,4 */ + GPS_FORMAT_P_CWORD82_F_GP0, /* P_CWORD82_F,GP,0 */ + GPS_FORMAT_P_CWORD82_J_GP1, /* P_CWORD82_J,GP,1 */ + GPS_FORMAT_P_CWORD82_I_GP, /* P_CWORD82_I,GP */ + GPS_FORMAT_P_CWORD82_E_GP0, /* P_CWORD82_E,GP,0 */ + GPS_FORMAT_P_CWORD82_J_GP0, /* P_CWORD82_J,GP,0 */ + GPS_FORMAT_P_CWORD82_E_GP2, /* P_CWORD82_E,GP,2 */ + GPS_FORMAT_P_CWORD82_G_GP0, /* P_CWORD82_G,GP,0 */ + GPS_FORMAT_P_CWORD82_J_GP7, /* P_CWORD82_J,GP,7 */ + GPS_FORMAT_P_CWORD82_J_GP8, /* P_CWORD82_J,GP,8 */ + GPS_FORMAT_MON_VER, /* MON-VER */ + GPS_FORMAT_AID_INI, /* AID-INI */ + GPS_FORMAT_ACK_ACKNACK, /* ACK-ACKNACK */ + GPS_FORMAT_NAV_TIMEUTC, /* NAV-TIMEUTC */ + GPS_FORMAT_NAV_CLOCK, /* NAV-CLOCK */ + GPS_FORMAT_RXM_RTC5, /* RXM-RTC5 */ + GPS_FORMAT_NAV_SVINFO, /* NAV-SVINFO */ + GPS_FORMAT_CFG_NAVX5, /* CFG-NAVX5 */ + GPS_FORMAT_NMEA, /* NMEA (Sentence unspecified) */ + GPS_FORMAT_UBX, /* UBX Protocol (binary) */ + GPS_FORMAT_MAX /* Maximum Receive Format */ +} TG_GPS_OUTPUT_FORMAT; + +// Return value +#define RETRY_OFF 0 /* No retry */ +#define RETRY_ON 1 /* Retry exists */ +#define RETRY_OUT 2 /* Retry out */ +#define NG_RETRYOUT 4 /* Transmission failure retry out */ + +#define GPSRET_NOPCMD (0) /* Non notification target command #GPF_60_024 */ +#define GPSRET_SNDCMD (1) /* Notification target command #GPF_60_024 */ +#define GPSRET_CMDERR (-1) /* No applicable command #GPF_60_024 */ + +// Number of Retries +#define SRSET_MAX 4 /* Maximum serial reset retry value */ +#define HRSET_MAX 3 /* Maximum value of hard reset retry */ +#define SNDNG_MAX 4 /* Maximum retry value of transmission failure */ + +/* The actual number of retries is SNDNG_MAX-1. */ +#define GPS_RECV_ERR_MAX 5 /* Maximum number of reception errors #GPF_60_024 */ + +// Others +#define SAV_MAX 11 +#define GPS_ALTITUDE_INVALID_VAL (-1000000) /* Value when fix altitude is invalid */ +#define GPS_HEADING_INVALID_VAL 36100 /* Value when measurement orientation is invalid */ + +#define GPS_RECVOK 0 /* Normal reception #13 */ +#define GPS_RECVNG 1 /* Reception error #13 */ +#define GPS_OVERRUN 2 /* Overrun detection #13 */ +#define GPS_PARITY 3 /* Parity detection #13 */ +#define GPS_FRAMING 4 /* Framing detection #13 */ + +// For the communication management thread +#define GPS_NON_RECV 0 /* Not received */ +#define GPS_RECV_ACK 1 /* ACK received */ +#define GPS_RECV_DATA 2 /* Receive response command */ + +#define GPS_SEND_ERR_MAX 5 /* Maximum number of transmission errors */ +#define GPS_CNCT_ERR_MAX 5 /* Maximum number of connection errors */ + +#define GPS_START_EVT "GPS_EVT" /* Interthread synchronization events */ +#define GPS_EVT_VAL 99 /* Event settings */ +#define GPS_MSGDAT_BUF_SZ 500 /* Receive message buffer size */ + +// Definition for receive command analysis table #GPF_60_024 +#define SENTENCE_STR_MAX 12 /* Maximum string for judging sentence */ + +#define SENSOR_MSG_VSINFO_DSIZE 1904 //!< \~english message body max size + +/*---------------------------------------------------------------------------*/ +// Structure + +/******************************************************************************** + * TAG :TG_GPS_MNG + * ABSTRACT :GPS process management information + * NOTE : + ********************************************************************************/ +typedef struct GpsMng { + u_int32 sts; /* Matrix state */ + u_int32 event; /* Matrix event code */ + u_int32 rcvsts; /* Response reception status */ + u_int32 sndcnt; /* Number of transmission attempts */ + u_int32 hrsetcnt; /* Number of tries before hard reset */ + u_int32 sndngcnt; /* Number of failed-to-send attempts */ + TG_GPS_OUTPUT_FORMAT rcv_cmd; /* Types of received GPS commands */ + TG_GPS_OUTPUT_FORMAT resp_cmd; /* Types of GPS commands during response monitoring */ + PNO resp_pno; /* Destination PNO */ + RID resp_rid; /* RID to be notified when responding */ + u_int8 resp_rst_flag; /* Response command reset flag */ + BOOL rcv_err_flag; /* GPS receiver error detection flag */ +} TG_GPS_MNG; + +/******************************************************************************** + * TAG :TG_GPS_SAVECMD + * ABSTRACT :GPS process pending command + * NOTE : + ********************************************************************************/ +typedef struct GpsSaveCmd { + TG_GPS_SND_DATA sndcmd; /* Sending commands(_CWORD82_ command) */ + PNO us_pno; /* Result notification destination process number(Unused) */ + RID uc_rid; /* Result Notification Resource ID(Unused) */ + u_int8 uc_rst; /* Reset flag */ + TG_GPS_OUTPUT_FORMAT e_cmd_info; /* Command information */ +} TG_GPS_SAVECMD; + +/******************************************************************************** + * TAG :TG_GPS_SAVEBUF + * ABSTRACT :GPS process pending buffer management information + * NOTE : + ********************************************************************************/ +typedef struct GpsSaveBuf { + u_int32 saveno; /* Current pending index number */ + u_int32 sendno; /* Current send index number */ + u_int32 bufsav; /* Number of pending buffers used */ + TG_GPS_SAVECMD savebuf[SAV_MAX]; /* Pending buffer */ +} TG_GPS_SAVEBUF; + +/******************************************************************************** + * TAG :TG_GPS_MSGRCV + * ABSTRACT :Receive message + * NOTE : + ********************************************************************************/ +typedef struct GpsMsgRcv { + T_APIMSG_MSGBUF_HEADER header; /* Message header */ + u_int8 msgdat[SENSOR_MSG_VSINFO_DSIZE + 12]; +} TG_GPS_MSGRCV; + +/******************************************************************************** + * TAG :TG_GPS_MSGEVTCHNG + * ABSTRACT :GPS message event translation table + * NOTE : + ********************************************************************************/ +typedef struct GpsMsgEvtChng { + u_int32 cid; /* Matrix state */ + u_int32 event; /* Matrix event code */ +} TG_GPS_MSGEVTCHNG; + +/******************************************************************************** + * TAG :TG_GPS_TIMEVTCHNG + * ABSTRACT :GPS timer ID and event translation table + * NOTE : + ********************************************************************************/ +typedef struct GpsTimEvtChng { + u_int16 tim_id; /* Timer ID information */ + u_int16 reserve; /* Reserved */ + u_int32 event; /* Matrix event code */ +} TG_GPS_TIMEVTCHNG; + +/******************************************************************************** + * TAG :Gps_Jmp_Tbl + * ABSTRACT :GPS jump table + * NOTE : + ********************************************************************************/ +typedef struct GpsJmpTbl { + void (*func)( void ); /* Jump-to module */ +} TG_GPS_JMP_TBL; + +/******************************************************************************** + * TAG :TG_GPS_CMD_ANA_TBL + * ABSTRACT :Receive command analysis table structure + * NOTE : + ********************************************************************************/ +typedef struct TgGpsCmdAnaTbl { + u_char c_sentence[SENTENCE_STR_MAX]; /* Sentence identifier */ + u_int32 ul_length; /* Length of the command */ + u_int32 ul_rcv_kind; /* Receiving type */ + BOOL b_snd_cmd_flg; /* Command notification flag */ + TG_GPS_OUTPUT_FORMAT e_rcv_format; /* Receive Format */ +} TG_GPS_CMD_ANA_TBL; + +/******************************************************************************** + * TAG :TG_GPS_TIME_BCD + * ABSTRACT :Time information (BCD) structure + * NOTE : + ********************************************************************************/ +typedef struct GpsTimeBCD { + u_int8 uc_year; /* Year information */ + u_int8 uc_month; /* Month information */ + u_int8 uc_day; /* Day information */ + u_int8 uc_hour; /* Time information */ + u_int8 uc_min; /* Minute information */ + u_int8 uc_sec; /* Second information */ +} TG_GPS_TIME_BCD; + +/*---------------------------------------------------------------------------*/ +// Global values + +#define MDEV_GPSMSGCHKC_MAX 10 +#define MDEV_PSTIMCHKC_MAX 11 +extern const TG_GPS_MSGEVTCHNG kGpsMsgchkC[MDEV_GPSMSGCHKC_MAX]; /* Ignore -> MISRA-C:2004 Rule 8.12 */ +extern const TG_GPS_TIMEVTCHNG kGpsTimchkC[MDEV_PSTIMCHKC_MAX]; /* Ignore -> MISRA-C:2004 Rule 8.12 */ +extern const TG_GPS_JMP_TBL kGpsMatxC[GPS_STS_NUM][GPS_EVT_NUM]; + +#define MDEV_GPSCMDANATBLNMEA_MAX 20 +extern const TG_GPS_CMD_ANA_TBL* kGpsCmdAnaTbl; +extern const TG_GPS_CMD_ANA_TBL kGpsCmdAnaTblUblox[MDEV_GPSCMDANATBLNMEA_MAX]; + +extern TG_GPS_SAVEBUF g_gps_save_cmdr; +extern TG_GPS_MSGRCV g_gps_msg_rcvr; +extern TG_GPS_MNG g_gps_mngr; + +extern u_int16 g_wsend_err; /* Number of transmission errors */ +extern u_int16 g_wcnct_err; /* Number of connection errors */ +extern TG_GPS_OUTPUT_FORMAT g_rcv_format; /* Receive Format */ + +/*---------------------------------------------------------------------------*/ +// Prototype + +EFrameworkunifiedStatus DevGpsMainThread(HANDLE h_app); +BOOL DevGpsInit(void); +void DevGpsRcvMsg(void); +void DevGpsMsgEventCheck(void); +void DevGpsTimEventCheck(void); +void DevGpsGpsVersionCheck(void); + +/*---------------------------------------------------------------------------*/ +#endif // INC_GPSCOMMON_MDEV_GPS_MAIN_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/inc/GpsCommon/MDev_Gps_Mtrx.h b/inc/GpsCommon/MDev_Gps_Mtrx.h new file mode 100644 index 00000000..610b8d42 --- /dev/null +++ b/inc/GpsCommon/MDev_Gps_Mtrx.h @@ -0,0 +1,90 @@ +/* + * @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.h +*/ + +#ifndef INC_GPSCOMMON_MDEV_GPS_MTRX_H_ +#define INC_GPSCOMMON_MDEV_GPS_MTRX_H_ + +/*---------------------------------------------------------------------------*/ +// Include files + +/*---------------------------------------------------------------------------*/ +// Prototype + +void DevGpsNop(void); +void DevGpsInitStartSendReq(void); +void DevGpsInitStartGPSResetReq(void); +void DevGpsInitStartRcvCyclCmd(void); +void DevGpsInitStartCyclDataTimeOut(void); +void DevGpsInitStartNaviDataTimeOut(void); /* #GPF_60_077 */ +void DevGpsInitStartDiagClkGuardTimeOut(void); /* #GPF_60_077 */ +void DevGpsInitStartAccOffStart(void); /* #GPF_60_024 */ +void DevGpsInitStartNaviInfoDeliver(void); /* #GPF_60_077 */ +void DevGpsInitStartNaviSpeedDeliver(void); /* #GPF_60_077 */ +void DevGpsInitStartSettingTime(void); /* #GPF_60_077 */ +void DevGpsInitStartNmeaDataGuardTimeOut(void); /* #GPF_60_109 */ + +void DevGpsInitStartBackupDataLoadReq(void); +void DevGpsInitStartStopReq(void); +void DevGpsInitStartGpsweekcorcntNtf(void); +void DevGpsInitStartRecoveryTimeOut(void); +void DevGpsInitStartGpsReceiverErrTimeOut(void); + +void DevGpsNormalSendReq(void); +void DevGpsNormalGPSResetReq(void); +void DevGpsNormalRcvCyclCmd(void); +void DevGpsNormalCyclDataTimeOut(void); +void DevGpsNormalNaviDataTimeOut(void); /* #GPF_60_077 */ +void DevGpsNormalDiagClkGuardTimeOut(void); /* #GPF_60_077 */ +void DevGpsNormalAccOffStart(void); /* #GPF_60_024 */ +void DevGpsNormalNaviInfoDeliver(void); /* #GPF_60_077 */ +void DevGpsNormalNaviSpeedDeliver(void); /* #GPF_60_077 */ +void DevGpsNormalSettingTime(void); /* #GPF_60_077 */ +void DevGpsNormalNmeaDataGuardTimeOut(void); /* #GPF_60_109 */ + +void DevGpsNormalBackupDataLoadReq(void); +void DevGpsNormalStopReq(void); +void DevGpsNormalGpsweekcorcntNtf(void); +void DevGpsNormalRecoveryTimeOut(void); +void DevGpsNormalGpsReceiverErrTimeOut(void); + +void DevGpsSendSendReq(void); +void DevGpsSendGPSResetReq(void); +void DevGpsSendRcvCyclCmd(void); +void DevGpsSendRcvRspCmd(void); /* #GPF_60_040 */ +void DevGpsSendRspDataTimeOut(void); /* #GPF_60_040 */ +void DevGpsSendCyclDataTimeOut(void); +void DevGpsSendNaviDataTimeOut(void); /* #GPF_60_077 */ +void DevGpsSendDiagClkGuardTimeOut(void); /* #GPF_60_077 */ +void DevGpsSendAccOffStart(void); /* #GPF_60_040 */ +void DevGpsSendNaviInfoDeliver(void); /* #GPF_60_077 */ +void DevGpsSendNaviSpeedDeliver(void); /* #GPF_60_077 */ +void DevGpsSendSettingTime(void); /* #GPF_60_077 */ +void DevGpsSendNmeaDataGuardTimeOut(void); /* #GPF_60_109 */ + +void DevGpsSendBackupDataLoadReq(void); +void DevGpsSendStopReq(void); +void DevGpsSendGpsweekcorcntNtf(void); +void DevGpsSendRecoveryTimeOut(void); +void DevGpsSendGpsReceiverErrTimeOut(void); + +/*---------------------------------------------------------------------------*/ +#endif // INC_GPSCOMMON_MDEV_GPS_MTRX_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/inc/GpsCommon/MDev_Gps_Nmea.h b/inc/GpsCommon/MDev_Gps_Nmea.h new file mode 100644 index 00000000..a19e3d9a --- /dev/null +++ b/inc/GpsCommon/MDev_Gps_Nmea.h @@ -0,0 +1,313 @@ +/* + * @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_GpsUblox.h +*/ + +#ifndef INC_GPSCOMMON_MDEV_GPS_NMEA_H_ +#define INC_GPSCOMMON_MDEV_GPS_NMEA_H_ + +/*---------------------------------------------------------------------------*/ + +#include "gps_hal.h" +#include "positioning_def.h" + +/*---------------------------------------------------------------------------*/ +// Value define + +#define TEST_NMEA (0) /* 0:Normal mode 1:Testing mode */ + +#define GPS_MAX_NUM_VISIBLE_SATELLITES (20) /* Maximum Number of Visible Satellites *Assume the GSV1~GSV5 */ + +/* UBX command *****************************/ + +/* CFG */ +#define UBX_CMD_SIZE_CFG_RST (12) /* size of CFG-RST of UBX sentences */ +#define UBX_CMD_SIZE_CFG_MSG (11) /* size of CFG-MSG of UBX sentences */ +#define UBX_CMD_SIZE_CFG_NAV5 (44) /* size of CFG-NAV5 of UBX sentences */ +#define UBX_CMD_SIZE_CFG_NAV5_POLL (8) +#define UBX_CMD_SIZE_CFG_NAV5_PLUS_POLL (UBX_CMD_SIZE_CFG_NAV5 + UBX_CMD_SIZE_CFG_NAV5_POLL) +#define UBX_CMD_SIZE_CFG_NAVX5_POLL (8) +#define UBX_CMD_OFFSET_WKNROLLOVER (18) /* offset from the top of 'payload' */ + +/* MON */ +#define UBX_CMD_SIZE_MON_VER (108) /* size of MON-VER of UBX sentences */ +#define UBX_CMD_SIZE_MON_VER_POLL (8) +#define UBX_CMD_SIZE_MON_VER_SW_VER (30) + +/* AID */ +#define UBX_CMD_SIZE_AID_INI (56) /* size of AID-INI of UBX sentences */ +#define UBX_CMD_SIZE_AID_INI_POLL (8) +#define UBX_CMD_SIZE_AID_INI_PLUS_POLL (UBX_CMD_SIZE_AID_INI + UBX_CMD_SIZE_AID_INI_POLL) + +/* ACK */ +#define UBX_CMD_OFFSET_ACKNAK (3) /* offset to Ack/Nack Infomation */ + +/* NAV */ +#define UBX_CMD_MSK_NAV_TIMEUTC_VALID_TOW (1) /* mask bit for 'validTow' of NAV-TIMEUTC */ +#define UBX_CMD_MSK_NAV_TIMEUTC_VALID_WKN (2) /* mask bit for 'validKwn' of NAV-TIMEUTC */ +#define UBX_CMD_MSK_NAV_TIMEUTC_VALID_UTC (4) /* mask bit for 'validUtc' of NAV-TIMEUTC */ + +/* Common */ +#define UBX_CMD_SIZE_HDR (2) /* size of 'header' of UBX sentence */ +#define UBX_CMD_SIZE_ID (2) /* size of 'id' of UBX sentence */ +#define UBX_CMD_SIZE_LEN (2) /* size of 'length' of UBX sentence */ +#define UBX_CMD_SIZE_MAX (400) /* size of NAV-SVINFO (numCh=32) */ +#define UBX_CMD_OFFSET_PAYLOAD (UBX_CMD_SIZE_HDR + UBX_CMD_SIZE_ID + UBX_CMD_SIZE_LEN) + /* offset to 'payload' of UBX sentence */ + +/* Types of UBX Poll Request Commands ************/ +#define UBX_POLL_CMD_KIND_AID_INI (1) +#define UBX_POLL_CMD_KIND_CFG_NAV5 (2) + +// GPS-command (_CWORD82_ NMEA) related definitions #GPF_60_024 +/* Command identification character string */ +#define GPS_CMD_NMEA_GGA ("$GPGGA") /* GGA/double precision GGAs */ +#define GPS_CMD_NMEA_VTG ("$GPVTG") /* VTG */ +#define GPS_CMD_NMEA_RMC ("$GPRMC") /* RMC/double precision RMCs */ +#define GPS_CMD_NMEA_GLL ("$GPGLL") /* GLL/double precision GLL */ +#define GPS_CMD_NMEA_GSA ("$GPGSA") /* GSA */ +#define GPS_CMD_NMEA_GSV_1 ("$GPGSV,*,1") /* GSV(1) */ +#define GPS_CMD_NMEA_GSV_2 ("$GPGSV,*,2") /* GSV(2) */ +#define GPS_CMD_NMEA_GSV_3 ("$GPGSV,*,3") /* GSV(3) */ +#define GPS_CMD_NMEA__CWORD44__GP_3 ("$_CWORD44_,GP,3") /* _CWORD44_,GP,3 */ +#define GPS_CMD_NMEA__CWORD44__GP_4 ("$_CWORD44_,GP,4") /* _CWORD44_,GP,4 */ +#define GPS_CMD_NMEA_P_CWORD82_F_GP_0 ("$P_CWORD82_F,GP,0") /* P_CWORD82_F,GP,0 */ +#define GPS_CMD_NMEA_P_CWORD82_J_GP_1 ("$P_CWORD82_J,GP,1") /* P_CWORD82_J,GP,1 */ +#define GPS_CMD_NMEA_P_CWORD82_I_GP ("$P_CWORD82_I,GP") /* P_CWORD82_I,GP */ +#define GPS_CMD_NMEA_P_CWORD82_E_GP_0 ("$P_CWORD82_E,GP,0") /* P_CWORD82_E,GP,0 */ +#define GPS_CMD_NMEA_P_CWORD82_J_GP_0 ("$P_CWORD82_J,GP,0") /* P_CWORD82_J,GP,0 */ +#define GPS_CMD_NMEA_P_CWORD82_E_GP_2 ("$P_CWORD82_E,GP,2") /* P_CWORD82_E,GP,2 */ +#define GPS_CMD_NMEA_P_CWORD82_G_GP_0 ("$P_CWORD82_G,GP,0") /* P_CWORD82_G,GP,0 */ +#define GPS_CMD_NMEA_P_CWORD82_J_GP_7 ("$P_CWORD82_J,GP,7") /* P_CWORD82_J,GP,7 */ +#define GPS_CMD_NMEA_P_CWORD82_J_GP_8 ("$P_CWORD82_J,GP,8") /* P_CWORD82_J,GP,8 */ +#define GPS_CMD_NMEA_GST ("$GPGST") /* GST */ +#define GPS_CMD_NMEA_GSV_4 ("$GPGSV,*,4") /* GSV(4) */ +#define GPS_CMD_NMEA_GSV_5 ("$GPGSV,*,5") /* GSV(5) */ + +/*------------------------------------------------------------------------------- + * GPS-command (_CWORD82_ Binary) related definitions #GPF_60_024 + -------------------------------------------------------------------------------*/ +#define GPS_CMD_BINARY (0xC6) /* Standard binary */ +#define GPS_CMD_FULLBIN (0xB0) /* FULL binaries */ + +#define ENDMARK "ENDENDEND" /* Table termination */ +#define GPSCMDANATBL_MAX 25 /* Maximum number of elements in table */ +#define RCV_CYCLE 0x01 /* Reception type cycle reception command*/ +#define RCV_RESP 0x02 /* Reception type response command */ + +// Length of FULLBINARY commands +#define GPS_CMD_FULLBIN1_SZ 30 +#define GPS_CMD_FULLBIN2_SZ 160 +#define GPS_CMD_FULLBIN3_SZ 25 +#define GPS_CMD_FULLBIN4_SZ 26 +#define GPS_CMD_FULLBIN5_SZ 14 +#define GPS_CMD_FULLBIN6_SZ 61 + +/* Start offset of FULLBINARY command */ +#define GPS_CMD_FULLBIN1_OFFSET 0 +#define GPS_CMD_FULLBIN2_OFFSET (GPS_CMD_FULLBIN1_OFFSET + GPS_CMD_FULLBIN1_SZ) +#define GPS_CMD_FULLBIN3_OFFSET (GPS_CMD_FULLBIN2_OFFSET + GPS_CMD_FULLBIN2_SZ) +#define GPS_CMD_FULLBIN4_OFFSET (GPS_CMD_FULLBIN3_OFFSET + GPS_CMD_FULLBIN3_SZ) +#define GPS_CMD_FULLBIN5_OFFSET (GPS_CMD_FULLBIN4_OFFSET + GPS_CMD_FULLBIN4_SZ) +#define GPS_CMD_FULLBIN6_OFFSET (GPS_CMD_FULLBIN5_OFFSET + GPS_CMD_FULLBIN6_SZ) + + +/* SUM value offset of FULLBINARY commands */ +#define GPS_CMD_FULLBIN1_SUMOFFSET (GPS_CMD_FULLBIN1_OFFSET + GPS_CMD_FULLBIN1_SZ - 2) +#define GPS_CMD_FULLBIN2_SUMOFFSET (GPS_CMD_FULLBIN2_OFFSET + GPS_CMD_FULLBIN2_SZ - 2) +#define GPS_CMD_FULLBIN3_SUMOFFSET (GPS_CMD_FULLBIN3_OFFSET + GPS_CMD_FULLBIN3_SZ - 2) +#define GPS_CMD_FULLBIN4_SUMOFFSET (GPS_CMD_FULLBIN4_OFFSET + GPS_CMD_FULLBIN4_SZ - 2) +#define GPS_CMD_FULLBIN5_SUMOFFSET (GPS_CMD_FULLBIN5_OFFSET + GPS_CMD_FULLBIN5_SZ - 2) +#define GPS_CMD_FULLBIN6_SUMOFFSET (GPS_CMD_FULLBIN6_OFFSET + GPS_CMD_FULLBIN6_SZ - 2) +#define GPS_CMD_FULLBIN_MASK 0x8F + +#define GPS_CMD_RESET 1 /* Reset request command */ +#define GPS_CMD_TIMESET 2 /* Time setting request command */ +#define GPS_CMD_VERSION 3 /* GPS version request command */ +#define GPS_CMD_SENTENCEADD_NMEAGST 4 /* Add sentence command NMEA GST */ +#define GPS_CMD_SENTENCEADD_NAVTIMEUTC 5 /* Add sentence command NAV-TIMEUTC */ +#define GPS_CMD_SENTENCEADD_NAVCLOCK 6 /* Add sentence command NAV-CLOCK */ +#define GPS_CMD_SENTENCEADD_RXMRTC5 7 /* Add sentence command RXM-RTC5 */ +#define GPS_CMD_SENTENCEADD_NAVSVINFO 8 /* Add sentence command NAV-SVINFO */ +#define GPS_CMD_AUTOMOTIVEMODE 9 /* Automatic mode request command */ +#define GPS_CMD_WKNROLLOVER 10 /* GPS rollover standard week number request command */ +#define GPS_CMD_NOTRST 0 /* Other request command */ + +/*---------------------------------------------------------------------------*/ +// Struct + +/*! + @brief UBX command headers +*/ +typedef struct GpsUbxPacketHeader { + u_int8 uc_sync_char1; /* u-blox starts with 2 Bytes 0xB5 0x62 */ + u_int8 uc_sync_char2; /* u-blox starts with 2 Bytes 0xB5 0x62 */ + u_int8 uc_class; /* Class */ + u_int8 uc_id; /* ID */ + u_int16 us_length; /* Length */ +} TG_GPS_UBX_PACKET_HEADER; + +/*! + @brief Command-structure without UBX data +*/ +typedef struct GpsUbxCmdNoData { + TG_GPS_UBX_PACKET_HEADER header; + u_int8 uc_cka; /* Checksum_A */ + u_int8 uc_ckb; /* Checksum_B */ +} TG_GPS_UBX_COMMAND_NO_DATA; + +/*! + @brief +*/ +typedef struct GpsUbxAidIniPolled { + u_int8 uc_sync_char1; /* u-blox starts with 2 Bytes 0xB5 0x62 */ + u_int8 uc_sync_char2; /* u-blox starts with 2 Bytes 0xB5 0x62 */ + u_int8 uc_class; /* Class */ + u_int8 uc_id; /* ID */ + u_int8 uc_length[2]; /* Length */ + u_int8 uc_ecefx_or_lat[4]; /* WGS84 ECEF X coordinate or latitude */ + u_int8 uc_ecefy_or_lon[4]; /* WGS84 ECEF Y coordinate or longitude */ + u_int8 uc_ecefz_or_alt[4]; /* WGS84 ECEF Z coordinate or altitude */ + u_int8 uc_pos_acc[4]; /* Position accuracy (stddev) */ + u_int8 uc_tm_cfg[2]; /* Time mark configuration */ + u_int8 wn[2]; /* Actual week number */ + u_int8 tow[4]; /* Actual time of week */ + u_int8 uc_tow_ns[4]; /* Sub-millisecond part of time of week */ + u_int8 uc_tacc_ms[4]; /* Milliseconds part of time accuracy */ + u_int8 uc_tacc_ns[4]; /* Nanoseconds part of time accuracy */ + u_int8 uc_clk_d_or_freq[4]; /* Clock drift or frequency */ + u_int8 uc_clk_dacc_or_freqacc[4]; /* Accuracy of clock drift or frequency */ + u_int8 uc_flags[4]; /* Bitmask with the following flags */ + u_int8 uc_cka; /* Checksum_A */ + u_int8 uc_ckb; /* Checksum_B */ +} TG_GPS_UBX_AID_INI_POLLED; + +/*! + @brief UBX ACK command data structures +*/ +typedef struct GpsUbxAckData { + uint8_t uc_msg_class; /* Message Class */ + uint8_t uc_msg_id; /* Message Identifier */ +} TG_GPS_UBX_ACK_DATA; + +/*! + @brief UBX NAV-TIMEUTC command data structures +*/ +typedef struct GpsUbxNavUTC { + uint32_t ul_itow; /* GPS time of week */ + uint32_t ul_tacc; /* Time accuracy estimate (UTC) */ + int32_t nano; /* Fraction of second */ + uint16_t year; /* Year, range 1999..2099 (UTC) */ + uint8_t month; /* Month, range 1..12 (UTC) */ + uint8_t day; /* Day of month, range 1..31 (UTC) */ + uint8_t hour; /* Hour of day, range 0..23 (UTC) */ + uint8_t min; /* Minute of hour, range 0..59 (UTC) */ + uint8_t sec; /* Seconds of minute, range 0..60 (UTC) */ + uint8_t valid; /* Validity Flags */ +} TG_GPS_UBX_NAV_TIMEUTC_DATA; + +/*! + @brief USB NAV-CLOCK command data structures +*/ +typedef struct GpsUbxNavClock { + uint32_t ul_itow; /**< GPS Time of week */ + int32_t l_clkb; /**< Clock bias */ + int32_t l_clkd; /**< Clock drift */ + uint32_t ul_tacc; /**< Time accuaracy estimate */ + uint32_t ul_facc; /**< Frequency accuracy estimate */ +} TG_GPS_UBX_NAV_CLOCK; + +/*! + @brief GPS RXM-RTC5 command data structures +*/ +typedef struct GpsUbxRxmRtc5 { + uint32_t ul_rtag_hw; /**< RTAG high word */ + uint32_t ul_rtag_lw; /**< RTAG low word */ + uint32_t freq; /**< Clock frequency */ + uint32_t ul_freq_frac; /**< Clock frequency fractional part */ + uint32_t ul_tow_frac; /**< Time of week fractional part */ + uint32_t tow; /**< Time of week */ + uint16_t wno; /**< GPS week number */ + uint8_t uc_tow_valid; /**< TOW is valid flag */ + uint8_t uc_freq_valid; /**< Frequency is valid flag */ +} TG_GPS_UBX_RXM_RTC5; + +/******************************************************************************** + * TAG :TG_GPS_RCVDATA_NMEA + * ABSTRACT :Cyclic data (NMEA) area + * NOTE :I/F information between communication management thread and vehicle sensor + ********************************************************************************/ +typedef struct GpsRcvDataNMEA { + u_int8 uc_rcv_flag[GPS_NMEA_INDEX_MAX]; /* Receive flag */ + u_int8 u_reserve[3]; /* Reserved */ + TG_GPS_NMEA_DAT st_nmea[GPS_NMEA_INDEX_MAX]; /* NMEA data area */ +} TG_GPS_RCVDATA_NMEA; + +/******************************************************************************** + * TAG :TG_GPS_RCVDATA_BINARY + * ABSTRACT :Cyclic data (standard binary) area + * NOTE :I/F information between communication management thread and vehicle sensor + ********************************************************************************/ +typedef struct GpsRcvDataBinary { + u_int8 uc_rcv_flag; /* Receive flag */ + u_int8 u_reserve[3]; /* Reserved */ + u_int8 uc_data[GPS_CMD_BINARY_SZ]; /* Standard binary data area */ + u_int8 u_reserve2[3]; /* Reserved */ +} TG_GPS_RCVDATA_BINARY; + +/******************************************************************************** + * TAG :TG_GPS_RCVDATA_FULLBIN + * ABSTRACT :Cyclic data (full binary) area + * NOTE :I/F information between communication management thread and vehicle sensor + ********************************************************************************/ +typedef struct GpsRcvDataFullbin { + u_int8 uc_rcv_flag; /* Receive flag */ + u_int8 u_reserve[3]; /* Reserved */ + u_int8 uc_data[GPS_CMD_FULLBIN_SZ]; /* Full binary data area */ + u_int8 u_reserve2; /* Reserved */ +} TG_GPS_RCVDATA_FULLBIN; + +/******************************************************************************** + * TAG :TG_GPS_RCVDATA + * ABSTRACT :Cyclic data storage area + * NOTE :I/F information between communication management thread and vehicle sensor + ********************************************************************************/ +typedef struct GpsRcvData { + u_int8 uc_sns_cnt; /* Sensor counter value */ + u_int8 u_reserve[3]; /* Reserved */ + TG_GPS_RCVDATA_NMEA st_nmea_data; /* NMEA data area */ + TG_GPS_RCVDATA_BINARY st_binary_data; /* Standard binary region */ + TG_GPS_RCVDATA_FULLBIN st_fullbin_data; /* Full binary data area */ +} TG_GPS_RCVDATA; + +/*---------------------------------------------------------------------------*/ +// Prototype + +void DevGpsSndCycleDataNmea(void); +void DevGpsRcvCyclCmd(void); +u_int8 DevGpsGetGpsRcvSts(u_int8 sv); +void DevGpsAnalyzeNmea(NAVIINFO_ALL* navilocinfo); +void DevGpsCycleDataClear(void); +void DevGpsCycleDataSetNmea(const u_int8*, u_int32, ENUM_GPS_NMEA_INDEX); +BOOL DevGpsCycleDataGetNmea(u_int8*, u_int32, ENUM_GPS_NMEA_INDEX); +void DevGpsCmdEventCheckNmea(void); +void DevGpsSetChkSum(u_int8* buffer, u_int32 length); +/*---------------------------------------------------------------------------*/ +#endif // INC_GPSCOMMON_MDEV_GPS_NMEA_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/inc/GpsCommon/MDev_Gps_TimerCtrl.h b/inc/GpsCommon/MDev_Gps_TimerCtrl.h new file mode 100644 index 00000000..96616efe --- /dev/null +++ b/inc/GpsCommon/MDev_Gps_TimerCtrl.h @@ -0,0 +1,113 @@ +/* + * @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.h +*/ + +#ifndef INC_GPSCOMMON_MDEV_GPS_TIMERCTRL_H_ +#define INC_GPSCOMMON_MDEV_GPS_TIMERCTRL_H_ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "positioning_def.h" + +/*---------------------------------------------------------------------------*/ +// Value define + +/* GPS-related timer value */ +#define TIMVAL_GPS_STARTUP 500 /* 5Sec Start confirmation monitoring timer */ +#define TIMVAL_GPS_RCVCYCLDAT 500 /* 5Sec Periodic reception data monitoring timer */ +#define TIMVAL_GPS_RCVACK 500 /* 5Sec ACK reception monitoring timer */ +#define TIMVAL_GPS_RCVDAT 500 /* 5Sec Data reception monitoring timer(Unused) */ +#define TIMVAL_GPS_NAVIFST 3000 /* 30sec Initial navigation monitoring timer */ +#define TIMVAL_GPS_NAVICYCLE 300 /* 3sec Navigation monitoring timer */ +#define TIMVAL_GPS_NAVIDISRPT 1000 /* 10Sec Navigation monitoring disruption log output timer */ +#define TIMVAL_GPS_DIAGCLKGUARD 1000 /* 10sec Diagnosis provision time guard monitoring timer */ +#define TIMVAL_GPS_NMEADATAGUARD 1000 /* 10sec NMEA data-providing guard monitoring timer */ +#define TIMVAL_GPS_RECOVERY 60000 /* 600sec GPS recovery timer */ +#define TIMVAL_GPS_RECEIVERERR 60000 /* 600sec GPS receiver anomaly detection timer */ + +/* Sensor-related timer value */ +#define TIMVAL_SNS_RCVFSTDAT 3000 /* 30Sec Initial sensor data reception monitoring timer */ +#define TIMVAL_SNS_RCVCYCLDAT 300 /* 3Sec Cyclic sensor data reception monitoring timer */ +#define TIMVAL_SNS_RCVDISRPT 1000 /* 10Sec Cyclic sensor data interruption log output timer */ + +/* Timer management table */ +#define TIM_NON 0x00 /* Timer counter initial value */ +#define TIM_CNTMIN 0x01 /* Timer counter minimum value */ +#define TIM_CNTMAX 0xff /* Maximum value of timer counter */ +#define TIMER_OFF 0 /* Timer enable flag OFF */ +#define TIMER_ON 1 /* Timer enable flag ON */ + +/*! + @brief Timer type + */ +typedef enum GpsTimKind { + GPS_STARTUP_TIMER = 0, /* 0 Start confirmation monitoring timer */ + GPS_CYCL_TIMER, /* 1 Cyclic GPS data reception monitoring timer */ + GPS_RECV_ACK_TIMER, /* 2 ACK reception monitoring timer */ + GPS_NAVIFST_TIMER, /* 3 Initial navigation monitoring timer */ + GPS_NAVICYCLE_TIMER, /* 4 Navigation monitoring timer */ + GPS_NAVIDISRPT_TIMER, /* 5 Navigation monitoring disruption log output timer */ + GPS_DIAGCLK_GUARDTIMER, /* 6 Diagnosis provision time guard monitoring timer */ + GPS_NMEADATA_GUARDTIMER, /* 7 NMEA data-providing guard monitoring timer */ + GPS_RECOVERY_TIMER, /* 8 GPS recovery timer */ + GPS_RECEIVERERR_TIMER, /* 9 GPS receiver anomaly detection timer */ + SNS_FST_TIMER, /* 10 Initial sensor data reception monitoring timer */ + SNS_CYCLE_TIMER, /* 11 Cyclic sensor data reception monitoring timer */ + SNS_DISRPT_TIMER, /* 12 Cyclic sensor data interruption log output timer */ + TIM_NUM /* 13 Number of timer types */ +} GPS_TIM_KIND; + +/*! + @brief Timer status +*/ +typedef struct GpsTimSts { + u_int8 flag; /**< Timer flag OFF:Stop,ON:Start */ + u_int8 cnt; /**< Start counter */ + int8 rsv[2]; /**< Reserved */ +} GPS_TIM_STS; + +/*! + @brief Timer status management table + */ +typedef struct GpsTimMng { + GPS_TIM_STS sts[TIM_NUM]; /**< Timer status */ +} GPS_TIM_MNG; + +/*! + @brief Timer setting information table +*/ +typedef struct GpsTimInfo { + uint32_t timer_val; /**< Timer value */ + PNO pno; /**< Event notification destination process number */ +} GPS_TIM_INFO; + +/*---------------------------------------------------------------------------*/ +// Prototype + +void DevGpsTimInit(void); +BOOL DevGpsTimeSet(GPS_TIM_KIND tim_kind); +BOOL DevGpsTimeStop(GPS_TIM_KIND tim_kind); +u_int16 DevGpsTimeMakSeqNo(GPS_TIM_KIND tim_kind); +BOOL DevGpsTimeJdgKind(u_int16 seqno); + +/*---------------------------------------------------------------------------*/ +#endif // INC_GPSCOMMON_MDEV_GPS_TIMERCTRL_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/inc/LineSensDrv/LineSensDrv_Sensor.h b/inc/LineSensDrv/LineSensDrv_Sensor.h new file mode 100644 index 00000000..1b996b78 --- /dev/null +++ b/inc/LineSensDrv/LineSensDrv_Sensor.h @@ -0,0 +1,183 @@ +/* + * @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_Sensor.h +*/ + +#ifndef INC_LINESENSDRV_LINESENSDRV_SENSOR_H_ +#define INC_LINESENSDRV_LINESENSDRV_SENSOR_H_ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "positioning_def.h" + +/*---------------------------------------------------------------------------*/ +// Macro definitions + +/* Message related */ +/* Message identification number definition received from the ICR */ +#define LSDRV_RCVMSG_INVALID 0x00 /* Not applicable */ +#define LSDRV_RCVMSG_SENSOR 0x12 /* Sensor data */ + +/* LineSensor vehicle signal notification */ + +#define LSDRV_SENS_MASK 0xFFF /* Sensor data MASK */ +#define LSDRV_PLSMAX 65536 /* Maximum vehicle speed pulse number */ +/* #011 start */ +#define LSDRV_SENS_COEFFICIENT 14.1287284284144427 /* Vehicle speed calculation factor */ +#define LSDRV_SPKMPH_METHOD_FST 0 /* Vehicle speed calculation method(Initial Sensor Data) */ +#define LSDRV_SPKMPH_METHOD_NML 1 /* Vehicle speed calculation method(Sensor data) */ +#define LSDRV_SPKMPH_AVE_TIME 29 /* Effective time for average calculation data(in units of 100ms) */ /* #013 */ +#define LSDRV_SPKMPH_NOISE_TIME (LSDRV_SPKMPH_AVE_TIME + 2) /* Effective Time for Noise Detection(in units of 100ms) */ /* #017 */ +#define LSDRV_SPKMPH_INVALID 1 /* No pulse of previous vehicle speed */ +#define LSDRV_SPKMPH_VALID 0 /* Last vehicle speed pulse */ +#define LSDRV_SENSCNT_BRW_ADD 256 /* Measures for borrow occurrence during search of sensor counters */ +/* #011 end */ +#define LSDRV_FSENS_NRCV 0 /* Not received */ +#define LSDRV_FSENS_RCV 1 /* Receiving */ +#define LSDRV_FSENS_RCVCOMP 2 /* Reception completion */ +#define LSDRV_FSENS_NORMAL 0 /* Receive data normal */ /* #001 */ +#define LSDRV_FSENS_ERROR 1 /* Receive data anomaly */ /* #001 */ +/* #016 start */ +#define LSDRV_PLSDATA_NRCV 0 /* Not received */ +#define LSDRV_PLSDATA_RCV 1 /* Received */ +#define LSDRV_SENSCNT_MAX 255 /* Maximum value of sensor counter */ +#define LSDRV_SPKMPH_MIN_DATA_N 6 /* Number of data required for vehicle speed calculation */ +#define LSDRV_FSPDPLS_NUM 3 /* Number of cumulative pulses registered for the first time */ +/* #016 end */ +#define LSDRV_REV_MASK 0x0040 /* REV-data MASK(Before endian conversion) */ +#define LSDRV_BITSHIFT_REV 6 /* REV data bit shift */ +#define LSDRV_TEMP_MASK 0x07FF /* Gyro temperature data MASK(after endian conversion) */ +#define LSDRV_PLSTIME_LEN 232 /* Data length when inter-pulse time exists */ + + +#define LSDRV_DETAILED_DATA_MULTI_ENABLE TRUE /* Detailed data multiplexing of initial sensor data Enabled/Disabled */ + +#if LSDRV_DETAILED_DATA_MULTI_ENABLE +#define LSDRV_DETAILED_DATA_MULTI_MAX_NUM 2 /* Detailed data multiplexing of initial sensor data Maximum number */ +#else +#define LSDRV_DETAILED_DATA_MULTI_MAX_NUM 1 /* Detailed data multiplexing of initial sensor data Maximum number */ +#endif + +/* Total number of partitions of initial sensor data : Maximum value */ +#define LSDRV_FSENS_TOTAL_NUM_MAX LSDRV_FSTSNS_SENSOR_FIRST_SEND_NUM + +/* Detailed data of initial sensor data : Maximum number of acquisitions */ +#define LSDRV_DETAILED_DATA_MAX_NUM (LSDRV_FSENS_TOTAL_NUM_MAX * LSDRV_DETAILED_DATA_MULTI_MAX_NUM) + +/* Initial sensor data : Maximum number of saved messages */ +#define LSDRV_FSENS_SAVE_MSG_NUM (LSDRV_FSENS_TOTAL_NUM_MAX + 1) + +/* Initial sensor data : Maximum number of data stored */ +#define LSDRV_FSENS_SAVE_DATA_NUM (LSDRV_FSENS_SAVE_MSG_NUM * LSDRV_DETAILED_DATA_MULTI_MAX_NUM) + +#define LSDRV_DETAILED_DATA_GYRO_NUM LSDRV_FSTSNS_DETAILED_DATA_GYRO_NUM +#define LSDRV_DETAILED_DATA_GSENSOR_NUM LSDRV_FSTSNS_DETAILED_DATA_GSENSOR_NUM +#define LSDRV_DETAILED_DATA_GSENSOR_X_OFFSET 0 /* G-Sensor data X-axis offset included in detail data */ +#define LSDRV_DETAILED_DATA_GSENSOR_Y_OFFSET 1 /* G-Sensor data Y-axis offsets included in the detail data */ +#define LSDRV_DETAILED_DATA_GSENSOR_OFFSET 3 /* G-Sensor data access offset included in the detail data */ + +/* Gyro Output Specifications:11500 ~ 54036LSB */ +#define GYRO_OUTPUPT_SPEC_LOWER_LIMIT 11500U +#define GYRO_OUTPUPT_SPEC_UPPER_LIMIT 54036U + +/* Gyro Failure Status : How many normal status continues should be made to Gyro Failure Status normal when it is judged to be "error -> normal" */ +#define GYRO_TROUBLE_NORMAL_COUNTER_THRESHOLD 10U /* 10 ms (Gyro data acquisition interval) * 10 times = 100ms */ + +/* Gyro Failure Status : How many error status continues should be made to Gyro Failure Status error when it is judged to be "normal -> error" */ +#define GYRO_TROUBLE_ERROR_COUNTER_THRESHOLD 3000U /* 10 ms (Gyro data acquisition interval) * 3000 times = 30s */ + +/* SYS GPS Interrupt Signal : How many disable continues should be made to SYS GPS Interrupt Signal disable when it is judged to be "enable -> disable" */ +#define SYS_GPS_INTERRUPT_SIGNAL_INVALID_COUNTER_THRESHOLD 10U + +/* Gyro Failure Status : Match values with SENSORMOTION_NORMAL and ERROR,UNFIXED in SensorMotion_API.h */ +#define GYRO_NORMAL (0U) +#define GYRO_ERROR (1U) +#define GYRO_UNFIXED (2U) + +/* Gyro connection status : Match values with SENSOR_CONNECT and SENSOR_DISCONNECT in Sensor_API.h */ +#define GYRO_DISCONNECT (0U) +#define GYRO_CONNECT (1U) + +/*** Data table for calculating vehicle speed *****************************************/ +#define LSDRV_SPKMPH_TBL_NUM 32 /* Number of data tables */ /* #50836 */ +#define LSDRV_SPKMPH_DATA_EN 0x01 /* Data storage flag valid */ +#define LSDRV_SPKMPH_DATA_DIS 0x00 /* Data storage flag invalid */ + +/*** Operation code *********************************************/ +#define LSDRV_OPC_WILDCARD 0xFF /* Wildcarded #012 */ + +#define LSDRV_OPC_EXTTERM_R 0x89 /* External pin status request */ +#define LSDRV_OPC_EXTTERM_A 0x99 /* External pin status notification */ +#define LSDRV_OPC_SENSOR 0xC1 /* Sensor data */ +#define LSDRV_OPC_FSENS_R 0xE4 /* Initial sensor data request */ +#define LSDRV_OPC_FSENS_A 0xF4 /* Initial sensor data response */ + +/*---------------------------------------------------------------------------*/ +// Enum + +/*---------------------------------------------------------------------------*/ +// Struct + +typedef struct { + u_int16 uc_sensor_x; /* G-Sensor X data */ + u_int16 uc_sensor_y; /* G-Sensor Y data */ + u_int16 uc_sensor_z; /* G-Sensor Z data */ +} SENSORINPUT_INFO_DAT_GSENS; + +// TAG : LSDRV_SPEEDKMPH_DAT +// ABSTRACT : Data Table Structure for Vehicle Speed Calculation +typedef struct LsDrvSpeedKmph { + u_int8 uc_flag; /* Data storage flag */ + u_int8 uc_sens_cnt; /* Sensor counter */ + u_int16 us_speed_pulse; /* Vehicle speed pulse */ + u_int8 uc_noise_flag; /* Noise flag */ /* #017 */ + u_int8 u_reserve[3]; /* Reserved */ /* #017 */ +} LSDRV_SPEEDKMPH_DAT_DAT; + +typedef struct LsDrvSpeedKmphLast { + u_int8 uc_flag; /* Data storage flag */ + u_int8 uc_sens_cnt; /* Sensor counter */ + u_int16 us_speed_pulse; /* Vehicle speed pulse */ +} LSDRV_SPEEDKMPH_LAST_DAT; + +typedef struct LsDrvSpeedKmphDat { + u_int8 uc_ptr; /* Data storage pointer */ + u_int8 uc_fstsns_rcv; /* Initial sensor data reception status */ /* #016 */ + u_int8 uc_sns_rcv; /* Sensor data reception status */ /* #016 */ + u_int8 uc_calc_start; /* Vehicle speed calculation start flag */ /* #016 */ + LSDRV_SPEEDKMPH_LAST_DAT st_last_data; /* Previous vehicle speed pulse information */ + LSDRV_SPEEDKMPH_DAT_DAT st_data[LSDRV_SPKMPH_TBL_NUM]; /* Data portion */ +} LSDRV_SPEEDKMPH_DAT; + +/*---------------------------------------------------------------------------*/ +// Prototype + +void LineSensDrvSensor(u_int8*); +u_int16 LineSensDrvSpeedCalc(u_int8); +void LineSensDrvSpeedKmphDataInit(void); +void LineSensDrvSpeedPulseSave(u_int16, u_int16, u_int8); +u_int8 LineSensDrvCheckNoise(u_int8); + +u_int8 LineSensDrvGetLastSpeedPulse(u_int16*, u_int16, u_int8); +void LineSensDrvSetLastSpeedPulse(u_int16, u_int8); + +/*---------------------------------------------------------------------------*/ +#endif // INC_LINESENSDRV_LINESENSDRV_SENSOR_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/inc/LineSensDrv/LineSensDrv_Thread.h b/inc/LineSensDrv/LineSensDrv_Thread.h new file mode 100644 index 00000000..54a01dc0 --- /dev/null +++ b/inc/LineSensDrv/LineSensDrv_Thread.h @@ -0,0 +1,66 @@ +/* + * @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.h +*/ + +#ifndef INC_LINESENSDRV_LINESENSDRV_THREAD_H_ +#define INC_LINESENSDRV_LINESENSDRV_THREAD_H_ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "LineSensDrv_Api.h" +#include "LineSensDrv_Sensor.h" + +/*---------------------------------------------------------------------------*/ +// Define + +// Return value +#define RET_LSDRV_SUCCESS RET_NORMAL // API processing success +#define RET_LSDRV_ERROR RET_ERROR // API processing failed + +/*---------------------------------------------------------------------------*/ +// Rcv Message Related + +// Size of data section +#define LSDRV_RCVMSG_MSGBUF 512 // Byte count of RcvMSG data section + +/*---------------------------------------------------------------------------*/ +// Struct + +// TAG : LSDRV_RCVDATA +// ABSTRACT : Message receive buffer structure +typedef struct LsDrvRcvData { + T_APIMSG_MSGBUF_HEADER st_head; // Message header + u_int8 uc_data[LSDRV_RCVMSG_MSGBUF]; // Data portion +} LSDRV_RCVDATA; + +/*---------------------------------------------------------------------------*/ +// Prototype + +int32 LineSensDrvMainThreadInit(HANDLE); +void LineSensDrvParamInit(void); + +EFrameworkunifiedStatus LineSensDrvThread(HANDLE); +void LineSensDrvThreadStopProcess(void); +u_int8 LineSensDrvGetSysRecvFlag(void); + +/*---------------------------------------------------------------------------*/ +#endif // INC_LINESENSDRV_LINESENSDRV_THREAD_H_ + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/src/Common/positioning_common.cpp b/src/Common/positioning_common.cpp new file mode 100644 index 00000000..00b3a8bc --- /dev/null +++ b/src/Common/positioning_common.cpp @@ -0,0 +1,388 @@ +/* + * @copyright Copyright (c) 2017-2020 TOYOTA MOTOR CORPORATION. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/** +* @file positioning_common.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "positioning_common.h" +#include "MDev_Gps_Common.h" +#include "MDev_GpsRecv.h" +#include "LineSensDrv_Thread.h" + +/*---------------------------------------------------------------------------*/ +// Value define + +#define DATMOD_RETRY (3) /* Number of shared memory generation retries */ +#define DATMOD_PREINIT (0) /* Shared Memory State [Before initialization] */ +#define PRIM_NAME_MAX (32) /* Maximum Name Size */ +#define VEHICLE_SHARE_NAME ("POS_VEHICLE_SHARE_MEMORY") /* Shared memory name */ + +/* Mask for managing various notification reception conditions */ +#define NTFY_MSK_NONE (0x00) +/* Service availability notification */ +#define NTFY_MSK_COMMUNICATION_AVAILABILITY (0x01) +#define NTFY_MSK_PS_COMMUSB_AVAILABILITY (0x02) +#define NTFY_MSK_PS_PSMSHADOW_AVAILABILITY (0x04) +#define NTFY_MSK_CLOCK_AVAILABILITY (0x08) +#define NTFY_MSK_NS_BACKUPMGR_AVAILABILITY (0x10) +#define NTFY_MSK_SS_DEVDETSRV_AVAILABILITY (0x20) +/* Other Notices */ +#define NTFY_MSK_PS_PSMSHADOW_INIT_COMP (0x01) /* PSMShadow startup completion notice */ +#define NTFY_MSK_PS_LANSERVER_DEVICE_UPDATE (0x02) /* LanServer device configuration change notification */ +#define NTFY_MSK_PS_LANSERVER_WAKEUP_COMP (0x04) /* LanServer start completion notice */ + +/* Thread state */ +#define THREAD_STS_NOEXIST (0x00) +#define THREAD_STS_CREATING (0x01) +#define THREAD_STS_CREATED (0x02) + +/*---------------------------------------------------------------------------*/ +// ENUMERATION + +/*---------------------------------------------------------------------------*/ +// STRUCTURE + +typedef struct StThreadInfo { + EnumTID_POS e_id; /**< Thread ID */ + const int8_t* p_name; /**< Thread name */ + PNO p_no; /**< Process number */ + CbFuncPtr cb_routine; /**< Start routine */ + uint8_t msk_available; /**< Dependent services Availability */ + uint8_t msk_ntfy; /**< Dependent notification */ + uint8_t msk_thread; /**< Dependent threads */ + BOOL b_is_depended; /**< Positioning/Availability->TRUE change dependency */ + uint8_t uc_status; /**< Thread activation state */ + uint8_t uc_order; /**< Boot Sequence(Performance) */ + uint8_t u_reserve[2]; +} ST_THREAD_CREATE_INFO; + +typedef struct { + char share_data_name[PRIM_NAME_MAX]; /**< Shared data name */ + u_int32 data_size; /**< Shared data size */ +} ST_SHAREDATA; + +static const int8_t kThreadNamePosMain[15] = "POS_Main"; +static const int8_t kThreadNamePosSens[15] = "POS_Sens"; +static const int8_t kThreadNamePosGps[15] = "POS_Gps"; +static const int8_t kThreadNamePosGpsRecv[15] = "POS_Gps_Recv"; +static const int8_t kThreadNamePosGpsRollover[15] = "POS_Gps_Rolovr"; + +static ST_THREAD_CREATE_INFO g_pos_thread_create_info[] = { + { /* POS_Main */ + ETID_POS_MAIN, /* (1) */ + NULL, /* (2) */ + 0, /* (3) */ + NULL, /* (4) */ + (NTFY_MSK_NONE), /* (5) */ + (NTFY_MSK_NONE), /* (6) */ + 0, /* (7) */ + FALSE, /* (8) */ + THREAD_STS_NOEXIST, /* (9) */ + 0 /* (10) */ + }, + { /* POS_sens */ + ETID_POS_SENS, /* (1) */ + kThreadNamePosSens, /* (2) */ + PNO_LINE_SENS_DRV, /* (3) */ + &LineSensDrvThread, /* (4) */ + (NTFY_MSK_NONE), /* (5) */ + (NTFY_MSK_NONE), /* (6) */ + 0, /* (7) */ + FALSE, /* (8) */ + THREAD_STS_NOEXIST, /* (9) */ + 0 /* (10) */ + }, + { /* POS_Gps */ + ETID_POS_GPS, /* (1) */ + kThreadNamePosGps, /* (2) */ + PNO_NAVI_GPS_MAIN, /* (3) */ + &DevGpsMainThread, /* (4) */ + (NTFY_MSK_NONE), /* (5) */ + (NTFY_MSK_NONE), /* (6) */ + 0, /* (7) */ + TRUE, /* (8) */ + THREAD_STS_NOEXIST, /* (9) */ + 0 /* (10) */ + }, + { /* POS_Gps_Recv */ + ETID_POS_GPS_RECV, /* (1) */ + kThreadNamePosGpsRecv, /* (2) */ + PNO_NAVI_GPS_RCV, /* (3) */ + &DevGpsRecvThread, /* (4) */ + (NTFY_MSK_NONE), /* (5) */ + (NTFY_MSK_NONE), /* (6) */ + THREAD_STS_MSK_POS_GPS, /* (7) */ + FALSE, /* (8) */ + THREAD_STS_NOEXIST, /* (9) */ + 0 /* (10) */ + }, + { /* POS_Gps_Rolovr */ + ETID_POS_GPS_ROLLOVER, /* (1) */ + NULL, /* (2) */ + 0, /* (3) */ + NULL, /* (4) */ + (NTFY_MSK_NONE), /* (5) */ + (NTFY_MSK_NONE), /* (6) */ + 0, /* (7) */ + FALSE, /* (8) */ + THREAD_STS_NOEXIST, /* (9) */ + 0 /* (10) */ + }, + { /* Termination */ + ETID_POS_MAX, /* (1) */ + NULL, /* (2) */ + 0, /* (3) */ + NULL, /* (4) */ + NTFY_MSK_NONE, /* (5) */ + NTFY_MSK_NONE, /* (6) */ + 0, /* (7) */ + FALSE, /* (8) */ + THREAD_STS_NOEXIST, /* (9) */ + 0 /* (10) */ + }, +}; + +static ST_SHAREDATA g_sharedata_tbl[] = { + /* Shared data name to be generated, Shared data size */ + { {VEHICLE_SHARE_NAME}, 512 * 11 }, /* Vehicle sensor information acquisition */ +#if 0 /* Not used in _CWORD71_ below */ + { {"SENSOR_SHARE_MEMORY"}, 512 * 11 }, /* Vehicle sensor information Pkg acquisition */ + { {"GPS_INT_SIGNAL_SHARE_MEMORY"}, 4 }, /* GPS interrupt signal acquisition */ + { {"LOG_SETTING_SHARE_MEMORY"}, 36 }, /* DR feature log acquisition */ + { {"GYRO_CONNECT_STTS_SHARE_MEMORY"}, 4 }, /* Gyro connection status acquisition */ + { {"EPHEMERIS_NUM_SHARE_MEMORY"}, 4 }, /* Effective ephemeris count acquisition at shutdown */ + { {"LOCALTIME_SHARE_MEMORY"}, 12 }, /* Local time acquisition at shutdown */ + { {"LONLAT_SHARE_MEMORY"}, 8 }, /* Location acquisition at shutdown */ +#endif + { {(int8)NULL}, 0 } /* Termination */ +}; + + +/*---------------------------------------------------------------------------*/ +// Functions + +static EFrameworkunifiedStatus PosStopThreadDummy(HANDLE h_app) { + return eFrameworkunifiedStatusOK; +} + +static uint32_t PosGetMsg(HANDLE h_app, void** p_buf, uint32_t ul_size) { + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusOK; + uint32_t tmp_size = 0; + void* p_rev_buf = NULL; + + if ((h_app == NULL) || (p_buf == NULL)) { + POSITIONING_LOG("Argument ERROR!! [h_app = %p, pBuf = %p]", h_app , p_buf); + } else { + /* Check the size of received data */ + tmp_size = FrameworkunifiedGetMsgLength(h_app); + if (tmp_size > ul_size) { + POSITIONING_LOG("Message ul_size ERROR!! [tmp_size = %d, maxsize = %d]", tmp_size, ul_size); + tmp_size = 0; + } else { + /* Obtain data */ + e_status = FrameworkunifiedGetDataPointer(h_app, &p_rev_buf); + + if (e_status == eFrameworkunifiedStatusOK) { + *p_buf = p_rev_buf; + } else if (e_status == eFrameworkunifiedStatusInvldBufSize) { + e_status = FrameworkunifiedGetMsgDataOfSize(h_app, *p_buf, tmp_size); + + if (e_status != eFrameworkunifiedStatusOK) { + POSITIONING_LOG("FrameworkunifiedGetMsgDataOfSize ERROR [e_status:%d]", e_status); + tmp_size = 0; + } + } else { + POSITIONING_LOG("FrameworkunifiedGetDataPointer ERROR [e_status:%d]", e_status); + tmp_size = 0; + } + } + } + + return tmp_size; +} + +static void PosCreateSharedMemory(void) { + RET_API ret_api = RET_NORMAL; + void *mod_exec_dmy; /* Module data pointer(dummy) */ + int retry; /* Retry counter */ + ST_SHAREDATA *p_shm_tbl; + + /* Configure Shared Data Generating Tables */ + p_shm_tbl = g_sharedata_tbl; + + while (*(p_shm_tbl->share_data_name) != (int8)NULL) { + for (retry = 0; retry < DATMOD_RETRY; retry++) { + /* Shared Memory Generation */ + ret_api = _pb_CreateShareData(p_shm_tbl->share_data_name, p_shm_tbl->data_size, &mod_exec_dmy); + if (ret_api == RET_NORMAL) { + /* Set the shared memory status flag to "Before initialization (0)" */ + *reinterpret_cast<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/src/Common/positioning_hal.cpp b/src/Common/positioning_hal.cpp new file mode 100644 index 00000000..94f79c4a --- /dev/null +++ b/src/Common/positioning_hal.cpp @@ -0,0 +1,49 @@ +/* + * @copyright Copyright (c) 2017-2020 TOYOTA MOTOR CORPORATION. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/** +* @file positioning_hal.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include <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/src/GpsCommon/MDev_GpsRecv.cpp b/src/GpsCommon/MDev_GpsRecv.cpp new file mode 100644 index 00000000..13159b47 --- /dev/null +++ b/src/GpsCommon/MDev_GpsRecv.cpp @@ -0,0 +1,608 @@ +/* + * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/** +* @file MDev_GpsRecv.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "MDev_GpsRecv.h" +#include "MDev_Gps_Common.h" +#include "positioning_common.h" +#include "MDev_Gps_API.h" +//extern "C" { +//#include <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/src/GpsCommon/MDev_GpsRollOver.cpp b/src/GpsCommon/MDev_GpsRollOver.cpp new file mode 100644 index 00000000..62227c26 --- /dev/null +++ b/src/GpsCommon/MDev_GpsRollOver.cpp @@ -0,0 +1,275 @@ +/* + * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/** +* @file MDev_GpsRollOver.cpp +*/ + +#include "MDev_GpsRollOver.h" + +#define TIM_ROLOVR_CALCORCNT_DAYS (1024 * 7) /*1024 weeks * 7 days */ + +#define TMT_OK (1) /* Definition indicating normal status */ +#define TMT_NG (0) /* Definitions indicating abnormal status */ +#define TMT_TRUE (1) /* Definitions indicating the status for which the condition is true */ +#define TMT_FALSE (0) /* Definitions indicating a status in which the condition is false */ + +#define TIM_ROLOVR_LEAPYEARDAYS (366) /*Number of days in the leap year */ +#define TIM_ROLOVR_NOTLEAPYEARDAYS (365) /*Year days in non-leap years */ + +static const WORD kMonth[2][12] = { + {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}, /* Number of days per month(For non-leap year) */ + {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31} /* Number of days per month(For leap years) */ +}; +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * FUNCTION : ChkLeapYear + * Function : Leap year determination processing + * Feature Overview : Determine whether it is a leap year + * Input : u_int16 year + * Output : None + * Return value : u_int32 + TMT_TRUE Leap year + TMT_FALSE Non-leap year + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +static u_int32 ChkLeapYear(u_int16 year) { + int32 leap_year; + + /* Leap year determination processing (select subscripts in the [LEAP YEAR] table) *The processing that matches the time correction processing after time difference value addition/subtraction is used. */ + leap_year = TMT_FALSE; + if ((year % 4) == 0) { + leap_year = TMT_TRUE; + } + if ((year % 100) == 0) { + leap_year = TMT_FALSE; + } + + if ((year % 400) == 0) { + leap_year = TMT_TRUE; + } + + return (leap_year); +} + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * FUNCTION : RollOverFormerLaterDays + * Function : Calculation of the number of days before and after + * Feature Overview : The number of days from the beginning of the month to the previous day of the specified date, + and calculates the number of days after a given date, up to the end of the month + * Input : TG_TIM_ROLOVR_YMD* base_ymd Reference date + * Output : u_int32* former_days Number of days before (number of days before the specified date from the beginning of the month) + u_int32* later_days Number of days after(Number of days following the specified date until the end of the month) + * Return value : None + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +static void RollOverFormerLaterDays(TG_TIM_ROLOVR_YMD* base_ymd, u_int32* former_days, u_int32* later_days) { + int32 leap_year; + + leap_year = ChkLeapYear(base_ymd->year); + + *former_days = base_ymd->day - 1; + + *later_days = kMonth[leap_year][base_ymd->month - 1] - base_ymd->day; +} + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * FUNCTION : RollOverCalDaysWithinBY + * Function : Calculation of the difference in the number of days in the base year + * Feature Overview : Calculate the difference in the number of days between the base date and the conversion target date. + [Restriction]If the base date > conversion target date, the number of days difference is 0. + * Input : TG_TIM_ROLOVR_YMD* base_ymd Reference date + TG_TIM_ROLOVR_YMD* conv_ymd Conversion Date + * Output : None + * Return value : u_int32 Difference in the number of days(positive only) + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +static u_int32 RollOverCalDaysWithinBY(TG_TIM_ROLOVR_YMD* base_ymd, TG_TIM_ROLOVR_YMD* conv_ymd) { + int32 leap_year; /* Subscripts in the [LEAP YEAR] table */ + u_int32 days; /* Day Difference Calculation Result */ + u_int32 l_days; /* Number of days after the reference month */ + u_int32 f_days; /* Number of days before the conversion target month */ + u_int32 dmy_days; /* Dummy days(For storing unnecessary number of days in this function) */ + u_int32 m_days; /* Sum of the number of days in the month between the base month and the month to be converted */ + int32 cnt; /* A counter that sums the number of days in a month between the base month and the month to be converted */ + u_int16 diff_month; /* Difference between the conversion target month and the base month */ + u_int16 chk_month; /* Number of days of month to be acquired */ + + days = 0; + + if (base_ymd->month == conv_ymd->month) { + if (base_ymd->day <= conv_ymd->day) { + days = conv_ymd->day - base_ymd->day; + } else { + days = 0; /* Set the difference to 0(Negative difference in number of days is not calculated.) */ + } + } else if (base_ymd->month < conv_ymd->month) { + RollOverFormerLaterDays(base_ymd, &dmy_days, &l_days); + + m_days = 0; + diff_month = conv_ymd->month - base_ymd->month; + + leap_year = ChkLeapYear(base_ymd->year); + + /* Calculate the sum of the number of days in the month between the base month and the conversion target month B */ + chk_month = base_ymd->month + 1; + for (cnt = 0; cnt < (diff_month - 1); cnt++) { + m_days += kMonth[leap_year][chk_month - 1]; + chk_month++; + } + + RollOverFormerLaterDays(conv_ymd, &f_days, &dmy_days); + + days = l_days + m_days + f_days + 1; /* Calculate the difference in number of days as A+B+C+1 */ + + } else { + days = 0; /* Set the difference to 0(Negative difference in number of days is not calculated.) */ + } + + return days; +} + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * FUNCTION : RollOverGetAYearDays + * Function : Process of obtaining the number of days per year + * Feature Overview : Get the number of days in a given year + * Input : u_int16 base_year Year + * Output : None + * Return value : u_int32 Number of days + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +static u_int32 RollOverGetAYearDays(u_int16 base_year) { + int32 leap_year; /* Subscripts in the [LEAP YEAR] table */ + u_int32 days; + + leap_year = ChkLeapYear(base_year); /* Leap year determination processing */ + + if (leap_year == TMT_TRUE) { + days = TIM_ROLOVR_LEAPYEARDAYS; + } else { + days = TIM_ROLOVR_NOTLEAPYEARDAYS; + } + + return days; +} + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * FUNCTION : RollOverConvYMDWithoutBY + * Function : Date conversion processing outside the base year, month, and day + * Feature Overview : Calculate the date shifted by the specified number of days from the specified date. + *If less than (1024 weeks x 7 days) is specified as the number of days difference, the calculation result is + returns the specified date(No correction) + * Input : TG_TIM_ROLOVR_YMD* base_ymd Reference date + u_int32 days Difference in the number of days + u_int32 days_by Number of days after + number of days after month + * Output : TG_TIM_ROLOVR_YMD* conv_ymd Date of the conversion result + * Return value : None + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +static void RollOverConvYMDWithoutBY(TG_TIM_ROLOVR_YMD* base_ymd, u_int32 days, + u_int32 days_by, TG_TIM_ROLOVR_YMD* conv_ymd) { + int32 leap_year; /* Subscripts in the [LEAP YEAR] table */ + u_int32 rest_days; /* Remaining number of days */ + u_int16 cal_year; /* For year calculation */ + u_int16 cal_month; /* For monthly calculation */ + u_int32 cal_days; /* To calculate the number of days */ + + rest_days = days - days_by; /* Remaining number of days is different from the number of initialization days.-(Number of days after + Number of days after month) */ + cal_year = base_ymd->year + 1; /* The year is set to the year following the year of the initialization reference date. */ /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ + + cal_days = RollOverGetAYearDays(cal_year); /* Process of obtaining the number of days per year */ + + while (rest_days > cal_days) { + rest_days -= cal_days; /* Remaining Days = Remaining Days-Updated as Days of Year */ + + cal_year++; /* Increment Year */ + + cal_days = RollOverGetAYearDays(cal_year); /* Process of obtaining the number of days per year */ + } + + /* Year Finalization */ + conv_ymd->year = cal_year; /* Year Calculated */ + + cal_month = 1; /* Initialize Month January */ + + leap_year = ChkLeapYear(conv_ymd->year); /* Leap year determination processing */ + + cal_days = kMonth[leap_year][cal_month - 1]; /* Acquisition processing of the number of days/month */ /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ + + while (rest_days > cal_days) { + rest_days -= cal_days; /* Remaining Days = Remaining Days-Updated as Days of Month */ + + cal_month++; /* Increment month */ + + cal_days = kMonth[leap_year][cal_month - 1]; /* Acquisition processing of the number of days/month */ /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ + } + + /* Fix month */ + conv_ymd->month = cal_month; /* Month Calculated */ + + /* Date set */ + conv_ymd->day = (u_int16)rest_days; /* Day calculated Remaining number of days */ +} + + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * FUNCTION : RollOverConvYMD + * Function : Date conversion processing + * Feature Overview : Calculate the date shifted by the specified number of days from the specified date. + *If the base date is shifted by the number of days but the year is the same as the base date, the result returns the base date + * Input : TG_TIM_ROLOVR_YMD* base_ymd Reference date + u_int32 days Difference in the number of days + * Output : TG_TIM_ROLOVR_YMD* conv_ymd Date of the conversion result + * Return value : None + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +static void RollOverConvYMD(TG_TIM_ROLOVR_YMD* base_ymd, u_int32 days, TG_TIM_ROLOVR_YMD* conv_ymd) { + u_int32 days_by; /* Number of days after + number of days after month(=difference in number of days from the base date to the end of the year) */ + TG_TIM_ROLOVR_YMD tmp_ymd; + + + /* (number of days after + number of days after)Set calculation date( 12/31 ) */ + tmp_ymd.year = base_ymd->year; + tmp_ymd.month = 12; + tmp_ymd.day = 31; + + days_by = RollOverCalDaysWithinBY(base_ymd, &tmp_ymd); /* Calculation of the difference in the number of days in the base year */ + + if (days_by >= days) { + memcpy(conv_ymd, base_ymd, sizeof(TG_TIM_ROLOVR_YMD)); /* Returns the base date regardless of the number of days difference */ + + } else { + RollOverConvYMDWithoutBY(base_ymd, days, days_by, conv_ymd); + } +} + + + +/** + * @brief + * Conversion to a time that takes the GPS week correction counter into account + * + * Converting the GPS Week Correction Counter to a Considered Time + * + * @param[in] TG_TIM_ROLOVR_YMD* base_ymd Reference date + * @param[in] u_int8 gpsweekcorcnt GPS weekly correction counter + * @param[out] TG_TIM_ROLOVR_YMD* conv_ymd Correction Date + * @return none + * @retval none + */ +void GPSRollOverConvTime(TG_TIM_ROLOVR_YMD* base_ymd, TG_TIM_ROLOVR_YMD* conv_ymd, u_int8 gpsweekcorcnt) { + u_int32 days; /* Difference in the number of days */ + + days = TIM_ROLOVR_CALCORCNT_DAYS * gpsweekcorcnt; /* (1024 weeks x 7 days) x GPS week correction counter to calculate the difference in the number of days */ + + RollOverConvYMD(base_ymd, days, conv_ymd); /* Date conversion processing */ +} diff --git a/src/GpsCommon/MDev_Gps_API.cpp b/src/GpsCommon/MDev_Gps_API.cpp new file mode 100644 index 00000000..35f52016 --- /dev/null +++ b/src/GpsCommon/MDev_Gps_API.cpp @@ -0,0 +1,491 @@ +/* + * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/** +* @file MDev_Gps_API.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "MDev_Gps_API.h" +#include "positioning_hal.h" +#include "positioning_def.h" +#include "MDev_Gps_Nmea.h" + +/*---------------------------------------------------------------------------*/ +// Functions + +/****************************************************************************** +@brief SendNmeaGps<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/src/GpsCommon/MDev_Gps_Common.cpp b/src/GpsCommon/MDev_Gps_Common.cpp new file mode 100644 index 00000000..c8b5c9a6 --- /dev/null +++ b/src/GpsCommon/MDev_Gps_Common.cpp @@ -0,0 +1,2105 @@ +/* + * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/** +* @file MDev_Gps_Common.h +*/ + +/*---------------------------------------------------------------------------*/ + +#include "MDev_Gps_Common.h" + +#include <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/src/GpsCommon/MDev_Gps_Main.cpp b/src/GpsCommon/MDev_Gps_Main.cpp new file mode 100644 index 00000000..0b9a4be6 --- /dev/null +++ b/src/GpsCommon/MDev_Gps_Main.cpp @@ -0,0 +1,362 @@ +/* + * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/** +* @file MDev_Gps_Main.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "MDev_Gps_Main.h" +#include "positioning_common.h" +#include "MDev_Gps_Mtrx.h" +#include "MDev_Gps_TimerCtrl.h" +#include "MDev_Gps_Common.h" +#include "MDev_Gps_Nmea.h" +#include "MDev_GpsRecv.h" +#include "LineSensDrv_Thread.h" + +/*---------------------------------------------------------------------------*/ +// Global values + +BOOL g_gps_rcv_thread_stop = FALSE; + +/******************************************************************************** + * Matrix event translation table(Message input) + ********************************************************************************/ +const TG_GPS_MSGEVTCHNG kGpsMsgchkC[MDEV_GPSMSGCHKC_MAX] = { + /* ++ #GPF_60_024 */ + /* Matrix Event Translation Table for _CWORD82_ NMEA */ + {CID_GPS__CWORD82__REQUEST, GPS_EVT_SENDREQ}, /* NMEA transmission request */ + {CID_INI_INTERNAL_ACCOFF_START, GPS_EVT_ACC_OFF}, /* In-function ACC-OFF instructions */ + {CID_GPS_REQRESET, GPS_EVT_RESETREQ}, /* GPS reset request */ + {CID_NAVIINFO_DELIVER, GPS_EVT_NAVI_LOCATIONINFO}, /* Providing navigation information */ + {CID_NAVIINFO_SPEED_DELIVER, GPS_EVT_NAVI_SPEEDINFO}, /* Navigation speed information service */ + {CID_GPS_TIMESETTING, GPS_EVT_TIMESETTING}, /* GPS time setting instruction */ + {CID_GPS_BACKUPDATA_LOAD, GPS_EVT_BACKUPDATA_LOAD}, /* Backup memory read request */ + {CID_THREAD_STOP_REQ, GPS_EVT_STOPREQ}, /* Thread stop request */ + {CID_GPSWEEKCOR_CNT_NOTIFICATION, GPS_EVT_WEEKCOR_CNT_NOTIFICATIO}, /* GPS Week Adjustment Counter Notification */ + {DAT_END, DAT_END} /* Table end */ + /* -- #GPF_60_024 */ +}; + +/******************************************************************************** + * Matrix event translation table(Input Timeout [ Timer ID]) + ********************************************************************************/ +const TG_GPS_TIMEVTCHNG kGpsTimchkC[MDEV_PSTIMCHKC_MAX] = { + {GPS_STARTUP_TIMER, 0, GPS_EVT_TIMEOUT_CYCLDAT}, /* Start confirmation monitoring timer */ + {GPS_CYCL_TIMER, 0, GPS_EVT_TIMEOUT_CYCLDAT}, /* Periodic reception data monitoring timer */ + {GPS_RECV_ACK_TIMER, 0, GPS_EVT_TIMEOUT_RSPDAT}, /* ACK reception monitoring timer */ + {GPS_NAVIFST_TIMER, 0, GPS_EVT_TIMEOUT_NAVI}, /* Initial NAVI monitoring timer */ + {GPS_NAVICYCLE_TIMER, 0, GPS_EVT_TIMEOUT_NAVI}, /* NAVIGATION monitoring timer */ + {GPS_NAVIDISRPT_TIMER, 0, GPS_EVT_TIMEOUT_NAVI}, /* NAVIGATION Disruption Log Output Timer */ + {GPS_DIAGCLK_GUARDTIMER, 0, GPS_EVT_TIMEOUT_DIAGCLKGUARD}, /* Diagnosis provision time guard monitoring timer */ + {GPS_NMEADATA_GUARDTIMER, 0, GPS_EVT_TIMEOUT_NMEADATAGUARD}, /* NMEA data-providing guard monitoring timer */ + {GPS_RECOVERY_TIMER, 0, GPS_EVT_TIMEOUT_RECOVERY}, /* GPS recovery timer */ + {GPS_RECEIVERERR_TIMER, 0, GPS_EVT_TIMEOUT_RECEIVERERR}, /* GPS receiver anomaly detection timer */ + {DAT_END, 0, DAT_END} /* Table end */ +}; + +/******************************************************************************** + * Processing Branch Table + ********************************************************************************/ +/******************************************************************************** + * TAG : TG_GPS_JMP_TBL + * ABSTRACT : GPS jump table + * NOTE : GPS Communication Management Matrix Table + ********************************************************************************/ +const TG_GPS_JMP_TBL kGpsMatxC[GPS_STS_NUM][GPS_EVT_NUM] = { + /* - - - - - Confirming Start - - - - - */ + { + {&DevGpsInitStartSendReq}, /* Transmission request */ + {&DevGpsInitStartGPSResetReq}, /* GPS reset request */ + {&DevGpsInitStartRcvCyclCmd}, /* Cyclic reception command reception */ + {&DevGpsNop}, /* Receive Response Command */ + {&DevGpsNop}, /* Response monitoring timeout */ + {&DevGpsInitStartCyclDataTimeOut}, /* Periodic reception data monitoring timeout */ + {&DevGpsInitStartNaviDataTimeOut}, /* Navigation providing data monitoring timeout */ + {&DevGpsInitStartDiagClkGuardTimeOut}, /* Diagnosis provision time guard monitoring timeout */ + {&DevGpsInitStartAccOffStart}, /* In-function ACC-OFF instructions */ + {&DevGpsInitStartNaviInfoDeliver}, /* Providing navigation information */ + {&DevGpsInitStartNaviSpeedDeliver}, /* Navigation speed information service */ + {&DevGpsInitStartSettingTime}, /* GPS time setting instruction */ + {&DevGpsInitStartNmeaDataGuardTimeOut}, /* NMEA data-providing guard monitoring timeout */ + {&DevGpsInitStartBackupDataLoadReq}, /* Backup data read request */ + {&DevGpsInitStartStopReq}, /* Thread stop request */ + {&DevGpsInitStartGpsweekcorcntNtf}, /* GPS Week Adjustment Counter Notification */ + {&DevGpsInitStartRecoveryTimeOut}, /* GPS error monitoring timer out */ + {&DevGpsInitStartGpsReceiverErrTimeOut} /* GPS receiver anomaly detection timeout */ + }, + /* - - - - - - In operation - - - - - - */ + { + {&DevGpsNormalSendReq}, /* Transmission request */ + {&DevGpsNormalGPSResetReq}, /* GPS reset request */ + {&DevGpsNormalRcvCyclCmd}, /* Cyclic reception command reception */ + {&DevGpsNop}, /* Receive Response Command */ + {&DevGpsNop}, /* Response monitoring timeout */ + {&DevGpsNormalCyclDataTimeOut}, /* Periodic reception data monitoring timeout */ + {&DevGpsNormalNaviDataTimeOut}, /* Navigation providing data monitoring timeout */ + {&DevGpsNormalDiagClkGuardTimeOut}, /* Diagnosis provision time guard monitoring timeout */ + {&DevGpsNormalAccOffStart}, /* In-function ACC-OFF instructions */ + {&DevGpsNormalNaviInfoDeliver}, /* Providing navigation information */ + {&DevGpsNormalNaviSpeedDeliver}, /* Navigation speed information service */ + {&DevGpsNormalSettingTime}, /* GPS time setting instruction */ + {&DevGpsNormalNmeaDataGuardTimeOut}, /* NMEA data-providing guard monitoring timeout */ + {&DevGpsNormalBackupDataLoadReq}, /* Backup data read request */ + {&DevGpsNormalStopReq}, /* Thread stop request */ + {&DevGpsNormalGpsweekcorcntNtf}, /* GPS Week Adjustment Counter Notification */ + {&DevGpsNormalRecoveryTimeOut}, /* GPS error monitoring timer out */ + {&DevGpsNormalGpsReceiverErrTimeOut} /* GPS receiver anomaly detection timeout */ + }, + /* - - - - -- Sending - - - - - - */ + { + {&DevGpsSendSendReq}, /* Transmission request */ + {&DevGpsSendGPSResetReq}, /* GPS reset request */ + {&DevGpsSendRcvCyclCmd}, /* Cyclic reception command reception */ + {&DevGpsSendRcvRspCmd}, /* Receive Response Command */ + {&DevGpsSendRspDataTimeOut}, /* Response monitoring timeout */ + {&DevGpsSendCyclDataTimeOut}, /* Periodic reception data monitoring timeout */ + {&DevGpsSendNaviDataTimeOut}, /* Navigation providing data monitoring timeout */ + {&DevGpsSendDiagClkGuardTimeOut}, /* Diagnosis provision time guard monitoring timeout */ + {&DevGpsSendAccOffStart}, /* In-function ACC-OFF instructions */ + {&DevGpsSendNaviInfoDeliver}, /* Providing navigation information */ + {&DevGpsSendNaviSpeedDeliver}, /* Navigation speed information service */ + {&DevGpsSendSettingTime}, /* GPS time setting instruction */ + {&DevGpsSendNmeaDataGuardTimeOut}, /* NMEA data-providing guard monitoring timeout */ + {&DevGpsSendBackupDataLoadReq}, /* Backup data read request */ + {&DevGpsSendStopReq}, /* Thread stop request */ + {&DevGpsSendGpsweekcorcntNtf}, /* GPS Week Adjustment Counter Notification */ + {&DevGpsSendRecoveryTimeOut}, /* GPS error monitoring timer out */ + {&DevGpsSendGpsReceiverErrTimeOut} /* GPS receiver anomaly detection timeout */ + } +}; + +// Receive command analysis table(NMEA format) +const TG_GPS_CMD_ANA_TBL* kGpsCmdAnaTbl = kGpsCmdAnaTblUblox; + +//GPS process pending buffer management table +TG_GPS_MSGRCV g_gps_msg_rcvr; /* Incoming message */ +TG_GPS_MNG g_gps_mngr; /* GPS process management information */ + +// Global variable for GPS communication management +u_int16 g_wsend_err; /* Number of transmission errors */ +u_int16 g_wcnct_err; /* Number of connection errors */ +TG_GPS_OUTPUT_FORMAT g_rcv_format; /* Receive Format */ + +// Global variable for the receive thread +TG_GPS_RECV_RcvData g_gps_rcvdata; /* Serial receive buffer */ +TG_GPS_RECV_RcvBuf g_gps_rcvbuf; /* Receive data storage buffer */ +TG_GPS_RECV_AnaDataBuf g_gps_ana_databuf; /* Analysis data buffer */ +TG_GPS_RECV_RcvFrameBuf g_gps_rcv_framebuf; /* Receive frame buffer */ +TG_GPS_SAVEBUF g_gps_save_cmdr; /* Command pending buffer */ +u_int16 g_wrecv_err; /* Number of reception errors */ + +/*---------------------------------------------------------------------------*/ +// Functions + +/******************************************************************************** + * MODULE : DEV_Gps_MainThread + * ABSTRACT : Communication Management Thread Domain Processing + * FUNCTION : Receive a request from the host application and send a command to the serial port. + * ARGUMENT : PVOID pv....Thread Creation Arguments + * NOTE : + * RETURN : TRUE :Normal completion + * FALSE:ABENDs + ********************************************************************************/ +EFrameworkunifiedStatus DevGpsMainThread(HANDLE h_app) { + BOOL ret = FALSE; + + PosSetupThread(h_app, ETID_POS_GPS); + + ret = DevGpsInit(); + + if (TRUE == ret) { + g_gps_rcv_thread_stop = FALSE; + + /* Start receiving message */ + DevGpsRcvMsg(); + } else { + /* Initialization failed */ + POSITIONING_LOG("DevGpsMainThread Init Fail \n"); + } + + return eFrameworkunifiedStatusFail; +} + + +/******************************************************************************** + * MODULE : DEV_Gps_Init + * ABSTRACT : Communication management thread initialization processing + * FUNCTION : Initialize internal tables and serial ports + * ARGUMENT : None + * NOTE : + * RETURN : TRUE :Normal completion + * FALSE:ABENDs + ********************************************************************************/ +BOOL DevGpsInit(void) { + BOOL ret = FALSE; + + /* Global variable initialization(GPS Communication Management Thread Internal Use Variable) */ + /* Clear process management information */ + memset(&g_gps_mngr, 0x00, sizeof(g_gps_mngr)); /* #05 */ + + /* Clear the message receive buffer */ + memset(&g_gps_msg_rcvr, 0x00, sizeof(g_gps_msg_rcvr)); /* #05 */ + + /* Clear the pending buffer */ + memset(&g_gps_save_cmdr, 0x00, sizeof(g_gps_save_cmdr)); /* #05 */ + + /* Clear RTC backup-related data */ + g_gps_mngr.rcv_cmd = GPS_FORMAT_MIN; /* #GPF_60_040 */ + g_gps_mngr.resp_cmd = GPS_FORMAT_MIN; /* #GPF_60_040 */ + g_gps_mngr.rcv_err_flag = FALSE; + + g_wsend_err = 0; /* Initialization of the number of transmission errors */ + g_wcnct_err = 0; /* Connection error count initialization */ + g_rcv_format = GPS_FORMAT_MIN; /* Initialize Receive Format #GPF_60_024*/ + + /* Initialize the timer function initialization processing management table */ + DevGpsTimInit(); + + /* Start confirmation monitoring timer setting(5Sec) */ + ret = DevGpsTimeSet(GPS_STARTUP_TIMER); + + /* GPS receiver anomaly detection timer setting(600Sec) */ + ret = DevGpsTimeSet(GPS_RECEIVERERR_TIMER); + + // Set command table + kGpsCmdAnaTbl = kGpsCmdAnaTblUblox; + + /* State transition processing(Start Confirmation Monitor) */ + ChangeStatusGpsCommon(GPS_STS_STARTUP); + + return(ret); +} + +/****************************************************************************** +@brief DEV_Gps_RcvMsg<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/src/GpsCommon/MDev_Gps_Mtrx.cpp b/src/GpsCommon/MDev_Gps_Mtrx.cpp new file mode 100644 index 00000000..b36a617c --- /dev/null +++ b/src/GpsCommon/MDev_Gps_Mtrx.cpp @@ -0,0 +1,866 @@ +/* + * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/** +* @file MDev_Gps_Mtrx.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files +#include "MDev_Gps_Mtrx.h" +#include "positioning_def.h" + +#include "MDev_Gps_Main.h" +#include "MDev_Gps_API.h" +#include "MDev_GpsRecv.h" +#include "MDev_Gps_Common.h" +#include "MDev_Gps_Nmea.h" +#include "MDev_Gps_TimerCtrl.h" + +extern uint8_t g_gps_reset_cmd_sending; +/*---------------------------------------------------------------------------*/ +// Functions + +/******************************************************************************** + * MODULE : DEV_Gps_Nop + * ABSTRACT : No processing + * FUNCTION : NOP processing of the matrix + * ARGUMENT : None + * NOTE : + * RETURN : None + ********************************************************************************/ +void DevGpsNop(void) { + return; +} + +/******************************************************************************** + * MODULE : DEV_Gps_InitStart_SendReq + * ABSTRACT : Startup confirmation - Transmit request reception matrix function + * FUNCTION : + * ARGUMENT : + * NOTE : + * RETURN : + ********************************************************************************/ +void DevGpsInitStartSendReq(void) { + /** Send Request Receive Common Processing Call */ + SendReqGpsCommon(); + /* -- #GPF_60_040 */ + return; +} + +/******************************************************************************** + * MODULE : DEV_Gps_InitStart_GPSResetReq + * ABSTRACT : Startup confirmation - GPS reset request reception matrix function + * FUNCTION : + * ARGUMENT : + * NOTE : + * RETURN : + ********************************************************************************/ +void DevGpsInitStartGPSResetReq(void) { + /** Reset request reception common processing call */ + GPSResetReqGpsCommon(); + return; +} + +/******************************************************************************** + * MODULE : DEV_Gps_InitStart_RcvCyclCmd + * ABSTRACT : Startup confirmation - Cyclic receive command receive matrix function + * FUNCTION : + * ARGUMENT : + * NOTE : + * RETURN : + ********************************************************************************/ +void DevGpsInitStartRcvCyclCmd(void) { + BOOL* pb_rcverr = &(g_gps_mngr.rcv_err_flag); /* GSP receiver error detection status */ + + /* Stop start confirmation monitoring timer */ + (void)DevGpsTimeStop(GPS_STARTUP_TIMER); /* Ignore -> MISRA-C:2004 Rule 16.10 */ /* QAC 3200 */ + + /* GPS receiver anomaly detection timer stopped */ + (void)DevGpsTimeStop(GPS_RECEIVERERR_TIMER); + + /* State transition processing(in operation) */ + ChangeStatusGpsCommon(GPS_STS_NORMAL); + + // Send NAV-TIMEUTC sentence addition requests + DevGpsNavTimeUTCAddReq(); + + DevGpsWknRolloverGetReq(); + + /* Receive command analysis(u-blox) */ + if (g_gps_reset_cmd_sending == FALSE) { + DevGpsRcvCyclCmd(); + } + + /* If a GPS receiver error is detected, the diagnosis code (current) is deleted. */ + if (*pb_rcverr == TRUE) { + *pb_rcverr = FALSE; + DevSendGpsConnectError(FALSE); + } + // } + /* Reset retry counter */ + RtyResetGpsCommon(); + + /* Sending pending commands */ + SendSaveCmdGpsCommon(); /* State transition if pending commands exist( -> Sending) */ + + /* Cyclic reception data monitoring timer setting */ + (void)DevGpsTimeSet(GPS_CYCL_TIMER); /* Ignore -> MISRA-C:2004 Rule 16.10 */ /* QAC 3200 */ + + /* GPS receiver anomaly detection timer setting */ + (void)DevGpsTimeSet(GPS_RECEIVERERR_TIMER); + + /* -- #GPF_60_024 */ + return; +} + +/******************************************************************************** + * MODULE : DEV_Gps_InitStart_CyclDataTimeOut + * ABSTRACT : Startup confirmation - Reception cycle data monitoring timeout matrix function + * FUNCTION : + * ARGUMENT : + * NOTE : + * RETURN : + ********************************************************************************/ +void DevGpsInitStartCyclDataTimeOut(void) { + uint8_t ret = RETRY_OFF; + + POSITIONING_LOG("DEV_Gps_InitStart_CyclDataTimeOut"); + + g_wcnct_err++; /* Count number of connection errors */ + + /* Stop all timers */ + DevGpsTimeStop(GPS_STARTUP_TIMER); + + DevGpsTimeStop(GPS_CYCL_TIMER); + + DevGpsTimeStop(GPS_RECV_ACK_TIMER); + + /* Hard reset judgment processing */ + ret = HardResetChkGpsCommon(); + if (RETRY_ON == ret) { + DevGpsResetReq(PNO_NONE, 0); + + SendSaveCmdGpsCommon(); /* State transition if pending commands exist( -> Sending) */ + } else if (RETRY_OFF == ret) { + /* Fixed number of hard resets completed */ + } else { + /* Retrying(with hard reset) */ + } + /* Clear cyclic receive data up to now */ + DevGpsCycleDataClear(); + /* Start confirmation monitoring timer setting */ + DevGpsTimeSet(GPS_STARTUP_TIMER); + return; +} + +/****************************************************************************** +@brief DEV_Gps_InitStart_NaviDataTimeOut<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/src/GpsCommon/MDev_Gps_Nmea.cpp b/src/GpsCommon/MDev_Gps_Nmea.cpp new file mode 100644 index 00000000..60fa1d8d --- /dev/null +++ b/src/GpsCommon/MDev_Gps_Nmea.cpp @@ -0,0 +1,928 @@ +/* + * @copyright Copyright (c) 2017-2020 TOYOTA MOTOR CORPORATION. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/** +* @file MDev_Gps_Nmea.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "MDev_Gps_Nmea.h" +#include "positioning_common.h" +#include "MDev_Gps_API.h" +#include "MDev_Gps_Main.h" +#include "MDev_GpsRecv.h" +#include "MDev_Gps_Common.h" +#include "MDev_GpsRollOver.h" + +/*---------------------------------------------------------------------------*/ +// Value define +#define ROLOVR_GPSWEEKCOR_NG 0xFF /* WEEK COMP. CORRECT CORRECTOR INVALUE */ +#define TMT_TGET_INI_INTERVAL (100) /* GPS time compensation interval immediately after startup (value with 10[msec] as 1) */ +#define TMT_DISCONTINUITY_TIME_TOUT_SEQ (0x5051) /* Sequence number of GPS time acquisition timeout based on date/time discontinuity */ + +/*---------------------------------------------------------------------------*/ +// Global values + +static TG_GPS_RCVDATA g_st_gpscycle_data; +u_int8 g_gps_week_cor_cnt = ROLOVR_GPSWEEKCOR_NG; + +extern uint8_t g_gpstime_raw_tdsts; + +const TG_GPS_CMD_ANA_TBL kGpsCmdAnaTblUblox[MDEV_GPSCMDANATBLNMEA_MAX] = { + /* Sentences Maximum length Receiving type Presence of notification Receive Format */ + /* NMEA */ + {{GPS_CMD_NMEA_RMC}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_RMC }, /* RMC information */ + {{GPS_CMD_NMEA_VTG}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_VTG }, /* VTG information */ + {{GPS_CMD_NMEA_GGA}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GGA }, /* GGA information */ + {{GPS_CMD_NMEA_GSA}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GSA }, /* GSA information */ + {{GPS_CMD_NMEA_GSV_1}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GSV1 }, /* GSV information1 */ + {{GPS_CMD_NMEA_GSV_2}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GSV2 }, /* GSV information2 */ + {{GPS_CMD_NMEA_GSV_3}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GSV3 }, /* GSV information3 */ + {{GPS_CMD_NMEA_GSV_4}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GSV4 }, /* GSV information4 */ + {{GPS_CMD_NMEA_GSV_5}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GSV5 }, /* GSV information5 */ + {{GPS_CMD_NMEA_GLL}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GLL }, /* GLL information */ + {{GPS_CMD_NMEA_GST}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_GST }, /* GST information */ + /* UBX */ + {{0xB5, 0x62, 0x0A, 0x04}, GPS_NMEA_MAX_SZ, RCV_RESP, TRUE, GPS_FORMAT_MON_VER }, /* MON-VER */ + {{0xB5, 0x62, 0x0B, 0x01}, GPS_NMEA_MAX_SZ, RCV_RESP, TRUE, GPS_FORMAT_AID_INI }, /* AID-INI */ + {{0xB5, 0x62, 0x05 }, GPS_NMEA_MAX_SZ, RCV_RESP, TRUE, GPS_FORMAT_ACK_ACKNACK }, /* ACK-ACKNACK */ + {{0xB5, 0x62, 0x01, 0x21}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_NAV_TIMEUTC }, /* NAV-TIMEUTC */ + {{0xB5, 0x62, 0x01, 0x22}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_NAV_CLOCK }, /* NAV-CLOCK */ + {{0xB5, 0x62, 0x02, 0x23}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_RXM_RTC5 }, /* RXM-RTC5 */ + {{0xB5, 0x62, 0x01, 0x30}, GPS_NMEA_MAX_SZ, RCV_CYCLE, TRUE, GPS_FORMAT_NAV_SVINFO }, /* NAV-SVINFO */ + {{0xB5, 0x62, 0x06, 0x23}, GPS_NMEA_MAX_SZ, RCV_RESP, TRUE, GPS_FORMAT_CFG_NAVX5 }, /* CFG-NAVX5 */ + {{ENDMARK}, 0, 0, FALSE, GPS_FORMAT_MIN } /* Table termination */ +}; + +/*---------------------------------------------------------------------------*/ +// Functions +/** + * @brief + * Time Calculation Process Considering Rollover + * + * @param[in/out] + * + * @return none + * @retval none + */ +static BOOL DevCalcRollOverTime(TG_TIM_ROLOVR_YMD* base_ymd, TG_TIM_ROLOVR_YMD* conv_ymd) { + BOOL ret = TRUE; + static u_int16 year_old = 0; + static u_int16 confirm_cnt = 0; + u_int8 gps_week_corr = g_gps_week_cor_cnt; + + if (gps_week_corr == ROLOVR_GPSWEEKCOR_NG) { + gps_week_corr = 0; + ret = FALSE; + } else { + if (year_old > base_ymd->year) { + confirm_cnt++; + if (confirm_cnt >= 10) { + confirm_cnt = 0; + year_old = base_ymd->year; + g_gps_week_cor_cnt = ROLOVR_GPSWEEKCOR_NG; + /* Starting the GPS time acquisition timer (1 second timer) based on date/time discontinuity */ + _pb_ReqTimerStart(PNO_CLK_GPS, TMT_DISCONTINUITY_TIME_TOUT_SEQ, TIMER_TYPE_USN, TMT_TGET_INI_INTERVAL); + } + ret = FALSE; + } else { + confirm_cnt = 0; + year_old = base_ymd->year; + } + } + /* Calculate time for rollover */ + GPSRollOverConvTime(base_ymd, conv_ymd, gps_week_corr); + + return ret; +} +/******************************************************************************************************/ + + +/** + * @brief + * NMEA data notification + */ +void DevGpsSndCycleDataNmea(void) { + /* Notifying vehicle sensor of NMEA */ + + MDEV_GPS_NMEA st_nmea; + TG_GPS_NMEA_INFO st_gps_nmea_info; + RET_API l_ret = RET_NORMAL; + BOOL b_get = FALSE; + u_int8 uc_nmea_data[GPS_NMEA_MAX_SZ]; + u_int32 ul_strlen = 0; + u_int16 us_offset = sizeof(TG_GPS_NMEA_INFO); + + memset(&st_nmea, 0x00, sizeof(st_nmea) ); /* QAC 3200 */ + memset(&st_gps_nmea_info, 0x00, sizeof(st_gps_nmea_info) ); /* QAC 3200 */ + memset(uc_nmea_data, 0x00, sizeof(uc_nmea_data) ); /* QAC 3200 */ + + /* Get received NMEA data from storage area(GGA) */ + b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_GGA); + if (b_get == TRUE) { + /* Data present */ + st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GGA; /* Receive flag */ + ul_strlen = strlen(reinterpret_cast<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/src/GpsCommon/MDev_Gps_TimerCtrl.cpp b/src/GpsCommon/MDev_Gps_TimerCtrl.cpp new file mode 100644 index 00000000..8dbf8dc7 --- /dev/null +++ b/src/GpsCommon/MDev_Gps_TimerCtrl.cpp @@ -0,0 +1,293 @@ +/* + * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/** +* @file MDev_Gps_TimerCtrl.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "MDev_Gps_TimerCtrl.h" + +// #include "MDev_Gps_Main.h" +// #include "MDev_GpsRecv.h" + +/*---------------------------------------------------------------------------*/ +// Global values + +static GPS_TIM_MNG g_st_tim_mng; + +/** Timer setting information table */ +static const GPS_TIM_INFO kTimInfo[TIM_NUM] = { + /* GSP related */ + {TIMVAL_GPS_STARTUP, PNO_NAVI_GPS_MAIN}, /* Start confirmation monitoring timer */ + {TIMVAL_GPS_RCVCYCLDAT, PNO_NAVI_GPS_MAIN}, /* Periodic reception data monitoring timer */ + {TIMVAL_GPS_RCVACK, PNO_NAVI_GPS_MAIN}, /* ACK reception monitoring timer */ + {TIMVAL_GPS_NAVIFST, PNO_NAVI_GPS_MAIN}, /* Initial Navigation Monitoring Timer */ + {TIMVAL_GPS_NAVICYCLE, PNO_NAVI_GPS_MAIN}, /* Navi monitoring timer */ + {TIMVAL_GPS_NAVIDISRPT, PNO_NAVI_GPS_MAIN}, /* Navigation Monitoring Disruption Log Output Timer */ + {TIMVAL_GPS_DIAGCLKGUARD, PNO_NAVI_GPS_MAIN}, /* Diagnosis provision time guard monitoring timer */ + {TIMVAL_GPS_NMEADATAGUARD, PNO_NAVI_GPS_MAIN}, /* NMEA data providing guard monitoring timer */ + {TIMVAL_GPS_RECOVERY, PNO_NAVI_GPS_MAIN}, /* GPS recovery timer */ + {TIMVAL_GPS_RECEIVERERR, PNO_NAVI_GPS_MAIN}, /* GPS receiver anomaly detection timer */ + /* Sensor related */ + {TIMVAL_SNS_RCVFSTDAT, PNO_VEHICLE_SENSOR}, /* Initial cyclic sensor data reception monitoring timer */ + {TIMVAL_SNS_RCVCYCLDAT, PNO_VEHICLE_SENSOR}, /* Cyclic sensor data reception monitoring timer */ + {TIMVAL_SNS_RCVDISRPT, PNO_VEHICLE_SENSOR}, /* Cyclic sensor data interruption log output timer */ +}; + +/*---------------------------------------------------------------------------*/ +// Functions + +static uint16_t TimeMakSeqNo(GPS_TIM_KIND tim_kind) { + GPS_TIM_MNG* pst_tim_mng = NULL; + u_int16 seq_no = 0; /* Timer sequence number */ + + pst_tim_mng = &g_st_tim_mng; + + /*------------------------------------------------------------------------*/ + /* Definition of Sequence Number */ + /* |------------------- Sequence number(2Byte) -----------------------| */ + /* 15 8 7 0 */ + /* +-------------------------------+-----------------------------------+ */ + /* | Timer type(1Byte) | Counter(1Byte)(0x01 ? 0xFF) | */ + /* +-------------------------------+-----------------------------------+ */ + /* The timer type is 0x00. ? (Number of timers-1) */ + /* counters is 0x01 ? 0xFF(Do not use 0x00) */ + /* (Counters are counted up each time a timer is started. */ + /* If the counter counts up when it is 0xFF, */ + /* be counted up from the 0x01.) */ + /*------------------------------------------------------------------------*/ + seq_no = ((u_int16)tim_kind << 8) | (pst_tim_mng->sts[tim_kind].cnt); + + return seq_no; +} + +static BOOL VehicleUtilitySetTimer(GPS_TIM_KIND tim_kind) { + GPS_TIM_MNG* pst_tim_mng = NULL; + const uint32_t * p_time_val; + const PNO* p_pno; + RET_API api_ret = RET_NORMAL; + u_int16 seq_no = 0; + BOOL ret = TRUE; + + // Initialize + pst_tim_mng = &g_st_tim_mng; + p_time_val = &(kTimInfo[tim_kind].timer_val); /* Timer set value */ + p_pno = &(kTimInfo[tim_kind].pno); /* Notify party PNO */ + + if (pst_tim_mng->sts[tim_kind].flag == TIMER_ON) { + /*-----------------------------------------------------------------------*/ + /* When the same timer has already started, */ + /* terminate without starting the timer because the timer is set multiple times. */ + /*-----------------------------------------------------------------------*/ + ret = FALSE; + } else { + /*-----------------------------------------------------------------------*/ + /* Count up the timer counter of the corresponding timer by 1. */ + /*-----------------------------------------------------------------------*/ + if (pst_tim_mng->sts[tim_kind].cnt >= TIM_CNTMAX) { + /*-----------------------------------------------------------------------*/ + /* When the count reaches the maximum number, it counts again from 1. */ + /*-----------------------------------------------------------------------*/ + pst_tim_mng->sts[tim_kind].cnt = TIM_CNTMIN; + } else { + /*-----------------------------------------------------------------------*/ + /* If the count has not reached the maximum, it is counted up. */ + /*-----------------------------------------------------------------------*/ + pst_tim_mng->sts[tim_kind].cnt++; + } + + /*-----------------------------------------------------------------------*/ + /* Creating timer sequence numbers */ + /*-----------------------------------------------------------------------*/ + seq_no = TimeMakSeqNo(tim_kind); + + /*-----------------------------------------------------------------------*/ + /* Start the timer */ + /*-----------------------------------------------------------------------*/ + api_ret = _pb_ReqTimerStart(*p_pno, seq_no, TIMER_TYPE_USN, static_cast<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/src/LineSensDrv/LineSensDrv_Api.cpp b/src/LineSensDrv/LineSensDrv_Api.cpp new file mode 100644 index 00000000..d363a4bd --- /dev/null +++ b/src/LineSensDrv/LineSensDrv_Api.cpp @@ -0,0 +1,136 @@ +/* + * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/** +* @file LineSensDrv_Api.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "LineSensDrv_Api.h" +#include "positioning_def.h" + +/*---------------------------------------------------------------------------*/ +// Macro definitions + +#define LSDRV_GPS_DATA_TOP_TH 5 // Threshold for determining the beginning of GPS data +#define LSDRV_SENS_DATA_RCV_WAIT_TIME 400 // Sensor data wait time of GPS reception flag ON(Total) +#define LSDRV_SENS_DATA_RCV_WAIT_TERM 50 // Sensor data wait time of GPS reception flag ON(1 time) + +#define LSDRV_EST_GPS_CNT_ARRAY_NUM 3U // Number of arrays for holding estimated GPS counter +#define LSDRV_EST_GPS_CNT_ENABLE_TH 5U // Estimated GPS counter enable/disable determination threshold +#define LSDRV_EST_GPS_CNT_ADD_VALUE 10U // Estimated GPS counter update addition value + +// GPS data reception cycle:1sec sensor counters: 100ms + +// for debug +#define LINE_SENS_DRV_API_DEBUG_SWITCH 0 // 0:OFF 1:ON + +/*---------------------------------------------------------------------------*/ +// Global variable + +static HANDLE g_gps_irq_mutex = NULL; // GPS received flag Mutex handles +static BOOL g_rcv_pps_int = FALSE; // PPS interrupt reception flag (GPS->_CWORD102_) +static BOOL g_rcv_gps_irq = FALSE; // GPS reception flag (GPS->_CWORD56_) +static u_int8 g_rcv_gps_sens_cnt_tmp = 0; // Sensor counter when GPS reception flag is ON(for retention) +static u_int8 g_rcv_gps_sens_cnt = 0; // Sensor counter when GPS reception flag is ON +static u_int8 g_gps_sens_cnt_top = 0; // Sensor counter when first GPS data is received +static u_int8 g_est_gps_cnt[LSDRV_EST_GPS_CNT_ARRAY_NUM]; // Array for storing estimated GPS counter values +static BOOL g_est_gps_cnt_available = FALSE; // Estimated GPS counter value enable/disable judgment + +typedef struct VehicleSensDataMaster { + DID ul_did; // Data ID + u_int16 us_size; // Size of the data + u_int8 uc_rcv_flag; // Receive flag + u_int8 uc_sns_cnt; // Sensor counter + u_int8 uc_data[132]; // Vehicle sensor data +} VEHICLESENS_DATA_MASTER; + + +/******************************************************************************* + * MODULE : DeliveryLineSensorDataPositioning + * ABSTRACT : LineSensor vehicle signaling notification messages sending process + * FUNCTION : Send LineSensor vehicle signalling notification messages + * ARGUMENT : *pstSendBuf:Transmitted data + * : uc_data_num :Number of sent data + * NOTE : + * RETURN : None + ******************************************************************************/ +void DeliveryLineSensorDataPositioning(LSDRV_MSG_LSDATA_G* pst_send_buf, u_int8 uc_data_num) { + if (pst_send_buf != NULL) { + /* Initializing sent messages */ + memset(reinterpret_cast<void *>(&(pst_send_buf->st_head)), 0, sizeof(T_APIMSG_MSGBUF_HEADER)); + + /* Message Header Settings */ + pst_send_buf->st_head.hdr.sndpno = PNO_LINE_SENS_DRV; /* Source process number */ + pst_send_buf->st_head.hdr.cid = CID_LINESENS_VEHICLE_DATA_G; /* Command ID */ + pst_send_buf->st_head.hdr.msgbodysize = sizeof(LSDRV_MSG_LSDATA_DAT_G); /* Message data body length */ + + /* Message data is already set */ + pst_send_buf->st_para.uc_data_num = uc_data_num; + + /* Send messages */ + (void)_pb_ZcSndMsg(PNO_VEHICLE_SENSOR, sizeof( LSDRV_MSG_LSDATA_G ), 0); + } +} + +/******************************************************************************* + * MODULE : LineSensDrvApi_Initialize + * ABSTRACT : LineSensDrvApi initialization process + * FUNCTION : LineSensDrvApi initialization process + * ARGUMENT : - + * NOTE : + * RETURN : - + ******************************************************************************/ +BOOL LineSensDrvApiInitialize(void) { + BOOL ret = TRUE; + + g_gps_irq_mutex = _pb_CreateMutex(NULL, FALSE, MUTEX_GPS_IRQ_FLG); + + if (g_gps_irq_mutex == 0) { + ret = FALSE; + } else { + g_rcv_gps_irq = FALSE; + g_rcv_gps_sens_cnt_tmp = 0; + LineSensDrvApiInitEstGpsCnt(); + ret = TRUE; + } + + return (ret); +} + +/******************************************************************************* + * MODULE : LineSensDrvApi_InitEstGpsCnt + * ABSTRACT : Estimated GPS counter related parameter initialization processing + * FUNCTION : Estimated GPS counter related parameter initialization processing + * ARGUMENT : - + * NOTE : + * RETURN : - + ******************************************************************************/ +void LineSensDrvApiInitEstGpsCnt(void) { + /* Initializing process */ + g_rcv_pps_int = FALSE; + g_rcv_gps_sens_cnt = 0; + g_gps_sens_cnt_top = 0; + + (void)memset(reinterpret_cast<void *>(&g_est_gps_cnt[0]), 0, sizeof(g_est_gps_cnt)); + g_est_gps_cnt_available = FALSE; + + return; +} + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/src/LineSensDrv/LineSensDrv_Sensor.cpp b/src/LineSensDrv/LineSensDrv_Sensor.cpp new file mode 100644 index 00000000..78ae4881 --- /dev/null +++ b/src/LineSensDrv/LineSensDrv_Sensor.cpp @@ -0,0 +1,622 @@ +/* + * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/** +* @file LineSensDrv_Snesor.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "LineSensDrv_Sensor.h" +#include "LineSensDrv_Api.h" + +/*---------------------------------------------------------------------------*/ +// Value Define + +#define LSDRV_MASK_WORD_L 0x00FF +#define LSDRV_MASK_WORD_U 0xFF00 + +#define LINE_SENS_DRV_SENSOR_DEBUG_FACTORY 0 +#define LINE_SENS_DRV_SENSOR_DEBUG_DIAG 0 + +#define VEHICLE_SNS_INFO_PULSE_NUM 32 + +/*---------------------------------------------------------------------------*/ +// Global variable + +static LSDRV_SPEEDKMPH_DAT g_speed_kmph_data; +static uint8_t g_rcv_data_len; /* Receive SYS data length */ +extern uint8_t g_uc_vehicle_reverse; +/*---------------------------------------------------------------------------*/ +// Functions + +/******************************************************************************* + * MODULE : LineSensDrv_Sensor + * ABSTRACT : Sensor data reception processing + * FUNCTION : Convert sensor data to delivery format + * ARGUMENT : *ucRcvdata : Data pointer + * NOTE : + * RETURN : None + ******************************************************************************/ +void LineSensDrvSensor(u_int8* uc_rcvdata) { + u_int8 uc_sens_cnt = 0; + u_int16 us_sp_kmph = 0; /* Vehicle speed(km/h) */ + u_int16 us_sp_pls1 = 0; /* Total vehicle speed pulse(Latest) */ + u_int16 us_sp_pls2 = 0; /* Total vehicle speed pulse(Last time) */ + u_int8 us_sp_ret = 0; /* Last vehicle speed information acquisition result */ + u_int8 uc_size = 0; /* Size of the data */ + u_int16 us_cnt = 0; /* Data counter */ + u_int16 us_cnt2 = 0; /* Data counter */ + u_int8* uc_data_pos = NULL; /* Data storage location */ + LSDRV_MSG_LSDATA_G* p_snd_buf = NULL; + LSDRV_LSDATA_G* p_snd_data_base = NULL; + LSDRV_LSDATA_G* p_snd_data = NULL; + RET_API ret = RET_NORMAL; + + /* Receive sensor data top address acquisition */ + uc_data_pos = (uc_rcvdata); + + /* Create send buffer/delivery all received data */ + ret = _pb_GetZcSndBuf(PNO_VEHICLE_SENSOR, reinterpret_cast<void**>(&p_snd_buf)); + if ((ret == RET_NORMAL) && (p_snd_buf != NULL)) { + p_snd_data_base = p_snd_buf->st_para.st_data; + (void)memset(p_snd_data_base, 0, LSDRV_KINDS_MAX * sizeof( LSDRV_LSDATA_G )); + + /* Sensor counter */ + p_snd_data = p_snd_data_base + LSDRV_SNS_COUNTER; + uc_sens_cnt = (u_int8)*(uc_data_pos); + p_snd_data->ul_did = VEHICLE_DID_SNS_COUNTER; /* Data ID */ + p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_1; /* Size of the data */ + p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ + p_snd_data->uc_data[0] = uc_sens_cnt; /* Data content */ + uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_1); + + /* Gyro output */ + p_snd_data = p_snd_data_base + LSDRV_GYRO_EXT; + p_snd_data->ul_did = VEHICLE_DID_GYRO_EXT; /* Data ID */ + p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */ + p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ + for (us_cnt = 0; us_cnt < LSDRV_SNDMSG_DTSIZE_20; us_cnt++) { + /* Since [0] is older and [9] is newer, the order of received data is switched. */ + /* Be in reverse order for endian conversion */ + p_snd_data->uc_data[LSDRV_SNDMSG_DTSIZE_20 - (us_cnt + 1)] = (u_int8)*(uc_data_pos + us_cnt); + } + uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20); + + p_snd_data = p_snd_data_base + LSDRV_GYRO_X; + p_snd_data->ul_did = VEHICLE_DID_GYRO; /* Data ID */ + p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */ + p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ + for (us_cnt = 0; us_cnt < LSDRV_SNDMSG_DTSIZE_20; us_cnt++) { + /* Since [0] is older and [9] is newer, the order of received data is switched. */ + /* Be in reverse order for endian conversion */ + p_snd_data->uc_data[LSDRV_SNDMSG_DTSIZE_20 - (us_cnt + 1)] = (u_int8)*(uc_data_pos + us_cnt); + } + uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20); + + /* Reverse flag */ + p_snd_data = p_snd_data_base + LSDRV_REV; + p_snd_data->ul_did = VEHICLE_DID_REV; /* Data ID */ + p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_1; /* Size of the data */ + p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor Counter */ + p_snd_data->uc_data[0] = g_uc_vehicle_reverse; + /* Gyro Temperature */ + p_snd_data = p_snd_data_base + LSDRV_GYRO_TEMP; + p_snd_data->ul_did = VEHICLE_DID_GYRO_TEMP; /* Data ID */ + p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_2; /* Size of the data */ + p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ + p_snd_data->uc_data[1] = (u_int8)(*(uc_data_pos ) & (u_int8)(LSDRV_TEMP_MASK >> 8)); + p_snd_data->uc_data[0] = (u_int8)(*(uc_data_pos + 1) & (u_int8)(LSDRV_TEMP_MASK)); + uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_2); + + /* Vehicle speed pulse */ + p_snd_data = p_snd_data_base + LSDRV_SPEED_PULSE; + p_snd_data->ul_did = VEHICLE_DID_SPEED_PULSE; /* Data ID */ + p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */ + p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ + for (us_cnt = 0; us_cnt < LSDRV_SNDMSG_DTSIZE_20; us_cnt++) { + /* Since [0] is older and [9] is newer, the order of received data is switched. */ + /* Be in reverse order for endian conversion */ + p_snd_data->uc_data[LSDRV_SNDMSG_DTSIZE_20 - (us_cnt + 1)] = (u_int8)*(uc_data_pos + us_cnt); + } + uc_data_pos = (uc_data_pos + LSDRV_SNDMSG_DTSIZE_20); + + /* Vehicle speed(km/h) */ + p_snd_data = p_snd_data_base + LSDRV_SPEED_PULSE; + us_sp_kmph = 0; + us_sp_pls1 = (u_int16)p_snd_data->uc_data[1]; + us_sp_pls1 = (u_int16)((us_sp_pls1 << 8) | p_snd_data->uc_data[0]); + us_sp_ret = LineSensDrvGetLastSpeedPulse(&us_sp_pls2, us_sp_pls1, uc_sens_cnt); + + LineSensDrvSetLastSpeedPulse(us_sp_pls1, uc_sens_cnt); + + p_snd_data = p_snd_data_base + LSDRV_SPEED_KMPH; + if (us_sp_ret != LSDRV_SPKMPH_INVALID) { + /* Vehicle speed pulse before 100 ms is valid */ + LineSensDrvSpeedPulseSave(us_sp_pls1, us_sp_pls2, uc_sens_cnt); + us_sp_kmph = LineSensDrvSpeedCalc(uc_sens_cnt); + /* The size can be set only when the vehicle speed [km/h] is calculated. "0" is notified to the vehicle sensor when the size cannot be set. */ + p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_2; /* Size of the data */ + } + p_snd_data->ul_did = VEHICLE_DID_SPEED_KMPH; /* Data ID */ + p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ + p_snd_data->uc_data[0] = (u_int8)(us_sp_kmph & 0x00FF); + p_snd_data->uc_data[1] = (u_int8)(us_sp_kmph >> 8); + + POSITIONING_LOG("[LOG, %d(cnt), %d(km/h), %d(pls1), %d(pls2)] \r\n", + uc_sens_cnt, + us_sp_kmph, + us_sp_pls1, + us_sp_pls2); + + /* G-Sensor X-axes */ + p_snd_data = p_snd_data_base + LSDRV_GSENSOR_X; + p_snd_data->ul_did = VEHICLE_DID_GSNS_X; /* Data ID */ + p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */ + p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ + us_cnt2 = p_snd_data->uc_size - 1; + for (us_cnt = 0; us_cnt < 10; us_cnt++) { + /* Since [0] is older and [9] is newer, the order of received data is switched. */ + p_snd_data->uc_data[us_cnt2 ] = (u_int8)*( uc_data_pos + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt) ); + p_snd_data->uc_data[us_cnt2 - 1] = (u_int8)*( uc_data_pos + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt) + 1); + us_cnt2 = us_cnt2 - 2; + } + + /* G-Sensor Y-axes */ + p_snd_data = p_snd_data_base + LSDRV_GSENSOR_Y; + p_snd_data->ul_did = VEHICLE_DID_GSNS_Y; /* Data ID */ + p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_20; /* Size of the data */ + p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ + us_cnt2 = p_snd_data->uc_size - 1; + for (us_cnt = 0; us_cnt < 10; us_cnt++) { + /* Since [0] is older and [9] is newer, the order of received data is switched. */ + p_snd_data->uc_data[us_cnt2] = (u_int8)*( (uc_data_pos + sizeof(u_int16)) \ + + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt)); + + p_snd_data->uc_data[us_cnt2-1] = (u_int8)*( (uc_data_pos + sizeof(u_int16)) \ + + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * us_cnt) + 1); + us_cnt2 = us_cnt2 - 2; + } + uc_data_pos = ( uc_data_pos + (sizeof(SENSORINPUT_INFO_DAT_GSENS) * 10) ); + + /* Inter-Pulse time */ + p_snd_data = p_snd_data_base + LSDRV_PULSE_TIME; + p_snd_data->ul_did = VEHICLE_DID_PULSE_TIME; /* Data ID */ + p_snd_data->uc_size = LSDRV_SNDMSG_DTSIZE_132; /* Size of the data */ + p_snd_data->uc_sns_cnt = uc_sens_cnt; /* Sensor counter */ + + /* Clear the buffer for variable length */ + (void)memset(&p_snd_data->uc_data[0], 0x00, sizeof(p_snd_data->uc_data)); + + if (g_rcv_data_len == LSDRV_PLSTIME_LEN) { + /* Inter-Pulse time (number of items + time). The number of items is stored at the beginning. */ + uc_size = (u_int8)*uc_data_pos; + if (uc_size > VEHICLE_SNS_INFO_PULSE_NUM) { + uc_size = VEHICLE_SNS_INFO_PULSE_NUM; + } + p_snd_data->uc_data[0] = uc_size; + p_snd_data->uc_data[1] = 0x00; + p_snd_data->uc_data[2] = 0x00; + p_snd_data->uc_data[3] = 0x00; + uc_data_pos = ( uc_data_pos + sizeof(u_int8) ); + + /* Since [0] is old and [31] is new in the received time, the order is changed. */ + /* Be in reverse order for endian conversion */ + for (us_cnt = 0; us_cnt < (uc_size * sizeof(u_int32)); us_cnt++) { + p_snd_data->uc_data[(uc_size * sizeof(u_int32)) - (us_cnt + 1) + 4] = (u_int8)*(uc_data_pos + us_cnt); + } + } + + /* Messaging */ + DeliveryLineSensorDataPositioning(p_snd_buf, LSDRV_KINDS_MAX); + } + + return; +} + +/******************************************************************************* + * MODULE : LineSensDrv_SpeedCalc + * ABSTRACT : Vehicle speed calculation processing for sensor data + * FUNCTION : Calculate vehicle speed for sensor data + * ARGUMENT : uc_sens_cnt : Sensor counter + * NOTE : + * RETURN : Vehicle speed(0.01km/h) + ******************************************************************************/ +u_int16 LineSensDrvSpeedCalc(u_int8 uc_sens_cnt) { + u_int32 ul_sp_caluc = 0; /* Vehicle speed(2^-8km/h) */ + u_int16 us_speed = 0; /* Vehicle speed(km/h) */ + u_int8 uc_ptr = 0; /* Data storage pointer for sensor data */ /* #010 */ + u_int8 uc_ptr2 = 0; /* Data storage pointer for sensor data */ /* #010 */ + u_int32 ul_work = 0; /* Variables for unsigned 32-bit type calculations */ /* #010 */ + double d_work = 0.0; /* Variables for calculating double types */ /* #010 */ /* Ignore -> MISRA-C++:2008 Rule 3-9-2 */ + u_int16 us_sens_cnt_search = 0; /* Sensor counter to be searched */ /* #010 */ + u_int16 us_sens_cnt_ref = 0; /* Sensor counters to be compared */ /* #010 */ + int32 i = 0; /* Generic counters */ /* #010 */ + int32 j = 0; /* Generic counters */ /* #010 */ + u_int16 us_offset = 0; /* Offset value */ /* #010 */ + + /* #016 start */ + /* Is the number of data that can be calculated at the vehicle speed already received? */ + if (LSDRV_PLSDATA_NRCV == g_speed_kmph_data.uc_calc_start) { + /* Do not compute if there is not enough data. */ + } else { + if (0 == g_speed_kmph_data.uc_ptr) { + uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1; + uc_ptr2 = LSDRV_SPKMPH_TBL_NUM - 1; + } else { + uc_ptr = g_speed_kmph_data.uc_ptr - 1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ + uc_ptr2 = g_speed_kmph_data.uc_ptr - 1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ + } + + ul_work = 0; + if (LSDRV_SPKMPH_AVE_TIME > uc_sens_cnt) { + us_offset = LSDRV_SENSCNT_BRW_ADD; + } else { + us_offset = 0; + } + + us_sens_cnt_search = (u_int16)uc_sens_cnt + us_offset - LSDRV_SPKMPH_AVE_TIME; + + for (i = 0; i < LSDRV_SPKMPH_TBL_NUM; i++) { + /* Invalid data is detected, and the search is completed. */ + if (LSDRV_SPKMPH_DATA_DIS == g_speed_kmph_data.st_data[uc_ptr].uc_flag) { + break; + } + + /* When the sensor counter is 29 or less, the borrow is considered. */ + if (LSDRV_SPKMPH_AVE_TIME > g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt) { + us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt + us_offset; + } else { + us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt; + } + + /* Checking the sensor counter to finish search */ + if (us_sens_cnt_search >= us_sens_cnt_ref) { + break; + } + + /* Add to calculate average value */ + ul_work += (u_int32)g_speed_kmph_data.st_data[uc_ptr].us_speed_pulse; + + if (0 == uc_ptr) { + uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1; /* To the end of the data table */ + } else { + uc_ptr--; /* To the previous data */ + } + } + + /* Averaging computation */ + if (0 == i) { + d_work = 0; + } else { + if (ul_work == 1) { + for (j = 0; j < LSDRV_SPKMPH_TBL_NUM; j++) { + if ((g_speed_kmph_data.st_data[uc_ptr2].us_speed_pulse == 1) + && (g_speed_kmph_data.st_data[uc_ptr2].uc_noise_flag == 1)) { + ul_work = 0; + break; + } else if (g_speed_kmph_data.st_data[uc_ptr2].us_speed_pulse == 1 && + g_speed_kmph_data.st_data[uc_ptr2].uc_noise_flag == 0) { + ul_work = 1; + break; + } else { + /* nop */ + } + + /* Borrow actions for pointer values */ + if (0 == uc_ptr2) { + uc_ptr2 = LSDRV_SPKMPH_TBL_NUM - 1; /* To the end of the data table */ + } else { + uc_ptr2--; /* To the previous data */ + } + } + } + + d_work = static_cast<double>(ul_work); + d_work = d_work / static_cast<double>(i); + d_work = d_work * static_cast<double>(LSDRV_SENS_COEFFICIENT); + d_work = d_work * 100; /* [1km/h] -> [0.01km/h] */ + d_work = d_work + 0.5; /* Preparation for rounding */ + } + + ul_sp_caluc = static_cast<int32>(d_work); + + /* When the vehicle speed calculation result overflows, the upper limit value is used for clipping. */ + if (LSDRV_PLSMAX - 1 >= ul_sp_caluc) { + us_speed = (u_int16)ul_sp_caluc; + } else { + us_speed = (u_int16)(LSDRV_PLSMAX - 1); + } + } + + return us_speed; +} + +/******************************************************************************* + * MODULE : LineSensDrv_SpeedKmphDataInit + * ABSTRACT : Data table initialization process for vehicle speed calculation + * FUNCTION : Initialize the data table for calculating the vehicle speed + * ARGUMENT : None + * NOTE : + * RETURN : None + ******************************************************************************/ +void LineSensDrvSpeedKmphDataInit(void) { + int32 i = 0; + + memset(reinterpret_cast<void*>(&g_speed_kmph_data), static_cast<int32>(0), (size_t)sizeof(g_speed_kmph_data)); + + /* Disable all data storage flags */ + for (i = 0; i < LSDRV_SPKMPH_TBL_NUM; i++) { + g_speed_kmph_data.st_data[i].uc_flag = LSDRV_SPKMPH_DATA_DIS; + } + + return; +} + +/******************************************************************************* + * MODULE : LineSensDrv_SpeedPulseSave + * ABSTRACT : Sensor data vehicle speed pulse save processing + * FUNCTION : Saving the vehicle speed pulse of the sensor data to the data table for calculating the vehicle speed + * ARGUMENT : us_sp1 : Vehicle speed pulse of the latest information + * : us_sp2 : Vehicle speed pulse of the previous information + * : uc_sens_cnt : Sensor counter + * NOTE : + * RETURN : None + ******************************************************************************/ +void LineSensDrvSpeedPulseSave(u_int16 us_sp1, u_int16 us_sp2, u_int8 uc_sens_cnt) { + u_int16 us_sp_diff = 0; /* Vehicle speed pulse difference */ + u_int8 uc_ptr = 0; /* Data storage pointer for sensor data */ /* #010 */ + u_int8 us_last_ptr = 0; /* Last pointer */ /* #010 */ + int32 i = 0; /* Generic counters */ /* #010 */ + u_int8 uc_fstsns_cnt = 0; /* Initial sensor data sensor counter value */ /* #016 */ + int32 l_data_num = 0; /* Number of registered data */ /* #016 */ + u_int8 uc_noise_flag = 0; /* Noise flag */ /* #017 */ + + /* Calculate the vehicle speed pulse difference between the latest and last 100ms information */ + if (us_sp1 >= us_sp2) { + /* The cumulative pulse number of the latest information is larger. */ + us_sp_diff = us_sp1 - us_sp2; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ + } else { + /* The cumulative pulse number of the latest information is smaller (the accumulated pulse overflows) */ + us_sp_diff = (LSDRV_PLSMAX - us_sp2) + us_sp1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ + } + + /* Call noise check only if us_sp_diff is 1 */ + if (us_sp_diff == 1) { + uc_noise_flag = LineSensDrvCheckNoise(uc_sens_cnt); + } + + /* Saving sensor data vehicle speed pulse in data table for vehicle speed calculation */ + if (LSDRV_PLSDATA_NRCV == g_speed_kmph_data.uc_sns_rcv) { + if (LSDRV_PLSDATA_RCV == g_speed_kmph_data.uc_fstsns_rcv) { + /* If the sensor data has already been received for the first time, set the temporary sensor counter value when the sensor data has been saved for the first time. */ + uc_fstsns_cnt = uc_sens_cnt; + for (i = (LSDRV_SPKMPH_TBL_NUM - 1); i >= 0 ; i--) { + if (LSDRV_SPKMPH_DATA_EN == g_speed_kmph_data.st_data[i].uc_flag) { + /* Data storage flag is valid */ + if (0 != uc_fstsns_cnt) { + uc_fstsns_cnt--; + } else { + uc_fstsns_cnt = LSDRV_SENSCNT_MAX; + } + + g_speed_kmph_data.st_data[i].uc_sens_cnt = uc_fstsns_cnt; + } + } + } + + /* Sensor data reception status <- "Received" */ + g_speed_kmph_data.uc_sns_rcv = LSDRV_PLSDATA_RCV; + } + + uc_ptr = g_speed_kmph_data.uc_ptr; + + /* If the sensor counter is the same as the previous one, overwrite update */ + if (0 == uc_ptr) { + us_last_ptr = LSDRV_SPKMPH_TBL_NUM - 1; + } else { + us_last_ptr = uc_ptr - 1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ + } + + if (g_speed_kmph_data.st_data[us_last_ptr].uc_sens_cnt == uc_sens_cnt) { + /* Next update of the data storage location */ + if (0 == g_speed_kmph_data.uc_ptr) { + g_speed_kmph_data.uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1; + } else { + g_speed_kmph_data.uc_ptr--; + } + + uc_ptr = g_speed_kmph_data.uc_ptr; + } + + /* Input into data table for calculation of vehicle speed */ + g_speed_kmph_data.st_data[uc_ptr].uc_flag = LSDRV_SPKMPH_DATA_EN; /* Data validity */ + g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt = uc_sens_cnt; /* Sensor counter input */ + g_speed_kmph_data.st_data[uc_ptr].us_speed_pulse = us_sp_diff; /* Vehicle speed pulse difference */ + g_speed_kmph_data.st_data[uc_ptr].uc_noise_flag = uc_noise_flag; /* Noise flag */ /* #017 */ + + /* Next update of the data storage location */ + if ((LSDRV_SPKMPH_TBL_NUM - 1) <= g_speed_kmph_data.uc_ptr) { + g_speed_kmph_data.uc_ptr = 0; + } else { + g_speed_kmph_data.uc_ptr++; + } + + /* Determine whether the vehicle speed can be calculated. */ + if (g_speed_kmph_data.uc_calc_start == LSDRV_PLSDATA_NRCV) { + /* Data count detection */ + l_data_num = 0; + for (i = 0; i < LSDRV_SPKMPH_TBL_NUM; i++) { + if (LSDRV_SPKMPH_DATA_EN == g_speed_kmph_data.st_data[i].uc_flag) { + l_data_num++; + } + } + + if (LSDRV_SPKMPH_MIN_DATA_N <= l_data_num) { + /* Vehicle Speed Calculation Required Data Number Received */ + g_speed_kmph_data.uc_calc_start = LSDRV_PLSDATA_RCV; + } + } + + return; +} + +/******************************************************************************* + * MODULE : LineSensDrv_CheckNoise + * ABSTRACT : Sensor data noise check processing + * FUNCTION : The vehicle speed pulse is saved in the data table for the vehicle speed calculation. + * ARGUMENT : uc_sens_cnt : Sensor counter of the latest data + * NOTE : + * RETURN : Noise flag + ******************************************************************************/ +u_int8 LineSensDrvCheckNoise(u_int8 uc_sens_cnt) { + int32 i = 0; /* Generic counters */ + u_int16 us_sens_cnt_search = 0; /* Sensor counter to be searched*/ + u_int8 uc_ptr = 0; /* Data storage pointer */ + u_int16 us_offset = 0; /* Offset value */ + u_int8 noise_flag = 0; /* Noise flag */ + u_int16 us_sens_cnt_ref = 0; /* Sensor counters to be compared */ + + /* If there is no point where the difference in vehicle speed pulse is 1 or more + between -1 and -14 of sensor counter of the latest data, + set the noise flag of the latest data to 1. */ + /* Set the noise flag to 1 */ + noise_flag = 1; + + /* The Target is the one before the storage location of the latest data. */ + if (0 == g_speed_kmph_data.uc_ptr) { + uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1; + } else { + uc_ptr = g_speed_kmph_data.uc_ptr - 1; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ + } + + if (LSDRV_SPKMPH_NOISE_TIME > uc_sens_cnt) { + us_offset = LSDRV_SENSCNT_BRW_ADD; + } else { + us_offset = 0; + } + + us_sens_cnt_search = (u_int16)uc_sens_cnt + us_offset - LSDRV_SPKMPH_NOISE_TIME; + + for (i = 0; i < LSDRV_SPKMPH_NOISE_TIME; i++) { + /* When the sensor counter is 15 or less, the borrow is considered. */ + if (LSDRV_SPKMPH_NOISE_TIME > g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt) { + us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt + us_offset; + } else { + us_sens_cnt_ref = g_speed_kmph_data.st_data[uc_ptr].uc_sens_cnt; /* Ignore -> MISRA-C++:2008 Rule 5-0-5 */ + } + + /* Checking the sensor Counter to Finish Search */ + if (us_sens_cnt_ref <= us_sens_cnt_search) { + noise_flag = 1; + break; + } else { + if (g_speed_kmph_data.st_data[uc_ptr].us_speed_pulse >= 1) { + noise_flag = 0; + break; + } + } + + /* Borrow actions for pointer values */ + if (0 == uc_ptr) { + uc_ptr = LSDRV_SPKMPH_TBL_NUM - 1; /* To the end of the data table */ + } else { + uc_ptr--; /* To the previous data */ + } + } + + return noise_flag; +} + +/** + * @brief + * Return the last (100ms ago) vehicle speed pulse + * + * @param[out] us_sp2 Last vehicle speed pulse + * @param[in] us_sp1 The latest vehicle speed pulse + * @param[in] uc_sens_cnt Latest sensor counter + * + * @return LSDRV_SPKMPH_INVALID Vehicle speed pulse information valid<br> + * LSDRV_SPKMPH_VALID Vehicle speed pulse information invalid + */ +u_int8 LineSensDrvGetLastSpeedPulse(u_int16* us_sp2, u_int16 us_sp1, u_int8 uc_sens_cnt) { + u_int8 ret = LSDRV_SPKMPH_INVALID; /* Return value */ + u_int16 sp_pls_diff = 0; /* Vehicle speed pulse difference */ + u_int16 sp_pls = 0; /* Vehicle speed pulse every 100 ms */ + u_int8 cnt_diff = 0; /* Sensor counter difference */ + + /* Check if the last vehicle speed pulse has been set */ + if (g_speed_kmph_data.st_last_data.uc_flag == LSDRV_SPKMPH_DATA_EN) { + /* Differential calculation of sensor counter */ + if (uc_sens_cnt >= g_speed_kmph_data.st_last_data.uc_sens_cnt) { + /* Latest sensor counter is larger */ + cnt_diff = uc_sens_cnt - g_speed_kmph_data.st_last_data.uc_sens_cnt; + } else { + /* Last sensor counter is larger(sensor counter overflows) */ + cnt_diff = (LSDRV_SENSCNT_MAX - g_speed_kmph_data.st_last_data.uc_sens_cnt) + uc_sens_cnt + 1; + } + + /* Check if sensor counter is continuous */ + if (cnt_diff <= 1) { + /* Continuous or same as the previous one, so the previous (100ms previous) vehicle speed pulse is set as it is */ + *us_sp2 = g_speed_kmph_data.st_last_data.us_speed_pulse; + } else { + /* Determine the vehicle speed pulse 100ms ago from the average considering the skipped portion because it is not consecutive. */ + if (us_sp1 >= g_speed_kmph_data.st_last_data.us_speed_pulse) { + /* Larger latest cumulative vehicle speed pulse */ + sp_pls_diff = us_sp1 - g_speed_kmph_data.st_last_data.us_speed_pulse; + } else { + /* Last cumulative vehicle speed pulse is larger(Cumulative vehicle speed pulse overflows) */ + sp_pls_diff = (LSDRV_PLSMAX - g_speed_kmph_data.st_last_data.us_speed_pulse) + us_sp1; + } + + /* Calculate average vehicle speed pulse including skip period */ + sp_pls = (u_int16)(sp_pls_diff / cnt_diff); + + /* Calculate the vehicle speed pulse 100ms ahead from the vehicle speed average */ + if (us_sp1 >= sp_pls) { + /* Does not overflow even if the 100ms vehicle speed pulse is pulled from the latest one. */ + *us_sp2 = us_sp1 - sp_pls; + } else { + /* Subtracting a 100ms vehicle speed pulse from the latest one overflows */ + *us_sp2 = LSDRV_PLSMAX - (sp_pls - us_sp1); + } + } + + ret = LSDRV_SPKMPH_VALID; + } + + return ret; +} + +/** + * @brief + * Return the last (100ms ago) vehicle speed pulse + * + * @param[in] us_sp Vehicle speed pulse + * @param[in] uc_sens_cnt Sensor counter + */ +void LineSensDrvSetLastSpeedPulse(u_int16 us_sp, u_int8 uc_sens_cnt) { + /* Vehicle speed pulse information valid setting */ + g_speed_kmph_data.st_last_data.uc_flag = LSDRV_SPKMPH_DATA_EN; + /* Sensor counter setting */ + g_speed_kmph_data.st_last_data.uc_sens_cnt = uc_sens_cnt; + /* Vehicle speed pulse setting */ + g_speed_kmph_data.st_last_data.us_speed_pulse = us_sp; +} + +/*---------------------------------------------------------------------------*/ +/*EOF*/ diff --git a/src/LineSensDrv/LineSensDrv_Thread.cpp b/src/LineSensDrv/LineSensDrv_Thread.cpp new file mode 100644 index 00000000..9fb1c299 --- /dev/null +++ b/src/LineSensDrv/LineSensDrv_Thread.cpp @@ -0,0 +1,125 @@ +/* + * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/** +* @file LineSensDrv_Thread.cpp +*/ + +/*---------------------------------------------------------------------------*/ +// Include files + +#include "LineSensDrv_Thread.h" +#include <native_service/frameworkunified_types.h> +#include <native_service/frameworkunified_framework_if.h> +#include "positioning_def.h" +#include "positioning_common.h" +/*---------------------------------------------------------------------------*/ +// Global variable + +static u_int8 g_sys_recv_flg = 0; // For debugging +char g_threadname[] = "POS_Sens"; +uint8_t g_uc_vehicle_reverse = 0; +uint8_t g_uc_reverse_state = 0; +static BOOL g_line_sens_thread_stop = FALSE; + +#define POS_REV_STATE_PROCESSING 0 +#define POS_REV_STATE_IDLE 1 + +/******************************************************************************* + * MODULE : LineSensDrv_Thread + * ABSTRACT : LineSensor driver thread main process + * FUNCTION : Main processing + * ARGUMENT : lpvPara : + * NOTE : + * RETURN : + ******************************************************************************/ +EFrameworkunifiedStatus LineSensDrvThread(HANDLE h_app) { + int32 l_ret = RET_LSDRV_SUCCESS; + EFrameworkunifiedStatus l_status = eFrameworkunifiedStatusOK; + + + (void)PosSetupThread(h_app, ETID_POS_SENS); + + /* Execute the initialization processes */ + l_ret = LineSensDrvMainThreadInit(h_app); + if (RET_LSDRV_SUCCESS != l_ret) { + l_status = eFrameworkunifiedStatusFail; + } + + return l_status; +} + +/******************************************************************************* + * MODULE : LineSensDrv_MainThread_Init + * ABSTRACT : Thread initialization process + * FUNCTION : Initialize thread + * ARGUMENT : None + * NOTE : + * RETURN : RET_LSDRV_SUCCESS:Success in initialization + * RET_LSDRV_ERROR :Initialization failed + ******************************************************************************/ +int32 LineSensDrvMainThreadInit(HANDLE h_app) { + int32 l_ret = RET_LSDRV_SUCCESS; + BOOL b_ret = TRUE; + + /****** Global variable initialization **********/ + LineSensDrvParamInit(); + + /****** LineSensDrvApi initialization **********/ + b_ret = LineSensDrvApiInitialize(); + + if (TRUE != b_ret) { + l_ret = RET_LSDRV_ERROR; + } + + return l_ret; +} + +/******************************************************************************* + * MODULE : LineSensDrv_Param_Init + * ABSTRACT : Global variable initialization processing + * FUNCTION : Initialize global variables + * ARGUMENT : None + * NOTE : + * RETURN : None + ******************************************************************************/ +void LineSensDrvParamInit(void) { + LineSensDrvSpeedKmphDataInit(); // Data table initialization process for vehicle speed calculation +} + +/** + * @brief + * Pos_Sens thread stop processing + */ +void LineSensDrvThreadStopProcess(void) { + g_line_sens_thread_stop = TRUE; + + if (POS_REV_STATE_IDLE == g_uc_reverse_state) { + PosTeardownThread(ETID_POS_SENS); + } + + return; +} + +/** + * @brief + * Get initial sensor data reception flag + */ +u_int8 LineSensDrvGetSysRecvFlag(void) { + return g_sys_recv_flg; +} + +/*---------------------------------------------------------------------------*/ +/*EOF*/ |