video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_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 * @file MDev_Gps_Common.h
  18 */
  19
  20 /*---------------------------------------------------------------------------*/
  21
  22 #include "MDev_Gps_Common.h"
  23
  24 #include <native_service/ns_backup.h>
  25 #include <native_service/ns_backup_id.h>
  26 #include "positioning_common.h"
  27 #include "MDev_Gps_API.h"
  28 #include "MDev_Gps_TimerCtrl.h"
  29 #include "MDev_Gps_Nmea.h"
  30 #include "MDev_GpsRecv.h"
  31
  32 /*---------------------------------------------------------------------------*/
  33 // Value define
  34
  35 #define BK_ID_POS_GPS_TD_STS_SZ   1         /* Datetime Status-Size                */
  36 #define BK_GPSFIXCNT_OFFSET_CHK   12        /* Fix Count-Offset-Check SRAM */
  37 #define BK_GPSFIXCNT_RAM_OK       0         /* Fix Count RAM Status OK         */
  38 #define BK_GPSFIXCNT_RAM_FILE_NG  1         /* Fix Count RAM Status FILE NG    */
  39 #define BK_GPSFIXCNT_RAM_NG       2         /* Fix Count RAM Status NG         */
  40
  41 #define JULIAN_DAY                      (-32044.0f)   /* Be based on March 1, 4800 BC */
  42 #define MODIFIED_JULIAN_DAY_OFFSET      (2400000.5f)
  43 #define GPS_WS_MAX                      (7 * 24 * 60 * 60)  /* GPS time(Weekly seconds)*/
  44 #define GET_METHOD_GPS          (1)  //!< \~english GPS
  45
  46 /* Presence information definitions DR */
  47 #define SENSORLOCATION_EXISTDR_NODR              (0)    /* Without DR   */
  48 #define SENSORLOCATION_EXISTDR_DR                (1)    /* There DR    */
  49 /* DR state definition */
  50 #define SENSORLOCATION_DRSTATUS_INVALID          (0)    /* Invalid            */
  51 #define SENSORLOCATION_DRSTATUS_GPS_NODR         (1)    /* Information use GPS, not yet implemented DR   */
  52 #define SENSORLOCATION_DRSTATUS_NOGPS_DR         (2)    /* No information available GPS, DR implementation */
  53 #define SENSORLOCATION_DRSTATUS_GPS_DR           (3)    /* Information use GPS, DR implementation    */
  54
  55 #define  SENSORLOCATION_STATUS_DISABLE  (0)  //!< \~english Not available
  56 #define  SENSORLOCATION_STATUS_ENABLE   (1)  //!< \~english Available
  57 /*---------------------------------------------------------------------------*/
  58 // Global values
  59
  60 uint8_t g_gps_reset_cmd_sending   = FALSE;  /* Reset command transmission in progress judgment flag     */
  61 uint8_t g_is_gpstime_sts = FALSE;            /* Date/Time Status Fixed Value Setting Indication Flag   */
  62 uint8_t g_gpstime_raw_tdsts = 0;
  63 extern u_int8 g_gps_week_cor_cnt;
  64 /*---------------------------------------------------------------------------*/
  65 // Functions
  66 void WriteGpsTimeToBackup(uint8_t flag, POS_DATETIME* pst_datetime) {
  67     int32_t ret;
  68     ST_GPS_SET_TIME buf;
  69
  70     memset(reinterpret_cast<void *>(&buf), 0, (size_t)sizeof(buf));
  71
  72     buf.flag   = flag;
  73     buf.year   = pst_datetime->year;
  74     buf.month  = pst_datetime->month;
  75     buf.date   = pst_datetime->date;
  76     buf.hour   = pst_datetime->hour;
  77     buf.minute = pst_datetime->minute;
  78     buf.second = pst_datetime->second;
  79
  80     ret = Backup_DataWt(D_BK_ID_POS_GPS_TIME_SET_INFO, &buf, 0, sizeof(ST_GPS_SET_TIME));
  81     if (ret != BKUP_RET_NORMAL) {
  82       POSITIONING_LOG("Backup_DataWt ERROR!! [ret=%d]", ret);
  83     }
  84
  85     return;
  86 }
  87
  88
  89
  90 /********************************************************************************
  91  * MODULE      : ChangeStatusGpsCommon
  92  * ABSTRACT    : State transition processing
  93  * FUNCTION    : Changes the state of the GPS communication management process to the specified state
  94  * ARGUMENT    : u_int32 sts : State of the transition destination
  95  * NOTE        :
  96  * RETURN      : None
  97  ********************************************************************************/
  98 void ChangeStatusGpsCommon(u_int32 sts ) {
  99   /* Set the transition destination state in the management information state  */
 100   g_gps_mngr.sts = sts;
 101 }
 102
 103 /********************************************************************************
 104  * MODULE      : RtyResetGpsCommon
 105  * ABSTRACT    : Reset retry counter(Periodic reception)
 106  * FUNCTION    : Reset the retry counter
 107  * ARGUMENT    : None
 108  * NOTE        :
 109  * RETURN      : None
 110  ********************************************************************************/
 111 void RtyResetGpsCommon(void) {
 112   g_gps_mngr.hrsetcnt = 0;      /* Initialization of the number of tries until a hard reset    */
 113   g_gps_mngr.sndngcnt = 0;      /* Initialization of the number of tries until serial reset  */
 114 }
 115
 116 /******************************************************************************
 117 @brief      SendRtyResetGpsCommon<BR>
 118             Reset retry counter(ACK response)
 119 @outline    Reset the retry counter
 120 @param[in]  none
 121 @param[out] none
 122 @return     none
 123 @retval     none
 124 *******************************************************************************/
 125 void SendRtyResetGpsCommon(void) {
 126   g_gps_mngr.sndcnt   = 0;      /* Initialization of the number of transmission retries            */
 127 }
 128
 129 uint8_t HardResetChkGpsCommon(void) {
 130   uint8_t      ret = RETRY_OFF;           /* Return value              */
 131   static uint8_t uc_hardreset_cnt = 0;      /* Number of hard resets      */
 132
 133   g_gps_mngr.hrsetcnt++;        /* Add number of tries until hard reset  */
 134
 135   if (g_gps_mngr.hrsetcnt >= HRSET_MAX) {
 136       // Todo For test don't have hardReset Method.
 137       // DEV_Gps_HardReset();
 138
 139       g_gps_mngr.hrsetcnt = 0;     /* Initialization of the number of tries until a hard reset */
 140       /* Discard all pending commands */
 141       DeleteAllSaveCmdGpsCommon();
 142       /* Determination of the number of hard reset executions */
 143       uc_hardreset_cnt++;
 144       if (uc_hardreset_cnt >= 3) {
 145           ret = RETRY_OFF;      /* Set the return value (No retry: Specified number of hard resets completed) */
 146           uc_hardreset_cnt = 0;         /* Clear the number of hard resets */
 147       } else {
 148           ret = RETRY_OUT;      /* Set the return value (retry out: hardware reset) */
 149       }
 150   } else {         /* When the number of tries until the hard reset is less than the maximum value  */
 151       ret = RETRY_ON;         /* Set return value (with retry)     */
 152   }
 153
 154   return (ret);            /* End of the process               */
 155 }
 156
 157
 158 /******************************************************************************
 159 @brief      SendReqGpsCommon<BR>
 160             Common-Transmit Request Reception Matrix Function
 161 @outline  Common processing when receiving a transmission request
 162 @param[in]  none
 163 @param[out]  none
 164 @return     none
 165 @retval     none
 166 *******************************************************************************/
 167 void SendReqGpsCommon(void) {
 168   RET_API               ret = RET_NORMAL;
 169   TG_GPS_SND_DATA*      p_rcv_data = NULL;
 170   TG_GPS_SAVECMD        s_send_data;
 171   TG_GPS_OUTPUT_FORMAT  s_output_format;
 172
 173   p_rcv_data = reinterpret_cast<TG_GPS_SND_DATA*>(&(g_gps_msg_rcvr.msgdat[0]));  /* Incoming message structure */
 174
 175   /** Transmit Data Format Check */
 176   ret = CheckSendCmdGpsCommon(p_rcv_data->ub_data, p_rcv_data->us_size, &s_output_format);
 177
 178   /** Normal format */
 179   if (ret == RET_NORMAL) {
 180     memset( &s_send_data, 0x00, sizeof(s_send_data) );  /* Ignore -> MISRA-C:2004 Rule 16.10 */
 181
 182     s_send_data.us_pno      = 0;                /* Result notification destination process number  */
 183     s_send_data.uc_rid      = 0;                /* Result Notification Resource ID    */
 184     s_send_data.uc_rst      = GPS_CMD_NOTRST;   /* Reset flag      */
 185     s_send_data.e_cmd_info  = s_output_format;  /* Command information        */
 186     memcpy( &s_send_data.sndcmd, p_rcv_data, sizeof(TG_GPS_SND_DATA) );  /* Sending commands */
 187
 188     /* Command is suspended and terminated. */
 189     ret = DevGpsSaveCmd(&s_send_data);  /* #GPF_60_040 */
 190
 191     if (ret != RET_NORMAL) {
 192         POSITIONING_LOG("GPS Command Reserve bufferFull ! \r\n");
 193     }
 194   } else {
 195     POSITIONING_LOG("# GpsCommCtl # GPS Command Format Error!! \r\n");
 196   }
 197
 198   return;
 199 }
 200
 201 /******************************************************************************
 202 @brief      GPSResetReqGpsCommon<BR>
 203             Common-GPS reset request reception matrix function
 204 @outline  Common processing at GPS reset reception
 205 @param[in]  none
 206 @param[out]  none
 207 @return     none
 208 @retval     none
 209 *******************************************************************************/
 210 void GPSResetReqGpsCommon(void) {
 211     RET_API         ret = RET_NORMAL;
 212     POS_RESETINFO   st_rcv_msg;
 213
 214     memset(&st_rcv_msg, 0x00, sizeof(st_rcv_msg));  /* 2015/03/31 Coverity CID: 21530 support */
 215
 216     if (g_gps_reset_cmd_sending == FALSE) {
 217       memcpy(&st_rcv_msg, reinterpret_cast<POS_RESETINFO *>(&(g_gps_msg_rcvr.msgdat)),
 218                               sizeof(POS_RESETINFO));
 219       ret = DevGpsResetReq(st_rcv_msg.respno, 0);
 220       if (ret == RET_NORMAL) {
 221             /* Set the reset command transmission judgment flag to transmission in progress */
 222             g_gps_reset_cmd_sending = TRUE;
 223             /* Send reset command(Normal response transmission)*/
 224             GpsSetPosBaseEvent(st_rcv_msg.sndpno, POS_RET_NORMAL);
 225         } else if (ret == RET_EV_NONE) {
 226             /* nop */
 227         } else {
 228             /* Send reset command(Internal Processing Error Response Transmission)*/
 229             GpsSetPosBaseEvent(st_rcv_msg.sndpno, POS_RET_ERROR_INNER);
 230         }
 231     } else {
 232       memcpy(&st_rcv_msg, reinterpret_cast<POS_RESETINFO*>(&(g_gps_msg_rcvr.msgdat)), sizeof(POS_RESETINFO));
 233       GpsSetPosBaseEvent(st_rcv_msg.sndpno, POS_RET_ERROR_BUSY);
 234     }
 235     return;
 236 }
 237
 238 /******************************************************************************
 239 @brief      CyclDataTimeOutGpsCommon<BR>
 240             Common-Receive data monitoring timeout matrix function
 241 @outline  Common processing at reception cycle data monitoring timeout
 242 @param[in]  none
 243 @param[out]  none
 244 @return     none
 245 @retval     none
 246 *******************************************************************************/
 247 void CyclDataTimeOutGpsCommon(void) {
 248   g_wcnct_err++;              /* Count number of connection errors      */
 249
 250   /* Stop all timers          */
 251   DevGpsTimeStop(GPS_STARTUP_TIMER);  /* Start confirmation monitoring timer      */  /* Ignore -> MISRA-C:2004 Rule 16.10 */
 252   DevGpsTimeStop(GPS_RECV_ACK_TIMER);  /* ACK reception monitoring timer      */  /* Ignore -> MISRA-C:2004 Rule 16.10 */
 253
 254   /* Clear cyclic receive data up to now */
 255   DevGpsCycleDataClear();
 256
 257   /* Start confirmation monitoring timer setting  */
 258   DevGpsTimeSet(GPS_STARTUP_TIMER);  /* Ignore -> MISRA-C:2004 Rule 16.10 */
 259
 260   /* State-transition(Start Confirmation Monitor)    */
 261   ChangeStatusGpsCommon(GPS_STS_STARTUP);
 262
 263   return;
 264 }
 265
 266 /********************************************************************************
 267  * MODULE      : CheckFrontStringPartGpsCommon
 268  * ABSTRACT    : Message start character string determination processing
 269  * FUNCTION    : Check that the message starts with the specified string
 270  * ARGUMENT    : pubTarget    : Message
 271  *             : pubStart    : Character string
 272  * NOTE        : When target is equal to start, it is also judged to be TRUE.
 273  *             : "*" in the start is treated as a single-character wildcard.
 274  * RETURN      : RET_NORMAL  : Decision success
 275  *             : RET_ERROR  : Decision failure
 276  ********************************************************************************/
 277 RET_API CheckFrontStringPartGpsCommon(const u_char *p_tartget, const u_char *p_start) {
 278   RET_API ret = RET_ERROR;
 279
 280   while (((*p_tartget == *p_start)
 281     || ('*' == *p_start))  /* #GPF_60_024 */  /* Ignore -> No applicable rules for MISRA-C */
 282     &&  (*p_tartget != '\0')
 283     &&  (*p_start != '\0')) {
 284     p_tartget++;  /* Ignore -> MISRA-C:2004 Rule 17.4 */
 285     p_start++;    /* Ignore -> MISRA-C:2004 Rule 17.4 */
 286   }
 287
 288   if (*p_start == '\0') {
 289     ret = RET_NORMAL;
 290   } else {
 291     ret = RET_ERROR;
 292   }
 293
 294   return ret;
 295 }
 296
 297 /******************************************************************************
 298 @brief      JudgeFormatGpsCommon<BR>
 299             Format determination processing
 300 @outline    Determine the format of the received GPS data
 301 @param[in]  pubSndData    : Message to be judged
 302 @param[in]  ul_length    : Message length
 303 @param[out] peFormat    : Where to Return the Format
 304 @input      none
 305 @return     int32
 306 @retval     GPSRET_SNDCMD  : Commands to be notified
 307 @retval     GPSRET_NOPCMD  : User unset command
 308 @retval     GPSRET_CMDERR  : No applicable command
 309 *******************************************************************************/
 310 int32  JudgeFormatGpsCommon(u_char *p_send_data, u_int32 ul_length, TG_GPS_OUTPUT_FORMAT *p_format) {
 311   int32     ret = GPSRET_NOPCMD; /* Return value    */
 312   u_int32   cnt = 0;              /* Ignore -> MISRA-C:2004 Rule 5.6 */
 313
 314   for (cnt = 0; cnt < (u_int32)GPSCMDANATBL_MAX; cnt++) {
 315     /** End-of-table decision */
 316     if (CheckFrontStringPartGpsCommon((const u_char*)ENDMARK, kGpsCmdAnaTbl[cnt].c_sentence) == RET_NORMAL) {
 317       /** When there is no matching sentence, */
 318       /** set return value. */
 319       ret = GPSRET_NOPCMD;  /* Ignore -> MISRA-C:2004 Rule 3.1 */
 320       break;
 321     }
 322
 323     /* ++ #GPF_60_040 */
 324     /* Sentence determination of received command */
 325
 326     if (kGpsCmdAnaTbl[cnt].e_rcv_format == g_gps_mngr.rcv_cmd) {
 327       /* Format set */
 328       (*p_format) = g_gps_mngr.rcv_cmd;
 329
 330       /* Notification judgment */
 331       if (kGpsCmdAnaTbl[cnt].b_snd_cmd_flg == TRUE) {
 332         /* Checksum implementation */
 333         if (CheckSumGpsCommon(reinterpret_cast<u_int8*>(p_send_data),
 334                               ul_length,
 335                               kGpsCmdAnaTbl[cnt].e_rcv_format) != RET_NORMAL) {
 336           /* Receive data anomaly */
 337           POSITIONING_LOG("# GpsCommCtl # GPS Data SUM ERROR! \r\n");
 338
 339           g_wrecv_err++;
 340
 341           /* Set data error in return value */
 342           ret = GPSRET_CMDERR;
 343         } else {
 344           /* Notification to vehicle sensor */
 345           ret = GPSRET_SNDCMD;
 346           g_wrecv_err = 0;
 347         }
 348       }
 349
 350       break;
 351     }
 352   }
 353
 354   return ret;
 355 }
 356
 357
 358 /******************************************************************************
 359 @brief      CheckSumGpsCommon<BR>
 360             Checksum processing
 361 @outline    Checking the integrity of GPS commands
 362 @param[in]  none
 363 @param[out] none
 364 @input      u_int8          : p_uc_data[]   ... Receive data pointer
 365 @input      u_int32         : ul_length    ... Data length
 366 @input      u_int32         : e_cmd_info    ... Command information<BR>
 367                                               -FORMAT_NMEA     NMEA data<BR>
 368                                               -FORMAT_BIN      Standard binary<BR>
 369                                               -FORMAT_FULLBIN  FULL binaries
 370 @return     RET_API
 371 @retval     RET_NORMAL      : Normal completion
 372 @retval     RET_ERROR       : ABENDs
 373 *******************************************************************************/
 374 RET_API CheckSumGpsCommon(const u_int8 p_uc_data[], u_int32 ul_length, TG_GPS_OUTPUT_FORMAT e_cmd_info) {
 375   RET_API l_ret = RET_ERROR;
 376   u_int32 i = 0;
 377   u_int8  uc_char = 0;
 378   u_int8  uc_sum = 0;
 379   u_int8  uc_cmp = 0;
 380   int32_t ret = 0;
 381
 382   static u_int8 uc_work[UBX_CMD_SIZE_MAX];
 383
 384   if ((GPS_FORMAT_MON_VER     == e_cmd_info)
 385     || (GPS_FORMAT_AID_INI     == e_cmd_info)
 386     || (GPS_FORMAT_ACK_ACKNACK == e_cmd_info)
 387     || (GPS_FORMAT_NAV_TIMEUTC == e_cmd_info)
 388     || (GPS_FORMAT_NAV_CLOCK   == e_cmd_info)
 389     || (GPS_FORMAT_RXM_RTC5    == e_cmd_info)
 390     || (GPS_FORMAT_NAV_SVINFO  == e_cmd_info)
 391     || (GPS_FORMAT_CFG_NAVX5   == e_cmd_info)) {
 392     (void)memcpy(uc_work, p_uc_data, ul_length - 2);
 393     DevGpsSetChkSum(uc_work, ul_length);
 394
 395     ret = memcmp(uc_work, p_uc_data, ul_length);
 396
 397     if (ret == 0) {
 398       l_ret = RET_NORMAL;
 399     } else {
 400     }
 401   } else {
 402     /** XOR each character between '$' and '*' */
 403     for ( i = 1; i < ul_length; i++ ) {
 404       if (p_uc_data[i] == (u_int8)'*') {
 405         /** '*'Detection */
 406         l_ret = RET_NORMAL;    /* #GPF_60_111 */
 407         break;
 408       } else {
 409         /** '*'Not detected */
 410         uc_char = p_uc_data[i];
 411         uc_sum ^= uc_char;
 412       }
 413     }
 414
 415     /** When the position of '*' is within two characters following '*' (access check of the receive buffer range) */
 416     if ( (l_ret == RET_NORMAL) && ((GPS_READ_LEN - 2) > i) ) {
 417       /** Convert two characters following '*' to two hexadecimal digits */
 418       uc_cmp = (AtoHGpsCommon(p_uc_data[i + 1]) * 16) + AtoHGpsCommon(p_uc_data[i + 2]);
 419
 420       /** Checksum result determination */
 421       if ( uc_cmp != uc_sum ) {
 422         /** Abnormality */
 423         l_ret = RET_ERROR;
 424       }
 425     } else {
 426       /** '*' Not detected, data length invalid */
 427       /** Abnormality */
 428       l_ret = RET_ERROR;
 429     }
 430   }
 431
 432   return l_ret;
 433 }
 434
 435 /******************************************************************************
 436 @brief      AtoHGpsCommon<BR>
 437             ASCII-> hexadecimal conversions
 438 @outline    Convert ASCII codes to hexadecimal
 439 @param[in]  none
 440 @param[out] none
 441 @input      u_int8          : ascii ... ASCII code('0' ~ '9', 'a' ~ 'f', 'A' ~ 'F')
 442 @return     u_int8
 443 @retval     Hexadecimal number
 444 *******************************************************************************/
 445 u_int8  AtoHGpsCommon(u_int8 ascii) {
 446   u_int8 hex = 0;
 447   if ((ascii >= '0') && (ascii <= '9')) {
 448     hex = ascii - '0';
 449   } else if ((ascii >= 'a') && (ascii <= 'f')) {
 450     hex = (ascii - 'a') + 0xa;  /* Ignore -> MISRA-C:2004 Rule 12.1 */
 451   } else if ( (ascii >= 'A') && (ascii <= 'F') ) {
 452     hex = (ascii - 'A') + 0xa;  /* Ignore -> MISRA-C:2004 Rule 12.1 */
 453   } else {
 454     /* nop */
 455   }
 456
 457   return hex;
 458 }
 459
 460 /******************************************************************************
 461 @brief      DevGpsSaveCmd<BR>
 462             Command pending processing
 463 @outline    Temporarily save commands to be sent to GPS
 464 @param[in]  TG_GPS_SND_DATA* pstSndMsg : Sending commands
 465 @param[out] none
 466 @return     RET_API
 467 @retval     RET_NORMAL      : Normal completion
 468 @retval     RET_ERROR       : ABENDs(buffer full)
 469 *******************************************************************************/
 470 RET_API DevGpsSaveCmd(TG_GPS_SAVECMD *p_send_data) {
 471   RET_API ret = RET_NORMAL;
 472   u_int32 savp = 0;            /* Holding buffer location storage      */
 473
 474   /*  Pending buffer full        */
 475   if ( g_gps_save_cmdr.bufsav >= SAV_MAX ) {
 476     /* Return an abend */
 477     ret = RET_ERROR;
 478   } else {
 479     savp = g_gps_save_cmdr.saveno;      /* Get pending buffer position        */
 480
 481     /* Copy data to pending buffer    */
 482     memset(&g_gps_save_cmdr.savebuf[savp], 0x00, sizeof(g_gps_save_cmdr.savebuf[savp]));
 483     memcpy(&g_gps_save_cmdr.savebuf[savp], p_send_data, sizeof(TG_GPS_SAVECMD));
 484
 485     g_gps_save_cmdr.saveno++;        /* Pending index addition        */
 486
 487     if (g_gps_save_cmdr.saveno >= SAV_MAX) {
 488       g_gps_save_cmdr.saveno = 0;      /* Reset Pending Index      */
 489     }
 490
 491     g_gps_save_cmdr.bufsav++;        /* Number of pending buffers added        */
 492   }
 493
 494   return ret;
 495 }
 496
 497 /******************************************************************************
 498 @brief      SendSaveCmdGpsCommon<BR>
 499             Pending command transmission processing
 500 @outline    Send Pending Commands to GPS
 501 @param[in]  none
 502 @param[out] none
 503 @return     none
 504 @retval     none
 505 *******************************************************************************/
 506 void SendSaveCmdGpsCommon(void) {
 507   u_int32           sndp = 0;             /* Holding buffer location storage  */
 508   // TG_GPS_SND_DATA*  p_send_data = NULL;   /* Pointer to the pending command */
 509   BOOL              b_ret = FALSE;        /* Command send return value       */
 510
 511   /** Retrieves one pending command, if any, and sends it. */
 512   if (g_gps_save_cmdr.bufsav != 0) {
 513     /** Send position acquisition        */
 514     sndp = g_gps_save_cmdr.sendno;
 515     // p_send_data = reinterpret_cast<TG_GPS_SND_DATA*>(&(g_gps_save_cmdr.savebuf[sndp]));
 516
 517     // todo For test No Uart
 518     // b_ret = DEV_Gps_CmdWrite( p_send_data->us_size, &(p_send_data->uc_data[0]) );/* #GPF_60_106 */
 519     if ( b_ret != TRUE ) {
 520       POSITIONING_LOG("DEV_Gps_CmdWrite fail. ret=[%d]\n", b_ret);
 521       // ucResult = SENSLOG_RES_FAIL;
 522     }
 523
 524     /* ++ #GPF_60_040 */
 525     /** Configure Monitored Commands */
 526     g_gps_mngr.resp_cmd       = g_gps_save_cmdr.savebuf[ sndp ].e_cmd_info;
 527     g_gps_mngr.resp_pno       = g_gps_save_cmdr.savebuf[ sndp ].us_pno;
 528     g_gps_mngr.resp_rid       = g_gps_save_cmdr.savebuf[ sndp ].uc_rid;
 529     g_gps_mngr.resp_rst_flag  = g_gps_save_cmdr.savebuf[ sndp ].uc_rst;
 530
 531     /** Perform response monitoring */
 532     DevGpsTimeSet(GPS_RECV_ACK_TIMER);  /* Ignore -> MISRA-C:2004 Rule 16.10 */
 533     /* State transition processing(sending)  */
 534     ChangeStatusGpsCommon(GPS_STS_SENT);
 535   } else {
 536     /** No monitored commands */
 537     g_gps_mngr.resp_cmd       = GPS_FORMAT_MIN;
 538     g_gps_mngr.resp_pno       = 0x0000;
 539     g_gps_mngr.resp_rid       = 0x00;
 540     g_gps_mngr.resp_rst_flag  = GPS_CMD_NOTRST;
 541   }
 542
 543   return;
 544 }
 545
 546 /* ++ #GPF_60_040 */
 547 /******************************************************************************
 548 @brief      DeleteSaveCmdGpsCommon<BR>
 549             Pending command deletion processing
 550 @outline    Deleting a Pending Command
 551 @param[in]  none
 552 @param[out] none
 553 @return     none
 554 @retval     none
 555 *******************************************************************************/
 556 void DeleteSaveCmdGpsCommon(void) {
 557   u_int32 sndp = 0; /* Holding buffer location storage  */
 558
 559   /** Delete one pending command, if any. */
 560   if (g_gps_save_cmdr.bufsav != 0) {
 561     /** Send position acquisition        */
 562     sndp = g_gps_save_cmdr.sendno;
 563
 564     /** Clear Stored Data */
 565     memset(&g_gps_save_cmdr.savebuf[sndp], 0x00, sizeof(g_gps_save_cmdr.savebuf[sndp]));
 566
 567     /** Transmit index addition    */
 568     g_gps_save_cmdr.sendno++;
 569     if ( g_gps_save_cmdr.sendno >= SAV_MAX ) {
 570       /** Transmit index MAX or higher  */
 571       /** Initialize transmission index      */
 572       g_gps_save_cmdr.sendno = 0;
 573     }
 574
 575     /** Subtract pending buffers      */
 576     g_gps_save_cmdr.bufsav--;
 577   }
 578
 579   return;
 580 }
 581
 582 /******************************************************************************
 583 @brief      DeleteAllSaveCmdGpsCommon<BR>
 584             Hold command abort processing
 585 @outline    Discards all pending commands and returns a reset response
 586 @param[in]  none
 587 @param[out] none
 588 @return     none
 589 @retval     none
 590 *******************************************************************************/
 591 void DeleteAllSaveCmdGpsCommon(void) {
 592   u_int32 sndp = 0;     /* Holding buffer location storage  */
 593   PNO     us_pno = 0;   /* Ignore -> MISRA-C:2004 Rule 5.6 */
 594   RID     uc_rid = 0;   /* Ignore -> MISRA-C:2004 Rule 5.6 */
 595
 596   while (g_gps_save_cmdr.bufsav != 0) {
 597     /** Determine whether it is a pending command that requires a response. */
 598     sndp = g_gps_save_cmdr.sendno;
 599
 600     if (g_gps_save_cmdr.savebuf[ sndp ].uc_rst == GPS_CMD_RESET) {
 601       /** In the case of a reset request, a reset response (communication error) is notified. */
 602       us_pno = g_gps_save_cmdr.savebuf[ sndp ].us_pno;
 603       uc_rid = g_gps_save_cmdr.savebuf[ sndp ].uc_rid;
 604       DevGpsRstAnsSend(us_pno, uc_rid, GPS_SENDNG);  /* Ignore -> MISRA-C:2004 Rule 16.10 */
 605     }
 606
 607     /** Delete */
 608     DeleteSaveCmdGpsCommon();
 609   }
 610
 611   return;
 612 }
 613 /******************************************************************************
 614 @brief      RcvCyclCmdNmeaGpsCommon<BR>
 615             Cyclic data (NMEA) notification process
 616 @outline    Notifying VehicleSensor of Cyclic Data (NMEA) Reception
 617 @param[in]  u_int8               : *p_uc_data ... Receive data pointer
 618 @param[in]  u_int32              : ul_len    ... Received data length
 619 @param[in]  TG_GPS_OUTPUT_FORMAT : s_output_format  ... Receive Format
 620 @param[out] none
 621 @return     none
 622 @retval     none
 623 *******************************************************************************/
 624 void RcvCyclCmdNmeaGpsCommon(u_int8 *p_uc_data, u_int32 ul_len, TG_GPS_OUTPUT_FORMAT s_output_format) {
 625   NAVIINFO_ALL  navilocinfo;            /* Navigation information */
 626   BOOL          bcycle_rcv_flag = FALSE;  /* Cyclic reception judgment result    */
 627
 628   if (JudgeFormatOrderGpsCommon(s_output_format, g_rcv_format) == 0) {
 629     /** If a sentence is received before the last received sentence, */
 630     /** determine as the beginning of cyclic data. */
 631     bcycle_rcv_flag = TRUE;
 632   }
 633
 634   if (bcycle_rcv_flag == TRUE) {
 635     DevGpsSndCycleDataNmea();
 636
 637     /* Reset navigation information */
 638     memset(&navilocinfo, 0x00, sizeof(NAVIINFO_ALL));
 639
 640     /* NMEA data analysis */
 641     DevGpsAnalyzeNmea(&navilocinfo);
 642
 643     /** Clear Buffer */
 644     // DevGpsCycleDataClear();
 645   }
 646
 647   DevGpsCycleDataSetNmea(p_uc_data, (u_int32)ul_len, GetNmeaIdxGpsCommon(s_output_format));
 648
 649   /** Update receive format */
 650   g_rcv_format = s_output_format;
 651
 652   return;
 653 }
 654
 655
 656 /**
 657  * @brief
 658  *  Cyclic data (UBX) notification processing
 659  *
 660  *  Notify vehicle sensor of reception of cyclic data (UBX)
 661  *
 662  * @param[in] u_int8               : *p_uc_data ... Receive data pointer
 663  * @param[in] u_int32              : ul_len    ... Received data length
 664  * @param[in] TG_GPS_OUTPUT_FORMAT : eFormat  ... Receive Format
 665  */
 666 void RcvCyclCmdExtGpsCommon(u_int8 *p_uc_data, u_int32 ul_len, TG_GPS_OUTPUT_FORMAT e_format) {
 667   SENSOR_GPSTIME_RAW            st_gpstime_raw;
 668   TG_GPS_UBX_NAV_TIMEUTC_DATA   *pst_navtime_utc;
 669   BOOL b_validtow;        /* Valid Time of Week   */
 670   BOOL b_validwkn;        /* Valid Week of Number */
 671   BOOL b_validutc;         /* Valid UTC Time       */
 672
 673   /* For NAV-TIMEUTC */
 674   if (e_format == GPS_FORMAT_NAV_TIMEUTC) {
 675     memset(&st_gpstime_raw, 0x00, sizeof(st_gpstime_raw));
 676
 677     /* NAV-TIMEUTC analysis */
 678     pst_navtime_utc = reinterpret_cast<TG_GPS_UBX_NAV_TIMEUTC_DATA*>(p_uc_data + UBX_CMD_OFFSET_PAYLOAD);
 679
 680     st_gpstime_raw.utc.year   = pst_navtime_utc->year;
 681     st_gpstime_raw.utc.month  = pst_navtime_utc->month;
 682     st_gpstime_raw.utc.date   = pst_navtime_utc->day;
 683     st_gpstime_raw.utc.hour   = pst_navtime_utc->hour;
 684     st_gpstime_raw.utc.minute = pst_navtime_utc->min;
 685     st_gpstime_raw.utc.second = pst_navtime_utc->sec;
 686     b_validtow = (BOOL)(((pst_navtime_utc->valid) & UBX_CMD_MSK_NAV_TIMEUTC_VALID_TOW)
 687                                   >> (uint8_t)(UBX_CMD_MSK_NAV_TIMEUTC_VALID_TOW / 2));
 688     b_validwkn = (BOOL)(((pst_navtime_utc->valid) & UBX_CMD_MSK_NAV_TIMEUTC_VALID_WKN)
 689                                                       >> (uint8_t)(UBX_CMD_MSK_NAV_TIMEUTC_VALID_WKN / 2));
 690     b_validutc = (BOOL)(((pst_navtime_utc->valid) & UBX_CMD_MSK_NAV_TIMEUTC_VALID_UTC)
 691                                                       >> (uint8_t)(UBX_CMD_MSK_NAV_TIMEUTC_VALID_UTC / 2));
 692
 693     if ((b_validtow == TRUE) && (b_validwkn == TRUE)) {
 694           st_gpstime_raw.tdsts = 2; /* Calibrated */
 695           g_gpstime_raw_tdsts = 2;
 696     } else {
 697           st_gpstime_raw.tdsts = 0; /* Uncalibrated */
 698           g_gpstime_raw_tdsts = 0;
 699     }
 700
 701     b_validutc = b_validutc;
 702     POSITIONING_LOG("year=%04d, month=%02d, date=%02d, hour=%02d, minute=%02d,"
 703                     " second=%02d, validTow=%d, validWkn=%d, validUtc=%d",
 704                     st_gpstime_raw.utc.year, st_gpstime_raw.utc.month, st_gpstime_raw.utc.date,
 705                     st_gpstime_raw.utc.hour, st_gpstime_raw.utc.minute, st_gpstime_raw.utc.second,
 706                     b_validtow, b_validwkn, b_validutc);
 707     /* Notify GPS time to vehicle sensor */
 708     SndGpsTimeRaw((const SENSOR_GPSTIME_RAW*)&st_gpstime_raw);
 709   } else {
 710     POSITIONING_LOG("Forbidden ERROR!![e_format=%d]", e_format);
 711   }
 712
 713     /** Update Receive Format */
 714     g_rcv_format = e_format;
 715
 716     return;
 717 }
 718
 719 /******************************************************************************
 720 @brief      CheckSendCmdGpsCommon<BR>
 721             Send command check processing
 722 @outline    Check the format of the send command
 723 @param[in]  const u_char*      pubRcvData  : Receive command
 724 @param[in]  u_int32          ul_length  : Length of the command
 725 @param[out] TG_GPS_OUTPUT_FORMAT*  peFormat  : Command format information
 726 @return     int32
 727 @retval     RET_NORMAL  : Normal
 728 @retval     RET_ERROR  : Abnormality
 729 *******************************************************************************/
 730 int32 CheckSendCmdGpsCommon(const u_char *p_rcv_data, u_int32 ul_length, TG_GPS_OUTPUT_FORMAT *p_format) {
 731   u_int32 ret = RET_NORMAL;
 732   u_int32 cnt = 0;          /* Ignore -> MISRA-C:2004 Rule 5.6 */
 733
 734   /** Analysis of received commands */
 735   for (cnt = 0; cnt < (u_int32)GPSCMDANATBL_MAX; cnt++) {
 736     /** End-of-table decision */
 737     if (CheckFrontStringPartGpsCommon((const u_char*)ENDMARK, kGpsCmdAnaTbl[cnt].c_sentence) == RET_NORMAL) {
 738       /** When there is no matching sentence */
 739
 740       /** Return value setting */
 741       ret = RET_ERROR;  /* Ignore -> MISRA-C:2004 Rule 3.1 */
 742       break;
 743     }
 744
 745     /** Sentence determination of received command */
 746     if (CheckFrontStringPartGpsCommon(p_rcv_data, kGpsCmdAnaTbl[cnt].c_sentence) == RET_NORMAL) {
 747       if (ul_length == kGpsCmdAnaTbl[cnt].ul_length) {
 748         /** When the sentences match */
 749
 750         /** Reception type determination */
 751         if ( kGpsCmdAnaTbl[cnt].ul_rcv_kind == RCV_RESP ) {
 752           /** Response monitor format setting */
 753           *p_format = kGpsCmdAnaTbl[cnt].e_rcv_format;
 754         } else {
 755           /** Return value setting */
 756           ret = RET_ERROR;  /* Ignore -> MISRA-C:2004 Rule 3.1 */
 757         }
 758
 759         break;  /* Ignore -> MISRA-C:2004 Rule 14.6 */
 760       }
 761     }
 762   }
 763
 764   return ret;
 765 }
 766
 767
 768 /**
 769  * @brief
 770  *   Get a string of fields from a NMEA sentence
 771  *
 772  *   Operation is not guaranteed if a pointer other than a pointer to the NMEA sentence is specified.
 773  *
 774  * @param[in]   field_number  Field No.
 775  * @param[in]   p_src  Pointers to NMEA sentences
 776  * @param[out]  p_dst  Pointer to the output area
 777  * @param[in]   size  Maximum output size
 778  *
 779  * @return Size
 780  */
 781 int32_t GetStringFromNmeaGpsCommon(u_int8 field_number, u_int8* p_src, char* p_dst, size_t size ) {
 782   uint8_t*  ptr = NULL;
 783   /* int32_t numeric; */
 784   uint32_t  i = 0;
 785   int32_t   cnt = 0;
 786   int8_t    buf[GPS_NMEA_FIELD_LEN_MAX] = {0};
 787
 788   /* Null check */
 789   if ((p_src == NULL) || (p_dst == NULL)) {
 790     POSITIONING_LOG("Argument ERROR [p_src=%p, p_dst=%p]", p_src, p_dst);
 791     return 0;
 792   }
 793
 794   ptr = p_src;
 795
 796   for (i = 0; i <= GPS_NMEA_MAX_SZ; i++) {
 797     if (cnt == field_number) {
 798       break;
 799     }
 800
 801     if (*ptr == NMEA_DATA_SEPARATOR) {
 802       cnt++;
 803     }
 804
 805     ptr++;
 806   }
 807
 808   i = 0;
 809   if (*ptr != NMEA_DATA_SEPARATOR) {
 810     for (i = 0; i < (GPS_NMEA_FIELD_LEN_MAX - 1); i++) {
 811       buf[i] = *ptr;
 812       ptr++;
 813
 814       if ((*ptr == NMEA_DATA_SEPARATOR) || (*ptr == NMEA_CHECKSUM_CHAR)) {
 815         i++;
 816         break;
 817       }
 818     }
 819   }
 820
 821   buf[i] = '\0';
 822   strncpy(p_dst, (const char *)buf, size);
 823
 824   return i;
 825 }
 826
 827 /**
 828  * @brief
 829  *   Get the number (real type) of a field from a NMEA sentence
 830  *
 831  *   Operation is not guaranteed if a pointer other than a pointer to the NMEA sentence is specified.
 832  *
 833  * @param[in]  field_number  Field No.
 834  * @param[in]  p_src  Pointers to NMEA sentences
 835  * @param[out] p_valid   Validity of numerical data
 836  *
 837  * @return  Real-number - double
 838  */
 839 double_t GetNumericFromNmeaGpsCommon(u_int8 field_number, u_int8* p_src, BOOL* p_valid) {
 840   double_t  numeric = 0;
 841   char      buf[GPS_NMEA_FIELD_LEN_MAX] = {0};
 842   int32_t   ret = 0;
 843
 844   /* Null check */
 845   if (p_src == NULL) {
 846     POSITIONING_LOG("Argument ERROR [p_src=%p]", p_src);
 847   }
 848
 849   ret = GetStringFromNmeaGpsCommon(field_number, p_src, buf, GPS_NMEA_FIELD_LEN_MAX);
 850
 851   if (ret > 0) {
 852     numeric = atof((const char *)buf);
 853     *p_valid = TRUE;
 854   } else {
 855     *p_valid = FALSE;
 856   }
 857
 858   return numeric;
 859 }
 860
 861 /**
 862  * @brief
 863  *   Get the integer value (integer type) of a field from a NMEA sentence
 864  *
 865  *   Operation is not guaranteed if a pointer other than a pointer to the NMEA sentence is specified.
 866  *
 867  * @param[in]  field_number  Field No.
 868  * @param[in]  p_src  Pointers to NMEA sentences
 869  *
 870  * @return  Integer - int32
 871  */
 872 int32 GetIntegerFromNmeaGpsCommon(u_int8 field_number, u_int8* p_src) {
 873   int32_t integer = 0;
 874   char    buf[GPS_NMEA_FIELD_LEN_MAX] = {0};
 875
 876   /* Null check */
 877   if (p_src == NULL) {
 878     POSITIONING_LOG("Argument ERROR [p_src=%p]", p_src);
 879   }
 880
 881   GetStringFromNmeaGpsCommon(field_number, p_src, buf, GPS_NMEA_FIELD_LEN_MAX);
 882   integer = atoi((const char *)buf);
 883
 884   return integer;
 885 }
 886
 887 /**
 888  * @brief
 889  *   Get the latitude/longitude of the given fi eld from the NMEA sentence
 890  *
 891  *   Operation is not guaranteed if a pointer other than a pointer to the NMEA sentence is specified.
 892  *
 893  * @param[in]  field_number  Field No.
 894  * @param[in]  p_src  Pointers to NMEA sentences
 895  * @param[out] p_valid   Validity of the data
 896  *
 897  * @return Latitude and longitude [1/256 sec]
 898  */
 899 int32_t GetLonLatFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src, BOOL *p_valid) {
 900   int32_t   result = 0;
 901   double_t  value = 0.0;
 902   int32_t   deg = 0;
 903   double_t  min = 0.0;
 904
 905   /* Null check */
 906   if (p_src == NULL) {
 907     POSITIONING_LOG("Argument ERROR [p_src=%p]", p_src);
 908   }
 909
 910   value = GetNumericFromNmeaGpsCommon(field_number, p_src, p_valid);
 911
 912   if ((value < 0.0) || (value > 0.0)) {
 913     deg = (int32_t)(value / 100.0f);
 914     min = (double_t)(value - (deg * 100.0f));
 915     result = (int32_t)(((60.0f * 60.0f * 256.0f) * deg) + ((60.0f * 256.0f) * min));
 916   }
 917
 918   return result;
 919 }
 920
 921 /**
 922  * @brief
 923  *   Get the orientation of any fields from a NMEA sentence
 924  *
 925  *   Operation is not guaranteed if a pointer other than a pointer to the NMEA sentence is specified.
 926  *
 927  * @param[in]  field_number  Field No.
 928  * @param[in]  p_src Pointers to NMEA sentences
 929  * @param[out] p_valid   Validity of the data
 930  *
 931  * @return Orientation[0.01 degree]
 932  */
 933 u_int16 GetHeadingFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src, BOOL *p_valid) {
 934   u_int16   result = 0;
 935   double_t  value = 0.0;
 936
 937   /* Null check */
 938   if (p_src == NULL) {
 939     POSITIONING_LOG("Argument ERROR [p_src=%p]", p_src);
 940   }
 941
 942   value = GetNumericFromNmeaGpsCommon(field_number, p_src, p_valid);
 943
 944   result = (u_int16)((value + 0.005f) * 100.0f);
 945
 946   return result;
 947 }
 948
 949
 950 /**
 951  * @brief
 952  *   Get the orientation of any fields from a NMEA sentence
 953  *
 954  *   Operation is not guaranteed if a pointer other than a pointer to the NMEA sentence is specified.
 955  *
 956  * @param[in]  field_number  Field No.
 957  * @param[in]  p_src Pointers to NMEA sentences
 958  * @param[out] p_valid   Validity of the data
 959  *
 960  * @return speed[0.01m/s]
 961  */
 962 u_int16 GetSpeedFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src, BOOL *p_valid) {
 963   u_int16   result = 0;
 964   double_t  value = 0.0;
 965
 966   /* Null check */
 967   if (p_src == NULL) {
 968     POSITIONING_LOG("Argument ERROR [p_src=%p]", p_src);
 969   }
 970
 971   value = GetNumericFromNmeaGpsCommon(field_number, p_src, p_valid);
 972
 973   // speed[0.01m/s]
 974   result = static_cast<u_int16>(value * 1.852 * 100 / 3.6);
 975
 976   return result;
 977 }
 978
 979 /**
 980  * @brief
 981  *   Get the altitude of certain fields from a NMEA sentence
 982  *
 983  *   Operation is not guaranteed if a pointer other than a pointer to the NMEA sentence is specified.
 984  *
 985  * @param[in]  field_number  Field No.
 986  * @param[in]  p_src Pointers to NMEA sentences
 987  * @param[out] p_valid   Validity of the data
 988  *
 989  * @return Altitude [0.01m]
 990  */
 991 int32 GetAltitudeFromNmeaGpsCommon(u_int8 field_number, u_int8 *p_src, BOOL *p_valid) {
 992   int32     result = 0;
 993   double_t  value = 0.0;
 994
 995   /* Null check */
 996   if (p_src == NULL) {
 997     POSITIONING_LOG("Argument ERROR [p_src=%p]", p_src);
 998   }
 999
1000   value = GetNumericFromNmeaGpsCommon(field_number, p_src, p_valid);
1001
1002   result = static_cast<int32>((value + 0.005f) * 100.0f);
1003
1004   return result;
1005 }
1006
1007 /**
1008  * @brief
1009  *  GPS Control Thread Stop Processing
1010  */
1011 void StopThreadGpsCommon(void) {
1012   /** Stop the start confirmation monitoring timer */
1013   (void)DevGpsTimeStop(GPS_STARTUP_TIMER);    /* Start confirmation monitoring timer      */
1014
1015   /** Stop the periodic reception data monitoring timer */
1016   (void)DevGpsTimeStop(GPS_CYCL_TIMER);      /* Periodic reception data monitoring timer    */
1017
1018   /** Stop the ACK reception monitoring timer */
1019   (void)DevGpsTimeStop(GPS_RECV_ACK_TIMER);    /* ACK reception monitoring timer      */
1020
1021   /** Stop the Navigation Providing Monitor Timer */
1022   (void)DevGpsTimeStop(GPS_NAVIFST_TIMER);    /* Initial Navigation Monitoring Timer      */
1023
1024   /** Stop the Navigation Providing Monitor Timer */
1025   (void)DevGpsTimeStop(GPS_NAVICYCLE_TIMER);    /* Navi monitoring timer        */
1026
1027   /** Stop the Navigation Providing Monitor Timer */
1028   (void)DevGpsTimeStop(GPS_NAVIDISRPT_TIMER);    /* Navigation Monitoring Disruption Log Output Timer  */
1029
1030   /** Stop Time Offering Monitor Timer */
1031   /* (void)DevGpsTimeStop(GPS_DIAGCLK_GUARDTIMER); */  /* Time Offering Monitoring Timer      */
1032
1033   PosTeardownThread(ETID_POS_GPS);
1034
1035   /* don't arrive here */
1036   return;
1037 }
1038
1039 /**
1040  * @brief
1041  *  GPS format order determination process
1042  *
1043  * @param[in]  s_format_1 GPS format1
1044  * @param[in]  s_format_2 GPS format2
1045  * @return  0 Format 1 first<br>
1046  *          1 Format 2 first<br>
1047  *          2 Same as<br>
1048  *          3 Unable to determine
1049  */
1050 uint8_t JudgeFormatOrderGpsCommon(TG_GPS_OUTPUT_FORMAT s_format_1, TG_GPS_OUTPUT_FORMAT s_format_2) {
1051   uint8_t ret = 3;
1052   u_int32 cnt = 0;
1053
1054   if (s_format_1 == s_format_2) {
1055     ret = 2;
1056   } else if ((s_format_1 == GPS_FORMAT_MAX) || (s_format_2 == GPS_FORMAT_MIN)) {
1057     ret = 1;
1058   } else {
1059     for (cnt = 0; cnt < static_cast<u_int32>(GPSCMDANATBL_MAX); cnt++) {
1060       /** End-of-table decision */
1061       if (CheckFrontStringPartGpsCommon((const u_char*)ENDMARK, kGpsCmdAnaTbl[cnt].c_sentence) == RET_NORMAL) {
1062         /** There is no matching sentence */
1063         ret = 3;
1064         break;
1065       }
1066
1067       if (kGpsCmdAnaTbl[cnt].e_rcv_format == s_format_1) {
1068         ret = 0;
1069         break;
1070       } else if (kGpsCmdAnaTbl[cnt].e_rcv_format == s_format_2) {
1071         ret = 1;
1072         break;
1073       }
1074     }
1075   }
1076
1077   return ret;
1078 }
1079
1080 /**
1081  * @brief
1082  *  Get NMEA index process
1083  *
1084  * @param[in]  iFmt GPS formats
1085  * @return  NMEA indexes
1086  */
1087 ENUM_GPS_NMEA_INDEX GetNmeaIdxGpsCommon(TG_GPS_OUTPUT_FORMAT s_output_format) {
1088   ENUM_GPS_NMEA_INDEX ret = GPS_NMEA_INDEX_MAX;
1089
1090   switch (s_output_format) {
1091   case GPS_FORMAT_GGA:
1092       /* GGA */
1093       ret = GPS_NMEA_INDEX_GGA;
1094       break;
1095   case GPS_FORMAT_DGGA:
1096       /* DGGA */
1097       ret = GPS_NMEA_INDEX_DGGA;
1098       break;
1099   case GPS_FORMAT_VTG:
1100       /* VTG */
1101       ret = GPS_NMEA_INDEX_VTG;
1102       break;
1103   case GPS_FORMAT_RMC:
1104       /* RMC */
1105       ret = GPS_NMEA_INDEX_RMC;
1106       break;
1107   case GPS_FORMAT_DRMC:
1108       /* DRMC */
1109       ret = GPS_NMEA_INDEX_DRMC;
1110       break;
1111   case GPS_FORMAT_GLL:
1112       /* GLL */
1113       ret = GPS_NMEA_INDEX_GLL;
1114       break;
1115   case GPS_FORMAT_DGLL:
1116       /* DGLL */
1117       ret = GPS_NMEA_INDEX_DGLL;
1118       break;
1119   case GPS_FORMAT_GSA:
1120       /* GSA */
1121       ret = GPS_NMEA_INDEX_GSA;
1122       break;
1123   case GPS_FORMAT_GSV1:
1124       /* GSV(1) */
1125       ret = GPS_NMEA_INDEX_GSV1;
1126       break;
1127   case GPS_FORMAT_GSV2:
1128       /* GSV(2) */
1129       ret = GPS_NMEA_INDEX_GSV2;
1130       break;
1131   case GPS_FORMAT_GSV3:
1132       /* GSV(3) */
1133       ret = GPS_NMEA_INDEX_GSV3;
1134       break;
1135   case GPS_FORMAT_GSV4:
1136       /* GSV(4) */
1137       ret = GPS_NMEA_INDEX_GSV4;
1138       break;
1139   case GPS_FORMAT_GSV5:
1140       /* GSV(5) */
1141       ret = GPS_NMEA_INDEX_GSV5;
1142       break;
1143   case GPS_FORMAT_GST:
1144       /* GST */
1145       ret = GPS_NMEA_INDEX_GST;
1146       break;
1147   case GPS_FORMAT__CWORD44__GP3:
1148       /* _CWORD44_,GP,3 */
1149       ret = GPS_NMEA_INDEX__CWORD44__GP_3;
1150       break;
1151   case GPS_FORMAT__CWORD44__GP4:
1152       /* _CWORD44_,GP,4 */
1153       ret = GPS_NMEA_INDEX__CWORD44__GP_4;
1154       break;
1155   case GPS_FORMAT_P_CWORD82_F_GP0:
1156       /* P_CWORD82_F,GP,0 */
1157       ret = GPS_NMEA_INDEX_P_CWORD82_F_GP_0;
1158       break;
1159   case GPS_FORMAT_P_CWORD82_J_GP1:
1160       /* P_CWORD82_J,GP,1 */
1161       ret = GPS_NMEA_INDEX_P_CWORD82_J_GP_1;
1162       break;
1163   default:
1164       /* nop */
1165       break;
1166   }
1167   return ret;
1168 }
1169
1170 /**
1171  * @brief
1172  *  GPS reset request command transmission processing
1173  */
1174 void SendResetReqGpsCommon(void) {
1175   T_APIMSG_MSGBUF_HEADER  s_msg_header;     /* Message header */
1176   POS_RESETINFO           s_send_msg;  /* Reset information     */
1177   u_int8                  uc_send_buf[(sizeof(T_APIMSG_MSGBUF_HEADER) + sizeof(POS_RESETINFO))]; /* Transmitting buffer */
1178   RET_API                 ret = RET_NORMAL;
1179
1180   /* Send reset request */
1181   POSITIONING_LOG("GPS Reset Request");
1182
1183   /* <<Creation of message header section>>>>      */
1184   s_msg_header.signo            = 0;                      /* Signal information setting                  */
1185   s_msg_header.hdr.sndpno       = PNO_NAVI_GPS_MAIN;      /* Source thread No. setting            */
1186   s_msg_header.hdr.respno       = PNO_NAVI_GPS_MAIN;      /* Destination process No.                */
1187   s_msg_header.hdr.cid          = CID_GPS_REQRESET;       /* Command ID setting                  */
1188   s_msg_header.hdr.msgbodysize  = sizeof(POS_RESETINFO);  /* Message data length setting        */
1189   s_msg_header.hdr.rid          = 0;                      /* Resource ID Setting                  */
1190   s_msg_header.hdr.reserve      = 0;                      /* Reserved area clear              */
1191
1192   (void)memcpy(&uc_send_buf[ 0 ], &s_msg_header, sizeof(T_APIMSG_MSGBUF_HEADER));
1193
1194   /* <<Creation of data section>>                  */
1195   s_send_msg.mode               = GPS_RST_COLDSTART;      /* Reset mode                    */
1196   s_send_msg.sndpno             = PNO_NAVI_GPS_MAIN;      /* Caller PNo.                        */
1197   s_send_msg.respno             = PNO_NAVI_GPS_MAIN;      /* Destination PNo.                        */
1198   s_send_msg.reserve[0]         = 0;
1199   s_send_msg.reserve[1]         = 0;
1200   s_send_msg.reserve[2]         = 0;
1201
1202   /* Copy data to send buffer        */
1203   (void)memcpy( &uc_send_buf[ sizeof( T_APIMSG_MSGBUF_HEADER ) ], &s_send_msg, sizeof(POS_RESETINFO) );
1204
1205   /* <<Messaging>>                */
1206   ret = _pb_SndMsg(PNO_NAVI_GPS_MAIN,  sizeof(uc_send_buf), reinterpret_cast<void *>(uc_send_buf), 0);
1207
1208   if (ret != RET_NORMAL) {
1209       POSITIONING_LOG("GPS Reset Request Error[ret = %d]", ret);
1210   }
1211 }
1212
1213
1214
1215 void DevGpsCommSettingTime(void) {
1216   TG_GPS_SND_DATA         *pst_rcv_data;
1217   RET_API                 ret = RET_NORMAL;
1218
1219   pst_rcv_data = reinterpret_cast<TG_GPS_SND_DATA*>(&(g_gps_msg_rcvr.msgdat[0]));
1220
1221   ret = DevGpsResetReq(PNO_NONE, 0);
1222   if (RET_NORMAL != ret) {
1223     POSITIONING_LOG("DevGpsResetReq failed.");
1224   } else {
1225     /* Time setting(u-blox) */
1226     DevGpsSettingTime(reinterpret_cast<POS_DATETIME *>(pst_rcv_data->ub_data));
1227   }
1228   return;
1229 }
1230
1231 void DevGpsReadGpsTime(POS_DATETIME* pst_datetime) {
1232     int32_t ret;
1233     ST_GPS_SET_TIME buf;
1234     POS_DATETIME st_datetime;
1235     memset(&st_datetime, 0, sizeof(st_datetime));
1236
1237     // if (isGpsTimeStatusProof == TRUE) {
1238         /* Clear the flag if the date and time status has already been calibrated. */
1239         // WriteGpsTimeToBackup(FALSE, &st_datetime);
1240         // POSITIONING_LOG("D_BK_ID_POS_GPS_TIME_SET_INFO:W:Clear");
1241     // } else {
1242         /* Read from backup RAM */
1243         ret = Backup_DataRd(D_BK_ID_POS_GPS_TIME_SET_INFO, 0, &buf, sizeof(ST_GPS_SET_TIME));
1244         if (ret == BKUP_RET_NORMAL) {
1245             pst_datetime->year   = buf.year;
1246             pst_datetime->month  = buf.month;
1247             pst_datetime->date   = buf.date;
1248             pst_datetime->hour   = buf.hour;
1249             pst_datetime->minute = buf.minute;
1250             pst_datetime->second = buf.second;
1251
1252             /* Update of the date/time status fixed value setting indication flag */
1253             if (buf.flag == TRUE) {
1254                 g_is_gpstime_sts = TRUE;
1255                 POSITIONING_LOG("g_is_gpstime_sts = TRUE");
1256                 POSITIONING_LOG("%d/%d/%d %d:%d:%d", buf.year,
1257                                                      buf.month,
1258                                                      buf.date,
1259                                                      buf.hour,
1260                                                      buf.minute,
1261                                                      buf.second);
1262             } else {
1263                 g_is_gpstime_sts = FALSE;
1264                 POSITIONING_LOG("g_is_gpstime_sts = FALSE");
1265             }
1266         } else {
1267             POSITIONING_LOG("Backup_DataRd ERROR!! [ret=%d]", ret);
1268         }
1269     // }
1270     return;
1271 }
1272
1273 RET_API GpsSetPosBaseEvent(uint16_t ul_snd_pno, int32_t event_val) {
1274   RET_API     ret = RET_ERROR;       /* Return value */
1275   EventID     ul_eventid = 0;          /* Event ID */
1276   char        event_name[32];         /* Event name character string buffer */
1277
1278   memset(reinterpret_cast<void *>(event_name), 0, sizeof(event_name));
1279   snprintf(event_name, sizeof(event_name), "VEHICLE_%X", ul_snd_pno);
1280   /* Event Generation */
1281   ul_eventid = _pb_CreateEvent(FALSE, 0, event_name);
1282
1283   // initialize EventID
1284   if (0 != ul_eventid) {
1285     ret = _pb_SetEvent(ul_eventid, SAPI_EVSET_ABSOLUTE, VEHICLE_EVENT_VAL_INIT);
1286     if (RET_NORMAL != ret) {
1287       /* Delete event and return event generation failed */
1288       ret = _pb_DeleteEvent(ul_eventid);
1289       ul_eventid = 0;
1290     }
1291   }
1292
1293   if (0 != ul_eventid) {
1294     /* Event publishing(Event wait release) */
1295     ret = _pb_SetEvent(ul_eventid, SAPI_EVSET_ABSOLUTE, event_val);
1296     if (RET_NORMAL != ret) {
1297         /* Event issuance failure */
1298         POSITIONING_LOG("set Event failed.");
1299     }
1300     /* Event deletion */
1301     _pb_DeleteEvent(ul_eventid);
1302   } else {
1303       /* Event generation failure */
1304       POSITIONING_LOG("create Event failed.");
1305   }
1306   return ret;
1307 }
1308
1309 /**
1310  * @brief
1311  *  GPS reset response command reception processing
1312  *
1313  * @param[in]  p_data Pointer to the received GPS command
1314  */
1315 BOOL DevGpsRcvProcGpsResetResp(TG_GPS_RCV_DATA* p_data) {
1316     RET_API ret;
1317     u_int32 ul_reset_sts = GPS_SENDNG;
1318     BOOL b_snd_flag = TRUE;
1319     uint8_t *p_gps_data;
1320     char date[6]; /* ddmmyy */
1321     char work[8]; /* Work buffer for converting String data */
1322
1323     if (p_data == NULL) {
1324         POSITIONING_LOG("Argument ERROR!! [p_data = %p]", p_data);
1325     } else {
1326         p_gps_data = p_data->bygps_data;
1327
1328         /* Checksum determination */
1329         ret = CheckSumGpsCommon(p_gps_data, p_data->bydata_len, GPS_FORMAT_RMC);
1330         if (ret == RET_NORMAL) {
1331             /* Get Date information */
1332             GetStringFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_DATE, p_gps_data, date, 6);
1333
1334             strncpy(work, &date[4], 2);
1335
1336             if (atoi(work) == 0) {
1337                 ul_reset_sts = GPS_SENDOK;  /* Reset Successful */
1338                 /* Reissuing commands to a GPS device from here */
1339                  /* Send NAV-TIMEUTC sentence addition requests */
1340                 DevGpsNavTimeUTCAddReq();
1341             }
1342             POSITIONING_LOG("reset flag = %d, year after reset = %s", ul_reset_sts, work);
1343         } else {
1344             b_snd_flag = FALSE; /* If the checksum is NG, wait for the next reply message without sending a callback. */
1345             POSITIONING_LOG("wrong checksum [ret = %d]", ret);
1346         }
1347
1348         if (b_snd_flag == TRUE) {
1349             /** Drop the time status to an abnormal time */
1350             // Todo Suntec MACRO __CWORD71_ Disable, This Functions do nothing actrually.
1351             // DEV_Gps_SetTimeStatus_NG();
1352
1353             /** Send Reset Response(Successful transmission) */
1354             DevGpsRstAnsSend(g_gps_mngr.resp_pno, g_gps_mngr.resp_rid, ul_reset_sts);
1355
1356             POSITIONING_LOG("DEV_Gps_RstAndSend CALL!! [ul_reset_sts = %d]", ul_reset_sts);
1357
1358             /** Clear reset command sending judgment flag */
1359             g_gps_reset_cmd_sending = FALSE;
1360         }
1361     }
1362     return b_snd_flag;
1363 }
1364
1365 /**
1366  * @brief
1367  *  GPS time setting response command reception processing
1368  *
1369  * @param[in]  p_data Pointer to the received GPS command
1370  */
1371 void DevGpsRcvProcTimeSetResp(TG_GPS_RCV_DATA* p_data) {
1372     u_int32 ul_reset_sts = GPS_SENDNG;
1373     BOOL b_snd_flag = TRUE;
1374     POS_DATETIME                    st_datetime;  /* Setting time */
1375     uint16_t set_gpsw;
1376     uint32_t set_gpsws;
1377     uint16_t rcv_gpsw;
1378     uint32_t rcv_gpsws;
1379     BOOL ret = FALSE;
1380     int32_t status;
1381
1382     if (p_data == NULL) {
1383       POSITIONING_LOG("Argument ERROR!! [p_data = %p]", p_data);
1384     } else {
1385       /* Read from backup RAM */
1386       status = Backup_DataRd(D_BK_ID_POS_GPS_TIME_SET_INFO, 0, &st_datetime, sizeof(ST_GPS_SET_TIME));
1387       if (status == BKUP_RET_NORMAL) {
1388           /* Convert to GSP week number, GPS week second */
1389           DevGpsYMD2GPSTIME(static_cast<int32>(st_datetime.year),
1390                             static_cast<int32>(st_datetime.month),
1391                             static_cast<int32>(st_datetime.date), static_cast<int32>(st_datetime.hour),
1392                             static_cast<int32>(st_datetime.minute),
1393                             static_cast<int32>(st_datetime.second), &set_gpsw, &set_gpsws);
1394
1395           memcpy(&rcv_gpsw,  &(p_data->bygps_data[24]), sizeof(uint16_t));
1396           memcpy(&rcv_gpsws, &(p_data->bygps_data[26]), sizeof(uint32_t));
1397           rcv_gpsws /= 1000; /* Convert week (ms) to week (s) */
1398           POSITIONING_LOG("recived gpsw[%d] gpsws[%d]\n", rcv_gpsw, rcv_gpsws);
1399
1400           /* Check if GPS week number and GPS week second are within valid range */
1401           ret = DevGpsCheckGpsTime(set_gpsw, set_gpsws, rcv_gpsw, rcv_gpsws, GPS_SETTIME_RANGE);
1402           if (ret == TRUE) {
1403               /* Date/Time Status Fixed Value Setting Indication Flag ON */
1404               g_is_gpstime_sts = TRUE;
1405               POSITIONING_LOG("g_is_gpstime_sts = TRUE");
1406
1407               /* Time Setting Successful : Update backup RAM time */
1408               ul_reset_sts = GPS_SENDOK;
1409               WriteGpsTimeToBackup(TRUE, &st_datetime);
1410               POSITIONING_LOG("SetGpsTime Success");
1411               POSITIONING_LOG("D_BK_ID_POS_GPS_TIME_SET_INFO:W:%d/%d/%d %d:%d:%d",
1412                                                     st_datetime.year,
1413                                                     st_datetime.month,
1414                                                     st_datetime.date,
1415                                                     st_datetime.hour,
1416                                                     st_datetime.minute,
1417                                                     st_datetime.second);
1418           } else {
1419               /* Time setting error (Difference in set time): Time deletion of backup RAM */
1420               memset(&st_datetime, 0, sizeof(st_datetime));
1421               WriteGpsTimeToBackup(FALSE, &st_datetime);
1422               POSITIONING_LOG("SetGpsTime failed\n");
1423               POSITIONING_LOG("D_BK_ID_POS_GPS_TIME_SET_INFO:W:Clear\n");
1424           }
1425       } else {
1426           /* Backup RAM read failed */
1427           b_snd_flag = FALSE;
1428           POSITIONING_LOG("Backup_DataRd ERROR!! [status=%d", status);
1429       }
1430
1431       if (b_snd_flag == TRUE) {
1432           /** Send Time Set Response */
1433           DevGpsTimesetAnsSend(g_gps_mngr.resp_pno, g_gps_mngr.resp_rid, ul_reset_sts);
1434
1435           POSITIONING_LOG("DEV_Gps_TimesetAndSend CALL!! [ul_reset_sts = %d]", ul_reset_sts);
1436       }
1437     }
1438     return;
1439 }
1440
1441 /**
1442  * @brief
1443  *   GPS rollover standard week number acquisition response reception processing
1444  *
1445  * @param[in]  p_data Pointer to the received GPS command
1446  */
1447 void DevGpsRcvProcWknRolloverGetResp(TG_GPS_RCV_DATA* p_data) {
1448   RET_API ret;
1449   SENSOR_WKN_ROLLOVER_HAL st_wkn_rollover;
1450
1451   if (p_data == NULL) {
1452       POSITIONING_LOG("Argument ERROR!! [p_data = %p]", p_data);
1453   } else {
1454       /* Checksum determination */
1455       ret = CheckSumGpsCommon(p_data->bygps_data, p_data->bydata_len, GPS_FORMAT_CFG_NAVX5);
1456       if (ret == RET_NORMAL) {
1457           /* Version information extraction */
1458         (void)memcpy(&(st_wkn_rollover.us_wkn),
1459                             &(p_data->bygps_data[UBX_CMD_OFFSET_PAYLOAD + UBX_CMD_OFFSET_WKNROLLOVER]),
1460                             sizeof(st_wkn_rollover.us_wkn));
1461         POSITIONING_LOG("wknRollover=%d", st_wkn_rollover.us_wkn);
1462
1463         // gwknRollover = st_wkn_rollover.us_wkn;
1464
1465         /* Send reference week number to vehicle sensor */
1466         ret = DevGpsSndWknRollover(&st_wkn_rollover);
1467         if (ret != RET_NORMAL) {
1468           POSITIONING_LOG("VehicleUtility_pb_SndMsg ERROR!! [ret=%d]", ret);
1469         }
1470       } else {
1471           POSITIONING_LOG("CheckSumGpsCommon ERROR!! [ret = %d]", ret);
1472       }
1473
1474       if (ret != RET_NORMAL) {
1475           /* GPS rollover standard week number acquisition request retransmission */
1476           ret = DevGpsWknRolloverGetReq();
1477           if (ret != RET_NORMAL) {
1478               POSITIONING_LOG("DevGpsWknRolloverGetReq ERROR!! [ret=%d]", ret);
1479           }
1480       }
1481   }
1482
1483   return;
1484 }
1485
1486 /**
1487  * @brief
1488  *   Receiving additional responses to NAV-SVINFO commands
1489  *
1490  * @param[in]  p_data Pointer to the received GPS command
1491  */
1492 void DevGpsRcvProcNavSvinfoAddResp(TG_GPS_RCV_DATA* p_data) {
1493     RET_API ret;
1494     /* Common Response Command Reception Processing */
1495     ret = DevGpsRcvProcCmnResp(p_data, GPS_CMD_SENTENCEADD_NAVSVINFO);
1496     if (ret != RET_NORMAL) {
1497         POSITIONING_LOG("DEV_Gps_RcvProcCmnResp(ADD_NAVSVINFO) ERROR!!");
1498         /* Retransmission of requests to add NAV-SVINFO commands */
1499         (void)DevGpsNavTimeUTCAddReq();
1500     }
1501     return;
1502 }
1503
1504
1505 /**
1506  * @brief
1507  *  Common processing when a GPS receiver error is detected
1508  */
1509 void GpsReceiverErrGpsCommon(void) {
1510   BOOL *pb_rcv_err = &(g_gps_mngr.rcv_err_flag); /* GSP receiver error detection status */
1511
1512   /* Update GPS receiver anomaly detection flag */
1513   *pb_rcv_err = TRUE;
1514
1515     DevSendGpsConnectError(TRUE);
1516
1517     return;
1518 }
1519
1520
1521 /**
1522  * @brief
1523  *  GPS device setting response reception processing
1524  *
1525  * @param[in]  p_data Pointer to the received GPS command
1526  * @param[in]  cmd   Command in the configuration request
1527  * @return  RET_NORMAL Normal completion<br>
1528  *          RET_ERROR  ABENDs
1529  */
1530 RET_API DevGpsRcvProcCmnResp(TG_GPS_RCV_DATA* p_data, uint8_t cmd) {
1531     RET_API ret = RET_ERROR;
1532     uint8_t is_ack; /* 1=ACK, 0=NACK */
1533     TG_GPS_UBX_ACK_DATA st_ack;
1534     uint8_t *puc_msg_class = &(st_ack.uc_msg_class);
1535     uint8_t *puc_msg_id    = &(st_ack.uc_msg_id);
1536
1537     if (p_data == NULL) {
1538         POSITIONING_LOG("Argument ERROR!![p_data=%p]", p_data);
1539     } else {
1540         /* Checksum determination */
1541         ret = CheckSumGpsCommon(p_data->bygps_data, p_data->bydata_len, GPS_FORMAT_ACK_ACKNACK);
1542         if (ret == RET_NORMAL) {
1543           is_ack = p_data->bygps_data[UBX_CMD_OFFSET_ACKNAK];
1544           if (is_ack == 1) {
1545             /* Response command information extraction */
1546             (void)memcpy(&st_ack, &(p_data->bygps_data[UBX_CMD_OFFSET_PAYLOAD]), sizeof(st_ack));
1547             POSITIONING_LOG("msgClass=0x%02x, msgID=0x%02x", *puc_msg_class, *puc_msg_id);
1548
1549             /* When the response wait command does not match the reception command */
1550             if (((cmd == GPS_CMD_SENTENCEADD_NMEAGST)    && ((*puc_msg_class != 0x06) || (*puc_msg_id != 0x01)))  ||
1551               ((cmd == GPS_CMD_SENTENCEADD_NAVTIMEUTC) && ((*puc_msg_class != 0x06) || (*puc_msg_id != 0x01)))  ||
1552               ((cmd == GPS_CMD_SENTENCEADD_NAVCLOCK)   && ((*puc_msg_class != 0x06) || (*puc_msg_id != 0x01)))  ||
1553               ((cmd == GPS_CMD_SENTENCEADD_RXMRTC5)    && ((*puc_msg_class != 0x06) || (*puc_msg_id != 0x01)))  ||
1554               ((cmd == GPS_CMD_SENTENCEADD_NAVSVINFO)  && ((*puc_msg_class != 0x06) || (*puc_msg_id != 0x01)))  ||
1555               ((cmd == GPS_CMD_AUTOMOTIVEMODE)         && ((*puc_msg_class != 0x06) || (*puc_msg_id != 0x24)))) {
1556               POSITIONING_LOG("Invalid ACK/NACK!! [cmd=%d, msgClass=0x%02x, msgID=0x%02x]\n",
1557                       cmd, *puc_msg_class, *puc_msg_id);
1558               ret = RET_ERROR;
1559             }
1560           } else {
1561             ret = RET_ERROR;
1562           }
1563         } else {
1564           POSITIONING_LOG("DEV_Gps_ChkSum ERROR!![ret=%d]", ret);
1565         }
1566     }
1567     return ret;
1568 }
1569
1570
1571 RET_API DevGpsResetReq(PNO us_pno, RID uc_rid) {
1572   RET_API                 ret;
1573   TG_GPS_SAVECMD          st_save_cmd;
1574   TG_GPS_SND_DATA*        pst_snd_data;
1575   uint8_t*                p_ublox_data;
1576
1577                                    /*  SYNC_CHAR_1_2   CLASS  ID     length(L/U) */
1578   static const u_int8  csnd_cmd[] = {   0xB5,   0x62,   0x06,  0x04,  0x04,   0x00,
1579                                    /* navBbrMask(L/U) resetMode/res   CK_A  CK_B */
1580                                       0xFF,   0xFF,   0x00,   0x00,   0x00,  0x00 };
1581
1582   pst_snd_data = &(st_save_cmd.sndcmd);
1583   p_ublox_data = pst_snd_data->ub_data;
1584
1585   memset(&st_save_cmd, 0x00, sizeof(st_save_cmd));
1586   memcpy(p_ublox_data, csnd_cmd, UBX_CMD_SIZE_CFG_RST);
1587
1588   /* Checksum assignment */
1589   DevGpsSetChkSum(p_ublox_data, UBX_CMD_SIZE_CFG_RST);
1590
1591   pst_snd_data->us_size = UBX_CMD_SIZE_CFG_RST;
1592   st_save_cmd.us_pno = us_pno;                    /* Result notification destination process number  */
1593   st_save_cmd.uc_rid = uc_rid;                    /* Result Notification Resource ID     */
1594   st_save_cmd.uc_rst = GPS_CMD_RESET;             /* Reset flag         */
1595   st_save_cmd.e_cmd_info = GPS_FORMAT_RMC;        /* Command information           */
1596
1597   /* Command is suspended and terminated. */
1598   ret = DevGpsSaveCmd(&st_save_cmd);
1599   if (ret != RET_NORMAL) {
1600       POSITIONING_LOG("# GpsCommCtl # GPS Command Reserve bufferFull !");
1601
1602       /** When the pending buffer is full, a reset response (communication error) is returned. */
1603       DevGpsRstAnsSend(st_save_cmd.us_pno, st_save_cmd.uc_rid, GPS_SENDNG);
1604   }
1605
1606   return ret;
1607 }
1608
1609 void DevGpsSettingTime(POS_DATETIME* pst_datetime) {
1610     RET_API                        ret;
1611     TG_GPS_SAVECMD                 st_save_cmd;
1612     TG_GPS_SND_DATA*               pst_snd_data;
1613     uint8_t*                       p_ublox_data;
1614     TG_GPS_UBX_AID_INI_POLLED*     pst_ublox_data;
1615
1616     uint16_t gpsw;
1617     uint32_t gpsws;
1618
1619     if (pst_datetime == NULL) {
1620         POSITIONING_LOG("Argument ERROR!! [pstDataTime = %p]", pst_datetime);
1621     } else {
1622         pst_snd_data = &(st_save_cmd.sndcmd);
1623         p_ublox_data = pst_snd_data->ub_data;
1624
1625         (void)memset(&st_save_cmd, 0x00, sizeof(st_save_cmd));
1626
1627         /* Convert to GSP week number, GPS week second */
1628         (void)DevGpsYMD2GPSTIME(static_cast<int32>(pst_datetime->year),
1629                                 static_cast<int32>(pst_datetime->month),
1630                                 static_cast<int32>(pst_datetime->date),
1631                                 static_cast<int32>(pst_datetime->hour),
1632                                 static_cast<int32>(pst_datetime->minute),
1633                                 static_cast<int32>(pst_datetime->second), &gpsw, &gpsws);
1634
1635
1636         /* Send command sequence generation */
1637         pst_ublox_data = reinterpret_cast<TG_GPS_UBX_AID_INI_POLLED*>(reinterpret_cast<void *>(p_ublox_data));
1638         pst_ublox_data->uc_sync_char1 = 0xB5;
1639         pst_ublox_data->uc_sync_char2 = 0x62;
1640         pst_ublox_data->uc_class     = 0x0B;
1641         pst_ublox_data->uc_id        = 0x01;
1642         pst_ublox_data->uc_length[0] = 0x30;
1643         pst_ublox_data->uc_length[1] = 0x00;
1644         pst_ublox_data->uc_flags[0]  = 0x02; /* Time setting: The second bit from the low order is 1. */
1645
1646         (void)memcpy(pst_ublox_data->wn, &gpsw, sizeof(uint16_t));
1647
1648         gpsws *= 1000;
1649         (void)memcpy(pst_ublox_data->tow, &gpsws, sizeof(uint32_t));
1650
1651         /* Checksum assignment */
1652         DevGpsSetChkSum(reinterpret_cast<u_int8 *>(reinterpret_cast<void*>(pst_ublox_data)),
1653                                                                         UBX_CMD_SIZE_AID_INI);
1654
1655         /* Concatenate Poll Request Command */
1656         ret = DevGpsCatPollReq(p_ublox_data, UBX_POLL_CMD_KIND_AID_INI);
1657         if (ret != RET_NORMAL) {
1658             POSITIONING_LOG("DEV_Gps_CatPollReqUblox ERROR!! [ret = %d]", ret);
1659         } else {
1660             pst_snd_data->us_size = UBX_CMD_SIZE_AID_INI_PLUS_POLL;
1661             st_save_cmd.us_pno = PNO_VEHICLE_SENSOR;   /* Result notification destination process number  */
1662             st_save_cmd.uc_rid = 0;                    /* Result Notification Resource ID      */
1663             st_save_cmd.uc_rst = GPS_CMD_TIMESET;      /* Response flag              */
1664             st_save_cmd.e_cmd_info = GPS_FORMAT_AID_INI;/* Command information            */
1665
1666             /* Command is suspended and terminated. */
1667             ret = DevGpsSaveCmd(&st_save_cmd);
1668             if (ret != RET_NORMAL) {
1669                 POSITIONING_LOG("DevGpsSaveCmd ERROR!! [ret = %d]", ret);
1670                 /* Todo: retransmission by timer */
1671             }
1672             /* Update non-volatile of time setting  */
1673             WriteGpsTimeToBackup(FALSE, pst_datetime);
1674             /* Waiting for a GPS device to respond */
1675             g_is_gpstime_sts = FALSE;
1676             POSITIONING_LOG("Start SetGpsTime\n");
1677             POSITIONING_LOG("D_BK_ID_POS_GPS_TIME_SET_INFO:W:%d/%d/%d %d:%d:%d\n",
1678                          pst_datetime->year,
1679                          pst_datetime->month,
1680                          pst_datetime->date,
1681                          pst_datetime->hour,
1682                          pst_datetime->minute,
1683                          pst_datetime->second);
1684         }
1685     }
1686     return;
1687 }
1688
1689 /**
1690  * @brief
1691  *  Converting from the Gregorian calendar to the Julian Day of Modification
1692  *
1693  * @param[in] y     year
1694  * @param[in] m     month
1695  * @param[in] d     day
1696  *
1697  * @return Modified Julian Date
1698  */
1699 int DevGpsYMD2JD(int y, int m, int d) {
1700   double jd = JULIAN_DAY;
1701   double tmp;
1702   int mjd = 0;
1703
1704   if (m <= 2) {  /* In January, February in December and 14th of the previous year */
1705       y--;
1706       m += 12;
1707   }
1708   y += 4800;
1709   m -= 3;
1710   d -= 1;
1711
1712   tmp = static_cast<double>(y / 400);
1713   y %= 400;
1714   jd += tmp * 146097;
1715
1716   tmp = static_cast<double>(y / 100);
1717   y %= 100;
1718   jd += tmp * 36524;
1719
1720   tmp = static_cast<double>(y / 4);
1721   y %= 4;
1722   jd += tmp * 1461;
1723   jd += static_cast<double>(y * 365);
1724
1725   tmp = static_cast<double>(m / 5);
1726   m %= 5;
1727   jd += tmp * 153;
1728
1729   tmp = static_cast<double>(m / 2);
1730   m %= 2;
1731   jd += tmp * 61;
1732   jd += static_cast<double>(m * 31);
1733   jd += static_cast<double>(d);
1734
1735   mjd = static_cast<int>(jd - MODIFIED_JULIAN_DAY_OFFSET); /* Julian Date-> Modified Julian Date */
1736
1737   return mjd;
1738 }
1739
1740 /**
1741  * @brief
1742  *  Converting from year, month, day, hour, minute and second (GPS hour) to GPS week and GPS week seconds
1743  *
1744  *  Converting y:year, m:month, d:day, hh:hour, mm:minute, ss:second (GPS hour) to GPS week/GPS week second
1745  *
1746  * @param[in] y     year
1747  * @param[in] m     month
1748  * @param[in] d     day
1749  * @param[in] hh    hour
1750  * @param[in] mm    munites
1751  * @param[in] ss    sec
1752  * @param[out] gpsw
1753  * @param[out] gpsws
1754  *
1755  * @return TRUE / FALSE
1756  */
1757 BOOL DevGpsYMD2GPSTIME(const int32 y, const int32 m, const int32 d, const int32 hh,
1758                               const int32 mm, const int32 ss, u_int16* gpsw, u_int32* gpsws) {
1759     /*
1760        MJD  :Y year M month D date modified Julian date (Convert date to modified Julian date)
1761        GPSW : GPS week
1762        GPSWS: GPS week second
1763     */
1764     int mjd = DevGpsYMD2JD(y, m, d);
1765
1766     *gpsw   = (uint16_t)((mjd - 44244) / 7);
1767     *gpsws  = (uint32_t)(((mjd - 44244 - (*gpsw * 7)) * 86400) +
1768                           (hh * 3600) + (mm * 60) + ss + 16); /* Add leap seconds ( 16 seconds) at 2015/1/1 */
1769
1770     if (*gpsws >= GPS_WS_MAX) {
1771         (*gpsw)++;
1772         *gpsws -= GPS_WS_MAX;
1773     }
1774
1775     POSITIONING_LOG("*gpsw = %d, *gpsws = %d", *gpsw, *gpsws);
1776
1777     return TRUE;
1778 }
1779
1780 /**
1781  * @brief
1782  *  Concatenating Poll Request commands to UBX commands
1783  *
1784  * @param[in/out]  *p_ublox_data     Pointer to the concatenated UBX command
1785  * @param[in]      kind          Additional command types
1786  *
1787  * @return RET_NORAML / RET_ERROR
1788  */
1789 RET_API DevGpsCatPollReq(uint8_t *p_ublox_data, uint16_t kind) {
1790   RET_API ret_api = RET_NORMAL;
1791   TG_GPS_UBX_PACKET_HEADER          *pst_ubx_pkg_hdr;
1792   TG_GPS_UBX_COMMAND_NO_DATA        *pst_ubx_poll_req;
1793   uint32_t length;
1794
1795                                          /* Header      ID          length      Checksum */
1796   static uint8_t c_aidini_poll_cmd[]  = {0xB5, 0x62, 0x0B, 0x01, 0x00, 0x00, 0x00, 0x00};
1797   static uint8_t c_cfgnav5_poll_cmd[] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x00, 0x00, 0x00};
1798
1799   uint8_t* p_poll_cmd;
1800   uint16_t poll_cmd_length;
1801
1802   if (p_ublox_data == NULL) {
1803     POSITIONING_LOG("Argument ERROR!! [p_ublox_data=%p]", p_ublox_data);
1804     ret_api = RET_ERROR;
1805   } else {
1806     if (kind == UBX_POLL_CMD_KIND_AID_INI) {
1807       p_poll_cmd   = c_aidini_poll_cmd;
1808       poll_cmd_length = UBX_CMD_SIZE_AID_INI_POLL;
1809     } else if (kind == UBX_POLL_CMD_KIND_CFG_NAV5) {
1810       p_poll_cmd   = c_cfgnav5_poll_cmd;
1811       poll_cmd_length = UBX_CMD_SIZE_CFG_NAV5_POLL;
1812     } else {
1813         POSITIONING_LOG("Argument ERROR!! [kind=%d]", kind);
1814         ret_api = RET_ERROR;
1815     }
1816
1817     if (ret_api != RET_ERROR) {
1818       pst_ubx_pkg_hdr = reinterpret_cast<TG_GPS_UBX_PACKET_HEADER*>(reinterpret_cast<void*>(p_ublox_data));
1819       length = pst_ubx_pkg_hdr->us_length;
1820
1821       if (length == 0) {
1822         POSITIONING_LOG("pst_ubx_pkg_hdr->us_length=0 ERROR!!");
1823         ret_api = RET_ERROR;
1824       } else {
1825         /* Add Poll Request Command */
1826         pst_ubx_poll_req = reinterpret_cast<TG_GPS_UBX_COMMAND_NO_DATA*>(reinterpret_cast<void*>(p_ublox_data +
1827                                                                                               length +
1828                                                                               sizeof(TG_GPS_UBX_COMMAND_NO_DATA)));
1829
1830         memcpy(reinterpret_cast<void*>(pst_ubx_poll_req), reinterpret_cast<void*>(p_poll_cmd), poll_cmd_length);
1831
1832         DevGpsSetChkSum(reinterpret_cast<u_int8 *>(reinterpret_cast<void*>(pst_ubx_poll_req)), poll_cmd_length);
1833       }
1834     }
1835   }
1836
1837   return ret_api;
1838 }
1839
1840 void DevGpsSetGpsweekcorcnt(void) {
1841     CLOCKGPS_GPSWEEKCOR_CNT_MSG gpsweekcor_cnt_msg;
1842     memcpy(&gpsweekcor_cnt_msg, &g_gps_msg_rcvr, sizeof(CLOCKGPS_GPSWEEKCOR_CNT_MSG));
1843     g_gps_week_cor_cnt = gpsweekcor_cnt_msg.gpsweekcorcnt;
1844     return;
1845 }
1846
1847 RET_API DevGpsNavTimeUTCAddReq(void) {
1848   RET_API                 ret;
1849   TG_GPS_SAVECMD          st_save_cmd;
1850   TG_GPS_SND_DATA*        pst_snd_data;
1851   uint8_t*                p_ublox_data;
1852
1853                                 /*  SYNC_CHAR_1_2  CLASS  ID    length(L/U)   Payload                 CK_A    CK_B */
1854   static const u_int8 csnd_cmd[] = {   0xB5, 0x62,  0x06,  0x01, 0x03,   0x00, 0x01,   0x21,   0x01,   0x00,   0x00  };
1855
1856   pst_snd_data = &(st_save_cmd.sndcmd);
1857   p_ublox_data = pst_snd_data->ub_data;
1858
1859   memset(&st_save_cmd, 0x00, sizeof(st_save_cmd));
1860
1861   /* Send command generation */
1862   memcpy(p_ublox_data, csnd_cmd, UBX_CMD_SIZE_CFG_MSG);
1863
1864   /* Checksum assignment */
1865   DevGpsSetChkSum(p_ublox_data, UBX_CMD_SIZE_CFG_MSG);
1866
1867   pst_snd_data->us_size = UBX_CMD_SIZE_CFG_MSG;
1868   st_save_cmd.us_pno = PNO_VEHICLE_SENSOR;             /* Result notification destination process number  */
1869   st_save_cmd.uc_rid = 0;                              /* Result notification resource ID      */
1870   st_save_cmd.uc_rst = GPS_CMD_SENTENCEADD_NAVTIMEUTC; /* Response flag            */
1871   st_save_cmd.e_cmd_info = GPS_FORMAT_ACK_ACKNACK;      /* Command information            */
1872
1873   /* Command is suspended and terminated. */
1874   ret = DevGpsSaveCmd(&st_save_cmd);
1875   if (ret != RET_NORMAL) {
1876       POSITIONING_LOG("# GpsCommCtl # GPS Command Reserve bufferFull !");
1877   }
1878
1879   return ret;
1880 }
1881
1882 RET_API DevGpsWknRolloverGetReq(void) {
1883   RET_API                 ret;
1884   TG_GPS_SAVECMD          st_save_cmd;
1885   TG_GPS_SND_DATA*        pst_snd_data;
1886   uint8_t*                p_ublox_data;
1887
1888                                       /*  SYNC_CHAR_1_2   CLASS   ID      length(L/U)     CK_A    CK_B */
1889   static const u_int8     csnd_cmd[] = {   0xB5,   0x62,   0x06,   0x23,   0x00,   0x00,   0x00,   0x00   };
1890
1891   pst_snd_data = &(st_save_cmd.sndcmd);
1892   p_ublox_data = pst_snd_data->ub_data;
1893
1894   (void)memset(&st_save_cmd, 0x00, sizeof(st_save_cmd)); /* QAC 3200 */
1895
1896   /* Send command generation */
1897   (void)memcpy(p_ublox_data, csnd_cmd, UBX_CMD_SIZE_CFG_NAVX5_POLL); /* QAC 3200 */
1898
1899   /* Checksum assignment */
1900   DevGpsSetChkSum(p_ublox_data, UBX_CMD_SIZE_CFG_NAVX5_POLL);
1901
1902   pst_snd_data->us_size = UBX_CMD_SIZE_CFG_NAVX5_POLL;
1903   st_save_cmd.us_pno = PNO_VEHICLE_SENSOR;           /* Result notification destination process number  */
1904   st_save_cmd.uc_rid = 0;                            /* Result notification resource ID      */
1905   st_save_cmd.uc_rst = GPS_CMD_WKNROLLOVER;          /* Response flag            */
1906   st_save_cmd.e_cmd_info = GPS_FORMAT_CFG_NAVX5;      /* Command information            */
1907
1908   /* Command is suspended and terminated. */
1909   ret = DevGpsSaveCmd(&st_save_cmd);
1910   if (ret != RET_NORMAL) {
1911       POSITIONING_LOG("# GpsCommCtl # GPS Command Reserve bufferFull !");
1912     /* If an error occurs at this point that is not expected due to a command queue full error, retransmission control is required. */
1913   }
1914   return ret;
1915 }
1916
1917 /**
1918  * @brief
1919  *  GPS time setting validity check process
1920  *
1921  * @param[in]  set_gpsw     Set GPS week
1922  * @param[in]  set_gpsws    Set GPS Week
1923  * @param[in]  rcv_gpsw     Answer GPS Week
1924  * @param[in]  rcv_gpsws    Answer GPS Weeksec
1925  * @param[in]  offset_range  Tolerance time difference[sec]
1926  * @return     BOOL         TRUE:Effective time / FALSE:Invalid time
1927  */
1928 BOOL DevGpsCheckGpsTime(uint16_t set_gpsw, uint32_t set_gpsws,
1929                         uint16_t rcv_gpsw, uint32_t rcv_gpsws, int32_t offset_range) {
1930   BOOL     ret = FALSE;
1931   int16_t  gap_gpsw = 0;
1932   int32_t  gap_gpsws = 0;
1933
1934   /* Check if GPS week number and GPS week second are within valid range */
1935   /*  NG when Set Time > Receive Time          */
1936   gap_gpsw = (int16_t)(rcv_gpsw - set_gpsw); /* QAC 3758 */
1937   if (gap_gpsw == 0) {
1938       /* There is no difference between the set GPS week and the received GPS week */
1939       gap_gpsws = (int32_t)(rcv_gpsws - set_gpsws); /* QAC 3771 */
1940       if ((0 <= gap_gpsws) && (gap_gpsws <= offset_range)) {
1941         /* Setting of GPS time succeeded if the difference between GPS week seconds is within the range. */
1942         ret = TRUE;
1943       }
1944   } else if (gap_gpsw == 1) {
1945       /* Difference between the set GPS week and the received GPS week is 1 (over 7 weeks) */
1946       gap_gpsws = (int32_t)((rcv_gpsws + GPS_WS_MAX) - set_gpsws); /* QAC 3771 */
1947       if ((0 <= gap_gpsws) && (gap_gpsws <= offset_range)) {
1948             /* Setting of GPS time succeeded if the difference between GPS week seconds is within the range. */
1949             ret = TRUE;
1950       }
1951   } else {
1952       /* Set GPS time failed except above */
1953   }
1954
1955   return ret;
1956 }
1957
1958
1959 /****************************************************************************
1960 @brief      DevGpsCnvLonLatNavi<BR>
1961             Longitude/latitude information conversion process(Navigation-provided data)
1962 @outline    Converts the latitude/longitude information provided by the navigation system into the format provided to the vehicle sensor.
1963 @param[in]  u_int8                         : fix_sts  ... Current positioning status
1964 @param[in]  int32                          : lon      ... Longitude information
1965 @param[in]  int32                          : lat      ... Latitude-information
1966 @param[out] SENSORLOCATION_LONLATINFO_DAT* : p_lonlat ... Latitude and longitude information after conversion
1967 @return     none
1968 @retval     none
1969 *******************************************************************************/
1970 void DevGpsCnvLonLatNavi(SENSORLOCATION_LONLATINFO_DAT* p_lonlat, u_int8 fix_sts, int32 lon, int32 lat) {
1971   int32 longtitude;
1972   int32 latitude;
1973   int64_t tmp;
1974   BOOL  b_ret_lon = TRUE;
1975   BOOL  b_ret_lat = TRUE;
1976   BOOL  b_fix_sts = TRUE;
1977
1978   /** Provides latitude/longitude info for positioning status (2D/3D) */
1979   if ((NAVIINFO_DIAG_GPS_FIX_STS_2D == fix_sts ) ||
1980           (NAVIINFO_DIAG_GPS_FIX_STS_3D == fix_sts)) {
1981       /** Get longitude information */
1982       tmp = (int64_t)lon * 10000000;
1983       longtitude = static_cast<int32>(tmp / (int64_t)(256 * 60 * 60));
1984       if (longtitude == -1800000000) {
1985           /** West longitude 180 degrees -> East longitude 180 degrees */
1986           longtitude = +1800000000;
1987       }
1988       if ((longtitude > +1800000000) ||
1989               (longtitude < -1800000000)) {
1990           /** Longitude range error */
1991           b_ret_lon = FALSE;
1992       }
1993       /** Get latitude information */
1994       tmp = (int64_t)lat * 10000000;
1995
1996       latitude = static_cast<int32>(tmp / (int64_t)(256 * 60 * 60));
1997
1998       if ((latitude > +900000000) ||
1999               (latitude < -900000000)) {
2000           /** Latitude range error */
2001           b_ret_lat = FALSE;
2002       }
2003   } else {
2004       /** Does not provide latitude/longitude information in non-positioning state. */
2005       b_fix_sts = FALSE;
2006   }
2007
2008   /** Provided data setting */
2009   p_lonlat->getMethod = GET_METHOD_GPS;
2010   p_lonlat->SyncCnt = 0x00;                /* Synchronization counter value(fixed to 0) */  /* Ignore -> No applicable rules for MISRA-C */
2011   /** Available/Unusable Decision */
2012   if ((b_fix_sts == TRUE) && (b_ret_lon == TRUE) && (b_ret_lat == TRUE)) {
2013     p_lonlat->isEnable  = SENSORLOCATION_STATUS_ENABLE;  /* Available status: Available */
2014     p_lonlat->Longitude = longtitude;            /* Longitude */  /* Ignore -> MISRA-C:2004 Rule 9.1 */
2015     p_lonlat->Latitude  = latitude;           /* Latitude */  /* Ignore -> MISRA-C:2004 Rule 9.1 */
2016   } else {
2017     p_lonlat->isEnable  = SENSORLOCATION_STATUS_DISABLE; /* Available status: Not available */
2018     p_lonlat->Longitude = 0;               /* Longitude 0 degree fixed */
2019     p_lonlat->Latitude  = 0;               /* Latitude fixed to 0 degrees */
2020   }
2021   return;
2022 }
2023
2024
2025 /****************************************************************************
2026 @brief      DevGpsCnvAltitudeNavi<BR>
2027             Altitude information conversion process(Navigation-provided data)
2028 @outline    Converts the altitude information provided by the navigation system into the format provided to the vehicle sensor.
2029 @param[in]  u_int8                           : fix_sts    ... Current positioning status
2030 @param[in]  int32                            : alt        ... Altitude information
2031 @param[out] SENSORLOCATION_ALTITUDEINFO_DAT* : p_altitude ... Converted altitude information
2032 @return     none
2033 @retval     none
2034 *******************************************************************************/
2035 void DevGpsCnvAltitudeNavi(SENSORLOCATION_ALTITUDEINFO_DAT* p_altitude, u_int8 fix_sts, int32 alt) {
2036   int32 altitude;
2037   BOOL  b_fix_sts = TRUE;
2038
2039   /** Provides altitude information when in Fix Status (2D/3D) */
2040   if (((NAVIINFO_DIAG_GPS_FIX_STS_2D == fix_sts) ||
2041           (NAVIINFO_DIAG_GPS_FIX_STS_3D == fix_sts)) &&
2042           (alt != GPS_ALTITUDE_INVALID_VAL)) {
2043       /* The unit of alt is [0.01m], so conversion is not necessary. */
2044       altitude = alt;
2045   } else {
2046       /** Does not provide altitude information in the non-positioning state. */
2047       b_fix_sts = FALSE;
2048   }
2049   /** Provided data setting */
2050   p_altitude->getMethod = GET_METHOD_GPS;
2051   p_altitude->SyncCnt = 0x00;                /* Synchronization counter value(fixed to 0) */  /* Ignore -> No applicable rules for MISRA-C */
2052   /** Available/Unusable Decision */
2053   if (b_fix_sts == TRUE) {
2054     p_altitude->isEnable = SENSORLOCATION_STATUS_ENABLE; /* Available status: Available */
2055     p_altitude->Altitude = altitude;            /* Altitude */  /* Ignore -> MISRA-C:2004 Rule 9.1 */
2056   } else {
2057     p_altitude->isEnable = SENSORLOCATION_STATUS_DISABLE;  /* Available status: Not available */
2058     p_altitude->Altitude = 0;                /* Altitude 0m fixed */
2059   }
2060   return;
2061 }
2062
2063 /****************************************************************************
2064 @brief      DevGpsCnvHeadingNavi<BR>
2065             Orientation information conversion processing(Navigation-provided data)
2066 @outline    Converts the orientation information provided by the navigation system into the format provided to the vehicle sensor.
2067 @param[in]  u_int8                        : fix_sts   ... Current positioning status
2068 @param[in]  u_int16                       : head     ... Bearing information
2069 @param[out] SENSORMOTION_HEADINGINFO_DAT* : p_heading ... Converted backward direction information
2070 @return     none
2071 @retval     none
2072 *******************************************************************************/
2073 void DevGpsCnvHeadingNavi(SENSORMOTION_HEADINGINFO_DAT* p_heading, u_int8 fix_sts, u_int16 head) {
2074   u_int16 us_heading;
2075   BOOL  b_fix_sts = TRUE;
2076
2077   /** Provides orientation when in Fix Status (2D/3D) */
2078   if (((NAVIINFO_DIAG_GPS_FIX_STS_2D == fix_sts ) ||
2079           (NAVIINFO_DIAG_GPS_FIX_STS_3D == fix_sts)) &&
2080           (head != GPS_HEADING_INVALID_VAL)) {
2081       /** Orientation information conversion */
2082       us_heading = head;
2083       /* Units of head are [0.01 degree] so no conversion is necessary. */
2084       us_heading -= (u_int16)((us_heading / 36000) * 36000);
2085   } else {
2086       /** Azimuth information is not provided in the non-positioning state. */
2087       b_fix_sts = FALSE;
2088   }
2089
2090   /** Provided data setting */
2091   p_heading->getMethod = GET_METHOD_GPS;
2092   p_heading->SyncCnt = 0x00;                 /* Synchronization counter value(fixed to 0) */  /* Ignore -> No applicable rules for MISRA-C */
2093   /** Available/Unusable Decision */
2094   if (b_fix_sts == TRUE) {
2095     p_heading->isEnable = SENSORLOCATION_STATUS_ENABLE;    /* Available status: Available */
2096     p_heading->Heading  = us_heading;           /* Orientation */  /* Ignore -> MISRA-C:2004 Rule 9.1 */
2097   } else {
2098     p_heading->isEnable = SENSORLOCATION_STATUS_DISABLE;   /* Available status: Not available */
2099     p_heading->Heading  = 0;               /* Azimuth 0 degree fixed */
2100   }
2101
2102   return;
2103 }
2104 /*---------------------------------------------------------------------------*/
2105 /*EOF*/