Harry Keane
/
X_NUCLEO_GNSS1A1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: A_TeseoLocationNEW A_TeseoLocation
Embed:
(wiki syntax)
Show/hide line numbers
NMEAParser.c
Go to the documentation of this file.
00001 /** 00002 ******************************************************************************* 00003 * @file NMEAParser.c 00004 * @author AST / Central Lab 00005 * @version V1.0.0 00006 * @date 18-May-2017 00007 * @brief NMEA sentence parser 00008 * 00009 ******************************************************************************* 00010 * @attention 00011 * 00012 * <h2><center>© COPYRIGHT(c) 2017 STMicroelectronics</center></h2> 00013 * 00014 * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 00015 * You may not use this file except in compliance with the License. 00016 * You may obtain a copy of the License at: 00017 * 00018 * http://www.st.com/software_license_agreement_liberty_v2 00019 * 00020 * Redistribution and use in source and binary forms, with or without modification, 00021 * are permitted provided that the following conditions are met: 00022 * 1. Redistributions of source code must retain the above copyright notice, 00023 * this list of conditions and the following disclaimer. 00024 * 2. Redistributions in binary form must reproduce the above copyright notice, 00025 * this list of conditions and the following disclaimer in the documentation 00026 * and/or other materials provided with the distribution. 00027 * 3. Neither the name of STMicroelectronics nor the names of its contributors 00028 * may be used to endorse or promote products derived from this software 00029 * without specific prior written permission. 00030 * 00031 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00032 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00033 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00034 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 00035 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00036 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 00037 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00038 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 00039 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 00040 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00041 * 00042 ******************************************************************************** 00043 */ 00044 00045 #include "string.h" 00046 #include "NMEAParser.h" 00047 #include "NMEAUtils.h" 00048 00049 /** @defgroup Middlewares 00050 * @brief Contains all platform independent modules (eg. NMEA Sentence Parser, ...). 00051 * @{ 00052 */ 00053 00054 /** @defgroup ST 00055 * @{ 00056 */ 00057 00058 /** @defgroup LIB_NMEA 00059 * @{ 00060 */ 00061 00062 /** @defgroup NMEA_PARSER 00063 * @{ 00064 */ 00065 00066 /** @addtogroup NMEA_PARSER_PUBLIC_FUNCTIONS 00067 * @{ 00068 */ 00069 /** 00070 * @brief Function that makes the parsing of the $GPGGA NMEA string with all Global Positioning System Fixed data. 00071 * @param gpgga_data Pointer to GPGGA_Infos struct 00072 * @param NMEA NMEA string read by the Gps expansion 00073 * @retval ParseStatus_Typedef PARSE_SUCC if the parsing process goes ok, PARSE_FAIL if it doesn't 00074 */ 00075 ParseStatus_Typedef parse_gpgga(GPGGA_Infos *gpgga_data, uint8_t *NMEA) 00076 { 00077 uint8_t app[MAX_MSG_LEN][MAX_MSG_LEN]; 00078 uint8_t valid_msg = 0; 00079 00080 ParseStatus_Typedef status = PARSE_FAIL; 00081 00082 if(NMEA == NULL) 00083 return status; 00084 00085 /* clear the app[][] buffer */ 00086 for (uint8_t i=0; i<MAX_MSG_LEN; i++) { 00087 memset(app[i], 0, MAX_MSG_LEN); 00088 } 00089 00090 for(unsigned i = 0, j = 0, k = 0; NMEA[i] != '\n' && i < strlen((char *)NMEA) - 1; i++) 00091 { 00092 if ((NMEA[i] == ',') || (NMEA[i] == '*')) { 00093 app[j][k] = '\0'; 00094 00095 if (strcmp((char *)app[0], "$GPGGA") == 0) { 00096 j++; 00097 k = 0; 00098 valid_msg = 1; 00099 continue; 00100 } 00101 else { 00102 while(NMEA[i++] != '\n'); 00103 j = k = 0; 00104 } 00105 } 00106 app[j][k++] = NMEA[i]; 00107 } 00108 00109 if (valid_msg == 1) { 00110 unsigned int valid; 00111 sscanf ((char *)app[6], "%u", &valid); 00112 gpgga_data->valid = (GPS_ValidTypedef)valid; 00113 if (gpgga_data->valid == VALID) { 00114 scan_utc ((char *)app[1], &gpgga_data->utc); 00115 sscanf ((char *)app[2], "%lf", &gpgga_data->xyz.lat); 00116 sscanf ((char *)app[3], "%c", &gpgga_data->xyz.ns); 00117 sscanf ((char *)app[4], "%lf", &gpgga_data->xyz.lon); 00118 sscanf ((char *)app[5], "%c", &gpgga_data->xyz.ew); 00119 sscanf ((char *)app[7], "%d", &gpgga_data->sats); 00120 sscanf ((char *)app[8], "%f", &gpgga_data->acc); 00121 sscanf ((char *)app[9], "%f", &gpgga_data->xyz.alt); 00122 sscanf ((char *)app[10], "%c", &gpgga_data->xyz.mis); 00123 sscanf ((char *)app[11], "%d", &gpgga_data->geoid.height); 00124 sscanf ((char *)app[12], "%c", &gpgga_data->geoid.mis); 00125 sscanf ((char *)app[13], "%d", &gpgga_data->update); 00126 sscanf ((char *)app[14], "%x", &gpgga_data->checksum); 00127 00128 valid_msg = 0; 00129 status = PARSE_SUCC; 00130 } 00131 } 00132 return status; 00133 } 00134 00135 /** 00136 * @brief Function that makes the parsing of the string read by the Gps expansion, capturing the right parameters from it. 00137 * @param gns_data Pointer to GNS_Infos struct 00138 * @param NMEA NMEA string read by the Gps expansion 00139 * @retval ParseStatus_Typedef PARSE_SUCC if the parsing process goes ok, PARSE_FAIL if it doesn't 00140 */ 00141 ParseStatus_Typedef parse_gnsmsg (GNS_Infos *gns_data, uint8_t *NMEA) 00142 { 00143 uint8_t app[MAX_MSG_LEN][MAX_MSG_LEN]; 00144 uint8_t valid_msg = 0; 00145 00146 ParseStatus_Typedef status = PARSE_FAIL; 00147 00148 if(NMEA == NULL) 00149 return status; 00150 00151 /* clear the app[][] buffer */ 00152 for (uint8_t i=0; i<MAX_MSG_LEN; i++) { 00153 memset(app[i], 0, MAX_MSG_LEN); 00154 } 00155 00156 for (unsigned i = 0, j = 0, k = 0; NMEA[i] != '\n' && i < strlen((char *)NMEA) - 1; i++) 00157 { 00158 if ((NMEA[i] == ',') || (NMEA[i] == '*')) { 00159 app[j][k] = '\0'; 00160 00161 if ((strcmp((char *)app[0], "$GPGNS") == 0) || 00162 (strcmp((char *)app[0], "$GLGNS") == 0) || 00163 (strcmp((char *)app[0], "$GAGNS") == 0) || 00164 (strcmp((char *)app[0], "$BDGNS") == 0) || 00165 (strcmp((char *)app[0], "$QZGNS") == 0) || 00166 (strcmp((char *)app[0], "$GNGNS") == 0)) 00167 { 00168 j++; 00169 k = 0; 00170 valid_msg = 1; 00171 continue; 00172 } 00173 else { 00174 while (NMEA[i++] != '\n'); 00175 j = k = 0; 00176 } 00177 } 00178 app[j][k++] = NMEA[i]; 00179 } 00180 00181 if (valid_msg == 1) { 00182 sscanf ((char *)app[0], "%s", gns_data->constellation); 00183 scan_utc ((char *)app[1], &gns_data->utc); 00184 sscanf ((char *)app[2], "%lf", &gns_data->xyz.lat); 00185 sscanf ((char *)app[3], "%c", &gns_data->xyz.ns); 00186 sscanf ((char *)app[4], "%lf", &gns_data->xyz.lon); 00187 sscanf ((char *)app[5], "%c", &gns_data->xyz.ew); 00188 sscanf ((char *)app[6], "%c", &gns_data->gps_mode); 00189 sscanf ((char *)app[7], "%c", &gns_data->glonass_mode); 00190 sscanf ((char *)app[8], "%d", &gns_data->sats); 00191 sscanf ((char *)app[9], "%f", &gns_data->hdop); 00192 sscanf ((char *)app[10], "%f", &gns_data->xyz.alt); 00193 sscanf ((char *)app[11], "%f", &gns_data->geo_sep); 00194 sscanf ((char *)app[12], "%c", &gns_data->dgnss_age); 00195 sscanf ((char *)app[13], "%c", &gns_data->dgnss_ref); 00196 sscanf ((char *)app[14], "%x", &gns_data->checksum); 00197 00198 valid_msg = 0; 00199 status = PARSE_SUCC; 00200 } 00201 00202 return status; 00203 } 00204 00205 /** 00206 * @brief Function that makes the parsing of the $GPGST NMEA string with GPS Pseudorange Noise Statistics. 00207 * @param GPGST_Infos Pointer to a GPGST_Infos struct 00208 * @param NMEA NMEA string read by the Gps expansion. 00209 * @retval ParseStatus_Typedef PARSE_SUCC if the parsing process goes ok, PARSE_FAIL if it doesn't 00210 */ 00211 ParseStatus_Typedef parse_gpgst (GPGST_Infos *gpgst_data, uint8_t *NMEA) 00212 { 00213 uint8_t app[MAX_MSG_LEN][MAX_MSG_LEN]; 00214 uint8_t valid_msg = 0; 00215 00216 ParseStatus_Typedef status = PARSE_FAIL; 00217 00218 if(NMEA == NULL) 00219 return status; 00220 00221 /* clear the app[][] buffer */ 00222 for (uint8_t i=0; i<MAX_MSG_LEN; i++) { 00223 memset(app[i], 0, MAX_MSG_LEN); 00224 } 00225 00226 for (unsigned i = 0, j = 0, k = 0; NMEA[i] != '\n' && i < strlen((char *)NMEA) - 1; i++) 00227 { 00228 if ((NMEA[i] == ',') || (NMEA[i] == '*')) { 00229 app[j][k] = '\0'; 00230 00231 if (strcmp((char *)app[0], "$GPGST") == 0) 00232 { 00233 j++; 00234 k = 0; 00235 valid_msg = 1; 00236 continue; 00237 } 00238 else { 00239 while (NMEA[i++] != '\n'); 00240 j = k = 0; 00241 } 00242 } 00243 app[j][k++] = NMEA[i]; 00244 } 00245 00246 if (valid_msg == 1) { 00247 scan_utc ((char *)app[1], &gpgst_data->utc); 00248 sscanf ((char *)app[2], "%f", &gpgst_data->EHPE); 00249 sscanf ((char *)app[3], "%f", &gpgst_data->semi_major_dev); 00250 sscanf ((char *)app[4], "%f", &gpgst_data->semi_minor_dev); 00251 sscanf ((char *)app[5], "%f", &gpgst_data->semi_major_angle); 00252 sscanf ((char *)app[6], "%f", &gpgst_data->lat_err_dev); 00253 sscanf ((char *)app[7], "%f", &gpgst_data->lon_err_dev); 00254 sscanf ((char *)app[8], "%f", &gpgst_data->alt_err_dev); 00255 sscanf ((char *)app[9], "%x", &gpgst_data->checksum); 00256 00257 valid_msg = 0; 00258 status = PARSE_SUCC; 00259 } 00260 00261 return status; 00262 } 00263 00264 /** 00265 * @brief Function that makes the parsing of the $GPRMC NMEA string with Recommended Minimum Specific GPS/Transit data. 00266 * @param GPRMC_Infos Pointer to a GPRMC_Infos struct 00267 * @param NMEA NMEA string read by the Gps expansion. 00268 * @retval ParseStatus_Typedef PARSE_SUCC if the parsing process goes ok, PARSE_FAIL if it doesn't 00269 */ 00270 ParseStatus_Typedef parse_gprmc (GPRMC_Infos *gprmc_data, uint8_t *NMEA) 00271 { 00272 uint8_t app[MAX_MSG_LEN][MAX_MSG_LEN]; 00273 uint8_t valid_msg = 0; 00274 00275 ParseStatus_Typedef status = PARSE_FAIL; 00276 00277 if(NMEA == NULL) 00278 return status; 00279 00280 /* clear the app[][] buffer */ 00281 for (uint8_t i=0; i<MAX_MSG_LEN; i++) { 00282 memset(app[i], 0, MAX_MSG_LEN); 00283 } 00284 00285 for (unsigned i = 0, j = 0, k = 0; NMEA[i] != '\n' && i < strlen((char *)NMEA) - 1; i++) 00286 { 00287 if ((NMEA[i] == ',') || (NMEA[i] == '*')) { 00288 app[j][k] = '\0'; 00289 00290 if (strcmp((char *)app[0], "$GPRMC") == 0) 00291 { 00292 j++; 00293 k = 0; 00294 valid_msg = 1; 00295 continue; 00296 } 00297 else { 00298 while (NMEA[i++] != '\n'); 00299 j = k = 0; 00300 } 00301 } 00302 app[j][k++] = NMEA[i]; 00303 } 00304 00305 if (valid_msg == 1) { 00306 scan_utc ((char *)app[1], &gprmc_data->utc); 00307 sscanf ((char *)app[2], "%c", &gprmc_data->status); 00308 sscanf ((char *)app[3], "%lf", &gprmc_data->xyz.lat); 00309 sscanf ((char *)app[4], "%c", &gprmc_data->xyz.ns); 00310 sscanf ((char *)app[5], "%lf", &gprmc_data->xyz.lon); 00311 sscanf ((char *)app[6], "%c", &gprmc_data->xyz.ew); 00312 sscanf ((char *)app[7], "%f", &gprmc_data->speed); 00313 sscanf ((char *)app[8], "%f", &gprmc_data->trackgood); 00314 sscanf ((char *)app[9], "%d", &gprmc_data->date); 00315 sscanf ((char *)app[10], "%f", &gprmc_data->mag_var); 00316 sscanf ((char *)app[11], "%c", &gprmc_data->mag_var_dir); 00317 /* WARNING: from received msg, it seems there is another data (app[12]) before the checksum */ 00318 sscanf ((char *)app[13], "%x", &gprmc_data->checksum); 00319 00320 valid_msg = 0; 00321 status = PARSE_SUCC; 00322 } 00323 00324 return status; 00325 } 00326 00327 /** 00328 * @brief Function that makes the parsing of the string read by the Gps expansion, capturing the right parameters from it. 00329 * @param GSA_Infos Pointer to a GSA_Infos struct 00330 * @param NMEA NMEA string read by the Gps expansion. 00331 * @retval ParseStatus_Typedef PARSE_SUCC if the parsing process goes ok, PARSE_FAIL if it doesn't 00332 */ 00333 ParseStatus_Typedef parse_gsamsg (GSA_Infos *gsa_data, uint8_t *NMEA) 00334 { 00335 uint8_t app[MAX_MSG_LEN][MAX_MSG_LEN]; 00336 uint8_t valid_msg = 0; 00337 00338 ParseStatus_Typedef status = PARSE_FAIL; 00339 00340 if(NMEA == NULL) 00341 return status; 00342 00343 /* clear the app[][] buffer */ 00344 for (uint8_t i=0; i<19; i++) { 00345 memset(app[i], 0, 19); 00346 } 00347 00348 for (unsigned i = 0, j = 0, k = 0; NMEA[i] != '\n' && i < strlen((char *)NMEA) - 1; i++) 00349 { 00350 if ((NMEA[i] == ',') || (NMEA[i] == '*')) { 00351 app[j][k] = '\0'; 00352 00353 if ((strcmp((char *)app[0], "$GPGSA") == 0) || 00354 (strcmp((char *)app[0], "$GLGSA") == 0) || 00355 (strcmp((char *)app[0], "$GAGSA") == 0) || 00356 (strcmp((char *)app[0], "$BDGSA") == 0) || 00357 (strcmp((char *)app[0], "$GNGSA") == 0)) 00358 { 00359 j++; 00360 k = 0; 00361 valid_msg = 1; 00362 continue; 00363 } 00364 else { 00365 while (NMEA[i++] != '\n'); 00366 j = k = 0; 00367 } 00368 } 00369 app[j][k++] = NMEA[i]; 00370 } 00371 00372 if (valid_msg == 1) { 00373 sscanf ((char *)app[0], "%s", gsa_data->constellation); 00374 sscanf ((char *)app[1], "%c", &gsa_data->operating_mode); 00375 sscanf ((char *)app[2], "%d", &gsa_data->current_mode); 00376 for (uint8_t i=0; i<MAX_SAT_NUM; i++) { 00377 sscanf ((char *)app[3+i], "%d", &gsa_data->sat_prn[i]); 00378 } 00379 sscanf ((char *)app[15], "%f", &gsa_data->pdop); 00380 sscanf ((char *)app[16], "%f", &gsa_data->hdop); 00381 sscanf ((char *)app[17], "%f", &gsa_data->vdop); 00382 sscanf ((char *)app[18], "%x", &gsa_data->checksum); 00383 00384 valid_msg = 0; 00385 status = PARSE_SUCC; 00386 } 00387 00388 return status; 00389 } 00390 00391 /** 00392 * @brief Function that makes the parsing of the string read by the Gps expansion, capturing the right parameters from it. 00393 * @param GSV_Infos Pointer to a GSV_Infos struct 00394 * @param NMEA NMEA string read by the Gps expansion. 00395 * @retval ParseStatus_Typedef PARSE_SUCC if the parsing process goes ok, PARSE_FAIL if it doesn't 00396 */ 00397 ParseStatus_Typedef parse_gsvmsg(GSV_Infos *gsv_data, uint8_t *NMEA) 00398 { 00399 uint8_t app[32][16]; 00400 uint8_t app_idx; 00401 uint8_t gsv_idx = 0; 00402 int msg_amount = 1; 00403 int curr_msg; 00404 uint8_t right_msg = 0; 00405 uint8_t valid_gsv_msg = 0; 00406 unsigned i, j, k; 00407 unsigned l = 0; 00408 00409 ParseStatus_Typedef status = PARSE_FAIL; 00410 00411 if(NMEA == NULL) 00412 return status; 00413 00414 while (right_msg < msg_amount) 00415 { 00416 /* clear the app[][] buffer */ 00417 for (uint8_t pos=0; pos<32; pos++) { 00418 memset(app[pos], 0, 16); 00419 } 00420 00421 for (i = l, j = 0, k = 0; NMEA[i] != '\n' && i < strlen((char *)NMEA) - 1; i++) 00422 { 00423 if (NMEA[i] == ',') { 00424 app[j][k] = '\0'; 00425 00426 if ((strcmp((char *)app[0], "$GPGSV") == 0) || 00427 (strcmp((char *)app[0], "$GLGSV") == 0) || 00428 (strcmp((char *)app[0], "$GAGSV") == 0) || 00429 (strcmp((char *)app[0], "$BDGSV") == 0) || 00430 (strcmp((char *)app[0], "$QZGSV") == 0) || 00431 (strcmp((char *)app[0], "$GNGSV") == 0)) 00432 { 00433 j++; 00434 k = 0; 00435 valid_gsv_msg = 1; 00436 continue; 00437 } 00438 else { 00439 while (NMEA[i++] != '\n'); 00440 j = k = 0; 00441 } 00442 } 00443 if ((NMEA[i] == '*') && (k!=0)) { 00444 j++; 00445 k=0; 00446 } 00447 app[j][k++] = NMEA[i]; 00448 } 00449 00450 l = i; 00451 00452 if (valid_gsv_msg == 1) { 00453 valid_gsv_msg = 0; 00454 sscanf((char *)app[1], "%d", &msg_amount); 00455 sscanf((char *)app[2], "%d", &curr_msg); 00456 if (curr_msg == right_msg+1) { 00457 valid_gsv_msg = 1; 00458 right_msg++; 00459 } 00460 } 00461 else { 00462 right_msg = msg_amount; 00463 } 00464 00465 if (valid_gsv_msg == 1) { 00466 sscanf((char *)app[0], "%s", gsv_data->constellation); 00467 sscanf((char *)app[1], "%d", &gsv_data->amount); 00468 sscanf((char *)app[2], "%d", &gsv_data->number); 00469 sscanf((char *)app[3], "%d", &gsv_data->tot_sats); 00470 app_idx = 4; 00471 while (app[app_idx][0] != '*') { 00472 sscanf((char *)app[app_idx++], "%d", &gsv_data->gsv_sat_i[gsv_idx].prn); 00473 sscanf((char *)app[app_idx++], "%d", &gsv_data->gsv_sat_i[gsv_idx].elev); 00474 sscanf((char *)app[app_idx++], "%d", &gsv_data->gsv_sat_i[gsv_idx].azim); 00475 if (app[app_idx][0] != '*') { 00476 sscanf((char *)app[app_idx++], "%d", &gsv_data->gsv_sat_i[gsv_idx].cn0); 00477 } 00478 else { 00479 sscanf("", "%d", &gsv_data->gsv_sat_i[gsv_idx].cn0); 00480 } 00481 gsv_idx++; 00482 } 00483 00484 valid_gsv_msg = 0; 00485 00486 status = PARSE_SUCC; 00487 } 00488 } 00489 00490 return status; 00491 } 00492 00493 /** 00494 * @brief 00495 * @param Geofence_Infos Pointer to a Geofence_Infos struct 00496 * @param NMEA NMEA string read by the Gps expansion. 00497 * @retval ParseStatus_Typedef PARSE_SUCC if the parsing process goes ok, PARSE_FAIL if it doesn't 00498 */ 00499 ParseStatus_Typedef parse_pstmgeofence (Geofence_Infos *geofence_data, uint8_t *NMEA) 00500 { 00501 uint8_t app[MAX_MSG_LEN][MAX_MSG_LEN]; 00502 uint8_t valid_msg = 0; 00503 00504 ParseStatus_Typedef status = PARSE_FAIL; 00505 00506 if(NMEA == NULL) 00507 return status; 00508 00509 /* clear the app[][] buffer */ 00510 for (uint8_t i=0; i<MAX_MSG_LEN; i++) { 00511 memset(app[i], 0, MAX_MSG_LEN); 00512 } 00513 00514 for(unsigned i = 0, j = 0, k = 0; NMEA[i] != '\n' && i < strlen((char *)NMEA) - 1; i++) 00515 { 00516 if ((NMEA[i] == ',') || (NMEA[i] == '*')) { 00517 app[j][k] = '\0'; 00518 00519 if ((strcmp((char *)app[0], "$PSTMCFGGEOFENCEOK") == 0) || 00520 (strcmp((char *)app[0], "$PSTMCFGGEOFENCEERROR") == 0) || 00521 (strcmp((char *)app[0], "$PSTMGEOFENCECFGOK") == 0) || 00522 (strcmp((char *)app[0], "$PSTMGEOFENCECFGERROR") == 0) || 00523 (strcmp((char *)app[0], "$PSTMGEOFENCESTATUS") == 0) || 00524 (strcmp((char *)app[0], "$PSTMGEOFENCE") == 0) || 00525 (strcmp((char *)app[0], "$PSTMGEOFENCEREQERROR") == 0)) { 00526 j++; 00527 k = 0; 00528 valid_msg = 1; 00529 continue; 00530 } 00531 else { 00532 while(NMEA[i++] != '\n'); 00533 j = k = 0; 00534 } 00535 } 00536 app[j][k++] = NMEA[i]; 00537 } 00538 00539 if (valid_msg == 1) { 00540 /* Enabling */ 00541 if (strcmp((char *)app[0], "$PSTMCFGGEOFENCEOK") == 0) { 00542 geofence_data->op = GNSS_FEATURE_EN_MSG; 00543 geofence_data->result = 0; 00544 } 00545 if (strcmp((char *)app[0], "$PSTMCFGGEOFENCEERROR") == 0) { 00546 geofence_data->op = GNSS_FEATURE_EN_MSG; 00547 geofence_data->result = 1; 00548 } 00549 /* Configuring */ 00550 if (strcmp((char *)app[0], "$PSTMGEOFENCECFGOK") == 0) { 00551 geofence_data->op = GNSS_GEOFENCE_CFG_MSG; 00552 geofence_data->result = 0; 00553 } 00554 if (strcmp((char *)app[0], "$PSTMGEOFENCECFGERROR") == 0) { 00555 geofence_data->op = GNSS_GEOFENCE_STATUS_MSG; 00556 geofence_data->result = 1; 00557 } 00558 /* Querying Status */ 00559 if (strcmp((char *)app[0], "$PSTMGEOFENCESTATUS") == 0) { 00560 geofence_data->op = GNSS_GEOFENCE_STATUS_MSG; 00561 geofence_data->result = 0; 00562 sscanf((char *)app[1], "%02d%02d%02d", &geofence_data->timestamp.hh,&geofence_data->timestamp.mm,&geofence_data->timestamp.ss); 00563 sscanf((char *)app[2], "%04d%02d%02d", &geofence_data->timestamp.year,&geofence_data->timestamp.month,&geofence_data->timestamp.day); 00564 for(uint8_t i = 0; i<MAX_GEOFENCES_NUM; i++) { 00565 sscanf((char *)app[3+i], "%d", &geofence_data->status[i]); 00566 } 00567 } 00568 /* Alarm Msg */ 00569 if (strcmp((char *)app[0], "$PSTMGEOFENCE") == 0) { 00570 geofence_data->op = GNSS_GEOFENCE_ALARM_MSG; 00571 geofence_data->result = 0; 00572 sscanf((char *)app[1], "%02d%02d%02d", &geofence_data->timestamp.hh,&geofence_data->timestamp.mm,&geofence_data->timestamp.ss); 00573 sscanf((char *)app[2], "%04d%02d%02d", &geofence_data->timestamp.year,&geofence_data->timestamp.month,&geofence_data->timestamp.day); 00574 sscanf((char *)app[3], "%d", &geofence_data->idAlarm); 00575 sscanf((char *)app[9], "%d", &geofence_data->status[geofence_data->idAlarm]); 00576 } 00577 00578 valid_msg = 0; 00579 status = PARSE_SUCC; 00580 } 00581 return status; 00582 } 00583 00584 /** 00585 * @brief 00586 * @param Odometer_Infos Pointer to a Odometer_Infos struct 00587 * @param NMEA NMEA string read by the Gps expansion. 00588 * @retval ParseStatus_Typedef PARSE_SUCC if the parsing process goes ok, PARSE_FAIL if it doesn't 00589 */ 00590 ParseStatus_Typedef parse_pstmodo (Odometer_Infos *odo_data, uint8_t *NMEA) 00591 { 00592 uint8_t app[MAX_MSG_LEN][MAX_MSG_LEN]; 00593 uint8_t valid_msg = 0; 00594 00595 ParseStatus_Typedef status = PARSE_FAIL; 00596 00597 if(NMEA == NULL) 00598 return status; 00599 00600 /* clear the app[][] buffer */ 00601 for (uint8_t i=0; i<MAX_MSG_LEN; i++) { 00602 memset(app[i], 0, MAX_MSG_LEN); 00603 } 00604 00605 for(unsigned i = 0, j = 0, k = 0; NMEA[i] != '\n' && i < strlen((char *)NMEA) - 1; i++) 00606 { 00607 if ((NMEA[i] == ',') || (NMEA[i] == '*')) { 00608 app[j][k] = '\0'; 00609 00610 if ((strcmp((char *)app[0], "$PSTMCFGODOOK") == 0) || 00611 (strcmp((char *)app[0], "$PSTMCFGODOERROR") == 0) || 00612 (strcmp((char *)app[0], "$PSTMODOSTARTOK") == 0) || 00613 (strcmp((char *)app[0], "$PSTMODOSTARTERROR") == 0) || 00614 (strcmp((char *)app[0], "$PSTMODOSTOPOK") == 0) || 00615 (strcmp((char *)app[0], "$PSTMODOSTOPERROR") == 0)) { 00616 j++; 00617 k = 0; 00618 valid_msg = 1; 00619 continue; 00620 } 00621 else { 00622 while(NMEA[i++] != '\n'); 00623 j = k = 0; 00624 } 00625 } 00626 app[j][k++] = NMEA[i]; 00627 } 00628 00629 if (valid_msg == 1) { 00630 /* Enabling */ 00631 if (strcmp((char *)app[0], "$PSTMCFGODOOK") == 0) { 00632 odo_data->op = GNSS_FEATURE_EN_MSG; 00633 odo_data->result = 0; 00634 } 00635 if (strcmp((char *)app[0], "$PSTMCFGODOERROR") == 0) { 00636 odo_data->op = GNSS_FEATURE_EN_MSG; 00637 odo_data->result = 1; 00638 } 00639 00640 /* Start */ 00641 if (strcmp((char *)app[0], "$PSTMODOSTARTOK") == 0) { 00642 odo_data->op = GNSS_ODO_START_MSG; 00643 odo_data->result = 0; 00644 } 00645 if (strcmp((char *)app[0], "$PSTMODOSTARTERROR") == 0) { 00646 odo_data->op = GNSS_ODO_START_MSG; 00647 odo_data->result = 1; 00648 } 00649 00650 /* Stop */ 00651 if (strcmp((char *)app[0], "$PSTMODOSTOPOK") == 0) { 00652 odo_data->op = GNSS_ODO_STOP_MSG; 00653 odo_data->result = 0; 00654 } 00655 if (strcmp((char *)app[0], "$PSTMODOSTOPERROR") == 0) { 00656 odo_data->op = GNSS_ODO_STOP_MSG; 00657 odo_data->result = 1; 00658 } 00659 00660 valid_msg = 0; 00661 status = PARSE_SUCC; 00662 } 00663 return status; 00664 } 00665 00666 /** 00667 * @brief 00668 * @param Datalog_Infos Pointer to a Datalog_Infos struct 00669 * @param NMEA NMEA string read by the Gps expansion. 00670 * @retval ParseStatus_Typedef PARSE_SUCC if the parsing process goes ok, PARSE_FAIL if it doesn't 00671 */ 00672 ParseStatus_Typedef parse_pstmdatalog (Datalog_Infos *datalog_data, uint8_t *NMEA) 00673 { 00674 uint8_t app[MAX_MSG_LEN][MAX_MSG_LEN]; 00675 uint8_t valid_msg = 0; 00676 00677 ParseStatus_Typedef status = PARSE_FAIL; 00678 00679 if(NMEA == NULL) 00680 return status; 00681 00682 /* clear the app[][] buffer */ 00683 for (uint8_t i=0; i<MAX_MSG_LEN; i++) { 00684 memset(app[i], 0, MAX_MSG_LEN); 00685 } 00686 00687 for(unsigned i = 0, j = 0, k = 0; NMEA[i] != '\n' && i < strlen((char *)NMEA) - 1; i++) 00688 { 00689 if ((NMEA[i] == ',') || (NMEA[i] == '*')) { 00690 app[j][k] = '\0'; 00691 00692 if ((strcmp((char *)app[0], "$PSTMCFGLOGOK") == 0) || 00693 (strcmp((char *)app[0], "$PSTMCFGLOGERROR") == 0) || 00694 (strcmp((char *)app[0], "$PSTMLOGCREATEOK") == 0) || 00695 (strcmp((char *)app[0], "$PSTMLOGCREATEERROR") == 0) || 00696 (strcmp((char *)app[0], "$PSTMLOGSTARTOK") == 0) || 00697 (strcmp((char *)app[0], "$PSTMLOGSTARTERROR") == 0) || 00698 (strcmp((char *)app[0], "$PSTMLOGSTOPOK") == 0) || 00699 (strcmp((char *)app[0], "$PSTMLOGSTOPERROR") == 0) || 00700 (strcmp((char *)app[0], "$PSTMLOGERASEOK") == 0) || 00701 (strcmp((char *)app[0], "$PSTMLOGERASEERROR") == 0)) { 00702 j++; 00703 k = 0; 00704 valid_msg = 1; 00705 continue; 00706 } 00707 else { 00708 while(NMEA[i++] != '\n'); 00709 j = k = 0; 00710 } 00711 } 00712 app[j][k++] = NMEA[i]; 00713 } 00714 00715 if (valid_msg == 1) { 00716 /* Enabling */ 00717 if (strcmp((char *)app[0], "$PSTMCFGLOGOK") == 0) { 00718 datalog_data->op = GNSS_FEATURE_EN_MSG; 00719 datalog_data->result = 0; 00720 } 00721 if (strcmp((char *)app[0], "$PSTMCFGLOGERROR") == 0) { 00722 datalog_data->op = GNSS_FEATURE_EN_MSG; 00723 datalog_data->result = 1; 00724 } 00725 /* Configuring */ 00726 if (strcmp((char *)app[0], "$PSTMLOGCREATEOK") == 0) { 00727 datalog_data->op = GNSS_DATALOG_CFG_MSG; 00728 datalog_data->result = 0; 00729 } 00730 if (strcmp((char *)app[0], "$PSTMLOGCREATEERROR") == 0) { 00731 datalog_data->op = GNSS_DATALOG_CFG_MSG; 00732 datalog_data->result = 1; 00733 } 00734 /* Start */ 00735 if (strcmp((char *)app[0], "$PSTMLOGSTARTOK") == 0) { 00736 datalog_data->op = GNSS_DATALOG_START_MSG; 00737 datalog_data->result = 0; 00738 } 00739 if (strcmp((char *)app[0], "$PSTMLOGSTARTERROR") == 0) { 00740 datalog_data->op = GNSS_DATALOG_START_MSG; 00741 datalog_data->result = 1; 00742 } 00743 /* Stop */ 00744 if (strcmp((char *)app[0], "$PSTMLOGSTOPOK") == 0) { 00745 datalog_data->op = GNSS_DATALOG_STOP_MSG; 00746 datalog_data->result = 0; 00747 } 00748 if (strcmp((char *)app[0], "$PSTMLOGSTOPERROR") == 0) { 00749 datalog_data->op = GNSS_DATALOG_STOP_MSG; 00750 datalog_data->result = 1; 00751 } 00752 /* Erase */ 00753 if (strcmp((char *)app[0], "$PSTMLOGERASEOK") == 0) { 00754 datalog_data->op = GNSS_DATALOG_ERASE_MSG; 00755 datalog_data->result = 0; 00756 } 00757 if (strcmp((char *)app[0], "$PSTMLOGERASEERROR") == 0) { 00758 datalog_data->op = GNSS_DATALOG_ERASE_MSG; 00759 datalog_data->result = 1; 00760 } 00761 00762 valid_msg = 0; 00763 status = PARSE_SUCC; 00764 } 00765 return status; 00766 } 00767 00768 /** 00769 * @brief 00770 * @param Ack_Info Ack from Teseo 00771 * @param NMEA NMEA string read by the Gps expansion. 00772 * @retval ParseStatus_Typedef PARSE_SUCC if the parsing process goes ok, PARSE_FAIL if it doesn't 00773 */ 00774 ParseStatus_Typedef parse_pstmsgl (Ack_Info *ack, uint8_t *NMEA) 00775 { 00776 uint8_t app[MAX_MSG_LEN][MAX_MSG_LEN]; 00777 uint8_t valid_msg = 0; 00778 00779 ParseStatus_Typedef status = PARSE_FAIL; 00780 00781 if(NMEA == NULL) 00782 return status; 00783 00784 /* clear the app[][] buffer */ 00785 for (uint8_t i=0; i<MAX_MSG_LEN; i++) { 00786 memset(app[i], 0, MAX_MSG_LEN); 00787 } 00788 00789 for(unsigned i = 0, j = 0, k = 0; NMEA[i] != '\n' && i < strlen((char *)NMEA) - 1; i++) 00790 { 00791 if ((NMEA[i] == ',') || (NMEA[i] == '*')) { 00792 app[j][k] = '\0'; 00793 00794 if ((strcmp((char *)app[0], "$PSTMCFGMSGLOK") == 0) || 00795 (strcmp((char *)app[0], "$PSTMCFGMSGLERROR") == 0)) { 00796 j++; 00797 k = 0; 00798 valid_msg = 1; 00799 continue; 00800 } 00801 else { 00802 while(NMEA[i++] != '\n'); 00803 j = k = 0; 00804 } 00805 } 00806 app[j][k++] = NMEA[i]; 00807 } 00808 00809 if (valid_msg == 1) { 00810 /* Enabling */ 00811 if (strcmp((char *)app[0], "$PSTMCFGMSGLOK") == 0) { 00812 *ack = 0; 00813 } 00814 if (strcmp((char *)app[0], "$PSTMCFGMSGLERROR") == 0) { 00815 *ack = 1; 00816 } 00817 00818 valid_msg = 0; 00819 status = PARSE_SUCC; 00820 } 00821 return status; 00822 } 00823 00824 /** 00825 * @brief 00826 * @param Ack_Info Ack from Teseo 00827 * @param NMEA NMEA string read by the Gps expansion. 00828 * @retval ParseStatus_Typedef PARSE_SUCC if the parsing process goes ok, PARSE_FAIL if it doesn't 00829 */ 00830 ParseStatus_Typedef parse_pstmsavepar (Ack_Info *ack, uint8_t *NMEA) 00831 { 00832 uint8_t app[MAX_MSG_LEN][MAX_MSG_LEN]; 00833 uint8_t valid_msg = 0; 00834 00835 ParseStatus_Typedef status = PARSE_FAIL; 00836 00837 if(NMEA == NULL) 00838 return status; 00839 00840 /* clear the app[][] buffer */ 00841 for (uint8_t i=0; i<MAX_MSG_LEN; i++) { 00842 memset(app[i], 0, MAX_MSG_LEN); 00843 } 00844 00845 for(unsigned i = 0, j = 0, k = 0; NMEA[i] != '\n' && i < strlen((char *)NMEA) - 1; i++) 00846 { 00847 if ((NMEA[i] == ',') || (NMEA[i] == '*')) { 00848 app[j][k] = '\0'; 00849 00850 if ((strcmp((char *)app[0], "$PSTMSAVEPAROK") == 0) || 00851 (strcmp((char *)app[0], "$PSTMSAVEPARERROR") == 0)) { 00852 j++; 00853 k = 0; 00854 valid_msg = 1; 00855 continue; 00856 } 00857 else { 00858 while(NMEA[i++] != '\n'); 00859 j = k = 0; 00860 } 00861 } 00862 app[j][k++] = NMEA[i]; 00863 } 00864 00865 if (valid_msg == 1) { 00866 /* Enabling */ 00867 if (strcmp((char *)app[0], "$PSTMSAVEPAROK") == 0) { 00868 *ack = 0; 00869 } 00870 if (strcmp((char *)app[0], "$PSTMSAVEPARERROR") == 0) { 00871 *ack = 1; 00872 } 00873 00874 valid_msg = 0; 00875 status = PARSE_SUCC; 00876 } 00877 return status; 00878 } 00879 00880 /** 00881 * @brief This function makes a copy of the datas stored into gps_t into the 'info' param 00882 * @param info Instance of a GPGGA_Infos object where there are the infos to be copied 00883 * @param gps_t Instance of a GPGGA_Infos object pointer where the infos stored into gps_t have to be copied 00884 * @retval None 00885 */ 00886 void copy_data(GPGGA_Infos *info, GPGGA_Infos gps_t){ 00887 info->acc = gps_t.acc; 00888 info->geoid.height = gps_t.geoid.height; 00889 info->geoid.mis = gps_t.geoid.mis; 00890 info->sats = gps_t.sats; 00891 info->update = gps_t.update; 00892 info->utc.hh = gps_t.utc.hh; 00893 info->utc.mm = gps_t.utc.mm; 00894 info->utc.ss = gps_t.utc.ss; 00895 info->utc.utc = gps_t.utc.utc; 00896 info->valid = gps_t.valid; 00897 info->xyz.alt = gps_t.xyz.alt; 00898 info->xyz.lat = gps_t.xyz.lat; 00899 info->xyz.lon = gps_t.xyz.lon; 00900 info->xyz.ew = gps_t.xyz.ew; 00901 info->xyz.ns = gps_t.xyz.ns; 00902 info->xyz.mis = gps_t.xyz.mis; 00903 info->checksum = gps_t.checksum; 00904 } 00905 /** 00906 * @} 00907 */ 00908 00909 /** 00910 * @} 00911 */ 00912 00913 /** 00914 * @} 00915 */ 00916 00917 /** 00918 * @} 00919 */ 00920 00921 /** 00922 * @} 00923 */
Generated on Wed Jul 13 2022 01:04:53 by
1.7.2