service/vehicle/positioning/server/src/Sensor/VehicleSens_Common.cpp

   1 /*
   2  * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION.
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  *      http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  */
  16
  17 /*******************************************************************************
  18  *  File name    :VehicleSens_Common.cpp
  19  *  System name    :_CWORD107_
  20  *  Subsystem name  :Vehicle sensor process
  21  *  Program name  :Vehicle sensor common processing(VEHICLE_COMMON)
  22  *  Module configuration  :VehicleSensmemcmp()        Functions for Common Processing Memory Block Comparisons
  23  *                    VehicleSensCheckDid()      Common Processing Data ID Check Function
  24  *                    VehicleSensGetDataMasterOffset()  Get function for common processing data master offset value
  25  ******************************************************************************/
  26
  27 #include <vehicle_service/positioning_base_library.h>
  28 #include "VehicleSens_Common.h"
  29 #include "POS_private.h"
  30 #include <system_service/ss_ver.h>
  31 #include <system_service/ss_package.h>
  32 #include "gps_hal.h"
  33 #include "VehicleSens_DataMaster.h"
  34
  35
  36 #define _pb_strcat(pdest, psrc, size)       (strncat(pdest, psrc, size) , (0))
  37
  38
  39 /*************************************************/
  40 /*           Global variable                      */
  41 /*************************************************/
  42 static const VEHICLESENS_DID_OFFSET_TBL kGstDidList[] = {
  43     /*      Data ID              Offset size    Reserved */
  44     {  VEHICLE_DID_DESTINATION,        VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  45     {  VEHICLE_DID_HV,              VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  46     {  VEHICLE_DID_STEERING_WHEEL,        VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  47     {  VEHICLE_DID_VB,              VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  48     {  VEHICLE_DID_IG,              VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  49     {  VEHICLE_DID_MIC,            VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  50     {  VEHICLE_DID_BACKDOOR,          VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  51     {  VEHICLE_DID_PKB,            VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  52     {  VEHICLE_DID_ADIM,            VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  53     {  VEHICLE_DID_ILL,            VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  54     {  VEHICLE_DID_RHEOSTAT,          VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  55     {  VEHICLE_DID_SYSTEMP,          VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  56     {  POSHAL_DID_SPEED_PULSE,        VEHICLESENS_OFFSET_20WORD,    {0, 0} },
  57     {  POSHAL_DID_SPEED_PULSE_FLAG,      VEHICLESENS_OFFSET_20WORD,    {0, 0} },
  58     {  POSHAL_DID_SPEED_KMPH,          VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  59     {  POSHAL_DID_GYRO_X,            VEHICLESENS_OFFSET_20WORD,    {0, 0} },
  60     {  POSHAL_DID_GYRO_Y,            VEHICLESENS_OFFSET_20WORD,    {0, 0} },
  61     {  POSHAL_DID_GYRO_Z,            VEHICLESENS_OFFSET_20WORD,    {0, 0} },
  62     {  POSHAL_DID_GSNS_X,            VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  63     {  POSHAL_DID_GSNS_Y,            VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  64     {  POSHAL_DID_GSNS_Z,            VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  65     {  POSHAL_DID_REV,            VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  66     {  POSHAL_DID_GPS_ANTENNA,        VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  67     {  POSHAL_DID_SNS_COUNTER,        VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  68     {  VEHICLE_DID_GPS_COUNTER,        VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  69     {  POSHAL_DID_GPS_VERSION,        VEHICLESENS_OFFSET_GPS_FORMAT,  {0, 0} },
  70     {  VEHICLE_DID_LOCATION,          VEHICLESENS_OFFSET_GPS_FORMAT,  {0, 0} },
  71     /* ++GPS _CWORD82_ support */
  72     {  POSHAL_DID_GPS__CWORD82___CWORD44_GP4,      VEHICLESENS_OFFSET_GPS_FORMAT,  {0, 0} },
  73     {  VEHICLE_DID_GPS__CWORD82__NMEA,        VEHICLESENS_OFFSET_GPS_NMEA,  {0, 0} },
  74     {  POSHAL_DID_GPS_NMEA,                   VEHICLESENS_OFFSET_GPS_NMEA,  {0, 0} },
  75     {  POSHAL_DID_GPS__CWORD82__FULLBINARY,      VEHICLESENS_OFFSET_GPS_FORMAT,  {0, 0} },
  76     /* --GPS _CWORD82_ support */
  77     {  VEHICLE_DID_REV_LINE,          VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  78     {  VEHICLE_DID_REV_CAN,          VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  79 #if CONFIG_SENSOR_EXT_VALID            /* Initial Sensor Support */
  80     {  POSHAL_DID_GYRO_EXT,          VEHICLESENS_OFFSET_20WORD,    {0, 0} },
  81     {  POSHAL_DID_SPEED_PULSE_FST,      VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  82     {  POSHAL_DID_GYRO_X_FST,        VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  83     {  POSHAL_DID_GYRO_Y_FST,        VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  84     {  POSHAL_DID_GYRO_Z_FST,        VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  85     {  POSHAL_DID_SPEED_PULSE_FLAG_FST,    VEHICLESENS_OFFSET_20WORD_FST,  {0, 0} },
  86     {  POSHAL_DID_REV_FST,          VEHICLESENS_OFFSET_20WORD_FST,  {0, 0} },
  87 #endif
  88     /* ++ PastModel002 support */
  89     {  VEHICLE_DID_GPS_UBLOX_NAV_POSLLH,    VEHICLESENS_OFFSET_GPS_UBLOX,  {0, 0} },
  90     {  VEHICLE_DID_GPS_UBLOX_NAV_STATUS,    VEHICLESENS_OFFSET_GPS_UBLOX,  {0, 0} },
  91     {  VEHICLE_DID_GPS_UBLOX_NAV_TIMEUTC,    VEHICLESENS_OFFSET_GPS_UBLOX,  {0, 0} },
  92     {  VEHICLE_DID_GPS_UBLOX_NAV_VELNED,    VEHICLESENS_OFFSET_GPS_UBLOX,  {0, 0} },
  93     {  VEHICLE_DID_GPS_UBLOX_NAV_DOP,      VEHICLESENS_OFFSET_GPS_UBLOX,  {0, 0} },
  94     {  VEHICLE_DID_GPS_UBLOX_NAV_TIMEGPS,    VEHICLESENS_OFFSET_GPS_UBLOX,  {0, 0} },
  95     {  VEHICLE_DID_GPS_UBLOX_NAV_SVINFO,    VEHICLESENS_OFFSET_GPS_UBLOX,  {0, 0} },
  96     {  VEHICLE_DID_GPS_UBLOX_NAV_CLOCK,    VEHICLESENS_OFFSET_GPS_UBLOX,  {0, 0} },
  97     {  VEHICLE_DID_GPS_UBLOX_MON_HW,      VEHICLESENS_OFFSET_GPS_UBLOX,  {0, 0} },
  98     {  VEHICLE_DID_GYRO_TROUBLE,        VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
  99     {  VEHICLE_DID_MAIN_GPS_INTERRUPT_SIGNAL,  VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 100     {  VEHICLE_DID_SYS_GPS_INTERRUPT_SIGNAL,  VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 101     {  VEHICLE_DID_GYRO_CONNECT_STATUS,    VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 102     /* -- PastModel002 support */
 103     {  POSHAL_DID_GPS_TIME,          VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 104     {  POSHAL_DID_GPS_TIME_RAW,        VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 105     {  POSHAL_DID_GPS_WKNROLLOVER,      VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 106     {  VEHICLE_DID_NAVIINFO_DIAG_GPS,      VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 107     {  POSHAL_DID_GYRO_TEMP,            VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 108     {  POSHAL_DID_GYRO_TEMP_FST,          VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 109     {  POSHAL_DID_GSNS_X_FST,            VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 110     {  POSHAL_DID_GSNS_Y_FST,            VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 111     {  POSHAL_DID_GSNS_Z_FST,            VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 112     {  POSHAL_DID_PULSE_TIME,            VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 113     {  VEHICLE_DID_LOCATION_LONLAT,      VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 114     {  VEHICLE_DID_LOCATION_ALTITUDE,      VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 115     {  VEHICLE_DID_MOTION_SPEED,        VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 116     {  VEHICLE_DID_MOTION_HEADING,        VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 117     {  VEHICLE_DID_LOCATION_LONLAT_NAVI,    VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 118     {  VEHICLE_DID_LOCATION_ALTITUDE_NAVI,    VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 119     {  VEHICLE_DID_MOTION_SPEED_NAVI,      VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 120     {  VEHICLE_DID_MOTION_HEADING_NAVI,    VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 121     {  VEHICLE_DID_SETTINGTIME,        VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 122     {  VEHICLE_DID_MOTION_SPEED_INTERNAL,    VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 123     {  POSHAL_DID_GPS_CLOCK_DRIFT,      VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 124     {  POSHAL_DID_GPS_CLOCK_FREQ,        VEHICLESENS_OFFSET_NORMAL,    {0, 0} },
 125     {  0,                    0,                {0, 0} }    /* Termination code */
 126 };
 127
 128 /*******************************************************************************
 129 * MODULE    : VehicleSensmemcmp
 130 * ABSTRACT  : Functions for Common Processing Memory Block Comparisons
 131 * FUNCTION  : Memory block comparison processing
 132 * ARGUMENT  : *vp_data1  : Comparison target address 1
 133 *           : *vp_data2  : Comparison target address 2
 134 *           : uc_size  : Comparison Size
 135 * NOTE      :
 136 * RETURN    : VEHICLESENS_EQ  : No data change
 137 *           : VEHICLESENS_NEQ  : Data change
 138 ******************************************************************************/
 139 u_int8 VehicleSensmemcmp(const void *vp_data1, const void *vp_data2, size_t uc_size) {
 140     u_int8 ret = VEHICLESENS_EQ;
 141     const u_int8 *ucp_data1 = (const u_int8 *)vp_data1;
 142     const u_int8 *ucp_data2 = (const u_int8 *)vp_data2;
 143
 144     /* Loop by data size */
 145     while (uc_size > 0) {
 146         if (*ucp_data1 != *ucp_data2) {
 147             /* Data mismatch */
 148             ret = VEHICLESENS_NEQ;
 149             break;
 150         }
 151         ucp_data1++;
 152         ucp_data2++;
 153         uc_size--;
 154     }
 155     return( ret );
 156 }
 157
 158 /*******************************************************************************
 159 * MODULE    : VehicleSensCheckDid
 160 * ABSTRACT  : Common Processing Data ID Check Function
 161 * FUNCTION  : Check if the specified DID corresponds to the vehicle sensor information
 162 * ARGUMENT  : ul_did  : Data ID
 163 * NOTE      :
 164 * RETURN    : VEHICLESENS_INVALID    :Disabled
 165 *           : VEHICLESENS_EFFECTIVE  :Enabled
 166 ******************************************************************************/
 167 int32 VehicleSensCheckDid(DID ul_did) {
 168     int32 i = 0;
 169     int32 ret = VEHICLESENS_INVALID;
 170
 171     while (0 != kGstDidList[i].ul_did) {  // LCOV_EXCL_BR_LINE 200: did always valid
 172         if (kGstDidList[i].ul_did == ul_did) {
 173             /* DID enabled */
 174             ret = VEHICLESENS_EFFECTIVE;
 175             break;
 176         }
 177         i++;
 178     }
 179     return( ret );
 180 }
 181
 182 /*******************************************************************************
 183 * MODULE    : VehicleSensGetDataMasterOffset
 184 * ABSTRACT  : Get function for common processing data master offset value
 185 * FUNCTION  : Get the fixed offset value for a given DID
 186 * ARGUMENT  : ul_did  : Data ID
 187 * NOTE      :
 188 * RETURN    : Offset value(Returns 0 if DID is invalid)
 189 ******************************************************************************/
 190 u_int16  VehicleSensGetDataMasterOffset(DID ul_did) {
 191     int32    i = 0;              /* Generic counters      */
 192     u_int16    ret = 0;            /* Return value of this function    */
 193
 194     while (0 != kGstDidList[i].ul_did) {  // LCOV_EXCL_BR_LINE 200: did always valid
 195         if (kGstDidList[i].ul_did == ul_did) {
 196             /* DID enabled */
 197             ret = kGstDidList[i].us_offset;
 198             break;
 199         }
 200         i++;
 201     }
 202     return( ret );
 203 }
 204
 205 /* Ignore->MISRA-C++:2008 Rule 2-7-2 */
 206 #if CONFIG_SENSOR_EXT_VALID        /* Initial Sensor Support */
 207 /*******************************************************************************
 208 * MODULE    : VehicleSensGetDataMasterExtOffset
 209 * ABSTRACT  : Get function for common processing data master offset value
 210 * FUNCTION  : Get the fixed offset value for the first package delivery of the specified DID
 211 * ARGUMENT  : ul_did  : Data ID
 212 * NOTE      :
 213 * RETURN    : Offset value(Returns 0 for unspecified DID)
 214 ******************************************************************************/
 215 u_int16  VehicleSensGetDataMasterExtOffset(DID ul_did) {
 216     u_int16    usRet = 0;            /* Return value of this function    */
 217
 218     switch (ul_did) {
 219         case POSHAL_DID_GYRO_EXT:
 220         case POSHAL_DID_GYRO_X:
 221         case POSHAL_DID_GYRO_Y:
 222         case POSHAL_DID_GYRO_Z:
 223         case POSHAL_DID_GSNS_X:   /* Ignore->MISRA-C++:2008 Rule 2-7-2 */
 224         case POSHAL_DID_GSNS_Y:   /* Ignore->MISRA-C++:2008 Rule 2-7-2 */
 225         case POSHAL_DID_GSNS_Z:
 226         case POSHAL_DID_SPEED_PULSE:
 227         {
 228             /* Returns the extended package size (one data 2byte) at the time of initial delivery */
 229             usRet    = VEHICLESENS_OFFSET_10WORD_PKG_EXT;
 230             break;
 231         }
 232         case POSHAL_DID_SNS_COUNTER:
 233         case POSHAL_DID_REV:
 234         {
 235             /* Returns the extended package size (one data 1byte) at the time of initial delivery */
 236             usRet    = VEHICLESENS_OFFSET_BYTE_PKG_EXT;
 237             break;
 238         }
 239         case POSHAL_DID_GYRO_TEMP:
 240         {
 241             /* Returns the extended package size (one data 2byte) at the time of initial delivery */
 242             usRet    = VEHICLESENS_OFFSET_WORD_PKG_EXT;
 243             break;
 244         }
 245         case POSHAL_DID_PULSE_TIME:
 246         {
 247             /* Returns the expansion package size (132 bytes per data) at the time of initial delivery */
 248             usRet = VEHICLESENS_OFFSET_32LONG_PKG_EXT;
 249             break;
 250         }
 251         default:  /* Other than the above */  /* Ignore->MISRA-C++:2008 Rule 6-3-1, 6-4-1 */
 252             break;
 253     }
 254     return( usRet );
 255 }
 256 #endif
 257 /* Ignore->MISRA-C++:2008 Rule 2-7-2 */
 258
 259 /**
 260  * @brief
 261  *  GPS version information setting request
 262  *
 263  *  Request to set GPS version information to SS
 264  *
 265  * @param[in]  pstData Pointer to received message data
 266  */
 267 void VehicleSensSetGpsVersion(const SENSOR_MSG_GPSDATA_DAT *pstData) {
 268     static BOOL      isExistGpsVersion = FALSE;
 269     SSVER_PkgInfo    info;
 270     CSSVer           cssVer;
 271     UNIT_TYPE        eType = UNIT_TYPE_NONE;         /* Supported HW Configuration Type   */
 272     EFrameworkunifiedStatus       ret;
 273
 274     if (isExistGpsVersion == FALSE) {
 275         memset(&info, 0x00, sizeof(info));
 276         /* Supported HW Configuration Check */
 277         eType = GetEnvSupportInfo();
 278         if (UNIT_TYPE_GRADE1 == eType) {  // LCOV_EXCL_BR_LINE 6:cannot be this env
 279             // LCOV_EXCL_START 8 : dead code
 280             AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
 281             /* GRADE1 */
 282             memcpy(info.version, pstData->uc_data, sizeof(info.version));
 283
 284             /* Calling setPkgInfo() */
 285             FRAMEWORKUNIFIEDLOG(ZONE_INFO, __FUNCTION__,
 286                                             "--- VehicleUtility_SetGpsVersion setPkgInfo -->");
 287             ret = cssVer.setPkgInfo(SS_PKG_NAVI_GPS, info);
 288             if (ret == eFrameworkunifiedStatusOK) {
 289                 FRAMEWORKUNIFIEDLOG(ZONE_INFO, __FUNCTION__,
 290                             "--- VehicleUtility_SetGpsVersion setPkgInfo <-- GPSVersion = %s", info.version);
 291                 isExistGpsVersion = TRUE; /* Update Flag */
 292             } else {
 293                 /* Error log */
 294                 FRAMEWORKUNIFIEDLOG(ZONE_ERR, __FUNCTION__,
 295                                             "Failed to set PkgInfo EpositioningStatus = %d", ret);
 296             }
 297             // LCOV_EXCL_STOP
 298         } else if ( UNIT_TYPE_GRADE2 == eType ) {
 299           /*
 300            *  Note.
 301            *  This feature branches processing depending on the unit type.
 302            */
 303         } else {
 304             /* nop */
 305         }
 306     }
 307     return;
 308 }
 309
 310 /**
 311  * @brief
 312 *  Acquisition of location and time information (dump)
 313  *
 314  * @param[out]     pBuf         Dump information
 315  * @param[in]      Uc_get_method Retrieval method
 316  */
 317 void VehicleSensGetDebugPosDate(void* pBuf, u_int8 uc_get_method) {  // LCOV_EXCL_START 7: uesd only in FrameworkunifiedOnDebugDump
 318     AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
 319     static uint8_t  buf[DEBUG_DUMP_MAX_SIZE];
 320     static uint8_t  bufTmp[256];
 321     VEHICLESENS_DATA_MASTER stSnsData;
 322     SENSORLOCATION_LONLATINFO_DAT *pstLonLat;
 323     SENSORLOCATION_ALTITUDEINFO_DAT *pstAltitude;
 324     SENSORMOTION_HEADINGINFO_DAT *pstHeading;
 325     SENSOR_MSG_GPSDATA_DAT stGpsData;
 326     SENSOR_MSG_GPSTIME *pstDateTimeGps;
 327     NAVIINFO_DIAG_GPS *pstDiagGps;
 328     uint8_t i;
 329
 330     memset(&buf, 0x00, sizeof(buf));
 331     /* Title */
 332     switch ( uc_get_method ) {
 333         case VEHICLESENS_GETMETHOD_GPS:
 334             snprintf(reinterpret_cast<char *>(&buf), sizeof(buf), "GPS Info");
 335             break;
 336         case VEHICLESENS_GETMETHOD_NAVI:
 337             snprintf(reinterpret_cast<char *>(&buf), sizeof(buf), "Navi Info");
 338             break;
 339         default:
 340             /* nop */
 341             break;
 342     }
 343
 344     /* Latitude,Longitude */
 345     VehicleSensGetLocationLonLat(&stSnsData, uc_get_method);
 346     pstLonLat = reinterpret_cast<SENSORLOCATION_LONLATINFO_DAT*>(stSnsData.uc_data);
 347     memset(&bufTmp[0], 0x00, sizeof(bufTmp));
 348     snprintf( reinterpret_cast<char *>(&bufTmp[0]), sizeof(bufTmp),
 349             "\n [LonLat] sync:%3d, Enable:%01d, Lon:%10d, Lat:%10d, PosSts:0x%02x, PosAcc:0x%04x",
 350             pstLonLat->SyncCnt,
 351             pstLonLat->isEnable,
 352             pstLonLat->Longitude,
 353             pstLonLat->Latitude,
 354             pstLonLat->posSts,
 355             pstLonLat->posAcc);
 356     _pb_strcat(reinterpret_cast<char *>(&buf[0]), reinterpret_cast<char *>(&bufTmp[0]), sizeof(bufTmp));
 357
 358     /* Altitude */
 359     VehicleSensGetLocationAltitude(&stSnsData, uc_get_method);
 360     pstAltitude = reinterpret_cast<SENSORLOCATION_ALTITUDEINFO_DAT*>(stSnsData.uc_data);
 361     memset(&bufTmp[0], 0x00, sizeof(bufTmp));
 362     snprintf(reinterpret_cast<char *>(&bufTmp[0]), sizeof(bufTmp),
 363             "\n [Alt]    sync:%3d, Enable:%01d, Alt:%10d",
 364             pstAltitude->SyncCnt,
 365             pstAltitude->isEnable,
 366             pstAltitude->Altitude);
 367     _pb_strcat(reinterpret_cast<char *>(&buf[0]), reinterpret_cast<char *>(&bufTmp[0]), sizeof(bufTmp));
 368
 369     /* Orientation */
 370     VehicleSensGetMotionHeading(&stSnsData, uc_get_method);
 371     pstHeading = reinterpret_cast<SENSORMOTION_HEADINGINFO_DAT*>(stSnsData.uc_data);
 372     memset(&bufTmp[0], 0x00, sizeof(bufTmp));
 373     snprintf(reinterpret_cast<char *>(&bufTmp[0]), sizeof(bufTmp),
 374             "\n [Head]   sync:%3d, Enable:%01d, Head:%5d, PosSts:0x%02x",
 375             pstHeading->SyncCnt,
 376             pstHeading->isEnable,
 377             pstHeading->Heading,
 378             pstHeading->posSts);
 379     _pb_strcat(reinterpret_cast<char *>(&buf[0]), reinterpret_cast<char *>(&bufTmp[0]), sizeof(bufTmp));
 380
 381     switch ( uc_get_method ) {
 382         case VEHICLESENS_GETMETHOD_GPS:
 383             /* Satellite information */
 384             VehicleSensGetNaviinfoDiagGPSg(&stGpsData);
 385             pstDiagGps = reinterpret_cast<NAVIINFO_DIAG_GPS*>(stGpsData.uc_data);
 386             memset(&bufTmp[0], 0x00, sizeof(bufTmp));
 387             snprintf(reinterpret_cast<char *>(&bufTmp[0]), sizeof(bufTmp),
 388                     "\n [Diag]\n  FixSts:0x%02x",
 389                     pstDiagGps->stFix.ucFixSts);
 390             _pb_strcat(reinterpret_cast<char *>(&buf[0]), reinterpret_cast<char *>(&bufTmp[0]), sizeof(bufTmp));
 391             for (i = 0; i < 12; i++) {
 392                 memset(&bufTmp[0], 0x00, sizeof(bufTmp));
 393                 snprintf(reinterpret_cast<char *>(&bufTmp[0]), sizeof(bufTmp),
 394                         "\n  [%02d] RcvSts:0x%02x, prn:0x%02x, elv:0x%02x, Lv:0x%02x, azm:0x%04x",
 395                         i,
 396                         pstDiagGps->stSat.stPrn[i].ucRcvSts,
 397                         pstDiagGps->stSat.stPrn[i].ucPrn,
 398                         pstDiagGps->stSat.stPrn[i].ucelv,
 399                         pstDiagGps->stSat.stPrn[i].ucLv,
 400                         pstDiagGps->stSat.stPrn[i].usAzm);
 401                 _pb_strcat(reinterpret_cast<char *>(&buf[0]), reinterpret_cast<char *>(&bufTmp[0]), sizeof(bufTmp));
 402             }
 403
 404             /* Time */
 405             VehicleSensGetGpsTime(&stGpsData, VEHICLESENS_GETMETHOD_GPS);
 406             pstDateTimeGps = reinterpret_cast<SENSOR_MSG_GPSTIME*>(stGpsData.uc_data);
 407             memset(&bufTmp[0], 0x00, sizeof(bufTmp));
 408             snprintf(reinterpret_cast<char *>(&bufTmp[0]), sizeof(bufTmp),
 409                     "\n [Time] %04d/%02d/%02d %02d:%02d:%02d, sts:%d",
 410                     pstDateTimeGps->utc.year,
 411                     pstDateTimeGps->utc.month,
 412                     pstDateTimeGps->utc.date,
 413                     pstDateTimeGps->utc.hour,
 414                     pstDateTimeGps->utc.minute,
 415                     pstDateTimeGps->utc.second,
 416                     pstDateTimeGps->tdsts);
 417             _pb_strcat(reinterpret_cast<char *>(&buf[0]), reinterpret_cast<char *>(&bufTmp[0]), sizeof(bufTmp));
 418             break;
 419         case VEHICLESENS_GETMETHOD_NAVI:
 420             /* nop */
 421             break;
 422         default:
 423             /* nop */
 424             break;
 425     }
 426     memcpy(pBuf, &buf[0], sizeof(buf));
 427     return;
 428 }
 429 // LCOV_EXCL_STOP