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 unsigned char nmea_parser_c2n(unsigned char ch)
  48 {
  49         if (ch <= '9')
  50                 return ch - '0';
  51         else
  52                 return (ch - 'A') + 10;
  53 }
  54
  55 int nmea_parser_verify_checksum(char *nmea_sen)
  56 {
  57         int ret = -1;
  58         int i;
  59         int checksum = 0;
  60         int sum = 0;
  61
  62         for (i = 0; i < strlen(nmea_sen) && (nmea_sen[i] != '*'); i++)
  63                 checksum ^= nmea_sen[i];
  64
  65         if (++i + 1 < strlen(nmea_sen))
  66                 sum = (nmea_parser_c2n(nmea_sen[i]) << 4) + nmea_parser_c2n(nmea_sen[i + 1]);
  67
  68         if (sum == checksum) {
  69                 ret = 0;
  70         } else {
  71                 LOG_PLUGIN(DBG_LOW, "NMEA checksum is INVALID");
  72                 ret = -1;
  73         }
  74         return ret;
  75 }
  76
  77 int nmea_parser_tokenize(char input[], char *token[])
  78 {
  79         char *s = input;
  80         int num_tokens = 0;
  81         int state;
  82
  83         token[num_tokens] = s;
  84         num_tokens++;
  85         state = 0;
  86         /* LOG_PLUGIN(DBG_LOW, "input:%s \n", input); */
  87
  88         while ((*s != 0) && (num_tokens < MAX_TOEKNS)) {
  89                 switch (state) {
  90                 case 0:
  91                         if (*s == ',') {
  92                                 *s = 0;
  93                                 state = 1;
  94                         }
  95                         break;
  96                 case 1:
  97                         token[num_tokens++] = s;
  98                         if (*s == ',')
  99                                 *s = 0;
 100                         else
 101                                 state = 0;
 102
 103                         break;
 104                 }
 105                 s++;
 106         }
 107         return num_tokens;
 108 }
 109
 110 static double nmea_parser_get_latitude(const char *lat, const char *bearing)
 111 {
 112         double latitude = 0.0;
 113         int ns;
 114         int deg;
 115         double remainder;
 116
 117         if ((*lat == 0) || (*bearing == 0))
 118                 return latitude;
 119
 120         ns = (*bearing == 'N') ? NORTH : SOUTH;
 121
 122         latitude = atof(lat);
 123         deg = (int)(latitude / 100.0);
 124         remainder = latitude - (deg * 100.0);
 125         latitude = (deg + (remainder / DECIMAL_TO_DEGREE)) * ns;
 126
 127         return latitude;
 128 }
 129
 130 static double nmea_parser_get_longitude(const char *lon, const char *bearing)
 131 {
 132         double longitude = 0.0;
 133         int ew;
 134         int deg;
 135         double remainder;
 136
 137         if (*lon == 0 || (*bearing == 0))
 138                 return longitude;
 139
 140         ew = (*bearing == 'E') ? EAST : WEST;
 141
 142         longitude = atof(lon);
 143         deg = (int)(longitude / 100.0);
 144         remainder = longitude - (deg * 100.0);
 145         longitude = (deg + (remainder / DECIMAL_TO_DEGREE)) * ew;
 146
 147         return longitude;
 148 }
 149
 150 static double nmea_parser_get_altitude(const char *alt, const char *unit)
 151 {
 152         double altitude;
 153
 154         if (*alt == 0)
 155                 return 0.0;
 156
 157         altitude = atof(alt);
 158         altitude = (*unit == 'M') ? altitude : altitude * METER_TO_FEET;
 159
 160         return altitude;
 161 }
 162
 163 static int nmea_parser_gpgga(char *token[], pos_data_t *pos, sv_data_t *sv)
 164 {
 165         double latitude, longitude, altitude;
 166         int quality;
 167
 168         quality = atoi(token[6]);
 169
 170         if (quality == 0) {
 171                 LOG_PLUGIN(DBG_LOW, "Not fixed");
 172                 sv->pos_valid = FALSE;
 173                 return READ_NOT_FIXED;
 174         }
 175
 176         /*      utctime = atoi(token[1]); */
 177         latitude = nmea_parser_get_latitude(token[2], token[3]);
 178         longitude = nmea_parser_get_longitude(token[4], token[5]);
 179         altitude = nmea_parser_get_altitude(token[9], token[10]);
 180         /*      num_of_sat_used = atoi(token[7]); */
 181         /*      eph = atof(token[8]); */
 182         /*      geoid = nmea_parser_get_altitude(token[11], token[12]); */
 183
 184         pos->latitude = latitude;
 185         pos->longitude = longitude;
 186         pos->altitude = altitude;
 187
 188         sv->pos_valid = TRUE;
 189
 190         return READ_SUCCESS;
 191 }
 192
 193 static int nmea_parser_gprmc(char *token[], pos_data_t *pos)
 194 {
 195         char *status;
 196         double latitude, longitude, speed, bearing;
 197
 198         status = token[2];      /*warn = *token[2]; */
 199         if (strcmp(status, "V") == 0) {
 200                 /* LOG_PLUGIN(DBG_LOW, "Not fixed"); */
 201                 return READ_NOT_FIXED;
 202         }
 203
 204         /*      utctime = atoi(token[1]); */
 205         latitude = nmea_parser_get_latitude(token[3], token[4]);
 206         longitude = nmea_parser_get_longitude(token[5], token[6]);
 207         speed = atof(token[7]);
 208         bearing = atof(token[8]);
 209         /*      date = atoi(token[9]); */
 210         /*      magvar = atof(token[10]); */
 211
 212         pos->latitude = latitude;
 213         pos->longitude = longitude;
 214         pos->speed = speed * KNOT_TO_MPS;
 215         pos->bearing = bearing;
 216
 217         return READ_SUCCESS;
 218 }
 219
 220 static int nmea_parser_gpgll(char *token[], pos_data_t *pos)
 221 {
 222         char *status;
 223         double latitude, longitude;
 224
 225         status = token[6];      /*warn = *token[2]; */
 226         if (strcmp(status, "V") == 0) {
 227                 /* LOG_PLUGIN(DBG_LOW, "Not fixed"); */
 228                 return READ_NOT_FIXED;
 229         }
 230
 231         latitude = nmea_parser_get_latitude(token[1], token[2]);
 232         longitude = nmea_parser_get_longitude(token[3], token[4]);
 233
 234         pos->latitude = latitude;
 235         pos->longitude = longitude;
 236
 237         return READ_SUCCESS;
 238 }
 239
 240 static int nmea_parser_gpgsa(char *token[], pos_data_t *pos)
 241 {
 242         int i, fix_type;
 243
 244         fix_type = atoi(token[2]);
 245         if (fix_type == 1) {
 246                 /* LOG_PLUGIN(DBG_LOW, "Not fixed"); */
 247                 return READ_NOT_FIXED;
 248         }
 249
 250         /*      selection_type = *token[1]; */
 251
 252         memset(used_sat, 0, sizeof(used_sat));
 253         for (i = 0; i < MAX_GPS_NUM_SAT_USED; i++)
 254                 used_sat[i] = atoi(token[i + 3]);
 255
 256
 257         /*      pdop = atof(token[15]); */
 258         /*      hdop = atof(token[16]); */
 259         /*      vdop = atof(token[17]); */
 260
 261         return READ_SUCCESS;
 262 }
 263
 264 static int nmea_parser_gpvtg(char *token[], pos_data_t *pos)
 265 {
 266         double true_course, kmh_speed;
 267
 268         true_course = atof(token[1]);
 269         /*      magnetic_course = atof(token[3]); */
 270         /*      knot_speed = atof(token[5]); */
 271         kmh_speed = atof(token[7]);
 272
 273         pos->speed = kmh_speed * KMPH_TO_MPS;
 274         pos->bearing = true_course;
 275
 276         return READ_SUCCESS;
 277 }
 278
 279 static int nmea_parser_gpgsv(char *token[], sv_data_t *sv)
 280 {
 281         int i, j;
 282         int p, q, iter;
 283         int msg_num, num_sv;
 284
 285         /*      num_sen = atoi(token[1]); */
 286         msg_num = atoi(token[2]);
 287         if (msg_num < 1) {
 288                 LOG_PLUGIN(DBG_LOW, "There is not GSV message");
 289                 return READ_ERROR;
 290         }
 291
 292         num_sv = atoi(token[3]);
 293         if (num_sv > MAX_GPS_NUM_SAT_IN_VIEW) {
 294                 LOG_PLUGIN(DBG_LOW, "num_of_sat(num_sv) size error");
 295                 return READ_ERROR;
 296         }
 297
 298         sv->num_of_sat = num_sv;
 299         iter = ((num_sv < (msg_num * 4)) ? (num_sv - ((msg_num - 1) * 4)) : 4);
 300         for (i = 0; i < iter; i++) {
 301                 q = (i + 1) * 4;
 302                 p = i + 4 * (msg_num - 1);
 303                 if (p > 31) {
 304                         LOG_PLUGIN(DBG_LOW, "Out of bounds");
 305                         return READ_ERROR;
 306                 }
 307                 sv->sat[p].prn = atoi(token[q]);
 308                 for (j = 0; j < MAX_GPS_NUM_SAT_USED; j++) {
 309                         if (sv->sat[p].prn == used_sat[j]) {
 310                                 sv->sat[p].used = 1;
 311                                 break;
 312                         } else {
 313                                 sv->sat[p].used = 0;
 314                         }
 315                 }
 316                 sv->sat[p].elevation = atoi(token[q + 1]);
 317                 sv->sat[p].azimuth = atoi(token[q + 2]);
 318                 sv->sat[p].snr = atoi(token[q + 3]);
 319         }
 320         return READ_SUCCESS;
 321 }
 322
 323 int nmea_parser_sentence(char *sentence, char *token[], pos_data_t *pos, sv_data_t *sv)
 324 {
 325         int ret = READ_SUCCESS;
 326         if (strcmp(sentence, "GPGGA") == 0)
 327                 ret = nmea_parser_gpgga(token, pos, sv);
 328         else if (strcmp(sentence, "GPRMC") == 0)
 329                 ret = nmea_parser_gprmc(token, pos);
 330         else if (strcmp(sentence, "GPGLL") == 0)
 331                 ret = nmea_parser_gpgll(token, pos);
 332         else if (strcmp(sentence, "GPGSA") == 0)
 333                 ret = nmea_parser_gpgsa(token, pos);
 334         else if (strcmp(sentence, "GPVTG") == 0)
 335                 ret = nmea_parser_gpvtg(token, pos);
 336         else if (strcmp(sentence, "GPGSV") == 0)
 337                 ret = nmea_parser_gpgsv(token, sv);
 338         else
 339                 LOG_PLUGIN(DBG_LOW, "Unsupported sentence : [%s]\n", sentence);
 340
 341
 342         return ret;
 343 }
 344
 345 int nmea_parser(char *data, pos_data_t *pos, sv_data_t *sv)
 346 {
 347         int ret = READ_SUCCESS;
 348         read_error_t err;
 349         int num_sen = 0;
 350         int count = 0;
 351         char *last = NULL;
 352         char *nmea_sen[MAX_NMEA_SENTENCES] = { 0, };
 353         char *token[MAX_TOEKNS] = { 0, };
 354
 355         nmea_sen[num_sen] = (char *)strtok_r((char *)data, "$", &last);
 356         while (nmea_sen[num_sen] != NULL) {
 357                 num_sen++;
 358                 nmea_sen[num_sen] = (char *)strtok_r(NULL, "$", &last);
 359         }
 360         /* LOG_PLUGIN(DBG_LOW, "Number of NMEA sentences:%d \n", num_sen); */
 361
 362         while (num_sen > 0) {
 363                 if (nmea_parser_verify_checksum(nmea_sen[count]) == 0) {
 364                         nmea_parser_tokenize(nmea_sen[count], token);
 365                         err = nmea_parser_sentence(token[0], token, pos, sv);
 366                         if (err == READ_NOT_FIXED) {
 367                                 /* LOG_PLUGIN(DBG_LOW, "NOT Fixed"); */
 368                                 ret = err;
 369                         } else if (err == READ_ERROR) {
 370                                 ret = err;
 371                         }
 372                 } else {
 373                         LOG_PLUGIN(DBG_ERR, "[NMEA Parser] %dth sentense : Invalid Checksum\n", count);
 374                 }
 375                 count++;
 376                 num_sen--;
 377         }
 378
 379         return ret;
 380 }