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_gpgsa(char *token[], pos_data_t * pos)
 231 {
 232         char selection_type;
 233         int i, fix_type;
 234         double pdop, hdop, vdop;
 235
 236         fix_type = atoi(token[2]);
 237         if (fix_type == 1) {
 238                 LOG_PLUGIN(DBG_LOW, "Not fixed");
 239                 return READ_NOT_FIXED;
 240         }
 241
 242         selection_type = *token[1];
 243
 244         memset(used_sat, 0, sizeof(used_sat));
 245         for (i = 0; i < MAX_GPS_NUM_SAT_USED; i++) {
 246                 used_sat[i] = atoi(token[i + 3]);
 247         }
 248
 249         pdop = atof(token[15]);
 250         hdop = atof(token[16]);
 251         vdop = atof(token[17]);
 252
 253         return READ_SUCCESS;
 254 }
 255
 256 static int nmea_parser_gpvtg(char *token[], pos_data_t * pos)
 257 {
 258         double true_course, magnetic_course, knot_speed, kmh_speed;
 259
 260         true_course = atof(token[1]);
 261         magnetic_course = atof(token[3]);
 262         knot_speed = atof(token[5]);
 263         kmh_speed = atof(token[7]);
 264
 265         pos->speed = kmh_speed * KMPH_TO_MPS;
 266         pos->bearing = true_course;
 267
 268         return READ_SUCCESS;
 269 }
 270
 271 static int nmea_parser_gpgsv(char *token[], sv_data_t * sv)
 272 {
 273         int i, j;
 274         int p, q, iter;
 275         int num_sen, msg_num, num_sv;
 276
 277         num_sen = atoi(token[1]);
 278         msg_num = atoi(token[2]);
 279         if (msg_num < 1) {
 280                 LOG_PLUGIN(DBG_LOW, "There is not GSV message");
 281                 return READ_ERROR;
 282         }
 283
 284         num_sv = atoi(token[3]);
 285         sv->num_of_sat = num_sv;
 286         iter = ((num_sv < (msg_num * 4)) ? (num_sv - ((msg_num - 1) * 4)) : 4);
 287         for (i = 0; i < iter; i++) {
 288                 q = (i + 1) * 4;
 289                 p = i + 4 * (msg_num - 1);
 290                 sv->sat[p].prn = atoi(token[q]);
 291                 for (j = 0; j < MAX_GPS_NUM_SAT_USED; j++) {
 292                         if (sv->sat[p].prn == used_sat[j]) {
 293                                 sv->sat[p].used = 1;
 294                                 break;
 295                         } else {
 296                                 sv->sat[p].used = 0;
 297                         }
 298                 }
 299                 sv->sat[p].elevation = atoi(token[q + 1]);
 300                 sv->sat[p].azimuth = atoi(token[q + 2]);
 301                 sv->sat[p].snr = atoi(token[q + 3]);
 302         }
 303         return READ_SUCCESS;
 304 }
 305
 306 int nmea_parser_sentence(char *sentence, char *token[], pos_data_t * pos, sv_data_t * sv)
 307 {
 308         int ret;
 309         if (strcmp(sentence, "GPGGA") == 0) {
 310                 ret = nmea_parser_gpgga(token, pos, sv);
 311         } else if (strcmp(sentence, "GPRMC") == 0) {
 312                 ret = nmea_parser_gprmc(token, pos);
 313         } else if (strcmp(sentence, "GPGSA") == 0) {
 314                 ret = nmea_parser_gpgsa(token, pos);
 315         } else if (strcmp(sentence, "GPVTG") == 0) {
 316                 ret = nmea_parser_gpvtg(token, pos);
 317         } else if (strcmp(sentence, "GPGSV") == 0) {
 318                 ret = nmea_parser_gpgsv(token, sv);
 319         } else {
 320                 LOG_PLUGIN(DBG_LOW, "Unsupported sentence : [%s]\n", sentence);
 321         }
 322
 323         return ret;
 324 }
 325
 326 int nmea_parser(char *data, pos_data_t * pos, sv_data_t * sv)
 327 {
 328         int ret = READ_SUCCESS;
 329         read_error_t err;
 330         int num_sen = 0;
 331         int count = 0;
 332         char *last = NULL;
 333         char *nmea_sen[MAX_NMEA_SENTENCES] = { 0, };
 334         char *token[MAX_TOEKNS] = { 0, };
 335
 336         nmea_sen[num_sen] = (char *)strtok_r((char *)data, "$", &last);
 337         while (nmea_sen[num_sen] != NULL) {
 338                 num_sen++;
 339                 nmea_sen[num_sen] = (char *)strtok_r(NULL, "$", &last);
 340         }
 341         LOG_PLUGIN(DBG_LOW, "Number of NMEA sentences:%d \n", num_sen);
 342
 343         while (num_sen > 0) {
 344                 if (nmea_parser_verify_checksum(nmea_sen[count]) == 0) {
 345                         nmea_parser_tokenize(nmea_sen[count], token);
 346                         err = nmea_parser_sentence(token[0], token, pos, sv);
 347                         if (err == READ_NOT_FIXED) {
 348                                 LOG_PLUGIN(DBG_LOW, "NOT Fixed");
 349                                 ret = err;
 350                         } else if (ret == READ_ERROR) {
 351                                 ret = err;
 352                                 break;
 353                         }
 354                 } else {
 355                         LOG_PLUGIN(DBG_ERR, "[NMEA Parser] %dth sentense : Invalid Checksum\n", count);
 356                 }
 357                 count++;
 358                 num_sen--;
 359         }
 360
 361         return ret;
 362 }