您的位置:首页 > 其它

I.MX6 GPS JNI HAL register init hacking

2015-11-03 10:58 615 查看
/************************************************************************************
*               I.MX6 GPS JNI HAL register init hacking
* 声明:
*     1. 本文主要是对Atheros GPS JNI、HAL层的代码进行初略的跟踪,主要想知道GPS设备
* 在这两层是如何注册、数据解析,目前还没分析Framework层进行分析。
*     2. 由于采用vim编辑文档,且分析文档的宽度超过博客园的文本宽度,如果想要阅读,
* 尽量cp到自己文档里面,宽度足够,用vim进行阅读比较方便。
*     3. 不要在本博客上直接阅读,请采纳2中的意见。
*
*                                   2015-11-3 深圳 阴 南山平山村 曾剑锋
***********************************************************************************/

\\\\\\\\\\\\-*- 目录 -*-/////////////
|  一、参考文章:
|  二、JNI、HAL函数注册:
|  三、Atheros GPS HAL初始化流程:
------------------------------------

一、参考文章:
GPS 研究二 (Android 2.3__gingerbread) http://blog.chinaunix.net/uid-25570748-id-184090.html android GPS HAL 回调函数实现 http://www.xuebuyuan.com/967335.html 
二、JNI、HAL函数注册:
cat /home/myzr/myandroid/frameworks/base/services/jni/com_android_server_location_GpsLocationProvider.cpp
...
static void android_location_GpsLocationProvider_class_init_native(JNIEnv* env, jclass clazz) {
int err;
hw_module_t* module;

method_reportLocation = env->GetMethodID(clazz, "reportLocation", "(IDDDFFFJ)V");
method_reportStatus = env->GetMethodID(clazz, "reportStatus", "(I)V");
method_reportSvStatus = env->GetMethodID(clazz, "reportSvStatus", "()V");
method_reportAGpsStatus = env->GetMethodID(clazz, "reportAGpsStatus", "(III)V");
method_reportNmea = env->GetMethodID(clazz, "reportNmea", "(J)V");
method_setEngineCapabilities = env->GetMethodID(clazz, "setEngineCapabilities", "(I)V");
method_xtraDownloadRequest = env->GetMethodID(clazz, "xtraDownloadRequest", "()V");
method_reportNiNotification = env->GetMethodID(clazz, "reportNiNotification",
"(IIIIILjava/lang/String;Ljava/lang/String;IILjava/lang/String;)V");
method_requestRefLocation = env->GetMethodID(clazz,"requestRefLocation","(I)V");
method_requestSetID = env->GetMethodID(clazz,"requestSetID","(I)V");
method_requestUtcTime = env->GetMethodID(clazz,"requestUtcTime","()V");

err = hw_get_module(GPS_HARDWARE_MODULE_ID, (hw_module_t const**)&module);
if (err == 0) {
hw_device_t* device;                     <----------------------------------------+
err = module->methods->open(module, GPS_HARDWARE_MODULE_ID, &device);   --------+ |
if (err == 0) {                                                                 | |
gps_device_t* gps_device = (gps_device_t *)device;                          | |
sGpsInterface = gps_device->get_gps_interface(gps_device);          --------*-*-+
}                                                                               | | |
}                                                                                   | | |
if (sGpsInterface) {                                                                | | |
sGpsXtraInterface =                                                             | | |
(const GpsXtraInterface*)sGpsInterface->get_extension(GPS_XTRA_INTERFACE);  | | |
sAGpsInterface =                                                                | | |
(const AGpsInterface*)sGpsInterface->get_extension(AGPS_INTERFACE);         | | |
sGpsNiInterface =                                                               | | |
(const GpsNiInterface*)sGpsInterface->get_extension(GPS_NI_INTERFACE);      | | |
sGpsDebugInterface =                                                            | | |
(const GpsDebugInterface*)sGpsInterface->get_extension(GPS_DEBUG_INTERFACE);| | |
sAGpsRilInterface =                                                             | | |
(const AGpsRilInterface*)sGpsInterface->get_extension(AGPS_RIL_INTERFACE);  | | |
}                                                                      |            | | |
}                                                                          +-------+    | | |
...                                                                                |    | | |
|    | | |
cat /home/myzr/myandroid/hardware/imx/libgps/gps.c                                     |    | | |
...                                                                                |    | | |
extern const GpsInterface* gps_get_hardware_interface();                ---------+ |    | | |
const GpsInterface* gps__get_gps_interface(struct gps_device_t* dev)    <--------*-*----*-*-+
{                                                                                | |    | | |
return gps_get_hardware_interface();                                ---------+ |    | | |
| |    | | |
}                                                                                | |    | | |
| |    | | |
static int open_gps(const struct hw_module_t* module, char const* name, <---------------+ | |
struct hw_device_t** device)                                             | |    | | |
{                                                                                | |    | | |
struct gps_device_t *dev = malloc(sizeof(struct gps_device_t));    ----------*-*----*-+ |
memset(dev, 0, sizeof(*dev));                                                | |    |   |
| |    |   |
dev->common.tag = HARDWARE_DEVICE_TAG;                                       | |    |   |
dev->common.version = 0;                                                     | |    |   |
dev->common.module = (struct hw_module_t*)module;                            | |    |   |
dev->get_gps_interface = gps__get_gps_interface;                   <---------*-*----*---+
| |    |
*device = (struct hw_device_t*)dev;                                          | |    |
return 0;                                                                    | |    |
}                                                                                | |    |
| |    |
static struct hw_module_methods_t gps_module_methods = {  <----- +               | |    |
.open = open_gps                                      <------*---------------*-*----+
};                                                               |               | |
|               | |
struct hw_module_t HAL_MODULE_INFO_SYM = {                       |               | |
.tag = HARDWARE_MODULE_TAG,                                  |               | |
.version_major = 1,                                          |               | |
.version_minor = 0,                                          |               | |
.id = GPS_HARDWARE_MODULE_ID,                                |               | |
.name = "Atheros GPS Module",                                |               | |
.author = "FSL MAD, Inc.",                                   |               | |
.methods = &gps_module_methods,               ---------------+               | |
};                                                                               | |
...                                                                              | |
| |
cat /home/myzr/myandroid/hardware/imx/libgps/athr_gps.c                              | |
...                                                                              | |
#ifdef ATHR_GPSNi                                                                | |
/*                                                                               | |
* Registers the callbacks for HAL to use                                        | |
*/                                                                              | |
static void athr_gps_ni_init(GpsNiCallbacks* callbacks)                          | |
{                                                                                | |
GpsState*  s = _gps_state;                                                   | |
| |
D("%s: entered", __FUNCTION__);                                              | |
| |
if (callbacks)                                                               | |
{                                                                            | |
s->ni_init = 1;                                                          | |
s->ni_callbacks = *callbacks;                                            | |
}                                                                            | |
}                                                                                | |
| |
/*                                                                               | |
* Sends a response to HAL                                                       | |
*/                                                                              | |
static void athr_gps_ni_respond(int notif_id, GpsUserResponseType user_response) | |
{                                                                                | |
// D("%s: entered", __FUNCTION__);                                           | |
}                                                                                | |
| |
static const GpsNiInterface athrGpsNiInterface = {                               | |
sizeof(GpsNiInterface),                                                      | |
athr_gps_ni_init,                                                            | |
athr_gps_ni_respond,                                                         | |
};                                                                               | |
#endif // ATHR_GPSNi                                                             | |
| |
| |
/**                                                                              | |
* 10-30 06:19:30.190: D/athr_gps(2570):                                        | |
*    athr_gps_get_extension: found xtra extension                              | |
* 10-30 06:19:30.190: D/athr_gps(2570):                                        | |
*    athr_gps_get_extension: no GPS extension for agps is found                | |
* 10-30 06:19:30.190: D/athr_gps(2570):                                        | |
*    athr_gps_get_extension: found ni extension                                | |
* 10-30 06:19:30.190: D/athr_gps(2570):                                        | |
*    athr_gps_get_extension: no GPS extension for gps-debug is found           | |
* 10-30 06:19:30.190: D/athr_gps(2570):                                        | |
*    athr_gps_get_extension: no GPS extension for agps_ril is foun             | |
*/                                                                             | |
static const void* athr_gps_get_extension(const char* name)      <---------------*-+
{                                                                                | |
if (strcmp(name, GPS_XTRA_INTERFACE) == 0)                                   | |
{                                                                            | |
#ifdef ATHR_GPSXtra                                                              | |
D("%s: found xtra extension", __FUNCTION__);                             | |
return (&athrGpsXtraInterface);                                          | |
#endif // ATHR_GPSXtra                                                           | |
}                                                                            | |
else if (strcmp(name, GPS_NI_INTERFACE) == 0)                                | |
{                                                                            | |
#ifdef ATHR_GPSNi                                                                | |
D("%s: found ni extension", __FUNCTION__);                               | |
return (&athrGpsNiInterface);                                            | |
#endif // ATHR_GPSNi                                                             | |
}                                                                            | |
D("%s: no GPS extension for %s is found", __FUNCTION__, name);               | |
return NULL;                                                                 | |
}                                                                                | |
| |
static const GpsInterface  athrGpsInterface = {      <----+                      | |
.size =sizeof(GpsInterface),                          |                      | |
.init = athr_gps_init,                                |                      | |
.start = athr_gps_start,                              |                      | |
.stop = athr_gps_stop,                                |                      | |
.cleanup = athr_gps_cleanup,                          |                      | |
.inject_time = athr_gps_inject_time,                  |                      | |
.inject_location = athr_gps_inject_location,          |                      | |
.delete_aiding_data = athr_gps_delete_aiding_data,    |                      | |
.set_position_mode = athr_gps_set_position_mode,      |                      | |
.get_extension = athr_gps_get_extension,              |                      | |
};                                                        |                      | |
|                      | |
const GpsInterface* gps_get_hardware_interface()          |  <-------------------+ |
{                                                         |                        |
return &athrGpsInterface;                        -----+                        |
}                                                                                  |
...                                                                                |
|
|
cat /home/myzr/myandroid/hardware/libhardware/include/hardware/gps.h                   |
...                                                                                |
/**                                                                                |
* Name for the GPS XTRA interface.                                                |
*/                                                                                |
#define GPS_XTRA_INTERFACE      "gps-xtra"                                         |
|
/**                                                                                |
* Name for the GPS DEBUG interface.                                               |
*/                                                                                |
#define GPS_DEBUG_INTERFACE      "gps-debug"                                       |
|
/**                                                                                |
* Name for the AGPS interface.                                                    |
*/                                                                                |
#define AGPS_INTERFACE      "agps"                                                 |
|
/**                                                                                |
* Name for NI interface                                                           |
*/                                                                                |
#define GPS_NI_INTERFACE "gps-ni"                                                  |
|
/**                                                                                |
* Name for the AGPS-RIL interface.                                                |
*/                                                                                |
#define AGPS_RIL_INTERFACE      "agps_ril"                <------------------------+
...

