2 * GPS manager replay plugin
4 * Copyright (c) 2011 Samsung Electronics Co., Ltd. All rights reserved.
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>
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
13 * http://www.apache.org/licenses/LICENSE-2.0
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.
26 #include <gps_manager_plugin_intf.h>
28 #include "gps_plugin_debug.h"
29 #include "nmea_parser.h"
32 #define MAX_NMEA_SENTENCES 32
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
45 int used_sat[MAX_GPS_NUM_SAT_USED] = { 0, };
47 static char nmea_parser_c2n(char ch)
52 return (ch - 'A') + 10;
56 int nmea_parser_verify_checksum(char *nmea_sen)
63 for (i = 0; i < strlen(nmea_sen) && (nmea_sen[i] != '*'); i++) {
64 checksum ^= nmea_sen[i];
67 if (++i + 1 < strlen(nmea_sen)) {
68 sum = (nmea_parser_c2n(nmea_sen[i]) << 4) + nmea_parser_c2n(nmea_sen[i + 1]);
71 if (sum == checksum) {
74 LOG_PLUGIN(DBG_LOW, "NMEA checksum is INVALID");
80 int nmea_parser_tokenize(char input[], char *token[])
86 token[num_tokens] = s;
89 LOG_PLUGIN(DBG_LOW, "input:%s \n", input);
91 while ((*s != 0) && (num_tokens < MAX_TOEKNS)) {
100 token[num_tokens++] = s;
116 static double nmea_parser_get_latitude(const char *lat, const char *bearing)
118 double latitude = 0.0;
123 if ((*lat == 0) || (*bearing == 0)) {
127 ns = (*bearing == 'N') ? NORTH : SOUTH;
129 latitude = atof(lat);
130 deg = (int)(latitude / 100.0);
131 remainder = latitude - (deg * 100.0);
132 latitude = (deg + (remainder / DECIMAL_TO_DEGREE)) * ns;
137 static double nmea_parser_get_longitude(const char *lon, const char *bearing)
139 double longitude = 0.0;
144 if (*lon == 0 || (*bearing == 0)) {
148 ew = (*bearing == 'E') ? EAST : WEST;
150 longitude = atof(lon);
151 deg = (int)(longitude / 100.0);
152 remainder = longitude - (deg * 100.0);
153 longitude = (deg + (remainder / DECIMAL_TO_DEGREE)) * ew;
158 static double nmea_parser_get_altitude(const char *alt, const char *unit)
166 altitude = atof(alt);
167 altitude = (*unit == 'M') ? altitude : altitude * METER_TO_FEET;
172 static int nmea_parser_gpgga(char *token[], pos_data_t * pos, sv_data_t * sv)
174 double latitude, longitude, altitude, eph, geoid;
175 int utctime, num_of_sat_used, quality;
177 quality = atoi(token[6]);
180 LOG_PLUGIN(DBG_LOW, "Not fixed");
181 sv->pos_valid = FALSE;
182 return READ_NOT_FIXED;
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]);
193 pos->latitude = latitude;
194 pos->longitude = longitude;
195 pos->altitude = altitude;
197 sv->pos_valid = TRUE;
202 static int nmea_parser_gprmc(char *token[], pos_data_t * pos)
206 double latitude, longitude, speed, bearing, magvar;
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;
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]);
222 pos->latitude = latitude;
223 pos->longitude = longitude;
224 pos->speed = speed * KNOT_TO_MPS;
225 pos->bearing = bearing;
230 static int nmea_parser_gpgll(char *token[], pos_data_t * pos)
233 double latitude, longitude;
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;
241 latitude = nmea_parser_get_latitude(token[1], token[2]);
242 longitude = nmea_parser_get_longitude(token[3], token[4]);
244 pos->latitude = latitude;
245 pos->longitude = longitude;
250 static int nmea_parser_gpgsa(char *token[], pos_data_t * pos)
254 double pdop, hdop, vdop;
256 fix_type = atoi(token[2]);
258 LOG_PLUGIN(DBG_LOW, "Not fixed");
259 return READ_NOT_FIXED;
262 selection_type = *token[1];
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]);
269 pdop = atof(token[15]);
270 hdop = atof(token[16]);
271 vdop = atof(token[17]);
276 static int nmea_parser_gpvtg(char *token[], pos_data_t * pos)
278 double true_course, magnetic_course, knot_speed, kmh_speed;
280 true_course = atof(token[1]);
281 magnetic_course = atof(token[3]);
282 knot_speed = atof(token[5]);
283 kmh_speed = atof(token[7]);
285 pos->speed = kmh_speed * KMPH_TO_MPS;
286 pos->bearing = true_course;
291 static int nmea_parser_gpgsv(char *token[], sv_data_t * sv)
295 int num_sen, msg_num, num_sv;
297 num_sen = atoi(token[1]);
298 msg_num = atoi(token[2]);
300 LOG_PLUGIN(DBG_LOW, "There is not GSV message");
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++) {
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]) {
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]);
326 int nmea_parser_sentence(char *sentence, char *token[], pos_data_t * pos, sv_data_t * sv)
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);
342 LOG_PLUGIN(DBG_LOW, "Unsupported sentence : [%s]\n", sentence);
348 int nmea_parser(char *data, pos_data_t * pos, sv_data_t * sv)
350 int ret = READ_SUCCESS;
355 char *nmea_sen[MAX_NMEA_SENTENCES] = { 0, };
356 char *token[MAX_TOEKNS] = { 0, };
358 nmea_sen[num_sen] = (char *)strtok_r((char *)data, "$", &last);
359 while (nmea_sen[num_sen] != NULL) {
361 nmea_sen[num_sen] = (char *)strtok_r(NULL, "$", &last);
363 LOG_PLUGIN(DBG_LOW, "Number of NMEA sentences:%d \n", num_sen);
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");
372 } else if (ret == READ_ERROR) {
377 LOG_PLUGIN(DBG_ERR, "[NMEA Parser] %dth sentense : Invalid Checksum\n", count);