hal/positioning_hal/src/GpsCommon/MDev_Gps_Nmea.cpp

   1 /*
   2  * @copyright Copyright (c) 2017-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_Nmea.cpp
  18 */
  19
  20 /*---------------------------------------------------------------------------*/
  21 // Include files
  22
  23 #include "MDev_Gps_Nmea.h"
  24 #include "positioning_common.h"
  25 #include "MDev_Gps_API.h"
  26 #include "MDev_Gps_Main.h"
  27 #include "MDev_GpsRecv.h"
  28 #include "MDev_Gps_Common.h"
  29 #include "MDev_GpsRollOver.h"
  30
  31 /*---------------------------------------------------------------------------*/
  32 // Value define
  33 #define ROLOVR_GPSWEEKCOR_NG            0xFF          /* WEEK COMP. CORRECT CORRECTOR INVALUE */
  34 #define TMT_TGET_INI_INTERVAL           (100)         /* GPS time compensation interval immediately after startup (value with 10[msec] as 1) */
  35 #define TMT_DISCONTINUITY_TIME_TOUT_SEQ (0x5051)      /* Sequence number of GPS time acquisition timeout based on date/time discontinuity */
  36
  37 /*---------------------------------------------------------------------------*/
  38 // Global values
  39
  40 static TG_GPS_RCVDATA  g_st_gpscycle_data;
  41 u_int8 g_gps_week_cor_cnt = ROLOVR_GPSWEEKCOR_NG;
  42
  43 extern uint8_t g_gpstime_raw_tdsts;
  44
  45 const TG_GPS_CMD_ANA_TBL kGpsCmdAnaTblUblox[MDEV_GPSCMDANATBLNMEA_MAX] = {
  46   /* Sentences                 Maximum length             Receiving type     Presence of notification  Receive Format    */
  47   /* NMEA */
  48   {{GPS_CMD_NMEA_RMC},        GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_RMC          },  /* RMC information        */
  49   {{GPS_CMD_NMEA_VTG},        GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_VTG          },  /* VTG information        */
  50   {{GPS_CMD_NMEA_GGA},        GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_GGA          },  /* GGA information        */
  51   {{GPS_CMD_NMEA_GSA},        GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_GSA          },  /* GSA information        */
  52   {{GPS_CMD_NMEA_GSV_1},      GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_GSV1         },  /* GSV information1       */
  53   {{GPS_CMD_NMEA_GSV_2},      GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_GSV2         },  /* GSV information2       */
  54   {{GPS_CMD_NMEA_GSV_3},      GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_GSV3         },  /* GSV information3       */
  55   {{GPS_CMD_NMEA_GSV_4},      GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_GSV4         },  /* GSV information4       */
  56   {{GPS_CMD_NMEA_GSV_5},      GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_GSV5         },  /* GSV information5       */
  57   {{GPS_CMD_NMEA_GLL},        GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_GLL          },  /* GLL information        */
  58   {{GPS_CMD_NMEA_GST},        GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_GST          },  /* GST information        */
  59   /* UBX */
  60   {{0xB5, 0x62, 0x0A, 0x04},  GPS_NMEA_MAX_SZ,  RCV_RESP,   TRUE,   GPS_FORMAT_MON_VER      },  /* MON-VER        */
  61   {{0xB5, 0x62, 0x0B, 0x01},  GPS_NMEA_MAX_SZ,  RCV_RESP,   TRUE,   GPS_FORMAT_AID_INI      },  /* AID-INI        */
  62   {{0xB5, 0x62, 0x05      },  GPS_NMEA_MAX_SZ,  RCV_RESP,   TRUE,   GPS_FORMAT_ACK_ACKNACK  },  /* ACK-ACKNACK    */
  63   {{0xB5, 0x62, 0x01, 0x21},  GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_NAV_TIMEUTC  },  /* NAV-TIMEUTC    */
  64   {{0xB5, 0x62, 0x01, 0x22},  GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_NAV_CLOCK    },  /* NAV-CLOCK      */
  65   {{0xB5, 0x62, 0x02, 0x23},  GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_RXM_RTC5     },  /* RXM-RTC5       */
  66   {{0xB5, 0x62, 0x01, 0x30},  GPS_NMEA_MAX_SZ,  RCV_CYCLE,  TRUE,   GPS_FORMAT_NAV_SVINFO   },  /* NAV-SVINFO     */
  67   {{0xB5, 0x62, 0x06, 0x23},  GPS_NMEA_MAX_SZ,  RCV_RESP,   TRUE,   GPS_FORMAT_CFG_NAVX5    },  /* CFG-NAVX5      */
  68   {{ENDMARK},                 0,                0,          FALSE,  GPS_FORMAT_MIN          }   /* Table termination */
  69 };
  70
  71 /*---------------------------------------------------------------------------*/
  72 // Functions
  73 /**
  74  * @brief
  75  *  Time Calculation Process Considering Rollover
  76  *
  77  * @param[in/out]
  78  *
  79  * @return      none
  80  * @retval      none
  81  */
  82 static BOOL DevCalcRollOverTime(TG_TIM_ROLOVR_YMD* base_ymd, TG_TIM_ROLOVR_YMD* conv_ymd) {
  83     BOOL ret = TRUE;
  84     static u_int16 year_old = 0;
  85     static u_int16 confirm_cnt = 0;
  86     u_int8 gps_week_corr = g_gps_week_cor_cnt;
  87
  88     if (gps_week_corr == ROLOVR_GPSWEEKCOR_NG) {
  89         gps_week_corr = 0;
  90         ret = FALSE;
  91     } else {
  92         if (year_old > base_ymd->year) {
  93             confirm_cnt++;
  94             if (confirm_cnt >= 10) {
  95                 confirm_cnt = 0;
  96                 year_old = base_ymd->year;
  97                 g_gps_week_cor_cnt = ROLOVR_GPSWEEKCOR_NG;
  98                 /* Starting the GPS time acquisition timer (1 second timer) based on date/time discontinuity */
  99                 _pb_ReqTimerStart(PNO_CLK_GPS, TMT_DISCONTINUITY_TIME_TOUT_SEQ, TIMER_TYPE_USN, TMT_TGET_INI_INTERVAL);
 100             }
 101             ret = FALSE;
 102         } else {
 103             confirm_cnt = 0;
 104             year_old = base_ymd->year;
 105         }
 106     }
 107     /* Calculate time for rollover */
 108     GPSRollOverConvTime(base_ymd, conv_ymd, gps_week_corr);
 109
 110     return ret;
 111 }
 112 /******************************************************************************************************/
 113
 114
 115 /**
 116  * @brief
 117  *  NMEA data notification
 118  */
 119 void DevGpsSndCycleDataNmea(void) {
 120   /* Notifying vehicle sensor of NMEA */
 121
 122   MDEV_GPS_NMEA    st_nmea;
 123   TG_GPS_NMEA_INFO st_gps_nmea_info;
 124   RET_API          l_ret = RET_NORMAL;
 125   BOOL             b_get = FALSE;
 126   u_int8           uc_nmea_data[GPS_NMEA_MAX_SZ];
 127   u_int32          ul_strlen = 0;
 128   u_int16          us_offset = sizeof(TG_GPS_NMEA_INFO);
 129
 130   memset(&st_nmea, 0x00, sizeof(st_nmea) ); /* QAC 3200 */
 131   memset(&st_gps_nmea_info, 0x00, sizeof(st_gps_nmea_info) ); /* QAC 3200 */
 132   memset(uc_nmea_data, 0x00, sizeof(uc_nmea_data) ); /* QAC 3200 */
 133
 134   /* Get received NMEA data from storage area(GGA) */
 135   b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_GGA);
 136   if (b_get == TRUE) {
 137     /* Data present */
 138     st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GGA;       /* Receive flag */
 139     ul_strlen = strlen(reinterpret_cast<const char *>(uc_nmea_data)); /* QAC 310 */
 140
 141     if (ul_strlen > GPS_NMEA_MAX_SZ) {
 142       ul_strlen = GPS_NMEA_MAX_SZ;
 143     }
 144
 145     if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) {
 146       (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen);  /* Data storage */
 147       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GGA].uc_size = (u_int8)ul_strlen;
 148       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GGA].us_offset = us_offset;
 149       us_offset += (u_int16)ul_strlen;
 150     }
 151   }
 152
 153   /* Get received NMEA data from storage area(GLL) */
 154   b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_GLL);
 155   if (b_get == TRUE) {
 156     /* Data present */
 157     st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GLL;       /* Receive flag */
 158     ul_strlen = strlen(reinterpret_cast<const char *>(uc_nmea_data)); /* QAC 310 */
 159
 160     if (ul_strlen > GPS_NMEA_MAX_SZ) {
 161         ul_strlen = GPS_NMEA_MAX_SZ;
 162     }
 163
 164     if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) {
 165       (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen);  /* Data storage */
 166       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GLL].uc_size = (u_int8)ul_strlen;
 167       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GLL].us_offset = us_offset;
 168       us_offset += (u_int16)ul_strlen;
 169     }
 170   }
 171
 172   /* Get received NMEA data from storage area(GSA) */
 173   b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_GSA);
 174   if (b_get == TRUE) {
 175     /* Data present */
 176     st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GSA1;       /* Receive flag */
 177     ul_strlen = strlen(reinterpret_cast<const char *>(uc_nmea_data)); /* QAC 310 */
 178     if (ul_strlen > GPS_NMEA_MAX_SZ) {
 179       ul_strlen = GPS_NMEA_MAX_SZ;
 180     }
 181
 182     if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) {
 183       (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen);  /* Data storage */
 184       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSA1].uc_size = (u_int8)ul_strlen;
 185       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSA1].us_offset = us_offset;
 186       us_offset += (u_int16)ul_strlen;
 187     }
 188   }
 189
 190   /* Get received NMEA data from storage area(GST) */
 191   b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_GST);
 192   if (b_get == TRUE) {
 193     /* Data present */
 194     st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GST;       /* Receive flag */
 195     ul_strlen = strlen(reinterpret_cast<const char *>(uc_nmea_data)); /* QAC 310 */
 196     if (ul_strlen > GPS_NMEA_MAX_SZ) {
 197       ul_strlen = GPS_NMEA_MAX_SZ;
 198     }
 199
 200     if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) {
 201       (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen);  /* Data storage */
 202       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GST].uc_size = (u_int8)ul_strlen;
 203       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GST].us_offset = us_offset;
 204       us_offset += (u_int16)ul_strlen;
 205     }
 206   }
 207
 208   /* Get received NMEA data from storage area(RMC) */
 209   b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_RMC);
 210   if (b_get == TRUE) {
 211     /* Data present */
 212     st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_RMC;       /* Receive flag */
 213     ul_strlen = strlen(reinterpret_cast<const char *>(uc_nmea_data)); /* QAC 310 */
 214     if (ul_strlen > GPS_NMEA_MAX_SZ) {
 215       ul_strlen = GPS_NMEA_MAX_SZ;
 216     }
 217
 218     if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) {
 219       (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen);  /* Data storage */
 220       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_RMC].uc_size = (u_int8)ul_strlen;
 221       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_RMC].us_offset = us_offset;
 222       us_offset += (u_int16)ul_strlen;
 223     }
 224   }
 225
 226   /* Get received NMEA data from storage area(VTG) */
 227   b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_VTG);
 228   if (b_get == TRUE) {
 229     /* Data present */
 230     st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_VTG;       /* Receive flag */
 231     ul_strlen = strlen(reinterpret_cast<const char *>(uc_nmea_data)); /* QAC 310 */
 232     if (ul_strlen > GPS_NMEA_MAX_SZ) {
 233       ul_strlen = GPS_NMEA_MAX_SZ;
 234     }
 235
 236     if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) {
 237       (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen);  /* Data storage */
 238       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_VTG].uc_size = (u_int8)ul_strlen;
 239       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_VTG].us_offset = us_offset;
 240       us_offset += (u_int16)ul_strlen;
 241     }
 242   }
 243
 244   /* Get received NMEA data from storage area(GSV1) */
 245   b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_GSV1);
 246   if (b_get == TRUE) {
 247     /* Data present */
 248     st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GSV1;       /* Receive flag */
 249     ul_strlen = strlen(reinterpret_cast<const char *>(uc_nmea_data)); /* QAC 310 */
 250     if (ul_strlen > GPS_NMEA_MAX_SZ) {
 251       ul_strlen = GPS_NMEA_MAX_SZ;
 252     }
 253
 254     if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) {
 255       (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen);  /* Data storage */
 256       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV1].uc_size = (u_int8)ul_strlen;
 257       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV1].us_offset = us_offset;
 258       us_offset += (u_int16)ul_strlen;
 259     }
 260   }
 261
 262   /* Get received NMEA data from storage area(GSV2) */
 263   b_get = DevGpsCycleDataGetNmea( uc_nmea_data, sizeof(uc_nmea_data), GPS_NMEA_INDEX_GSV2 );
 264   if (b_get == TRUE) {
 265     /* Data present */
 266     st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GSV2;       /* Receive flag */
 267     ul_strlen = strlen(reinterpret_cast<const char *>(uc_nmea_data)); /* QAC 310 */
 268     if (ul_strlen > GPS_NMEA_MAX_SZ) {
 269       ul_strlen = GPS_NMEA_MAX_SZ;
 270     }
 271
 272     if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) {
 273       (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen);  /* Data storage */
 274       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV2].uc_size = (u_int8)ul_strlen;
 275       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV2].us_offset = us_offset;
 276       us_offset += (u_int16)ul_strlen;
 277     }
 278   }
 279
 280   /* Get received NMEA data from storage area(GSV3) */
 281   b_get = DevGpsCycleDataGetNmea( uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_GSV3 );
 282   if (b_get == TRUE) {
 283     /* Data present */
 284     st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GSV3;       /* Receive flag */
 285     ul_strlen = strlen(reinterpret_cast<const char *>(uc_nmea_data)); /* QAC 310 */
 286     if (ul_strlen > GPS_NMEA_MAX_SZ) {
 287       ul_strlen = GPS_NMEA_MAX_SZ;
 288     }
 289
 290     if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) {
 291       (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen);
 292       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV3].uc_size = static_cast<u_int8>(ul_strlen);
 293       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV3].us_offset = us_offset;
 294       us_offset += (u_int16)ul_strlen;
 295     }
 296   }
 297
 298   /* Get received NMEA data from storage area(GSV4) */
 299   b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data), GPS_NMEA_INDEX_GSV4);
 300   if (b_get == TRUE) {
 301     /* Data present */
 302     st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GSV4;       /* Receive flag */
 303     ul_strlen = strlen(reinterpret_cast<const char *>(uc_nmea_data)); /* QAC 310 */
 304     if (ul_strlen > GPS_NMEA_MAX_SZ) {
 305       ul_strlen = GPS_NMEA_MAX_SZ;
 306     }
 307
 308     if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) {
 309       (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen);
 310       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV4].uc_size = static_cast<u_int8>(ul_strlen);
 311       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV4].us_offset = us_offset;
 312       us_offset += (u_int16)ul_strlen;
 313     }
 314   }
 315
 316   /* Get received NMEA data from storage area(GSV5) */
 317   b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data), GPS_NMEA_INDEX_GSV5);
 318   if (b_get == TRUE) {
 319     /* Data present */
 320     st_gps_nmea_info.ul_rcvsts |= POS_SNS_GPS_NMEA_GSV5;       /* Receive flag */
 321     ul_strlen = strlen(reinterpret_cast<const char *>(uc_nmea_data)); /* QAC 310 */
 322     if (ul_strlen > GPS_NMEA_MAX_SZ) {
 323       ul_strlen = GPS_NMEA_MAX_SZ;
 324     }
 325
 326     if ((us_offset + ul_strlen) < SENSOR_MSG_VSINFO_DSIZE) {
 327       (void)memcpy(&(st_nmea.uc_nmea_data[us_offset]), uc_nmea_data, ul_strlen);
 328       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV5].uc_size = static_cast<u_int8>(ul_strlen);
 329       st_gps_nmea_info.st_nmea_sentence_info[POS_SNS_GPS_NMEA_SNO_GSV5].us_offset = us_offset;
 330       us_offset += (u_int16)ul_strlen;
 331     }
 332   }
 333
 334   if (0 != st_gps_nmea_info.ul_rcvsts) {
 335     /* Receive flag */
 336     (void)memcpy(&(st_nmea.uc_nmea_data[0]), &st_gps_nmea_info, sizeof(st_gps_nmea_info));
 337
 338     /* Provided to vehicle sensor */
 339     l_ret = SendNmeaGps(&st_nmea);
 340
 341     if (RET_NORMAL != l_ret) {
 342       POSITIONING_LOG("SendNmeaGps SndMsg Error\n");
 343     }
 344   } else {
 345     /* Do not provide to vehicle sensor when data acquisition fails or no data */
 346   }
 347
 348   return;
 349 }
 350
 351 /**
 352  * @brief
 353  *  Analysis of the received command
 354  */
 355 void DevGpsRcvCyclCmd(void) {
 356   int32                 i_ret = 0;
 357   TG_GPS_OUTPUT_FORMAT  e_format = GPS_FORMAT_MIN;
 358
 359   TG_GPS_RCV_DATA      st_rcv_data; /* Pointer to the received message */
 360   (void)memcpy(&st_rcv_data, &(g_gps_msg_rcvr.msgdat[0]), sizeof(st_rcv_data) ); /* QAC 3200 */
 361
 362   /* Analysis of received commands */
 363   i_ret = JudgeFormatGpsCommon(reinterpret_cast<u_char*>(&(st_rcv_data.bygps_data[0])),
 364                                static_cast<u_int32>(st_rcv_data.bydata_len),
 365                                &e_format);
 366
 367   if (i_ret == GPSRET_SNDCMD) {
 368     /* For NMEA formats */
 369     if ((e_format == GPS_FORMAT_RMC)  ||
 370         (e_format == GPS_FORMAT_VTG)  ||
 371         (e_format == GPS_FORMAT_GGA)  ||
 372         (e_format == GPS_FORMAT_GSA)  ||
 373         (e_format == GPS_FORMAT_GSV1) ||
 374         (e_format == GPS_FORMAT_GSV2) ||
 375         (e_format == GPS_FORMAT_GSV3) ||
 376         (e_format == GPS_FORMAT_GSV4) ||
 377         (e_format == GPS_FORMAT_GSV5) ||
 378         (e_format == GPS_FORMAT_GLL)  ||
 379         (e_format == GPS_FORMAT_GST)) {
 380       /* NMEA reception process */
 381       RcvCyclCmdNmeaGpsCommon(&(st_rcv_data.bygps_data[0]), (u_int32)st_rcv_data.bydata_len, e_format);
 382     } else if ((e_format == GPS_FORMAT_MON_VER)     ||
 383                (e_format == GPS_FORMAT_AID_INI)     ||
 384                (e_format == GPS_FORMAT_ACK_ACKNACK) ||
 385                (e_format == GPS_FORMAT_NAV_TIMEUTC) ||
 386                (e_format == GPS_FORMAT_NAV_CLOCK)   ||
 387                (e_format == GPS_FORMAT_RXM_RTC5)    ||
 388                (e_format == GPS_FORMAT_NAV_SVINFO)) {
 389         /* UBX reception process */
 390       RcvCyclCmdExtGpsCommon(&(st_rcv_data.bygps_data[0]), (u_int32)st_rcv_data.bydata_len, e_format);
 391     } else {
 392       POSITIONING_LOG("Forbidden ERROR!![e_format=%d]", (int)e_format);
 393     }
 394   } else if (i_ret == GPSRET_CMDERR) {
 395     /* Receive command error */
 396
 397     /* Discard previously received data */
 398     DevGpsCycleDataClear();
 399     /* Initialize receive format */
 400     g_rcv_format = GPS_FORMAT_MIN;
 401   } else {
 402   }
 403
 404   return;
 405 }
 406
 407 /**
 408  * @brief
 409  *  Check of the received command
 410  */
 411 void DevGpsCmdEventCheckNmea(void) {
 412   u_int32         ul_cnt = 0;
 413   TG_GPS_RCV_DATA st_rcv_data;
 414   u_char*         pub_rcv_data = NULL;
 415   BOOL            brk_flg = FALSE;
 416
 417   memset(&st_rcv_data, 0, sizeof(TG_GPS_RCV_DATA));
 418   memcpy( &st_rcv_data, &(g_gps_msg_rcvr.msgdat[0]), sizeof(TG_GPS_RCV_DATA) );
 419   pub_rcv_data = reinterpret_cast<u_char*>(&(st_rcv_data.bygps_data[0]));
 420
 421   /* Analysis of received commands */
 422   for (ul_cnt = 0; ul_cnt < (u_int32)GPSCMDANATBL_MAX; ul_cnt++) {
 423     /* End-of-table decision */
 424     if (CheckFrontStringPartGpsCommon(reinterpret_cast<const u_char*>(ENDMARK),
 425         reinterpret_cast<const u_char*>(kGpsCmdAnaTbl[ul_cnt].c_sentence)) == RET_NORMAL ) {
 426       g_wrecv_err++;
 427
 428       /* Data error is set to Event ID. */
 429       g_gps_mngr.event = (u_int32)NG;
 430
 431       brk_flg = TRUE;
 432     } else if (CheckFrontStringPartGpsCommon(pub_rcv_data, kGpsCmdAnaTbl[ul_cnt].c_sentence) == RET_NORMAL) {
 433       /* Reception type determination */
 434
 435       /* Using $GPRMC in responses to resets */
 436       if ((g_gps_mngr.sts == GPS_STS_SENT) &&
 437           (g_gps_mngr.resp_cmd == GPS_FORMAT_RMC) &&
 438           (kGpsCmdAnaTbl[ul_cnt].e_rcv_format == GPS_FORMAT_RMC)) {
 439         POSITIONING_LOG("Received response ($GPRMC) form GPS device.\n");
 440
 441         /** Response command */
 442         g_gps_mngr.event = GPS_EVT_RECVRSPDAT;
 443
 444         /** Receive format setting */
 445         g_gps_mngr.rcv_cmd = kGpsCmdAnaTbl[ul_cnt].e_rcv_format;
 446       } else if (kGpsCmdAnaTbl[ul_cnt].ul_rcv_kind == RCV_CYCLE) {
 447         /* Cyclic receive command */
 448         g_gps_mngr.event = GPS_EVT_RECVCYCLDAT;
 449
 450         /* Receive format setting */
 451         g_gps_mngr.rcv_cmd = kGpsCmdAnaTbl[ul_cnt].e_rcv_format;
 452       } else if (kGpsCmdAnaTbl[ul_cnt].ul_rcv_kind == RCV_RESP) {
 453         /** Response command */
 454         g_gps_mngr.event = GPS_EVT_RECVRSPDAT;
 455
 456         /** Receive format setting */
 457         g_gps_mngr.rcv_cmd = kGpsCmdAnaTbl[ul_cnt].e_rcv_format;
 458       } else {
 459         /* Undefined value */
 460         /* Data error is set to Event ID. */
 461         g_gps_mngr.event = (u_int32)NG;
 462       }
 463
 464       brk_flg = TRUE;
 465     }
 466
 467     if (brk_flg == TRUE) {
 468       break;
 469     }
 470   }
 471
 472   return;
 473 }
 474
 475 /**
 476  * @brief
 477  *   Get GPS reception status
 478  *
 479  *   By analyzing the last received GSA-sentence and using the satellite-number as the Satellite number
 480  *   Determines the reception status based on whether or not notification has been made, and returns it.
 481  *
 482  * @param[in]  no_sv Satellite number
 483  *
 484  * @return  NAVIINFO_DIAG_GPS_RCV_STS_NOTUSEFIX  - Positioning not used
 485  *      NAVIIFNO_DIAG_GPS_RCV_STS_USEFIX  - Positioning use
 486  */
 487 u_int8 DevGpsGetGpsRcvSts(u_int8 sv) {
 488   u_int8  rcv_sts = NAVIINFO_DIAG_GPS_RCV_STS_TRACHING; /* Tracking in progress */
 489   u_int8  uc_nmea_data[GPS_NMEA_MAX_SZ];
 490   u_int8  uc_no = 0;
 491   BOOL    b_get = FALSE;
 492   int32   i = 0;
 493
 494   /* Get received NMEA data from storage area(GSA) */
 495   b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data), GPS_NMEA_INDEX_GSA);
 496
 497   if (b_get == TRUE) {
 498     for (i = 0; i < GPS_NMEA_NUM_GSA_SV; i++) {
 499       /* Get satellite number */
 500       uc_no = (uint8_t)GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSA_SV + (u_int8)(1 * i), uc_nmea_data);
 501
 502       if (uc_no == sv) {
 503         rcv_sts = NAVIINFO_DIAG_GPS_RCV_STS_USEFIX; /* Positioning use */
 504         break;
 505       }
 506     }
 507   }
 508
 509   return rcv_sts;
 510 }
 511
 512 /**
 513  * @brief
 514  *   GPS information analysis
 515  *
 516  *   Analyzing received NMEA sentences
 517  * @param[out]  navilocinfo Navigation information
 518  */
 519 void DevGpsAnalyzeNmea(NAVIINFO_ALL* navilocinfo) {
 520   u_int8  uc_nmea_data[GPS_NMEA_MAX_SZ];
 521   int32   no_sv = 0; /* number of Satellites in View */
 522   int32   __offset = 0;
 523   char    utc_time[12]; /* hhmmss.sss */
 524   char    _date[6]; /* ddmmyy */
 525   char    _status = 0; /* 'V' or 'A' */
 526   char    indicator; /* 'N' or 'S' or 'E' or 'W' */
 527   char    work[8]; /* Work buffer for converting String data */
 528   BOOL    b_get = FALSE;
 529   uint8_t fixsts = NAVIINFO_DIAG_GPS_FIX_STS_NON;
 530   uint8_t fixsts_gga;
 531   BOOL    bvalid_lon = FALSE;
 532   BOOL    bvalid_lat = FALSE;
 533   BOOL    bvalid = FALSE;
 534   TG_TIM_ROLOVR_YMD   base_ymd;
 535   TG_TIM_ROLOVR_YMD   conv_ymd;
 536   BOOL roll_over_sts;
 537   u_int8  _tdsts = g_gpstime_raw_tdsts;
 538
 539   GpsSatelliteInfo st_visible_satellite_list[GPS_MAX_NUM_VISIBLE_SATELLITES]; /* Visible satellite list */
 540   GpsSatelliteInfo st_tmp_buf;
 541
 542   int32 i = 0;
 543   int32 j = 0;
 544   int32 k = 0;
 545
 546   SENSORLOCATION_LONLATINFO_DAT     st_lonlat;
 547   SENSORLOCATION_ALTITUDEINFO_DAT   st_altitude;
 548   SENSORMOTION_HEADINGINFO_DAT      st_heading;
 549   // MDEV_GPS_RTC                      st_rtc;
 550   SENSOR_MSG_GPSTIME                st_gps_time;
 551   SENSORMOTION_SPEEDINFO_DAT        st_speed;
 552
 553   memset(&st_lonlat, 0x00, sizeof(SENSORLOCATION_LONLATINFO_DAT));
 554   memset(&st_altitude, 0x00, sizeof(SENSORLOCATION_ALTITUDEINFO_DAT));
 555   memset(&st_heading, 0x00, sizeof(SENSORMOTION_HEADINGINFO_DAT));
 556   // memset(&st_rtc, 0x00, sizeof(MDEV_GPS_RTC));
 557   memset(&st_gps_time, 0x00, sizeof(SENSOR_MSG_GPSTIME));
 558   memset(&st_speed, 0x00, sizeof(SENSORMOTION_SPEEDINFO_DAT));
 559
 560   /* Satellite signal strength list initialization */
 561   (void)memset(st_visible_satellite_list, 0x00, sizeof(GpsSatelliteInfo) * GPS_MAX_NUM_VISIBLE_SATELLITES);
 562
 563   /* Get received NMEA data from storage area(GSA) */
 564   b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data), GPS_NMEA_INDEX_GSA);
 565
 566   if (b_get == TRUE) {
 567     fixsts = static_cast<uint8_t>(GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSA_FS, uc_nmea_data));
 568   }
 569
 570   /* Get received NMEA data from storage area(RMC) */
 571   b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data) , GPS_NMEA_INDEX_RMC);
 572
 573   if (b_get == TRUE) {
 574     navilocinfo->stDiagGps.stFix.stWgs84.lLat =
 575           GetLonLatFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_LATITUDE, uc_nmea_data, &bvalid_lat);  /* GPS location information and latitude  */
 576
 577     GetStringFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_NS, uc_nmea_data, &indicator, sizeof(indicator));
 578
 579     if (indicator != GPS_NMEA_RMC_IND_NORTH) {
 580       navilocinfo->stDiagGps.stFix.stWgs84.lLat *= -1;
 581     }
 582
 583     POSITIONING_LOG("lLat = %d", navilocinfo->stDiagGps.stFix.stWgs84.lLat);
 584
 585     navilocinfo->stDiagGps.stFix.stWgs84.lLon =
 586           GetLonLatFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_LONGITUDE, uc_nmea_data, &bvalid_lon);  /* GPS position information and longitude  */
 587
 588     GetStringFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_EW, uc_nmea_data, &indicator, sizeof(indicator));
 589
 590     if (indicator != GPS_NMEA_RMC_IND_EAST) {
 591       navilocinfo->stDiagGps.stFix.stWgs84.lLon *= -1;
 592     }
 593
 594     st_lonlat.Longitude = navilocinfo->stDiagGps.stFix.stWgs84.lLon;
 595     st_lonlat.Latitude  = navilocinfo->stDiagGps.stFix.stWgs84.lLat;
 596
 597     POSITIONING_LOG("lLon = %d", navilocinfo->stDiagGps.stFix.stWgs84.lLon);
 598
 599     /* Get Date information */
 600     (void)GetStringFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_DATE, uc_nmea_data, _date, 6);
 601     (void)memset(&base_ymd,  0, sizeof(base_ymd)); /* QAC 3200 */
 602     (void)memset(&conv_ymd,  0, sizeof(conv_ymd)); /* QAC 3200 */
 603     (void)memset(work,  0, sizeof(work)); /* QAC 3200 */
 604
 605     (void)strncpy(work, &_date[4], 2); /* QAC 3200 */
 606     base_ymd.year = (u_int16)(2000 + atoi(work));  /* YEAR          */
 607
 608     st_gps_time.utc.year = (uint8_t)atoi(work);
 609
 610     (void)strncpy(work, &_date[2], 2); /* QAC 3200 */
 611     base_ymd.month = (u_int16)(atoi(work));      /* MONTH          */
 612
 613     st_gps_time.utc.month = (uint8_t)atoi(work);
 614
 615     (void)strncpy(work, &_date[0], 2); /* QAC 3200 */
 616     base_ymd.day = (u_int16)(atoi(work));      /* DAY          */
 617
 618     st_gps_time.utc.date = (uint8_t)atoi(work);
 619
 620     POSITIONING_LOG("year = %d", base_ymd.year);
 621     POSITIONING_LOG("month = %d", base_ymd.month);
 622     POSITIONING_LOG("date = %d", base_ymd.day);
 623
 624     /* UTC time information acquisition */
 625     (void)GetStringFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_UTC, uc_nmea_data, utc_time, 12);
 626
 627     (void)strncpy(work, &utc_time[0], 2); /* QAC 3200 */
 628     navilocinfo->stNaviGps.utc.hour = (uint8_t)atoi(work);      /* HOUR          */
 629
 630     st_gps_time.utc.hour = navilocinfo->stNaviGps.utc.hour;
 631     POSITIONING_LOG("hour = %d", navilocinfo->stNaviGps.utc.hour);
 632
 633     (void)strncpy(work, &utc_time[2], 2); /* QAC 3200 */
 634     navilocinfo->stNaviGps.utc.minute = (uint8_t)atoi(work);      /* MINUTE          */
 635
 636     st_gps_time.utc.minute = navilocinfo->stNaviGps.utc.minute;
 637     POSITIONING_LOG("minute = %d", navilocinfo->stNaviGps.utc.minute);
 638
 639     (void)strncpy(work, &utc_time[4], 2); /* QAC 3200 */
 640     navilocinfo->stNaviGps.utc.second = (uint8_t)atoi(work);      /* SECOND          */
 641
 642     st_gps_time.utc.second = navilocinfo->stNaviGps.utc.second;
 643     POSITIONING_LOG("second = %d", navilocinfo->stNaviGps.utc.second);
 644
 645     /* Compass information acquisition */
 646     navilocinfo->stNaviGps.heading =
 647         GetHeadingFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_COG, uc_nmea_data, &bvalid);
 648
 649     st_heading.Heading = navilocinfo->stNaviGps.heading;
 650     POSITIONING_LOG("heading = %u", navilocinfo->stNaviGps.heading);
 651
 652     st_speed.Speed = GetSpeedFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_SPEED, uc_nmea_data, &bvalid);
 653
 654     /* Fix Status/Time Status Calculation */
 655     (void)GetStringFromNmeaGpsCommon(GPS_NMEA_FNO_RMC_STATUS, uc_nmea_data, &_status, sizeof(_status));
 656
 657     if ((_status == GPS_NMEA_RMC_STS_VALID) && (bvalid_lat == TRUE) && (bvalid_lon == TRUE)) {
 658       /* Fix status information  */
 659       switch (fixsts) {
 660         case GPS_NMEA_GSA_FIX_STS_NON:
 661           navilocinfo->stDiagGps.stFix.ucFixSts = NAVIINFO_DIAG_GPS_FIX_STS_NON;
 662           break;
 663         case GPS_NMEA_GSA_FIX_STS_2D:
 664           navilocinfo->stDiagGps.stFix.ucFixSts = NAVIINFO_DIAG_GPS_FIX_STS_2D;
 665           break;
 666         case GPS_NMEA_GSA_FIX_STS_3D:
 667           navilocinfo->stDiagGps.stFix.ucFixSts = NAVIINFO_DIAG_GPS_FIX_STS_3D;
 668           break;
 669         default:
 670           POSITIONING_LOG("GSA Nav Mode Error [fixsts:%d]", fixsts);
 671           break;
 672       }
 673
 674       if (_tdsts == POS_TIMESTS_OK) {
 675         roll_over_sts = DevCalcRollOverTime(&base_ymd, &conv_ymd);
 676         navilocinfo->stNaviGps.utc.year   = conv_ymd.year;        /* year (after conversion)          */
 677         navilocinfo->stNaviGps.utc.month  = (u_int8)(conv_ymd.month);  /* month (after conversion)          */
 678         navilocinfo->stNaviGps.utc.date   = (u_int8)(conv_ymd.day);  /* dat (after conversion)          */
 679         if (roll_over_sts == FALSE) {
 680           navilocinfo->stNaviGps.tdsts = POS_TIMESTS_NG;
 681           /* Reserve[0] is time setting information: anomaly time, but can be calculated by rolling over. */
 682           navilocinfo->stNaviGps.reserve[0] = GPS_TIME_ROLOVR;
 683         } else {
 684           /* When the location information is normal, the time information is also judged to be normal. */
 685           navilocinfo->stNaviGps.tdsts = POS_TIMESTS_OK; /* Time calibration completed */
 686           /* Reserve[0] is time setting information: Use time status received from GPS device. */
 687           navilocinfo->stNaviGps.reserve[0] = GPS_TIME_RX;
 688         }
 689       } else {
 690         navilocinfo->stNaviGps.tdsts      = POS_TIMESTS_NG;  /* Time uncalibrated */
 691         /* Reserve[0] is time setting information: Use time status received from GPS device. */
 692         navilocinfo->stNaviGps.reserve[0] = GPS_TIME_RX;
 693         navilocinfo->stNaviGps.utc.year   = base_ymd.year;        /* year(after conversion)          */
 694         navilocinfo->stNaviGps.utc.month  = (u_int8)(base_ymd.month);  /* month (after conversion)          */
 695         navilocinfo->stNaviGps.utc.date   = (u_int8)(base_ymd.day);  /* day (after conversion)          */
 696       }
 697
 698       if (bvalid != TRUE) {
 699         /* Invalid value if measurement orientation is invalid. */
 700         navilocinfo->stNaviGps.heading    = GPS_HEADING_INVALID_VAL;
 701         // POSITIONING_LOG("RMC Heading[cog] Invalid");
 702       }
 703     } else {
 704       /* Fix status information: Non-position fix is set regardless of FS of GSA. */
 705       navilocinfo->stDiagGps.stFix.ucFixSts = NAVIINFO_DIAG_GPS_FIX_STS_NON;
 706       /* If the location information is invalid, the time information is also judged to be invalid. */
 707       /* Time not calibrated after receiver reset (time entry or master reset or CSF activation) */
 708       navilocinfo->stNaviGps.tdsts = POS_TIMESTS_NG;
 709       /* Reserve[0] is time setting information: Use time status received from GPS device. */
 710       navilocinfo->stNaviGps.reserve[0] = GPS_TIME_RX;
 711       navilocinfo->stNaviGps.utc.year = base_ymd.year;        /* year (after conversion)          */
 712       navilocinfo->stNaviGps.utc.month = (u_int8)(base_ymd.month);  /* month (after conversion)          */
 713       navilocinfo->stNaviGps.utc.date = (u_int8)(base_ymd.day);  /* day (after conversion)          */
 714       // POSITIONING_LOG("RMC Invalid[status:%d, bvalidLat:%d, bvalidLon:%d]", _status, bvalid_lat, bvalid_lon);
 715     }
 716
 717     // POSITIONING_LOG("year(Fix) = %d", navilocinfo->stNaviGps.utc.year);
 718     // POSITIONING_LOG("month(Fix) = %d", navilocinfo->stNaviGps.utc.month);
 719     // POSITIONING_LOG("date(Fix) = %d", navilocinfo->stNaviGps.utc.date);
 720     // POSITIONING_LOG("tdsts = %d", navilocinfo->stNaviGps.tdsts);
 721   }
 722
 723   /* Get received NMEA data from storage area(GGA) */
 724   b_get = DevGpsCycleDataGetNmea(uc_nmea_data, sizeof(uc_nmea_data), GPS_NMEA_INDEX_GGA);
 725
 726   if (b_get == TRUE) {
 727     /* Data status acquisition */
 728     fixsts_gga = (uint8_t)GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GGA_FS, uc_nmea_data);
 729
 730     /* Altitude information acquisition */
 731     if (((fixsts == GPS_NMEA_GSA_FIX_STS_2D) ||
 732             (fixsts == GPS_NMEA_GSA_FIX_STS_3D)) &&
 733             ((fixsts_gga == GPS_NMEA_GGA_FIX_STATUS_GPS) ||
 734              (fixsts_gga == GPS_NMEA_GGA_FIX_STATUS_DGPS) ||
 735              (fixsts_gga == GPS_NMEA_GGA_FIX_STATUS_DR))) {
 736       navilocinfo->stNaviGps.altitude =
 737           GetAltitudeFromNmeaGpsCommon(GPS_NMEA_FNO_GGA_MSL, uc_nmea_data, &bvalid);
 738
 739       if (bvalid != TRUE) {
 740         /* If the location information is invalid, set an invalid value. */
 741         navilocinfo->stNaviGps.altitude = GPS_ALTITUDE_INVALID_VAL;
 742         // POSITIONING_LOG("GGA Altitude[msl] Invalid");
 743       }
 744     } else {
 745       /* If the location information is invalid, set an invalid value. */
 746       navilocinfo->stNaviGps.altitude = GPS_ALTITUDE_INVALID_VAL;
 747       // POSITIONING_LOG("GGA Invalid[fixsts:%d, fixstsGGA:%d]", fixsts, fixsts_gga);
 748     }
 749
 750     st_altitude.Altitude = navilocinfo->stNaviGps.altitude;
 751     // POSITIONING_LOG("altitude = %d", navilocinfo->stNaviGps.altitude);
 752   }
 753
 754   DevGpsCnvLonLatNavi(&st_lonlat, navilocinfo->stDiagGps.stFix.ucFixSts, navilocinfo->stDiagGps.stFix.stWgs84.lLon,
 755                                                                          navilocinfo->stDiagGps.stFix.stWgs84.lLat);
 756
 757   DevGpsCnvAltitudeNavi(&st_altitude, navilocinfo->stDiagGps.stFix.ucFixSts, navilocinfo->stNaviGps.altitude);
 758
 759   DevGpsCnvHeadingNavi(&st_heading, navilocinfo->stDiagGps.stFix.ucFixSts, navilocinfo->stNaviGps.heading);
 760
 761   SendCustomGps(&st_gps_time, &st_lonlat, &st_altitude, &st_heading, &navilocinfo->stDiagGps);
 762   // For test todo don't needed
 763   // SendTimeGps(&st_rtc);
 764   // SendSpeedGps(&st_speed, 0);
 765
 766   /* Create visual satellite information list from GSV1~GSV5 and GSV */
 767   for (i = 0; i < 5; i++) {
 768     /* Get received NMEA data from storage area */
 769     b_get = DevGpsCycleDataGetNmea( uc_nmea_data, sizeof(uc_nmea_data),
 770                                             (ENUM_GPS_NMEA_INDEX)(GPS_NMEA_INDEX_GSV1 + i));
 771
 772     if (b_get == TRUE) {
 773       /* Get number of Satellites in View */
 774       no_sv = GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSV_NOSV, uc_nmea_data);
 775
 776       for (j = 0; j < GPS_NMEA_NUM_GSV_SINFO; j++) {
 777         if (__offset >= no_sv) {
 778           break;
 779         }
 780
 781         st_visible_satellite_list[__offset].sv  = static_cast<uint8_t>(
 782           GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSV_SV + (u_int8)(4 * j), uc_nmea_data));   /* Satellite information_No      */
 783         st_visible_satellite_list[__offset].elv = static_cast<uint8_t>(
 784           GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSV_ELV + (u_int8)(4 * j), uc_nmea_data));  /* Satellite information_elevation    */
 785         st_visible_satellite_list[__offset].az  = static_cast<uint16_t>(
 786           GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSV_AZ + (u_int8)(4 * j), uc_nmea_data));   /* Satellite Information_Azimuth    */
 787         st_visible_satellite_list[__offset].cno = static_cast<uint8_t>(
 788           GetIntegerFromNmeaGpsCommon(GPS_NMEA_FNO_GSV_CNO + (u_int8)(4 * j), uc_nmea_data));   /* Satellite information_level    */
 789         st_visible_satellite_list[__offset].sts = DevGpsGetGpsRcvSts(st_visible_satellite_list[__offset].sv);
 790
 791         /* Sort in ascending order of status (priority to use fix) and received signal strength */
 792         for (k = __offset; k > 0; k--) {
 793           if (((st_visible_satellite_list[k].sts == NAVIINFO_DIAG_GPS_RCV_STS_USEFIX) &&
 794            (st_visible_satellite_list[k - 1].sts == NAVIINFO_DIAG_GPS_RCV_STS_TRACHING)) ||
 795           ((st_visible_satellite_list[k - 1].sts == st_visible_satellite_list[k].sts) &&
 796            (st_visible_satellite_list[k].cno > st_visible_satellite_list[k - 1].cno))) {
 797             (void)memcpy(&st_tmp_buf, &st_visible_satellite_list[k], sizeof(GpsSatelliteInfo));
 798             (void)memcpy(&st_visible_satellite_list[k], &st_visible_satellite_list[k - 1], sizeof(GpsSatelliteInfo));
 799             (void)memcpy(&st_visible_satellite_list[k - 1], &st_tmp_buf, sizeof(GpsSatelliteInfo));
 800           } else {
 801             break;
 802           }
 803         }
 804
 805         __offset++;
 806       }
 807     }
 808   }
 809
 810   return;
 811 }
 812
 813 /****************************************************************************
 814 @brief      DevGpsCycleDataClear<BR>
 815             Cyclic data storage area clear processing
 816 @outline    Clear the cyclic data storage area
 817 @param[in]  none
 818 @param[out] none
 819 @return     none
 820 @retval     none
 821 *******************************************************************************/
 822 void DevGpsCycleDataClear(void) {
 823   int32 i = 0;
 824
 825   /* Sensor counter, reception flag initialization */
 826   g_st_gpscycle_data.uc_sns_cnt = 0;
 827
 828   for (i = 0; i < GPS_NMEA_INDEX_MAX; i++) {
 829     g_st_gpscycle_data.st_nmea_data.uc_rcv_flag[i] = GPS_CYCLECMD_NOTRCV;
 830   }
 831
 832   g_st_gpscycle_data.st_binary_data.uc_rcv_flag  = GPS_CYCLECMD_NOTRCV;
 833   g_st_gpscycle_data.st_fullbin_data.uc_rcv_flag = GPS_CYCLECMD_NOTRCV;
 834 }
 835
 836 /******************************************************************************
 837 @brief      DEV_Gps_CycleData_SetNmea<BR>
 838             NMEA data setting process
 839 @outline    Set NMEA data in cyclic data storage area
 840 @param[in]  u_int8*            : p_data   ... NMEA data to be set
 841 @param[in]  u_int32            : ul_length  ... Data length
 842 @param[in]  ENUM_GPS_NMEA_INDEX: e_format   ... Sentence identification
 843 @param[out] none
 844 @return     none
 845 @retval     none
 846 *******************************************************************************/
 847 void DevGpsCycleDataSetNmea(const u_int8* p_data, u_int32 ul_length, ENUM_GPS_NMEA_INDEX e_format) {
 848   u_int32 ul_copy_sz = 0;
 849
 850   /** Anomaly detection */
 851   if (e_format >= GPS_NMEA_INDEX_MAX) {
 852     POSITIONING_LOG("# GpsCommCtl_API # Set NMEA Sentence ERROR ! \r\n");
 853   } else {
 854     /** Storage size determination */
 855     if (GPS_NMEA_MAX_SZ < ul_length) {
 856       ul_copy_sz = GPS_NMEA_MAX_SZ;
 857       POSITIONING_LOG("# GpsCommCtl_API # Set NMEA Cmd Size ERROR ! \r\n");
 858     } else {
 859       ul_copy_sz = ul_length;
 860     }
 861
 862     /** Storing */
 863     g_st_gpscycle_data.st_nmea_data.uc_rcv_flag[e_format] = GPS_CYCLECMD_RCV;
 864     memset(&(g_st_gpscycle_data.st_nmea_data.st_nmea[e_format].uc_data[0]), 0x00, GPS_NMEA_MAX_SZ);
 865     memcpy(&(g_st_gpscycle_data.st_nmea_data.st_nmea[e_format].uc_data[0]), p_data, ul_copy_sz);
 866   }
 867
 868   return;
 869 }
 870
 871 /******************************************************************************
 872 @brief      DevGpsCycleDataGetNmea<BR>
 873             NMEA data setting process
 874 @outline    Set NMEA data in cyclic data storage area
 875 @param[in]  u_int32            : ul_buf_size ... Storage destination buffer size
 876 @param[in]  ENUM_GPS_NMEA_INDEX: e_format   ... Sentence identification
 877 @param[out] u_int8*            : p_data   ... Storage destination buffer pointer
 878 @return     BOOL
 879 @retval     TRUE               : Data present
 880 @retval     FALSE              : No data
 881 *******************************************************************************/
 882 BOOL DevGpsCycleDataGetNmea(u_int8 *p_data, u_int32 ul_buf_size, ENUM_GPS_NMEA_INDEX e_format) {
 883   BOOL ret = TRUE;
 884
 885   /** Determining whether data exists in the cyclic data area */
 886   if (GPS_CYCLECMD_RCV == g_st_gpscycle_data.st_nmea_data.uc_rcv_flag[e_format]) {
 887     if (GPS_NMEA_MAX_SZ <= ul_buf_size) {
 888       /** Copy to storage destination buffer */
 889       memcpy(p_data, &(g_st_gpscycle_data.st_nmea_data.st_nmea[e_format].uc_data[0]), GPS_NMEA_MAX_SZ);
 890     } else {
 891       /** Storage destination buffer size is small */
 892       ret = FALSE;
 893     }
 894   } else {
 895     /** Not received */
 896     ret = FALSE;
 897   }
 898
 899   return ret;
 900 }
 901
 902 /**
 903  * @brief
 904  *  Setting of the check sum
 905  *
 906  * @param[in]  buffer   Pointer of data
 907  * @param[in]  length   length of data
 908  */
 909 void DevGpsSetChkSum(u_int8* buffer, u_int32 length) {
 910   u_int16 i = 0;
 911   u_int8  ck_a = 0;
 912   u_int8  ck_b = 0;
 913
 914   if (buffer != NULL) {
 915     for (i = 2; i < (length - 2); i++) {
 916       ck_a = ck_a + buffer[i];
 917       ck_b = ck_b + ck_a;
 918     }
 919
 920     /* Checksum_Set */
 921     buffer[length - 2] = ck_a;
 922     buffer[length - 1] = ck_b;
 923   } else {
 924   }
 925 }
 926
 927 /*---------------------------------------------------------------------------*/
 928 /*EOF*/