三、Atheros GPS HAL初始化流程:
cat /home/myzr/myandroid/hardware/imx/libgps/athr_gps.c
......
typedef struct {
int     pos;
int     overflow;
int     utc_year;
int     utc_mon;
int     utc_day;
int     utc_diff;
GpsLocation  fix;
GpsSvStatus  sv_status;
int     sv_status_changed;
char    in[ NMEA_MAX_SIZE+1 ];
int     gsa_fixed;
AthTimemap_t timemap;
} NmeaReader;                             <----+
|
typedef struct {                               |
int                     init;              |
int                     fd;                |
GpsCallbacks            callbacks;         |     <--------+
pthread_t               thread;            |              |
pthread_t           nmea_thread;           |              |
pthread_t               tmr_thread;        |              |
int                     control[2];        |              |
int                     fix_freq;          |              |
sem_t                   fix_sem;           |              |
int                     first_fix;         |              |
NmeaReader              reader;      ------+              |
#ifdef ATHR_GPSXtra                                           |
int                     xtra_init;                        |
GpsXtraCallbacks        xtra_callbacks;                   |
#endif                                                        |
#ifdef ATHR_GPSNi                                             |
int                     ni_init;                          |
GpsNiCallbacks          ni_callbacks;                     |
GpsNiNotification       ni_notification;                  |
#endif                                                        |
} GpsState;              --------------------+                |
......                                       |                |
static GpsState  _gps_state[1];          <---+  <-----------+ |
static GpsState *gps_state = _gps_state; <---+              | |
......                                                      | |
GpsCallbacks* g_gpscallback = 0;    <-----------------------*-*-+
......                                                      | | |
| | |
static const GpsInterface  athrGpsInterface = {             | | |
.size =sizeof(GpsInterface),                            | | |
.init = athr_gps_init,               -----------------+ | | |
.start = athr_gps_start,                              | | | |
.stop = athr_gps_stop,                                | | | |
.cleanup = athr_gps_cleanup,                          | | | |
.inject_time = athr_gps_inject_time,                  | | | |
.inject_location = athr_gps_inject_location,          | | | |
.delete_aiding_data = athr_gps_delete_aiding_data,    | | | |
.set_position_mode = athr_gps_set_position_mode,      | | | |
.get_extension = athr_gps_get_extension,              | | | |
};                                                        | | | |
| | | |
static int athr_gps_init(GpsCallbacks* callbacks)  <------+ | | |
{                                                           | | |
GpsState*  s = _gps_state;                 -------------+ | |
| |
D("gps state initializing %d",s->init);                   | |
| |
s->callbacks = *callbacks;       -------------------------+ |
if (!s->init)                                               |
gps_state_init(s);           ----------+                |
|                |
if(!g_gpscallback)                         |                |
g_gpscallback = callbacks;   ----------*----------------+
|
return 0;                                  |
}                                              |
|
static void                                    |
gps_state_init( GpsState*  state )   <---------+
{
...
if ( socketpair( AF_LOCAL, SOCK_STREAM, 0, state->control ) < 0 ) {
ALOGE("could not create thread control socket pair: %s", strerror(errno));
goto Fail;
}

state->thread = state->callbacks.create_thread_cb("athr_gps", gps_state_thread, state);
if (!state->thread)                                                 |
{                                                                       |
ALOGE("could not create gps thread: %s", strerror(errno));          |
goto Fail;                                                          |
}                                                                       |
state->callbacks.set_capabilities_cb(GPS_CAPABILITY_SCHEDULING);    |
D("gps state initialized");                                             |
...                                                                     |
}                                                                           |
|
static void                                         <-----------------------+
gps_state_thread( void*  arg )
{
...
state->tmr_thread = state->callbacks.create_thread_cb("athr_gps_tmr", gps_timer_thread, state);
if (!state->tmr_thread)                                                      |
{                                                                            |
ALOGE("could not create gps timer thread: %s", strerror(errno));         |
started = 0;                                                             +--------------+
state->init = STATE_INIT;                                                               |
goto Exit;                                                                              |
}                                                                                           |
|
state->nmea_thread = state->callbacks.create_thread_cb("athr_nmea_thread", gps_nmea_thread, |
state);                                                                      |          |
if (!state->nmea_thread)                                                         |          |
{                                                                                |          |
ALOGE("could not create gps nmea thread: %s", strerror(errno));              |          |
started = 0;                                                                 |          |
state->init = STATE_INIT;                                                    |          |
goto Exit;                                                                   |          |
}                                                                                |          |
...                                                                              |          |
}                                                                                    |          |
|          |
static void                                                                          |          |
gps_nmea_thread( void*  arg )                                      <-----------------+          |
{                                                                                               |
GpsState *state = (GpsState *)arg;              <----+                                      |
NmeaReader  *reader;                   -----+        |                                      |
reader = &state->reader;               -----+   -----+                                      |
|                                               |
DFR("gps entered nmea thread");             |                                               |
int versioncnt = 0;                         |                                               |
|                                               |
// now loop                                 +------------+                                  |
while (continue_thread)                                  |                                  |
{                                                        |                                  |
|                                  |
if (FD_ISSET(state->fd, &readfds))                   |                                  |
{                                                    |                                  |
memset(buf,0,sizeof(buf));                       |                                  |
ret = read( state->fd, buf, sizeof(buf) );       |                                  |
if (ret > 0)                                     |                                  |
{                                                |                                  |
if (strstr(buf, "CFG_R"))                    |                                  |
{                                            |                                  |
ALOGI("ver %s",buf);                     |                                  |
}                                            |                                  |
|                                  |
for (nn = 0; nn < ret; nn++)                 |                                  |
nmea_reader_addc( reader, buf[nn] ); <---+       -----------+               |
|               |
/* ATHR in/out sentences*/                                      |               |
if ((buf[0] == 'O') && (buf[1] == 'A') && (buf[2] == 'P')) {    |               |
D("OAP200 sentences found");                                |               |
athr_reader_parse(buf, ret);                                |               |
/*                                                          |               |
for (nn = 0; nn < ret; nn++)                                |               |
D("%.2x ", buf[nn]);                                    |               |
*/                                                          |               |
}else if ((buf[0] == '#') && (buf[1] == '!') && \               |               |
(buf[2] == 'G') && (buf[3] == 'S') && \               |               |
(buf[4] == 'M') && (buf[5] == 'A')) {                 |               |
D("GSMA sentences found");                                  |               |
athr_reader_parse(buf, ret);                                |               |
}                                                               |               |
}                                                                   |               |
else                                                                |               |
{                                                                   |               |
DFR("Error on NMEA read :%s",strerror(errno));                  |               |
gps_closetty(state);                                            |               |
GPS_STATUS_CB(state->callbacks, GPS_STATUS_SESSION_END);        |               |
sleep(3); //wait Orion shutdown.                                |               |
bOrionShutdown = 1;                                             |               |
continue;                                                       |               |
}                                                                   |               |
}                                                                       |               |
|               |
if(!continue_thread)                                                    |               |
break;                                                              |               |
}                                                                           |               |
Exit:                                                                           |               |
DFR("gps nmea thread destroyed");                                           |               |
return;                                                                     |               |
}                                                                               |               |
|               |
static void                                                                     |               |
nmea_reader_addc( NmeaReader*  r, int  c )                 <--------------------+               |
{                                                                                               |
int cnt;                                                                                    |
|
if (r->overflow) {                                                                          |
r->overflow = (c != '\n');                                                              |
return;                                                                                 |
}                                                                                           |
|
if (r->pos >= (int) sizeof(r->in)-1 ) {                                                     |
r->overflow = 1;                                                                        |
r->pos      = 0;                                                                        |
return;                                                                                 |
}                                                                                           |
|
r->in[r->pos] = (char)c;                                                                    |
r->pos       += 1;                                                                          |
|
if (c == '\n') {                                                                            |
gps_state_lock_fix(gps_state);                                                          |
nmea_reader_parse( r );            --------+                                            |
gps_state_unlock_fix(gps_state);           |                                            |
r->pos = 0;                                |                                            |
}                                              |                                            |
}                                                  |                                            |
|                                            |
static void                                        |                                            |
nmea_reader_parse( NmeaReader*  r )        <-------+                                            |
{                                                                                               |
/* we received a complete sentence, now parse it to generate                                 |
* a new GPS fix...                                                                          |
*/                                                                                          |
NmeaTokenizer  tzer[1];                                                                     |
Token          tok;                                                                         |
|
if (r->pos < 9) {                                                                           |
return;                                                                                |
}                                                                                           |
|
if (gps_state->callbacks.nmea_cb) {                                                         |
struct timeval tv;                                                                      |
unsigned long long mytimems;                                                            |
gettimeofday(&tv,NULL);                                                                 |
mytimems = tv.tv_sec * 1000 + tv.tv_usec / 1000;                                        |
gps_state->callbacks.nmea_cb(mytimems, r->in, r->pos);                                  |
}                                                                                           |
|
nmea_tokenizer_init(tzer, r->in, r->in + r->pos);                                           |
#ifdef GPS_DEBUG_TOKEN                                                                          |
{                                                                                           |
int  n;                                                                                 |
D("Found %d tokens", tzer->count);                                                      |
for (n = 0; n < tzer->count; n++) {                                                     |
Token  tok = nmea_tokenizer_get(tzer,n);                                            |
D("%2d: '%.*s'", n, tok.end-tok.p, tok.p);                                          |
}                                                                                       |
}                                                                                           |
#endif                                                                                          |
|
tok = nmea_tokenizer_get(tzer, 0);                                                          |
|
if (tok.p + 5 > tok.end) {                                                                  |
/* for $PUNV sentences */                                                               |
if ( !memcmp(tok.p, "PUNV", 4) ) {                                                      |
Token tok_cfg = nmea_tokenizer_get(tzer,1);                                         |
|
if (!memcmp(tok_cfg.p, "CFG_R", 5)) {                                               |
} else if ( !memcmp(tok_cfg.p, "QUAL", 4) ) {                                       |
Token  tok_sigma_x   = nmea_tokenizer_get(tzer, 3);                             |
|
if (tok_sigma_x.p[0] != ',') {                                                  |
Token tok_accuracy      = nmea_tokenizer_get(tzer, 10);                     |
nmea_reader_update_accuracy(r, tok_accuracy);                               |
}                                                                               |
} else if (!memcmp(tok_cfg.p, "TIMEMAP", 7))                                        |
{                                                                                   |
Token systime = nmea_tokenizer_get(tzer, 8); // system time token               |
Token timestamp = nmea_tokenizer_get(tzer, 2); // UTC time token                |
nmea_reader_update_timemap(r, systime, timestamp);                              |
}                                                                                   |
}else{                                                                                  |
}                                                                                       |
return;                                                                                 |
}                                                                                           |
|
if ( !memcmp(tok.p, "GPG", 3) ) //GPGSA,GPGGA,GPGSV                                         |
bGetFormalNMEA = 1;                                                                     |
// ignore first two characters.                                                             |
tok.p += 2;                                                                                 |
|
if ( !memcmp(tok.p, "GGA", 3) ) {                                                           |
// GPS fix                                                                              |
Token  tok_fixstaus      = nmea_tokenizer_get(tzer,6);                                  |
|
if (tok_fixstaus.p[0] > '0') {                                                          |
|
Token  tok_time          = nmea_tokenizer_get(tzer,1);                                |
Token  tok_latitude      = nmea_tokenizer_get(tzer,2);                                |
Token  tok_latitudeHemi  = nmea_tokenizer_get(tzer,3);                                |
Token  tok_longitude     = nmea_tokenizer_get(tzer,4);                                |
Token  tok_longitudeHemi = nmea_tokenizer_get(tzer,5);                                |
Token  tok_altitude      = nmea_tokenizer_get(tzer,9);                                |
Token  tok_altitudeUnits = nmea_tokenizer_get(tzer,10);                               |
|
nmea_reader_update_time(r, tok_time);                                                 |
nmea_reader_update_latlong(r, tok_latitude,                                           |
tok_latitudeHemi.p[0],                                  |
tok_longitude,                                          |
tok_longitudeHemi.p[0]);                                |
nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits);                      |
}                                                                                       |
|
} else if ( !memcmp(tok.p, "GLL", 3) ) {                                                    |
|
Token  tok_fixstaus      = nmea_tokenizer_get(tzer,6);                                  |
|
if (tok_fixstaus.p[0] == 'A') {                                                          |
|
Token  tok_latitude      = nmea_tokenizer_get(tzer,1);                                |
Token  tok_latitudeHemi  = nmea_tokenizer_get(tzer,2);                                |
Token  tok_longitude     = nmea_tokenizer_get(tzer,3);                                |
Token  tok_longitudeHemi = nmea_tokenizer_get(tzer,4);                                |
Token  tok_time          = nmea_tokenizer_get(tzer,5);                                |
|
nmea_reader_update_time(r, tok_time);                                                 |
nmea_reader_update_latlong(r, tok_latitude,                                           |
tok_latitudeHemi.p[0],                                  |
tok_longitude,                                          |
tok_longitudeHemi.p[0]);                                |
}                                                                                       |
} else if ( !memcmp(tok.p, "GSA", 3) ) {                                                    |
|
Token  tok_fixStatus   = nmea_tokenizer_get(tzer, 2);                                   |
int i;                                                                                  |
|
if (tok_fixStatus.p[0] != '\0' && tok_fixStatus.p[0] != '1') {                          |
|
r->sv_status.used_in_fix_mask = 0ul;                                                  |
|
for (i = 3; i <= 14; ++i){                                                            |
|
Token  tok_prn  = nmea_tokenizer_get(tzer, i);                                      |
int prn = str2int(tok_prn.p, tok_prn.end);                                          |
|
/* only available for PRN 1-32 */                                                    |
if ((prn > 0) && (prn < 33)){                                                       |
r->sv_status.used_in_fix_mask |= (1ul << (prn-1));                                |
r->sv_status_changed = 1;                                                         |
/* mark this parameter to identify the GSA is in fixed state */                   |
r->gsa_fixed = 1;                                                                 |
}                                                                                   |
}                                                                                     |
|
}else {                                                                                 |
if (r->gsa_fixed == 1) {                                                              |
r->sv_status.used_in_fix_mask = 0ul;                                                |
r->sv_status_changed = 1;                                                           |
r->gsa_fixed = 0;                                                                   |
}                                                                                     |
}                                                                                       |
D("%s","zengjf GSA");                                                                   |
} else if ( !memcmp(tok.p, "GSV", 3) ) {                                                    |
|
Token  tok_noSatellites  = nmea_tokenizer_get(tzer, 3);                                 |
int    noSatellites = str2int(tok_noSatellites.p, tok_noSatellites.end);                |
|
if (noSatellites > 0) {                                                                 |
|
Token  tok_noSentences   = nmea_tokenizer_get(tzer, 1);                               |
Token  tok_sentence      = nmea_tokenizer_get(tzer, 2);                               |
|
int sentence = str2int(tok_sentence.p, tok_sentence.end);                             |
int totalSentences = str2int(tok_noSentences.p, tok_noSentences.end);                 |
int curr;                                                                             |
int i;                                                                                |
|
if (sentence == 1) {                                                                   |
r->sv_status_changed = 0;                                                         |
r->sv_status.num_svs = 0;                                                         |
}                                                                                     |
|
curr = r->sv_status.num_svs;                                                          |
|
i = 0;                                                                                |
|
while (i < 4 && r->sv_status.num_svs < noSatellites){                                 |
|
Token  tok_prn = nmea_tokenizer_get(tzer, i * 4 + 4);                          |
Token  tok_elevation = nmea_tokenizer_get(tzer, i * 4 + 5);                    |
Token  tok_azimuth = nmea_tokenizer_get(tzer, i * 4 + 6);                      |
Token  tok_snr = nmea_tokenizer_get(tzer, i * 4 + 7);                          |
|
r->sv_status.sv_list[curr].prn = str2int(tok_prn.p, tok_prn.end);              |
r->sv_status.sv_list[curr].elevation =                                         |
str2float(tok_elevation.p, tok_elevation.end);                              |
r->sv_status.sv_list[curr].azimuth =                                           |
str2float(tok_azimuth.p, tok_azimuth.end);                                  |
r->sv_status.sv_list[curr].snr = str2float(tok_snr.p, tok_snr.end);            |
|
r->sv_status.num_svs += 1;                                                     |
|
curr += 1;                                                                     |
|
i += 1;                                                                        |
}                                                                                     |
|
if (sentence == totalSentences) {                                                     |
r->sv_status_changed = 1;                                                         |
}                                                                                     |
}                                                                                       |
|
} else if ( !memcmp(tok.p, "RMC", 3) )                                                      |
|
Token  tok_fixStatus     = nmea_tokenizer_get(tzer,2);                                  |
|
if (tok_fixStatus.p[0] == 'A')                                                          |
{                                                                                       |
Token  tok_time          = nmea_tokenizer_get(tzer,1);                                |
Token  tok_latitude      = nmea_tokenizer_get(tzer,3);                                |
Token  tok_latitudeHemi  = nmea_tokenizer_get(tzer,4);                                |
Token  tok_longitude     = nmea_tokenizer_get(tzer,5);                                |
Token  tok_longitudeHemi = nmea_tokenizer_get(tzer,6);                                |
Token  tok_speed         = nmea_tokenizer_get(tzer,7);                                |
Token  tok_bearing       = nmea_tokenizer_get(tzer,8);                                |
Token  tok_date          = nmea_tokenizer_get(tzer,9);                                |
|
nmea_reader_update_date( r, tok_date, tok_time );                                   |
|
nmea_reader_update_latlong( r, tok_latitude,                                        |
tok_latitudeHemi.p[0],                               |
tok_longitude,                                       |
tok_longitudeHemi.p[0] );                            |
|
nmea_reader_update_bearing( r, tok_bearing );                                       |
nmea_reader_update_speed  ( r, tok_speed );                                         |
}                                                                                       |
D("%s","zengjf RMC");                                                                   |
|
} else if ( !memcmp(tok.p, "VTG", 3) ) {                                                    |
|
Token  tok_fixStatus     = nmea_tokenizer_get(tzer,9);                                  |
|
if (tok_fixStatus.p[0] != '\0' && tok_fixStatus.p[0] != 'N')                            |
{                                                                                       |
Token  tok_bearing       = nmea_tokenizer_get(tzer,1);                              |
Token  tok_speed         = nmea_tokenizer_get(tzer,5);                              |
|
nmea_reader_update_bearing( r, tok_bearing );                                       |
nmea_reader_update_speed  ( r, tok_speed );                                         |
}                                                                                       |
|
} else if ( !memcmp(tok.p, "ZDA", 3) ) {                                                    |
|
Token  tok_time;                                                                        |
Token  tok_year  = nmea_tokenizer_get(tzer,4);                                          |
|
if (tok_year.p[0] != '\0') {                                                            |
|
Token  tok_day   = nmea_tokenizer_get(tzer,2);                                        |
Token  tok_mon   = nmea_tokenizer_get(tzer,3);                                        |
|
nmea_reader_update_cdate( r, tok_day, tok_mon, tok_year );                            |
|
}                                                                                       |
|
tok_time  = nmea_tokenizer_get(tzer,1);                                                 |
|
if (tok_time.p[0] != '\0') {                                                            |
|
nmea_reader_update_time(r, tok_time);                                                 |
|
}                                                                                       |
|
|
} else {                                                                                    |
tok.p -= 2;                                                                             |
}                                                                                           |
|
if (!gps_state->first_fix &&                                                                |
gps_state->init == STATE_INIT &&                                                        |
r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {                                             |
|
ath_send_ni_notification(r);                                                            |
|
if (gps_state->callbacks.location_cb) {                                                 |
gps_state->callbacks.location_cb( &r->fix );                                        |
r->fix.flags = 0;                                                                   |
}                                                                                       |
|
gps_state->first_fix = 1;                                                               |
}                                                                                           |
}                                                                                               |
|
static void                                                                                     |
gps_timer_thread( void*  arg )                               <----------------------------------+
{
int need_sleep = 0;
int sleep_val = 0;

GpsState *state = (GpsState *)arg;

DFR("gps entered timer thread");

do {

while(started != 1 && continue_thread) //
{
usleep(500*1000);
}

gps_state_lock_fix(state);

if ((state->reader.fix.flags & GPS_LOCATION_HAS_LAT_LONG) != 0) {

//D("gps fix cb: 0x%x", state->reader.fix.flags);

if (state->callbacks.location_cb) {
state->callbacks.location_cb( &state->reader.fix );
state->reader.fix.flags = 0;
state->first_fix = 1;
}

if (state->fix_freq == 0) {
state->fix_freq = -1;
}
}

if (state->reader.sv_status_changed != 0) {

// D("gps sv status callback");

if (state->callbacks.sv_status_cb) {
state->callbacks.sv_status_cb( &state->reader.sv_status );
state->reader.sv_status_changed = 0;
}

}

need_sleep = (state->fix_freq != -1 && (state->init != STATE_QUIT) ? 1 : 0)     ;
sleep_val = state->fix_freq;

gps_state_unlock_fix(state);

if (need_sleep) {
sleep(sleep_val);
} else {
D("won't sleep because fix_freq=%d state->init=%d",state->fix_freq, sta     te->init);
}
ath_send_ni_notification(&state->reader);
if( state->init == STATE_INIT && lastcmd == CMD_STOP && started == 1)
{
int gap = 0;
D("Wait for NMEA coming,%d,%d,%d", state->init , lastcmd, started);

while (state->init != STATE_START && bGetFormalNMEA == 0 && continue_th     read && !bOrionShutdown)
{
usleep(300*1000);
if (++gap > 100)
break;
} ;

D("Get NMEA %d and state %d",bGetFormalNMEA,state->init);
// even we don't get nmea after 30 second, still force close it
bGetFormalNMEA |= (gap >= 100);

if( state->init == STATE_INIT && lastcmd == CMD_STOP && started == 1)
{
gps_sleep(state);
}
else
{
D("User enter LM before sending sleep, so drop it");
}
}

} while(continue_thread);

DFR("gps timer thread destroyed");

return;

}
内容来自用户分享和网络整理,不保证内容的准确性,如有侵权内容,可联系管理员处理 点击这里给我发消息
标签: