replay-plugin/src/nmea_parser.c

   1 /*
   2  * gps-manager 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_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                 }
 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, eph, geoid;
 172         int utctime, num_of_sat_used, 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         int date, utctime;
 202         char *status;
 203         double latitude, longitude, speed, bearing, magvar;
 204
 205         status = token[2];      //warn = *token[2];
 206         if (strcmp(status, "V") == 0) {
 207                 LOG_PLUGIN(DBG_LOW, "Not fixed");
 208                 return READ_NOT_FIXED;
 209         }
 210
 211         utctime = atoi(token[1]);
 212         latitude = nmea_parser_get_latitude(token[3], token[4]);
 213         longitude = nmea_parser_get_longitude(token[5], token[6]);
 214         speed = atof(token[7]);
 215         bearing = atof(token[8]);
 216         date = atoi(token[9]);
 217         magvar = atof(token[10]);
 218
 219         pos->latitude = latitude;
 220         pos->longitude = longitude;
 221         pos->speed = speed * KNOT_TO_MPS;
 222         pos->bearing = bearing;
 223
 224         return READ_SUCCESS;
 225 }
 226
 227 static int nmea_parser_gpgll(char *token[], pos_data_t * pos)
 228 {
 229         char *status;
 230         double latitude, longitude;
 231
 232         status = token[6];      //warn = *token[2];
 233         if (strcmp(status, "V") == 0) {
 234                 LOG_PLUGIN(DBG_LOW, "Not fixed");
 235                 return READ_NOT_FIXED;
 236         }
 237
 238         latitude = nmea_parser_get_latitude(token[1], token[2]);
 239         longitude = nmea_parser_get_longitude(token[3], token[4]);
 240
 241         pos->latitude = latitude;
 242         pos->longitude = longitude;
 243
 244         return READ_SUCCESS;
 245 }
 246
 247 static int nmea_parser_gpgsa(char *token[], pos_data_t * pos)
 248 {
 249         char selection_type;
 250         int i, fix_type;
 251         double pdop, hdop, vdop;
 252
 253         fix_type = atoi(token[2]);
 254         if (fix_type == 1) {
 255                 LOG_PLUGIN(DBG_LOW, "Not fixed");
 256                 return READ_NOT_FIXED;
 257         }
 258
 259         selection_type = *token[1];
 260
 261         memset(used_sat, 0, sizeof(used_sat));
 262         for (i = 0; i < MAX_GPS_NUM_SAT_USED; i++) {
 263                 used_sat[i] = atoi(token[i + 3]);
 264         }
 265
 266         pdop = atof(token[15]);
 267         hdop = atof(token[16]);
 268         vdop = atof(token[17]);
 269
 270         return READ_SUCCESS;
 271 }
 272
 273 static int nmea_parser_gpvtg(char *token[], pos_data_t * pos)
 274 {
 275         double true_course, magnetic_course, knot_speed, kmh_speed;
 276
 277         true_course = atof(token[1]);
 278         magnetic_course = atof(token[3]);
 279         knot_speed = atof(token[5]);
 280         kmh_speed = atof(token[7]);
 281
 282         pos->speed = kmh_speed * KMPH_TO_MPS;
 283         pos->bearing = true_course;
 284
 285         return READ_SUCCESS;
 286 }
 287
 288 static int nmea_parser_gpgsv(char *token[], sv_data_t * sv)
 289 {
 290         int i, j;
 291         int p, q, iter;
 292         int num_sen, msg_num, num_sv;
 293
 294         num_sen = atoi(token[1]);
 295         msg_num = atoi(token[2]);
 296         if (msg_num < 1) {
 297                 LOG_PLUGIN(DBG_LOW, "There is not GSV message");
 298                 return READ_ERROR;
 299         }
 300
 301         num_sv = atoi(token[3]);
 302         sv->num_of_sat = num_sv;
 303         iter = ((num_sv < (msg_num * 4)) ? (num_sv - ((msg_num - 1) * 4)) : 4);
 304         for (i = 0; i < iter; i++) {
 305                 q = (i + 1) * 4;
 306                 p = i + 4 * (msg_num - 1);
 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_ERROR;
 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                                 break;
 372                         }
 373                 } else {
 374                         LOG_PLUGIN(DBG_ERR, "[NMEA Parser] %dth sentense : Invalid Checksum\n", count);
 375                 }
 376                 count++;
 377                 num_sen--;
 378         }
 379
 380         return ret;
 381 }