gps-plugin/src/nmea_parser.c

   1 /*
   2  * gps replay plugin
   3  *
   4  * Copyright (c) 2011-2013 Samsung Electronics Co., Ltd. All rights reserved.
   5  *
   6  * Contact: Youngae Kang <youngae.kang@samsung.com>, Minjune Kim <sena06.kim@samsung.com>
   7  *                      Genie Kim <daejins.kim@samsung.com>
   8  *
   9  * Licensed under the Apache License, Version 2.0 (the "License");
  10  * you may not use this file except in compliance with the License.
  11  * You may obtain a copy of the License at
  12  *
  13  * http://www.apache.org/licenses/LICENSE-2.0
  14  *
  15  * Unless required by applicable law or agreed to in writing, software
  16  * distributed under the License is distributed on an "AS IS" BASIS,
  17  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  18  * See the License for the specific language governing permissions and
  19  * limitations under the License.
  20  */
  21
  22 #include <stdio.h>
  23 #include <stdlib.h>
  24 #include <string.h>
  25 #include <glib.h>
  26 #include <gps_plugin_intf.h>
  27
  28 #include "gps_plugin_debug.h"
  29 #include "nmea_parser.h"
  30 #include "setting.h"
  31
  32 #define MAX_NMEA_SENTENCES      32
  33 #define MAX_TOEKNS                      25
  34
  35 #define DECIMAL_TO_DEGREE       60.0
  36 #define METER_TO_FEET           3.2808399
  37 #define KNOT_TO_MPS                     0.5144444
  38 #define KMPH_TO_MPS                     0.2777778
  39
  40 #define NORTH            1
  41 #define SOUTH           -1
  42 #define EAST             1
  43 #define WEST            -1
  44
  45 int used_sat[MAX_GPS_NUM_SAT_USED] = { 0, };
  46
  47 static char nmea_parser_c2n(char ch)
  48 {
  49         if (ch <= '9') {
  50                 return ch - '0';
  51         } else {
  52                 return (ch - 'A') + 10;
  53         }
  54 }
  55
  56 int nmea_parser_verify_checksum(char *nmea_sen)
  57 {
  58         int ret = -1;
  59         int i;
  60         int checksum = 0;
  61         int sum = 0;
  62
  63         for (i = 0; i < strlen(nmea_sen) && (nmea_sen[i] != '*'); i++) {
  64                 checksum ^= nmea_sen[i];
  65         }
  66
  67         if (++i + 1 < strlen(nmea_sen)) {
  68                 sum = (nmea_parser_c2n(nmea_sen[i]) << 4) + nmea_parser_c2n(nmea_sen[i + 1]);
  69         }
  70
  71         if (sum == checksum) {
  72                 ret = 0;
  73         } else {
  74                 LOG_PLUGIN(DBG_LOW, "NMEA checksum is INVALID");
  75                 ret = -1;
  76         }
  77         return ret;
  78 }
  79
  80 int nmea_parser_tokenize(char input[], char *token[])
  81 {
  82         char *s = input;
  83         int num_tokens = 0;
  84         int state;
  85
  86         token[num_tokens] = s;
  87         num_tokens++;
  88         state = 0;
  89         LOG_PLUGIN(DBG_LOW, "input:%s \n", input);
  90
  91         while ((*s != 0) && (num_tokens < MAX_TOEKNS)) {
  92                 switch (state) {
  93                         case 0:
  94                                 if (*s == ',') {
  95                                         *s = 0;
  96                                         state = 1;
  97                                 }
  98                                 break;
  99                         case 1:
 100                                 token[num_tokens++] = s;
 101                                 if (*s == ',') {
 102                                         *s = 0;
 103                                 } else {
 104                                         state = 0;
 105                                 }
 106                                 break;
 107                 }
 108                 s++;
 109         }
 110         return num_tokens;
 111 }
 112
 113 static double nmea_parser_get_latitude(const char *lat, const char *bearing)
 114 {
 115         double latitude = 0.0;
 116         int ns;
 117         int deg;
 118         double remainder;
 119
 120         if ((*lat == 0) || (*bearing == 0)) {
 121                 return latitude;
 122         }
 123
 124         ns = (*bearing == 'N') ? NORTH : SOUTH;
 125
 126         latitude = atof(lat);
 127         deg = (int)(latitude / 100.0);
 128         remainder = latitude - (deg * 100.0);
 129         latitude = (deg + (remainder / DECIMAL_TO_DEGREE)) * ns;
 130
 131         return latitude;
 132 }
 133
 134 static double nmea_parser_get_longitude(const char *lon, const char *bearing)
 135 {
 136         double longitude = 0.0;
 137         int ew;
 138         int deg;
 139         double remainder;
 140
 141         if (*lon == 0 || (*bearing == 0)) {
 142                 return longitude;
 143         }
 144
 145         ew = (*bearing == 'E') ? EAST : WEST;
 146
 147         longitude = atof(lon);
 148         deg = (int)(longitude / 100.0);
 149         remainder = longitude - (deg * 100.0);
 150         longitude = (deg + (remainder / DECIMAL_TO_DEGREE)) * ew;
 151
 152         return longitude;
 153 }
 154
 155 static double nmea_parser_get_altitude(const char *alt, const char *unit)
 156 {
 157         double altitude;
 158
 159         if (*alt == 0) {
 160                 return 0.0;
 161         }
 162
 163         altitude = atof(alt);
 164         altitude = (*unit == 'M') ? altitude : altitude * METER_TO_FEET;
 165
 166         return altitude;
 167 }
 168
 169 static int nmea_parser_gpgga(char *token[], pos_data_t *pos, sv_data_t *sv)
 170 {
 171         double latitude, longitude, altitude;
 172         int quality;
 173
 174         quality = atoi(token[6]);
 175
 176         if (quality == 0) {
 177                 LOG_PLUGIN(DBG_LOW, "Not fixed");
 178                 sv->pos_valid = FALSE;
 179                 return READ_NOT_FIXED;
 180         }
 181
 182         /*      utctime = atoi(token[1]); */
 183         latitude = nmea_parser_get_latitude(token[2], token[3]);
 184         longitude = nmea_parser_get_longitude(token[4], token[5]);
 185         altitude = nmea_parser_get_altitude(token[9], token[10]);
 186         /*      num_of_sat_used = atoi(token[7]); */
 187         /*      eph = atof(token[8]); */
 188         /*      geoid = nmea_parser_get_altitude(token[11], token[12]); */
 189
 190         pos->latitude = latitude;
 191         pos->longitude = longitude;
 192         pos->altitude = altitude;
 193
 194         sv->pos_valid = TRUE;
 195
 196         return READ_SUCCESS;
 197 }
 198
 199 static int nmea_parser_gprmc(char *token[], pos_data_t *pos)
 200 {
 201         char *status;
 202         double latitude, longitude, speed, bearing;
 203
 204         status = token[2];      /*warn = *token[2]; */
 205         if (strcmp(status, "V") == 0) {
 206                 LOG_PLUGIN(DBG_LOW, "Not fixed");
 207                 return READ_NOT_FIXED;
 208         }
 209
 210         /*      utctime = atoi(token[1]); */
 211         latitude = nmea_parser_get_latitude(token[3], token[4]);
 212         longitude = nmea_parser_get_longitude(token[5], token[6]);
 213         speed = atof(token[7]);
 214         bearing = atof(token[8]);
 215         /*      date = atoi(token[9]); */
 216         /*      magvar = atof(token[10]); */
 217
 218         pos->latitude = latitude;
 219         pos->longitude = longitude;
 220         pos->speed = speed * KNOT_TO_MPS;
 221         pos->bearing = bearing;
 222
 223         return READ_SUCCESS;
 224 }
 225
 226 static int nmea_parser_gpgll(char *token[], pos_data_t *pos)
 227 {
 228         char *status;
 229         double latitude, longitude;
 230
 231         status = token[6];      /*warn = *token[2]; */
 232         if (strcmp(status, "V") == 0) {
 233                 LOG_PLUGIN(DBG_LOW, "Not fixed");
 234                 return READ_NOT_FIXED;
 235         }
 236
 237         latitude = nmea_parser_get_latitude(token[1], token[2]);
 238         longitude = nmea_parser_get_longitude(token[3], token[4]);
 239
 240         pos->latitude = latitude;
 241         pos->longitude = longitude;
 242
 243         return READ_SUCCESS;
 244 }
 245
 246 static int nmea_parser_gpgsa(char *token[], pos_data_t *pos)
 247 {
 248         int i, fix_type;
 249
 250         fix_type = atoi(token[2]);
 251         if (fix_type == 1) {
 252                 LOG_PLUGIN(DBG_LOW, "Not fixed");
 253                 return READ_NOT_FIXED;
 254         }
 255
 256         /*      selection_type = *token[1]; */
 257
 258         memset(used_sat, 0, sizeof(used_sat));
 259         for (i = 0; i < MAX_GPS_NUM_SAT_USED; i++) {
 260                 used_sat[i] = atoi(token[i + 3]);
 261         }
 262
 263         /*      pdop = atof(token[15]); */
 264         /*      hdop = atof(token[16]); */
 265         /*      vdop = atof(token[17]); */
 266
 267         return READ_SUCCESS;
 268 }
 269
 270 static int nmea_parser_gpvtg(char *token[], pos_data_t *pos)
 271 {
 272         double true_course, kmh_speed;
 273
 274         true_course = atof(token[1]);
 275         /*      magnetic_course = atof(token[3]); */
 276         /*      knot_speed = atof(token[5]); */
 277         kmh_speed = atof(token[7]);
 278
 279         pos->speed = kmh_speed * KMPH_TO_MPS;
 280         pos->bearing = true_course;
 281
 282         return READ_SUCCESS;
 283 }
 284
 285 static int nmea_parser_gpgsv(char *token[], sv_data_t *sv)
 286 {
 287         int i, j;
 288         int p, q, iter;
 289         int msg_num, num_sv;
 290
 291         /*      num_sen = atoi(token[1]); */
 292         msg_num = atoi(token[2]);
 293         if (msg_num < 1) {
 294                 LOG_PLUGIN(DBG_LOW, "There is not GSV message");
 295                 return READ_ERROR;
 296         }
 297
 298         num_sv = atoi(token[3]);
 299         sv->num_of_sat = num_sv;
 300         iter = ((num_sv < (msg_num * 4)) ? (num_sv - ((msg_num - 1) * 4)) : 4);
 301         for (i = 0; i < iter; i++) {
 302                 q = (i + 1) * 4;
 303                 p = i + 4 * (msg_num - 1);
 304                 sv->sat[p].prn = atoi(token[q]);
 305                 for (j = 0; j < MAX_GPS_NUM_SAT_USED; j++) {
 306                         if (sv->sat[p].prn == used_sat[j]) {
 307                                 sv->sat[p].used = 1;
 308                                 break;
 309                         } else {
 310                                 sv->sat[p].used = 0;
 311                         }
 312                 }
 313                 sv->sat[p].elevation = atoi(token[q + 1]);
 314                 sv->sat[p].azimuth = atoi(token[q + 2]);
 315                 sv->sat[p].snr = atoi(token[q + 3]);
 316         }
 317         return READ_SUCCESS;
 318 }
 319
 320 int nmea_parser_sentence(char *sentence, char *token[], pos_data_t *pos, sv_data_t *sv)
 321 {
 322         int ret = READ_SUCCESS;
 323         if (strcmp(sentence, "GPGGA") == 0) {
 324                 ret = nmea_parser_gpgga(token, pos, sv);
 325         } else if (strcmp(sentence, "GPRMC") == 0) {
 326                 ret = nmea_parser_gprmc(token, pos);
 327         } else if (strcmp(sentence, "GPGLL") == 0) {
 328                 ret = nmea_parser_gpgll(token, pos);
 329         } else if (strcmp(sentence, "GPGSA") == 0) {
 330                 ret = nmea_parser_gpgsa(token, pos);
 331         } else if (strcmp(sentence, "GPVTG") == 0) {
 332                 ret = nmea_parser_gpvtg(token, pos);
 333         } else if (strcmp(sentence, "GPGSV") == 0) {
 334                 ret = nmea_parser_gpgsv(token, sv);
 335         } else {
 336                 LOG_PLUGIN(DBG_LOW, "Unsupported sentence : [%s]\n", sentence);
 337         }
 338
 339         return ret;
 340 }
 341
 342 int nmea_parser(char *data, pos_data_t *pos, sv_data_t *sv)
 343 {
 344         int ret = READ_SUCCESS;
 345         read_error_t err;
 346         int num_sen = 0;
 347         int count = 0;
 348         char *last = NULL;
 349         char *nmea_sen[MAX_NMEA_SENTENCES] = { 0, };
 350         char *token[MAX_TOEKNS] = { 0, };
 351
 352         nmea_sen[num_sen] = (char *)strtok_r((char *)data, "$", &last);
 353         while (nmea_sen[num_sen] != NULL) {
 354                 num_sen++;
 355                 nmea_sen[num_sen] = (char *)strtok_r(NULL, "$", &last);
 356         }
 357         LOG_PLUGIN(DBG_LOW, "Number of NMEA sentences:%d \n", num_sen);
 358
 359         while (num_sen > 0) {
 360                 if (nmea_parser_verify_checksum(nmea_sen[count]) == 0) {
 361                         nmea_parser_tokenize(nmea_sen[count], token);
 362                         err = nmea_parser_sentence(token[0], token, pos, sv);
 363                         if (err == READ_NOT_FIXED) {
 364                                 LOG_PLUGIN(DBG_LOW, "NOT Fixed");
 365                                 ret = err;
 366                         } else if (err == READ_ERROR) {
 367                                 ret = err;
 368                         }
 369                 } else {
 370                         LOG_PLUGIN(DBG_ERR, "[NMEA Parser] %dth sentense : Invalid Checksum\n", count);
 371                 }
 372                 count++;
 373                 num_sen--;
 374         }
 375
 376         return ret;
 377 }