replay-plugin/src/nmea_parser.c

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