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;
113 static double nmea_parser_get_latitude(const char *lat, const char *bearing)
115 double latitude = 0.0;
120 if ((*lat == 0) || (*bearing == 0)) {
124 ns = (*bearing == 'N') ? NORTH : SOUTH;
126 latitude = atof(lat);
127 deg = (int)(latitude / 100.0);
128 remainder = latitude - (deg * 100.0);
129 latitude = (deg + (remainder / DECIMAL_TO_DEGREE)) * ns;
134 static double nmea_parser_get_longitude(const char *lon, const char *bearing)
136 double longitude = 0.0;
141 if (*lon == 0 || (*bearing == 0)) {
145 ew = (*bearing == 'E') ? EAST : WEST;
147 longitude = atof(lon);
148 deg = (int)(longitude / 100.0);
149 remainder = longitude - (deg * 100.0);
150 longitude = (deg + (remainder / DECIMAL_TO_DEGREE)) * ew;
155 static double nmea_parser_get_altitude(const char *alt, const char *unit)
163 altitude = atof(alt);
164 altitude = (*unit == 'M') ? altitude : altitude * METER_TO_FEET;
169 static int nmea_parser_gpgga(char *token[], pos_data_t * pos, sv_data_t * sv)
171 double latitude, longitude, altitude, eph, geoid;
172 int utctime, num_of_sat_used, quality;
174 quality = atoi(token[6]);
177 LOG_PLUGIN(DBG_LOW, "Not fixed");
178 sv->pos_valid = FALSE;
179 return READ_NOT_FIXED;
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]);
190 pos->latitude = latitude;
191 pos->longitude = longitude;
192 pos->altitude = altitude;
194 sv->pos_valid = TRUE;
199 static int nmea_parser_gprmc(char *token[], pos_data_t * pos)
203 double latitude, longitude, speed, bearing, magvar;
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;
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]);
219 pos->latitude = latitude;
220 pos->longitude = longitude;
221 pos->speed = speed * KNOT_TO_MPS;
222 pos->bearing = bearing;
227 static int nmea_parser_gpgll(char *token[], pos_data_t * pos)
230 double latitude, longitude;
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;
238 latitude = nmea_parser_get_latitude(token[1], token[2]);
239 longitude = nmea_parser_get_longitude(token[3], token[4]);
241 pos->latitude = latitude;
242 pos->longitude = longitude;
247 static int nmea_parser_gpgsa(char *token[], pos_data_t * pos)
251 double pdop, hdop, vdop;
253 fix_type = atoi(token[2]);
255 LOG_PLUGIN(DBG_LOW, "Not fixed");
256 return READ_NOT_FIXED;
259 selection_type = *token[1];
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]);
266 pdop = atof(token[15]);
267 hdop = atof(token[16]);
268 vdop = atof(token[17]);
273 static int nmea_parser_gpvtg(char *token[], pos_data_t * pos)
275 double true_course, magnetic_course, knot_speed, kmh_speed;
277 true_course = atof(token[1]);
278 magnetic_course = atof(token[3]);
279 knot_speed = atof(token[5]);
280 kmh_speed = atof(token[7]);
282 pos->speed = kmh_speed * KMPH_TO_MPS;
283 pos->bearing = true_course;
288 static int nmea_parser_gpgsv(char *token[], sv_data_t * sv)
292 int num_sen, msg_num, num_sv;
294 num_sen = atoi(token[1]);
295 msg_num = atoi(token[2]);
297 LOG_PLUGIN(DBG_LOW, "There is not GSV message");
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++) {
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]) {
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]);
323 int nmea_parser_sentence(char *sentence, char *token[], pos_data_t * pos, sv_data_t * sv)
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);
339 LOG_PLUGIN(DBG_LOW, "Unsupported sentence : [%s]\n", sentence);
345 int nmea_parser(char *data, pos_data_t * pos, sv_data_t * sv)
347 int ret = READ_SUCCESS;
352 char *nmea_sen[MAX_NMEA_SENTENCES] = { 0, };
353 char *token[MAX_TOEKNS] = { 0, };
355 nmea_sen[num_sen] = (char *)strtok_r((char *)data, "$", &last);
356 while (nmea_sen[num_sen] != NULL) {
358 nmea_sen[num_sen] = (char *)strtok_r(NULL, "$", &last);
360 LOG_PLUGIN(DBG_LOW, "Number of NMEA sentences:%d \n", num_sen);
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");
369 } else if (err == READ_ERROR) {
374 LOG_PLUGIN(DBG_ERR, "[NMEA Parser] %dth sentense : Invalid Checksum\n", count);