├── LICENSE ├── README.md └── gnsshal ├── Android.mk └── gps_zkw.c /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Jarod Lee 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 卫星定位接收机HAL驱动 2 | 3 | 适用于Android7或Android8 4 | 5 | 对于Android4, Android5, Android6, 请使用项目[android_hal_gpsbds](https://github.com/zxcwhale/android_hal_gpsbds). 6 | 7 | 对于Android9或Android10, 请使用项目[android9_gnss_hal_driver](https://github.com/zxcwhale/android9_gnss_hal_driver). 8 | ## 使用方法 9 | 10 | 1. 修改gps_zkw.c文件中的`GPS_CHANNEL_NAME`为接收机的TTY号. 11 | 2. 修改gps_zkw.c文件中的`TTY_BAUD`为接收机实际的波特率, 默认为`B9600`. 12 | 3. 在Android开发环境下编译. 13 | 4. 将编译结果gps.XXXX.so文件用adb push到Android设备的/system/lib/hw目录下. 如果是64位Android, 则push到/system/lib64/hw目录. 14 | 15 | ## 可能的问题 16 | 17 | 1. 如果编译出现找不到`ALOGD`, `ALOGE`的报错, 可以尝试将`ALOGD`改为`LOGD`, `ALOGE`改为`LOGE`. 18 | -------------------------------------------------------------------------------- /gnsshal/Android.mk: -------------------------------------------------------------------------------- 1 | #include $(all-subdir-makefiles) 2 | 3 | LOCAL_PATH := $(call my-dir) 4 | 5 | #ifeq ($(BOARD_GPS_LIBRARIES), libgps) 6 | 7 | #ifneq ($(TARGET_PRODUCT),sim) 8 | # HAL module implemenation, not prelinked and stored in 9 | # # hw/..so 10 | include $(CLEAR_VARS) 11 | 12 | LOCAL_PRELINK_MODULE := false 13 | LOCAL_MODULE_PATH := $(TARGET_OUT_SHARED_LIBRARIES)/hw 14 | #LOCAL_MULTILIB := both 15 | #LOCAL_MODULE_RELATIVE_PATH := hw 16 | #LOCAL_MULTILIB := TARGET_PREFER_64_BIT 17 | LOCAL_CFLAGS := -DHAVE_GPS_HARDWARE 18 | LOCAL_SHARED_LIBRARIES := liblog libcutils libhardware libc libutils 19 | LOCAL_SRC_FILES := gps_zkw.c 20 | 21 | LOCAL_MODULE := gps.$(TARGET_BOARD_PLATFORM) 22 | #LOCAL_MODULE := gps.default 23 | LOCAL_MODULE_TAGS := debug eng 24 | include $(BUILD_SHARED_LIBRARY) 25 | #endif 26 | #adb push $(TARGET_OUT_SHARED_LIBRARIES)/hw/gps.default /system/lib/hw/ 27 | #endif 28 | -------------------------------------------------------------------------------- /gnsshal/gps_zkw.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #define LOCATION_NLP_FIX "/data/misc/gps/LOCATION.DAT" 28 | #define C_INVALID_FD -1 29 | #define LOG_TAG "gps_zkw" 30 | #include 31 | #include 32 | #include 33 | #ifdef HAVE_LIBC_SYSTEM_PROPERTIES 34 | #define _REALLY_INCLUDE_SYS__SYSTEM_PROPERTIES_H_ 35 | #include 36 | #endif 37 | //#include 38 | #include 39 | 40 | /* the name of the controlled socket */ 41 | #define GPS_CHANNEL_NAME "/dev/ttyS1" 42 | #define TTY_BAUD B9600 // B115200 43 | // 44 | #define REDUCE_SV_FREQ 0 45 | #define TTY_BOOST 0 46 | 47 | #define GPS_DEBUG 1 48 | #define NEMA_DEBUG 1 /*the flag works if GPS_DEBUG is defined*/ 49 | #if GPS_DEBUG 50 | #define TRC(f) ALOGD("%s", __func__) 51 | #define ERR(f, ...) ALOGE("%s: line = %d, " f, __func__, __LINE__, ##__VA_ARGS__) 52 | #define WAN(f, ...) ALOGW("%s: line = %d, " f, __func__, __LINE__, ##__VA_ARGS__) 53 | #define DBG(f, ...) ALOGD("%s: line = %d, " f, __func__, __LINE__, ##__VA_ARGS__) 54 | #define VER(f, ...) ((void)0) // ((void)0) // 55 | #else 56 | # define DBG(...) ((void)0) 57 | # define VER(...) ((void)0) 58 | # define ERR(...) ((void)0) 59 | #endif 60 | static int flag_unlock = 0; 61 | GpsStatus gps_status; 62 | const char* gps_native_thread = "GPS NATIVE THREAD"; 63 | static GpsCallbacks callback_backup; 64 | static float report_time_interval = 0; 65 | static int started = 0; 66 | /*****************************************************************************/ 67 | 68 | /*****************************************************************/ 69 | /*****************************************************************/ 70 | /***** *****/ 71 | /***** C O N N E C T I O N S T A T E *****/ 72 | /***** *****/ 73 | /*****************************************************************/ 74 | /*****************************************************************/ 75 | 76 | /* commands sent to the gps thread */ 77 | enum { 78 | CMD_QUIT = 0, 79 | CMD_START = 1, 80 | CMD_STOP = 2, 81 | CMD_RESTART = 3, 82 | CMD_DOWNLOAD = 4, 83 | 84 | CMD_TEST_START = 10, 85 | CMD_TEST_STOP = 11, 86 | CMD_TEST_SMS_NO_RESULT = 12, 87 | }; 88 | 89 | static int gps_nmea_end_tag = 0; 90 | 91 | /*****************************************************************/ 92 | /*****************************************************************/ 93 | /***** *****/ 94 | /***** N M E A T O K E N I Z E R *****/ 95 | /***** *****/ 96 | /*****************************************************************/ 97 | /*****************************************************************/ 98 | 99 | typedef struct { 100 | const char* p; 101 | const char* end; 102 | } Token; 103 | 104 | #define MAX_NMEA_TOKENS 24 105 | 106 | typedef struct { 107 | int count; 108 | Token tokens[ MAX_NMEA_TOKENS ]; 109 | } NmeaTokenizer; 110 | 111 | static int 112 | nmea_tokenizer_init(NmeaTokenizer* t, const char* p, const char* end) 113 | { 114 | int count = 0; 115 | char* q; 116 | 117 | // the initial '$' is optional 118 | if (p < end && p[0] == '$') 119 | p += 1; 120 | 121 | // remove trailing newline 122 | if (end > p && (*(end-1) == '\n')) { 123 | end -= 1; 124 | if (end > p && (*(end-1) == '\r')) 125 | end -= 1; 126 | } 127 | 128 | // get rid of checksum at the end of the sentecne 129 | if (end >= p+3 && (*(end-3) == '*')) { 130 | end -= 3; 131 | } 132 | 133 | while (p < end) { 134 | const char* q = p; 135 | 136 | q = memchr(p, ',', end-p); 137 | if (q == NULL) 138 | q = end; 139 | 140 | if (q >= p) { 141 | if (count < MAX_NMEA_TOKENS) { 142 | t->tokens[count].p = p; 143 | t->tokens[count].end = q; 144 | count += 1; 145 | } 146 | } 147 | if (q < end) 148 | q += 1; 149 | 150 | p = q; 151 | } 152 | 153 | t->count = count; 154 | return count; 155 | } 156 | 157 | static Token 158 | nmea_tokenizer_get(NmeaTokenizer* t, int index) 159 | { 160 | Token tok; 161 | static const char* dummy = ""; 162 | 163 | if (index < 0 || index >= t->count) { 164 | tok.p = tok.end = dummy; 165 | } else 166 | tok = t->tokens[index]; 167 | 168 | return tok; 169 | } 170 | 171 | 172 | static int 173 | str2int(const char* p, const char* end) 174 | { 175 | int result = 0; 176 | int len = end - p; 177 | int sign = 1; 178 | 179 | if (*p == '-') 180 | { 181 | sign = -1; 182 | p++; 183 | len = end - p; 184 | } 185 | 186 | for (; len > 0; len--, p++) 187 | { 188 | int c; 189 | 190 | if (p >= end) 191 | goto Fail; 192 | 193 | c = *p - '0'; 194 | if ((unsigned)c >= 10) 195 | goto Fail; 196 | 197 | result = result*10 + c; 198 | } 199 | return sign*result; 200 | 201 | Fail: 202 | return -1; 203 | } 204 | 205 | static double 206 | str2float(const char* p, const char* end) 207 | { 208 | int result = 0; 209 | int len = end - p; 210 | char temp[16]; 211 | 212 | if (len >= (int)sizeof(temp)) 213 | return 0.; 214 | 215 | memcpy(temp, p, len); 216 | temp[len] = 0; 217 | return strtod(temp, NULL); 218 | } 219 | 220 | /*****************************************************************/ 221 | /*****************************************************************/ 222 | /***** *****/ 223 | /***** N M E A P A R S E R *****/ 224 | /***** *****/ 225 | /*****************************************************************/ 226 | /*****************************************************************/ 227 | 228 | // #define NMEA_MAX_SIZE 83 229 | #define MAX_SV_COUNT 300 230 | #define NMEA_MAX_SIZE 128 231 | /*maximum number of SV information in GPGSV*/ 232 | #define NMEA_MAX_SV_INFO 4 233 | #define LOC_FIXED(pNmeaReader) ((pNmeaReader->fix_mode == 2) || (pNmeaReader->fix_mode ==3)) 234 | typedef struct { 235 | int pos; 236 | int overflow; 237 | int utc_year; 238 | int utc_mon; 239 | int utc_day; 240 | int utc_diff; 241 | GpsLocation fix; 242 | 243 | /* 244 | * The fix flag extracted from GPGSA setence: 1: No fix; 2 = 2D; 3 = 3D 245 | * if the fix mode is 0, no location will be reported via callback 246 | * otherwise, the location will be reported via callback 247 | */ 248 | int fix_mode; 249 | /* 250 | * Indicate that the status of callback handling. 251 | * The flag is used to report GPS_STATUS_SESSION_BEGIN or GPS_STATUS_SESSION_END: 252 | * (0) The flag will be set as true when callback setting is changed via nmea_reader_set_callback 253 | * (1) GPS_STATUS_SESSION_BEGIN: receive location fix + flag set + callback is set 254 | * (2) GPS_STATUS_SESSION_END: receive location fix + flag set + callback is null 255 | */ 256 | int cb_status_changed; 257 | int sv_count; /*used to count the number of received SV information*/ 258 | GpsSvStatus sv_status_gps; 259 | GnssSvStatus sv_status_gnss; 260 | GpsCallbacks callbacks; 261 | char in[ NMEA_MAX_SIZE+1 ]; 262 | int sv_status_can_report; 263 | int location_can_report; 264 | int sv_used_in_fix[MAX_SV_COUNT]; 265 | } NmeaReader; 266 | 267 | 268 | static void 269 | nmea_reader_update_utc_diff(NmeaReader* const r) 270 | { 271 | time_t now = time(NULL); 272 | struct tm tm_local; 273 | struct tm tm_utc; 274 | unsigned long time_local, time_utc; 275 | 276 | gmtime_r(&now, &tm_utc); 277 | localtime_r(&now, &tm_local); 278 | 279 | 280 | time_local = mktime(&tm_local); 281 | 282 | 283 | time_utc = mktime(&tm_utc); 284 | 285 | // r->utc_diff = time_utc - time_local; 286 | r->utc_diff = time_local - time_utc; 287 | } 288 | 289 | 290 | static void 291 | nmea_reader_init(NmeaReader* const r) 292 | { 293 | memset(r, 0, sizeof(*r)); 294 | 295 | r->pos = 0; 296 | r->overflow = 0; 297 | r->utc_year = -1; 298 | r->utc_mon = -1; 299 | r->utc_day = -1; 300 | r->utc_diff = 0; 301 | r->callbacks.location_cb= NULL; 302 | r->callbacks.status_cb= NULL; 303 | r->callbacks.sv_status_cb= NULL; 304 | r->sv_count = 0; 305 | r->fix_mode = 0; /*no fix*/ 306 | r->cb_status_changed = 0; 307 | memset((void*)&r->sv_status_gps, 0x00, sizeof(r->sv_status_gps)); 308 | memset((void*)&r->sv_status_gnss, 0x00, sizeof(r->sv_status_gnss)); 309 | memset((void*)&r->in, 0x00, sizeof(r->in)); 310 | 311 | nmea_reader_update_utc_diff(r); 312 | } 313 | 314 | static void 315 | nmea_reader_set_callback(NmeaReader* const r, GpsCallbacks* const cbs) 316 | { 317 | if (!r) { /*this should not happen*/ 318 | return; 319 | } else if (!cbs) { /*unregister the callback */ 320 | return; 321 | } else {/*register the callback*/ 322 | r->fix.flags = 0; 323 | r->sv_count = 0; 324 | r->sv_status_gps.num_svs = 0; 325 | r->sv_status_gnss.num_svs = 0; 326 | } 327 | } 328 | 329 | 330 | static int 331 | nmea_reader_update_time(NmeaReader* const r, Token tok) 332 | { 333 | int hour, minute; 334 | double seconds; 335 | struct tm tm; 336 | struct tm tm_local; 337 | time_t fix_time; 338 | 339 | if (tok.p + 6 > tok.end) 340 | return -1; 341 | 342 | memset((void*)&tm, 0x00, sizeof(tm)); 343 | if (r->utc_year < 0) { 344 | // no date yet, get current one 345 | time_t now = time(NULL); 346 | gmtime_r(&now, &tm); 347 | r->utc_year = tm.tm_year + 1900; 348 | r->utc_mon = tm.tm_mon + 1; 349 | r->utc_day = tm.tm_mday; 350 | } 351 | 352 | hour = str2int(tok.p, tok.p+2); 353 | minute = str2int(tok.p+2, tok.p+4); 354 | seconds = str2float(tok.p+4, tok.end); 355 | 356 | tm.tm_hour = hour; 357 | tm.tm_min = minute; 358 | tm.tm_sec = (int) seconds; 359 | tm.tm_year = r->utc_year - 1900; 360 | tm.tm_mon = r->utc_mon - 1; 361 | tm.tm_mday = r->utc_day; 362 | tm.tm_isdst = -1; 363 | 364 | if (mktime(&tm) == (time_t)-1) 365 | ERR("mktime error: %d %s\n", errno, strerror(errno)); 366 | 367 | fix_time = mktime(&tm); 368 | localtime_r(&fix_time, &tm_local); 369 | 370 | // fix_time += tm_local.tm_gmtoff; 371 | // DBG("fix_time: %d\n", (int)fix_time); 372 | r->fix.timestamp = (long long)fix_time * 1000; 373 | return 0; 374 | } 375 | 376 | static int 377 | nmea_reader_update_date(NmeaReader* const r, Token date, Token time) 378 | { 379 | Token tok = date; 380 | int day, mon, year; 381 | 382 | if (tok.p + 6 != tok.end) { 383 | ERR("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p); 384 | return -1; 385 | } 386 | day = str2int(tok.p, tok.p+2); 387 | mon = str2int(tok.p+2, tok.p+4); 388 | year = str2int(tok.p+4, tok.p+6) + 2000; 389 | 390 | if ((day|mon|year) < 0) { 391 | ERR("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p); 392 | return -1; 393 | } 394 | 395 | r->utc_year = year; 396 | r->utc_mon = mon; 397 | r->utc_day = day; 398 | 399 | return nmea_reader_update_time(r, time); 400 | } 401 | 402 | 403 | static double 404 | convert_from_hhmm(Token tok) 405 | { 406 | double val = str2float(tok.p, tok.end); 407 | int degrees = (int)(floor(val) / 100); 408 | double minutes = val - degrees*100.; 409 | double dcoord = degrees + minutes / 60.0; 410 | return dcoord; 411 | } 412 | 413 | 414 | static int 415 | nmea_reader_update_latlong(NmeaReader* const r, 416 | Token latitude, 417 | char latitudeHemi, 418 | Token longitude, 419 | char longitudeHemi) 420 | { 421 | double lat, lon; 422 | Token tok; 423 | 424 | tok = latitude; 425 | if (tok.p + 6 > tok.end) { 426 | ERR("latitude is too short: '%.*s'", tok.end-tok.p, tok.p); 427 | return -1; 428 | } 429 | lat = convert_from_hhmm(tok); 430 | if (latitudeHemi == 'S') 431 | lat = -lat; 432 | 433 | tok = longitude; 434 | if (tok.p + 6 > tok.end) { 435 | ERR("longitude is too short: '%.*s'", tok.end-tok.p, tok.p); 436 | return -1; 437 | } 438 | lon = convert_from_hhmm(tok); 439 | if (longitudeHemi == 'W') 440 | lon = -lon; 441 | 442 | r->fix.flags |= GPS_LOCATION_HAS_LAT_LONG; 443 | r->fix.latitude = lat; 444 | r->fix.longitude = lon; 445 | return 0; 446 | } 447 | /* this is the state of our connection to the daemon */ 448 | typedef struct { 449 | int init; 450 | int fd; 451 | GpsCallbacks callbacks; 452 | pthread_t thread; 453 | int control[2]; 454 | int sockfd; 455 | int epoll_hd; 456 | int flag; 457 | int start_flag; 458 | // int thread_exit_flag; 459 | } GpsState; 460 | 461 | static GpsState _gps_state[1]; 462 | 463 | static int 464 | nmea_reader_update_altitude(NmeaReader* const r, 465 | Token altitude, 466 | Token units) 467 | { 468 | double alt; 469 | Token tok = altitude; 470 | 471 | if (tok.p >= tok.end) 472 | return -1; 473 | 474 | r->fix.flags |= GPS_LOCATION_HAS_ALTITUDE; 475 | r->fix.altitude = str2float(tok.p, tok.end); 476 | return 0; 477 | } 478 | 479 | 480 | static int 481 | nmea_reader_update_bearing(NmeaReader* const r, 482 | Token bearing) 483 | { 484 | double alt; 485 | Token tok = bearing; 486 | 487 | if (tok.p >= tok.end) 488 | return -1; 489 | 490 | r->fix.flags |= GPS_LOCATION_HAS_BEARING; 491 | r->fix.bearing = str2float(tok.p, tok.end); 492 | return 0; 493 | } 494 | 495 | 496 | static int 497 | nmea_reader_update_speed(NmeaReader* const r, 498 | Token speed) 499 | { 500 | double alt; 501 | Token tok = speed; 502 | 503 | if (tok.p >= tok.end) 504 | return -1; 505 | 506 | r->fix.flags |= GPS_LOCATION_HAS_SPEED; 507 | 508 | // knot to m/s 509 | r->fix.speed = str2float(tok.p, tok.end) / 1.942795467; 510 | return 0; 511 | } 512 | 513 | // Add by LCH for accuracy 514 | static int 515 | nmea_reader_update_accuracy(NmeaReader* const r, 516 | Token accuracy) 517 | { 518 | double alt; 519 | Token tok = accuracy; 520 | 521 | if (tok.p >= tok.end) 522 | return -1; 523 | 524 | r->fix.flags |= GPS_LOCATION_HAS_ACCURACY; 525 | r->fix.accuracy = str2float(tok.p, tok.end); 526 | return 0; 527 | } 528 | 529 | /* 530 | static int 531 | nmea_reader_update_sv_status_gps(NmeaReader* r, int sv_index, 532 | int id, Token elevation, 533 | Token azimuth, Token snr) 534 | { 535 | // int prn = str2int(id.p, id.end); 536 | int prn = id; 537 | if ((prn <= 0) || (prn < 65 && prn > GPS_MAX_SVS)|| (prn > 96) || (r->sv_count >= GPS_MAX_SVS)) { 538 | VER("sv_status_gps: ignore (%d)", prn); 539 | return 0; 540 | } 541 | sv_index = r->sv_count+r->sv_status_gps.num_svs; 542 | if (GPS_MAX_SVS <= sv_index) { 543 | ERR("ERR: sv_index=[%d] is larger than GPS_MAX_SVS.\n", sv_index); 544 | return 0; 545 | } 546 | r->sv_status_gps.sv_list[sv_index].prn = prn; 547 | r->sv_status_gps.sv_list[sv_index].snr = str2float(snr.p, snr.end); 548 | r->sv_status_gps.sv_list[sv_index].elevation = str2int(elevation.p, elevation.end); 549 | r->sv_status_gps.sv_list[sv_index].azimuth = str2int(azimuth.p, azimuth.end); 550 | r->sv_count++; 551 | VER("sv_status_gps(%2d): %2d, %2f, %3f, %2f", sv_index, 552 | r->sv_status_gps.sv_list[sv_index].prn, r->sv_status_gps.sv_list[sv_index].elevation, 553 | r->sv_status_gps.sv_list[sv_index].azimuth, r->sv_status_gps.sv_list[sv_index].snr); 554 | return 0; 555 | } 556 | */ 557 | 558 | static int 559 | get_svid(int prn, int sv_type) 560 | { 561 | if (sv_type == GNSS_CONSTELLATION_GLONASS && prn >= 1 && prn <= 32) 562 | return prn + 64; 563 | else if (sv_type == GNSS_CONSTELLATION_BEIDOU && prn >= 1 && prn <= 64) 564 | return prn + 200; 565 | 566 | return prn; 567 | } 568 | 569 | static int 570 | nmea_reader_update_sv_status_gnss(NmeaReader* r, int sv_index, 571 | int id, Token elevation, 572 | Token azimuth, Token snr) 573 | { 574 | int prn = id; 575 | sv_index = r->sv_count + r->sv_status_gnss.num_svs; 576 | if (GNSS_MAX_SVS <= sv_index) { 577 | ERR("ERR: sv_index=[%d] is larger than GNSS_MAX_SVS.\n", sv_index); 578 | return 0; 579 | } 580 | 581 | if ((prn > 0) && (prn < 32)) { 582 | r->sv_status_gnss.gnss_sv_list[sv_index].svid = prn; 583 | r->sv_status_gnss.gnss_sv_list[sv_index].constellation = GNSS_CONSTELLATION_GPS; 584 | } else if ((prn >= 65) && (prn <= 96)) { 585 | r->sv_status_gnss.gnss_sv_list[sv_index].svid = prn-64; 586 | r->sv_status_gnss.gnss_sv_list[sv_index].constellation = GNSS_CONSTELLATION_GLONASS; 587 | } else if ((prn >= 201) && (prn <= 264)) { 588 | r->sv_status_gnss.gnss_sv_list[sv_index].svid = prn-200; 589 | r->sv_status_gnss.gnss_sv_list[sv_index].constellation = GNSS_CONSTELLATION_BEIDOU; 590 | } else if ((prn >= 401) && (prn <= 436)) { 591 | r->sv_status_gnss.gnss_sv_list[sv_index].svid = prn-400; 592 | r->sv_status_gnss.gnss_sv_list[sv_index].constellation = GNSS_CONSTELLATION_GALILEO; 593 | } else { 594 | DBG("sv_status: ignore (%d)", prn); 595 | return 0; 596 | } 597 | 598 | r->sv_status_gnss.gnss_sv_list[sv_index].c_n0_dbhz = str2float(snr.p, snr.end); 599 | r->sv_status_gnss.gnss_sv_list[sv_index].elevation = str2int(elevation.p, elevation.end); 600 | r->sv_status_gnss.gnss_sv_list[sv_index].azimuth = str2int(azimuth.p, azimuth.end); 601 | r->sv_status_gnss.gnss_sv_list[sv_index].flags = 0; 602 | if (1 == r->sv_used_in_fix[prn]) { 603 | r->sv_status_gnss.gnss_sv_list[sv_index].flags |= GNSS_SV_FLAGS_USED_IN_FIX; 604 | } 605 | 606 | r->sv_count++; 607 | DBG("sv_status(%2d): %2d, %d, %2f, %3f, %2f, %d", 608 | sv_index, r->sv_status_gnss.gnss_sv_list[sv_index].svid, r->sv_status_gnss.gnss_sv_list[sv_index].constellation, 609 | r->sv_status_gnss.gnss_sv_list[sv_index].elevation, r->sv_status_gnss.gnss_sv_list[sv_index].azimuth, 610 | r->sv_status_gnss.gnss_sv_list[sv_index].c_n0_dbhz, r->sv_status_gnss.gnss_sv_list[sv_index].flags); 611 | return 0; 612 | } 613 | 614 | 615 | static void 616 | nmea_reader_parse(NmeaReader* const r) 617 | { 618 | /* we received a complete sentence, now parse it to generate 619 | * a new GPS fix... 620 | */ 621 | NmeaTokenizer tzer[1]; 622 | Token tok; 623 | GnssConstellationType sv_type = GNSS_CONSTELLATION_GPS; 624 | 625 | 626 | #if NEMA_DEBUG 627 | DBG("Received: '%.*s'", r->pos, r->in); 628 | #endif 629 | if (r->pos < 9) { 630 | ERR("Too short. discarded. '%.*s'", r->pos, r->in); 631 | return; 632 | } 633 | if (r->pos < (int)sizeof(r->in)) { 634 | nmea_tokenizer_init(tzer, r->in, r->in + r->pos); 635 | } 636 | #if NEMA_DEBUG 637 | { 638 | int n; 639 | DBG("Found %d tokens", tzer->count); 640 | for (n = 0; n < tzer->count; n++) { 641 | Token tok = nmea_tokenizer_get(tzer, n); 642 | DBG("%2d: '%.*s'", n, tok.end-tok.p, tok.p); 643 | } 644 | } 645 | #endif 646 | 647 | tok = nmea_tokenizer_get(tzer, 0); 648 | if (tok.p + 5 > tok.end) { 649 | ERR("sentence id '%.*s' too short, ignored.", tok.end-tok.p, tok.p); 650 | return; 651 | } 652 | 653 | // ignore first two characters. 654 | if (!memcmp(tok.p, "BD", 2)) { 655 | sv_type = GNSS_CONSTELLATION_BEIDOU; 656 | DBG("BDS SV type"); 657 | } 658 | else if (!memcmp(tok.p, "GL", 2)) { 659 | sv_type = GNSS_CONSTELLATION_GLONASS; 660 | DBG("GLN SV type"); 661 | } 662 | tok.p += 2; 663 | if (!memcmp(tok.p, "GGA", 3)) { 664 | // GPS fix 665 | Token tok_time = nmea_tokenizer_get(tzer, 1); 666 | Token tok_latitude = nmea_tokenizer_get(tzer, 2); 667 | Token tok_latitudeHemi = nmea_tokenizer_get(tzer, 3); 668 | Token tok_longitude = nmea_tokenizer_get(tzer, 4); 669 | Token tok_longitudeHemi = nmea_tokenizer_get(tzer, 5); 670 | Token tok_altitude = nmea_tokenizer_get(tzer, 9); 671 | Token tok_altitudeUnits = nmea_tokenizer_get(tzer, 10); 672 | 673 | nmea_reader_update_time(r, tok_time); 674 | nmea_reader_update_latlong(r, tok_latitude, 675 | tok_latitudeHemi.p[0], 676 | tok_longitude, 677 | tok_longitudeHemi.p[0]); 678 | nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits); 679 | 680 | } 681 | else if (!memcmp(tok.p, "GSA", 3)) { 682 | Token tok_fix = nmea_tokenizer_get(tzer, 2); 683 | Token tok_svs = nmea_tokenizer_get(tzer, 18); 684 | switch(tok_svs.p[0]) { 685 | case '1': 686 | sv_type = GNSS_CONSTELLATION_GPS; 687 | break; 688 | case '2': 689 | sv_type = GNSS_CONSTELLATION_GLONASS; 690 | break; 691 | case '4': 692 | sv_type = GNSS_CONSTELLATION_BEIDOU; 693 | break; 694 | default: 695 | break; 696 | } 697 | int idx, max = 12; /*the number of satellites in GPGSA*/ 698 | 699 | r->fix_mode = str2int(tok_fix.p, tok_fix.end); 700 | 701 | if (LOC_FIXED(r)) { /* 1: No fix; 2: 2D; 3: 3D*/ 702 | Token tok_accuracy = nmea_tokenizer_get(tzer, 15); 703 | nmea_reader_update_accuracy(r, tok_accuracy); // pdop 704 | 705 | for (idx = 0; idx < max; idx++) { 706 | Token tok_satellite = nmea_tokenizer_get(tzer, idx+3); 707 | if (tok_satellite.p == tok_satellite.end) { 708 | DBG("GSA: found %d active satellites\n", idx); 709 | break; 710 | } 711 | int prn = str2int(tok_satellite.p, tok_satellite.end); 712 | int svid = get_svid(prn, sv_type); 713 | if (svid >= 0 && svid < MAX_SV_COUNT) 714 | r->sv_used_in_fix[svid] = 1; 715 | 716 | /* 717 | if (sv_type == GNSS_CONSTELLATION_BEIDOU) { 718 | sate_id += 200; 719 | } 720 | else if (sv_type == GNSS_CONSTELLATION_GLONASS) { 721 | sate_id += 64; 722 | } 723 | if (sate_id >= 1 && sate_id <= 32) { // GP 724 | r->sv_used_in_fix[sate_id] = 1; 725 | } else if (sate_id >= 193 && sate_id <= 197) { 726 | r->sv_used_in_fix[sate_id] = 0; 727 | DBG("[debug mask]QZSS, just ignore. satellite id is %d\n ", sate_id); 728 | continue; 729 | } else if (sate_id >= 65 && sate_id <= 96) { // GL 730 | r->sv_used_in_fix[sate_id] = 1; 731 | } else if (sate_id >= 201 && sate_id <= 232) { // BD 732 | r->sv_used_in_fix[sate_id] = 1; 733 | } 734 | else { 735 | VER("GSA: invalid sentence, ignore!!"); 736 | break; 737 | } 738 | DBG("GSA:sv_used_in_fix[%d] = %d\n", svid, r->sv_used_in_fix[svid]); 739 | */ 740 | } 741 | } 742 | } 743 | else if (!memcmp(tok.p, "RMC", 3)) { 744 | Token tok_time = nmea_tokenizer_get(tzer, 1); 745 | Token tok_fixStatus = nmea_tokenizer_get(tzer, 2); 746 | Token tok_latitude = nmea_tokenizer_get(tzer, 3); 747 | Token tok_latitudeHemi = nmea_tokenizer_get(tzer, 4); 748 | Token tok_longitude = nmea_tokenizer_get(tzer, 5); 749 | Token tok_longitudeHemi = nmea_tokenizer_get(tzer, 6); 750 | Token tok_speed = nmea_tokenizer_get(tzer, 7); 751 | Token tok_bearing = nmea_tokenizer_get(tzer, 8); 752 | Token tok_date = nmea_tokenizer_get(tzer, 9); 753 | 754 | VER("in RMC, fixStatus=%c", tok_fixStatus.p[0]); 755 | if (tok_fixStatus.p[0] == 'A') 756 | { 757 | nmea_reader_update_date(r, tok_date, tok_time); 758 | 759 | nmea_reader_update_latlong(r, tok_latitude, 760 | tok_latitudeHemi.p[0], 761 | tok_longitude, 762 | tok_longitudeHemi.p[0]); 763 | 764 | nmea_reader_update_bearing(r, tok_bearing); 765 | nmea_reader_update_speed(r, tok_speed); 766 | r->location_can_report = 1; 767 | } 768 | r->sv_status_can_report = 1; 769 | } else if (!memcmp(tok.p, "GSV", 3)) { 770 | Token tok_num = nmea_tokenizer_get(tzer, 1); // number of messages 771 | Token tok_seq = nmea_tokenizer_get(tzer, 2); // sequence number 772 | Token tok_cnt = nmea_tokenizer_get(tzer, 3); // Satellites in view 773 | int num = str2int(tok_num.p, tok_num.end); 774 | int seq = str2int(tok_seq.p, tok_seq.end); 775 | int cnt = str2int(tok_cnt.p, tok_cnt.end); 776 | int sv_base = (seq - 1)*NMEA_MAX_SV_INFO; 777 | int sv_num = cnt - sv_base; 778 | int idx, base = 4, base_idx; 779 | if (sv_num > NMEA_MAX_SV_INFO) 780 | sv_num = NMEA_MAX_SV_INFO; 781 | if (seq == 1) /*if sequence number is 1, a new set of GSV will be parsed*/ 782 | r->sv_count = 0; 783 | for (idx = 0; idx < sv_num; idx++) { 784 | base_idx = base*(idx+1); 785 | Token tok_id = nmea_tokenizer_get(tzer, base_idx+0); 786 | int prn = str2int(tok_id.p, tok_id.end); 787 | int svid = get_svid(prn, sv_type); 788 | /* 789 | if (sv_type == GNSS_CONSTELLATION_BEIDOU) { 790 | sv_id += 200; 791 | DBG("It is BDS SV: %d", sv_id); 792 | } 793 | else if (sv_type == GNSS_CONSTELLATION_GLONASS) { 794 | sv_id += 64; 795 | DBG("It is GLN SV: %d", sv_id); 796 | } 797 | */ 798 | Token tok_ele = nmea_tokenizer_get(tzer, base_idx+1); 799 | Token tok_azi = nmea_tokenizer_get(tzer, base_idx+2); 800 | Token tok_snr = nmea_tokenizer_get(tzer, base_idx+3); 801 | nmea_reader_update_sv_status_gnss(r, sv_base+idx, svid, tok_ele, tok_azi, tok_snr); 802 | } 803 | if (seq == num) { 804 | if (r->sv_count <= cnt) { 805 | DBG("r->sv_count = %d", r->sv_count); 806 | r->sv_status_gnss.num_svs += r->sv_count; 807 | 808 | 809 | } else { 810 | ERR("GPGSV incomplete (%d/%d), ignored!", r->sv_count, cnt); 811 | r->sv_count = r->sv_status_gnss.num_svs = 0; 812 | } 813 | } 814 | } 815 | // Add for Accuracy 816 | else if (!memcmp(tok.p, "ACCURACY", 8)) { 817 | if ((r->fix_mode == 3) || (r->fix_mode == 2)) { 818 | Token tok_accuracy = nmea_tokenizer_get(tzer, 1); 819 | nmea_reader_update_accuracy(r, tok_accuracy); 820 | DBG("GPS get accuracy from driver:%f\n", r->fix.accuracy); 821 | } 822 | else { 823 | DBG("GPS get accuracy failed, fix mode:%d\n", r->fix_mode); 824 | } 825 | } 826 | else { 827 | tok.p -= 2; 828 | VER("unknown sentence '%.*s", tok.end-tok.p, tok.p); 829 | } 830 | //if (!LOC_FIXED(r)) { 831 | // VER("Location is not fixed, ignored callback\n"); 832 | //} else if (r->fix.flags != 0 && gps_nmea_end_tag) { 833 | if (r->location_can_report) { 834 | #if NEMA_DEBUG 835 | char temp[256]; 836 | char* p = temp; 837 | char* end = p + sizeof(temp); 838 | struct tm utc; 839 | 840 | p += snprintf(p, end-p, "sending fix"); 841 | if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) { 842 | p += snprintf(p, end-p, " lat=%g lon=%g", r->fix.latitude, r->fix.longitude); 843 | } 844 | if (r->fix.flags & GPS_LOCATION_HAS_ALTITUDE) { 845 | p += snprintf(p, end-p, " altitude=%g", r->fix.altitude); 846 | } 847 | if (r->fix.flags & GPS_LOCATION_HAS_SPEED) { 848 | p += snprintf(p, end-p, " speed=%g", r->fix.speed); 849 | } 850 | if (r->fix.flags & GPS_LOCATION_HAS_BEARING) { 851 | p += snprintf(p, end-p, " bearing=%g", r->fix.bearing); 852 | } 853 | if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) { 854 | p += snprintf(p, end-p, " accuracy=%g", r->fix.accuracy); 855 | DBG("GPS accuracy=%g\n", r->fix.accuracy); 856 | } 857 | gmtime_r((time_t*) &r->fix.timestamp, &utc); 858 | p += snprintf(p, end-p, " time=%s", asctime(&utc)); 859 | VER(temp); 860 | #endif 861 | 862 | callback_backup.location_cb(&r->fix); 863 | r->fix.flags = 0; 864 | r->location_can_report = 0; 865 | } 866 | 867 | DBG("r->sv_status_gnss.num_svs = %d, gps_nmea_end_tag = %d, sv_status_can_report = %d", r->sv_status_gnss.num_svs, gps_nmea_end_tag, r->sv_status_can_report); 868 | if (r->sv_status_can_report) { 869 | r->sv_status_can_report = 0; 870 | if (r->sv_status_gnss.num_svs != 0) { 871 | r->sv_status_gnss.size = sizeof(GnssSvStatus); 872 | DBG("Report sv status"); 873 | callback_backup.gnss_sv_status_cb(&r->sv_status_gnss); 874 | r->sv_count = r->sv_status_gnss.num_svs = 0; 875 | memset(r->sv_used_in_fix, 0, sizeof(r->sv_used_in_fix)); 876 | } 877 | } 878 | } 879 | 880 | 881 | static void 882 | nmea_reader_addc(NmeaReader* const r, int c) 883 | { 884 | if (r->overflow) { 885 | r->overflow = (c != '\n'); 886 | return; 887 | } 888 | 889 | if ((r->pos >= (int) sizeof(r->in)-1 ) || (r->pos < 0)) { 890 | r->overflow = 1; 891 | r->pos = 0; 892 | DBG("nmea sentence overflow\n"); 893 | return; 894 | } 895 | 896 | r->in[r->pos] = (char)c; 897 | r->pos += 1; 898 | 899 | if (c == '\n') { 900 | nmea_reader_parse(r); 901 | 902 | DBG("start nmea_cb\n"); 903 | r->in[r->pos] = 0; 904 | callback_backup.nmea_cb(r->fix.timestamp, r->in, r->pos); 905 | r->pos = 0; 906 | } 907 | } 908 | 909 | 910 | static void 911 | gps_state_done(GpsState* s) 912 | { 913 | char cmd = CMD_QUIT; 914 | 915 | write(s->control[0], &cmd, 1); 916 | close(s->control[0]); 917 | s->control[0] = -1; 918 | close(s->control[1]); 919 | s->control[1] = -1; 920 | close(s->fd); 921 | s->fd = -1; 922 | close(s->sockfd); 923 | s->sockfd = -1; 924 | close(s->epoll_hd); 925 | s->epoll_hd = -1; 926 | s->init = 0; 927 | return; 928 | } 929 | 930 | 931 | static void 932 | gps_state_start(GpsState* s) 933 | { 934 | char cmd = CMD_START; 935 | int ret; 936 | 937 | do { 938 | ret = write(s->control[0], &cmd, 1); 939 | } 940 | while (ret < 0 && errno == EINTR); 941 | 942 | if (ret != 1) 943 | ERR("%s: could not send CMD_START command: ret=%d: %s", 944 | __FUNCTION__, ret, strerror(errno)); 945 | } 946 | 947 | static void 948 | gps_state_stop(GpsState* s) 949 | { 950 | char cmd = CMD_STOP; 951 | int ret; 952 | 953 | do { 954 | ret = write(s->control[0], &cmd, 1); 955 | } 956 | while (ret < 0 && errno == EINTR); 957 | 958 | if (ret != 1) 959 | ERR("%s: could not send CMD_STOP command: ret=%d: %s", 960 | __FUNCTION__, ret, strerror(errno)); 961 | } 962 | 963 | static void 964 | gps_state_restart(GpsState* s) 965 | { 966 | char cmd = CMD_RESTART; 967 | int ret; 968 | 969 | do { 970 | ret = write(s->control[0], &cmd, 1); 971 | } 972 | while (ret < 0 && errno == EINTR); 973 | 974 | if (ret != 1) 975 | ERR("%s: could not send CMD_RESTART command: ret=%d: %s", 976 | __FUNCTION__, ret, strerror(errno)); 977 | } 978 | 979 | 980 | static int 981 | epoll_register(int epoll_fd, int fd) 982 | { 983 | struct epoll_event ev; 984 | int ret, flags; 985 | 986 | /* important: make the fd non-blocking */ 987 | flags = fcntl(fd, F_GETFL); 988 | fcntl(fd, F_SETFL, flags | O_NONBLOCK); 989 | 990 | ev.events = EPOLLIN; 991 | ev.data.fd = fd; 992 | do { 993 | ret = epoll_ctl(epoll_fd, EPOLL_CTL_ADD, fd, &ev); 994 | } while (ret < 0 && errno == EINTR); 995 | if (ret < 0) 996 | ERR("epoll ctl error, error num is %d\n, message is %s\n", errno, strerror(errno)); 997 | return ret; 998 | } 999 | 1000 | 1001 | static int 1002 | epoll_deregister(int epoll_fd, int fd) 1003 | { 1004 | int ret; 1005 | do { 1006 | ret = epoll_ctl(epoll_fd, EPOLL_CTL_DEL, fd, NULL); 1007 | } while (ret < 0 && errno == EINTR); 1008 | return ret; 1009 | } 1010 | 1011 | /*for reducing the function call to get data from kernel*/ 1012 | static char buff[2048]; 1013 | /* this is the main thread, it waits for commands from gps_state_start/stop and, 1014 | * when started, messages from the GPS daemon. these are simple NMEA sentences 1015 | * that must be parsed to be converted into GPS fixes sent to the framework 1016 | */ 1017 | void 1018 | gps_state_thread(void* arg) 1019 | { 1020 | static float count = 0; 1021 | GpsState* state = (GpsState*) arg; 1022 | // state->thread_exit_flag=0; 1023 | NmeaReader reader[1]; 1024 | int gps_fd = state->fd; 1025 | int control_fd = state->control[1]; 1026 | //int atc_fd = state->sockfd; 1027 | 1028 | int epoll_fd = state->epoll_hd; 1029 | int test_started = 0; 1030 | 1031 | nmea_reader_init(reader); 1032 | 1033 | // register control file descriptors for polling 1034 | if (epoll_register(epoll_fd, control_fd) < 0) 1035 | ERR("epoll register control_fd error, error num is %d\n, message is %s\n", errno, strerror(errno)); 1036 | if (epoll_register(epoll_fd, gps_fd) < 0) 1037 | ERR("epoll register gps_fd error, error num is %d\n, message is %s\n", errno, strerror(errno)); 1038 | //if (epoll_register(epoll_fd, atc_fd) < 0) 1039 | // ERR("epoll register atc_fd error, error num is %d\n, message is %s\n", errno, strerror(errno)); 1040 | 1041 | DBG("gps thread running: PPID[%d], PID[%d]\n", getppid(), getpid()); 1042 | DBG("HAL thread is ready, realease lock, and CMD_START can be handled\n"); 1043 | // now loop 1044 | for (;;) { 1045 | struct epoll_event events[4]; 1046 | int ne, nevents; 1047 | nevents = epoll_wait(epoll_fd, events, 4, -1); 1048 | if (nevents < 0) { 1049 | if (errno != EINTR) 1050 | ERR("epoll_wait() unexpected error: %s", strerror(errno)); 1051 | continue; 1052 | } 1053 | VER("gps thread received %d events", nevents); 1054 | for (ne = 0; ne < nevents; ne++) { 1055 | if ((events[ne].events & (EPOLLERR|EPOLLHUP)) != 0) { 1056 | ERR("EPOLLERR or EPOLLHUP after epoll_wait() !?"); 1057 | goto Exit; 1058 | } 1059 | if ((events[ne].events & EPOLLIN) != 0) { 1060 | int fd = events[ne].data.fd; 1061 | 1062 | if (fd == control_fd) { 1063 | char cmd = 255; 1064 | int ret; 1065 | DBG("gps control fd event"); 1066 | do { 1067 | ret = read(fd, &cmd, 1); 1068 | } while (ret < 0 && errno == EINTR); 1069 | 1070 | if (cmd == CMD_QUIT) { 1071 | DBG("gps thread quitting on demand"); 1072 | goto Exit; 1073 | } 1074 | else if (cmd == CMD_START) { 1075 | if (!started) { 1076 | DBG("gps thread starting location_cb=%p", &callback_backup); 1077 | started = 1; 1078 | nmea_reader_set_callback(reader, &state->callbacks); 1079 | } 1080 | } 1081 | else if (cmd == CMD_STOP) { 1082 | if (started) { 1083 | DBG("gps thread stopping"); 1084 | started = 0; 1085 | nmea_reader_set_callback(reader, NULL); 1086 | DBG("CMD_STOP has been receiving from HAL thread, release lock so can handle CLEAN_UP\n"); 1087 | } 1088 | } 1089 | else if (cmd == CMD_RESTART) { 1090 | reader->fix_mode = 0; 1091 | } 1092 | } 1093 | else if (fd == gps_fd) { 1094 | if (!flag_unlock) { 1095 | flag_unlock = 1; 1096 | DBG("got first NMEA sentence, release lock to set state ENGINE ON, SESSION BEGIN"); 1097 | } 1098 | VER("gps fd event"); 1099 | if (report_time_interval > ++count) { 1100 | DBG("[trace]count is %f\n", count); 1101 | int ret = read(fd, buff, sizeof(buff)); 1102 | continue; 1103 | } 1104 | count = 0; 1105 | for (;;) { 1106 | int nn, ret; 1107 | ret = read(fd, buff, sizeof(buff)); 1108 | if (ret < 0) { 1109 | if (errno == EINTR) 1110 | continue; 1111 | if (errno != EWOULDBLOCK) 1112 | ERR("error while reading from gps daemon socket: %s: %p", strerror(errno), buff); 1113 | break; 1114 | } 1115 | DBG("received %d bytes:\n", ret); 1116 | gps_nmea_end_tag = 0; 1117 | for (nn = 0; nn < ret; nn++) 1118 | { 1119 | if (nn == (ret-1)) 1120 | gps_nmea_end_tag = 1; 1121 | 1122 | nmea_reader_addc(reader, buff[nn]); 1123 | } 1124 | } 1125 | VER("gps fd event end"); 1126 | } 1127 | else 1128 | { 1129 | ERR("epoll_wait() returned unkown fd %d ?", fd); 1130 | } 1131 | } 1132 | } 1133 | } 1134 | Exit: 1135 | DBG("HAL thread is exiting, release lock to clean resources\n"); 1136 | return; 1137 | } 1138 | 1139 | static void 1140 | gpstty_boost(int tty_fd, int baud) 1141 | { 1142 | const char msg[] = "$PCAS01,5*19\r\n"; 1143 | 1144 | if (TTY_BOOST == 0) { 1145 | DBG("TTY Boost OFF."); 1146 | return; 1147 | } 1148 | 1149 | if (baud > B9600) { 1150 | DBG("No need to boost baudrate."); 1151 | return; 1152 | } 1153 | 1154 | write(tty_fd, msg, strlen(msg)); 1155 | 1156 | usleep(200 * 1000); // sleep 200ms to make sure msg is send at TTY_BAUD 1157 | 1158 | // Upgrade tty's baudrate to 115200 1159 | 1160 | struct termios cfg; 1161 | tcgetattr(tty_fd, &cfg); 1162 | cfmakeraw(&cfg); 1163 | cfsetispeed(&cfg, B115200); 1164 | cfsetospeed(&cfg, B115200); 1165 | tcsetattr(tty_fd, TCSANOW, &cfg); 1166 | 1167 | DBG("Boost baudrate to 115200."); 1168 | } 1169 | 1170 | 1171 | static void 1172 | gps_state_init(GpsState* state) 1173 | { 1174 | state->control[0] = -1; 1175 | state->control[1] = -1; 1176 | state->fd = -1; 1177 | 1178 | DBG("Try open gps hardware: %s", GPS_CHANNEL_NAME); 1179 | state->fd = open(GPS_CHANNEL_NAME, O_RDONLY | O_NONBLOCK | O_NOCTTY); // support poll behavior 1180 | //state->fd = open(GPS_CHANNEL_NAME, O_RDWR | O_NONBLOCK | O_NOCTTY); 1181 | int epoll_fd = epoll_create(2); 1182 | state->epoll_hd = epoll_fd; 1183 | 1184 | if (state->fd < 0) { 1185 | ERR("no gps hardware detected: %s:%d, %s", GPS_CHANNEL_NAME, state->fd, strerror(errno)); 1186 | return; 1187 | } 1188 | 1189 | struct termios cfg; 1190 | tcgetattr(state->fd, &cfg); 1191 | cfmakeraw(&cfg); 1192 | cfsetispeed(&cfg, TTY_BAUD); 1193 | cfsetospeed(&cfg, TTY_BAUD); 1194 | tcsetattr(state->fd, TCSANOW, &cfg); 1195 | 1196 | DBG("Open gps hardware succeed: %s", GPS_CHANNEL_NAME); 1197 | 1198 | if (socketpair(AF_LOCAL, SOCK_STREAM, 0, state->control) < 0) { 1199 | ERR("could not create thread control socket pair: %s", strerror(errno)); 1200 | goto Fail; 1201 | } 1202 | state->thread = callback_backup.create_thread_cb(gps_native_thread, gps_state_thread, state); 1203 | if (!state->thread) { 1204 | ERR("could not create gps thread: %s", strerror(errno)); 1205 | goto Fail; 1206 | } 1207 | 1208 | DBG("gps state initialized, the thread is %d\n", (int)state->thread); 1209 | 1210 | if (REDUCE_SV_FREQ) { 1211 | // Set GSA and GSV outputs once per 2 seconds 1212 | char msg[] = "$PCAS03,,,2,2,,,,,,,,,,*02\r\n"; /* NMEA command, write(fd, cmdbuf, strlen(cmdbuf)); */ 1213 | 1214 | write(state->fd, msg, strlen(msg)); 1215 | } 1216 | 1217 | // Try Boost baudrate 1218 | gpstty_boost(state->fd, TTY_BAUD); 1219 | 1220 | return; 1221 | 1222 | Fail: 1223 | gps_state_done(state); 1224 | } 1225 | 1226 | 1227 | /*****************************************************************/ 1228 | /*****************************************************************/ 1229 | /***** *****/ 1230 | /***** I N T E R F A C E *****/ 1231 | /***** *****/ 1232 | /*****************************************************************/ 1233 | /*****************************************************************/ 1234 | static int 1235 | zkw_gps_init(GpsCallbacks* callbacks) 1236 | { 1237 | GpsState* s = _gps_state; 1238 | int get_time = 20; 1239 | int res = 0; 1240 | if (s->init) 1241 | return 0; 1242 | 1243 | 1244 | s->callbacks = *callbacks; 1245 | callback_backup = *callbacks; 1246 | 1247 | gps_state_init(s); 1248 | if (s->fd < 0) { 1249 | return -1; 1250 | } 1251 | DBG("Set GPS_CAPABILITY_SCHEDULING \n"); 1252 | callback_backup.set_capabilities_cb(GPS_CAPABILITY_SCHEDULING); 1253 | s->init = 1; 1254 | return 0; 1255 | } 1256 | 1257 | static void 1258 | zkw_gps_cleanup(void) 1259 | { 1260 | GpsState* s = _gps_state; 1261 | 1262 | //if (s->init) 1263 | // gps_state_done(s); 1264 | DBG("zkw_gps_cleanup done"); 1265 | // return NULL; 1266 | } 1267 | 1268 | int 1269 | zkw_gps_start() 1270 | { 1271 | GpsState* s = _gps_state; 1272 | int err; 1273 | int count=0; 1274 | 1275 | if (!s->init) { 1276 | ERR("%s: called with uninitialized state !!", __FUNCTION__); 1277 | return -1; 1278 | } 1279 | 1280 | DBG("HAL thread has initialiazed\n"); 1281 | gps_state_start(s); 1282 | 1283 | gps_status.status = GPS_STATUS_ENGINE_ON; 1284 | DBG("gps_status = GPS_STATUS_ENGINE_ON\n"); 1285 | callback_backup.status_cb(&gps_status); 1286 | gps_status.status = GPS_STATUS_SESSION_BEGIN; 1287 | DBG("gps_status = GPS_STATUS_SESSION_BEGIN\n"); 1288 | callback_backup.status_cb(&gps_status); 1289 | callback_backup.acquire_wakelock_cb(); 1290 | s->start_flag = 1; 1291 | DBG("s->start_flag = 1\n"); 1292 | return 0; 1293 | } 1294 | int 1295 | zkw_gps_stop() 1296 | { 1297 | GpsState* s = _gps_state; 1298 | int err; 1299 | int count=0; 1300 | 1301 | if (!s->init) { 1302 | ERR("%s: called with uninitialized state !!", __FUNCTION__); 1303 | return -1; 1304 | } 1305 | 1306 | gps_state_stop(s); 1307 | 1308 | gps_status.status = GPS_STATUS_SESSION_END; 1309 | callback_backup.status_cb(&gps_status); 1310 | DBG("gps_status = GPS_STATUS_SESSION_END\n"); 1311 | gps_status.status = GPS_STATUS_ENGINE_OFF; 1312 | DBG("gps_status = GPS_STATUS_ENGINE_OFF\n"); 1313 | callback_backup.status_cb(&gps_status); 1314 | callback_backup.release_wakelock_cb(); 1315 | s->start_flag = 0; 1316 | DBG("s->start_flag = 0\n"); 1317 | return 0; 1318 | } 1319 | static int 1320 | zkw_gps_inject_time(GpsUtcTime time, int64_t timeReference, int uncertainty) 1321 | { 1322 | return 0; 1323 | } 1324 | 1325 | static int 1326 | zkw_gps_inject_location(double latitude, double longitude, float accuracy) 1327 | { 1328 | return 0; 1329 | } 1330 | 1331 | static void 1332 | zkw_gps_delete_aiding_data(GpsAidingData flags) 1333 | { 1334 | return; 1335 | } 1336 | 1337 | static int 1338 | zkw_gps_set_position_mode(GpsPositionMode mode, GpsPositionRecurrence recurrence, uint32_t min_interval, uint32_t preferred_accuracy, uint32_t preferred_time) 1339 | { 1340 | // FIXME - support fix_frequency 1341 | return 0; 1342 | } 1343 | 1344 | static const void* 1345 | zkw_gps_get_extension(const char* name) 1346 | { 1347 | DBG("zkw_gps_get_extension name=[%s]\n", name); 1348 | /* 1349 | TRC(); 1350 | if (strncmp(name, "agps", strlen(name)) == 0) { 1351 | return &zkwAGpsInterface; 1352 | } 1353 | if (strncmp(name, "gps-ni", strlen(name)) == 0) { 1354 | return &zkwGpsNiInterface; 1355 | } 1356 | if (strncmp(name, "agps_ril", strlen(name)) == 0) { 1357 | return &zkwAGpsRilInterface; 1358 | } 1359 | if (strncmp(name, "supl-certificate", strlen(name)) == 0) { 1360 | return &zkwSuplCertificateInterface; 1361 | } 1362 | if (strncmp(name, GPS_MEASUREMENT_INTERFACE, strlen(name)) == 0) { 1363 | return &zkwGpsMeasurementInterface; 1364 | } 1365 | if (strncmp(name, GPS_NAVIGATION_MESSAGE_INTERFACE, strlen(name)) == 0) { 1366 | return &zkwGpsNavigationMessageInterface; 1367 | }*/ 1368 | return NULL; 1369 | } 1370 | 1371 | static const GpsInterface zkwGpsInterface = { 1372 | sizeof(GpsInterface), 1373 | zkw_gps_init, 1374 | zkw_gps_start, 1375 | zkw_gps_stop, 1376 | zkw_gps_cleanup, 1377 | zkw_gps_inject_time, 1378 | zkw_gps_inject_location, 1379 | zkw_gps_delete_aiding_data, 1380 | zkw_gps_set_position_mode, 1381 | zkw_gps_get_extension, 1382 | }; 1383 | 1384 | const GpsInterface* gps__get_gps_interface(struct gps_device_t* dev) 1385 | { 1386 | DBG("gps__get_gps_interface HAL\n"); 1387 | 1388 | return &zkwGpsInterface; 1389 | } 1390 | 1391 | static int open_gps(const struct hw_module_t* module, char const* name, 1392 | struct hw_device_t** device) { 1393 | DBG("open_gps HAL 1\n"); 1394 | struct gps_device_t *dev = malloc(sizeof(struct gps_device_t)); 1395 | if (dev != NULL) { 1396 | memset(dev, 0, sizeof(*dev)); 1397 | 1398 | dev->common.tag = HARDWARE_DEVICE_TAG; 1399 | dev->common.version = 0; 1400 | dev->common.module = (struct hw_module_t*)module; 1401 | // dev->common.close = (int (*)(struct hw_device_t*))close_lights; 1402 | DBG("open_gps HAL 2\n"); 1403 | dev->get_gps_interface = gps__get_gps_interface; 1404 | DBG("open_gps HAL 3\n"); 1405 | *device = (struct hw_device_t*)dev; 1406 | } else { 1407 | DBG("malloc failed dev = NULL!\n"); 1408 | } 1409 | return 0; 1410 | } 1411 | 1412 | 1413 | static struct hw_module_methods_t gps_module_methods = { 1414 | .open = open_gps 1415 | }; 1416 | 1417 | 1418 | struct hw_module_t HAL_MODULE_INFO_SYM = { 1419 | .tag = HARDWARE_MODULE_TAG, 1420 | .version_major = 7, 1421 | .version_minor = 2, 1422 | .id = GPS_HARDWARE_MODULE_ID, 1423 | .name = "Hardware GPS Module", 1424 | .author = "", 1425 | .methods = &gps_module_methods, 1426 | }; 1427 | --------------------------------------------------------------------------------