updated gps libs to 1.5 (credit to tytung)
This commit is contained in:
parent
945d603e1a
commit
21c182ea64
@ -38,6 +38,8 @@
|
||||
|
||||
#define LOG_TAG "gps_leo_rpc"
|
||||
|
||||
#define MEASUREMENT_PRECISION 5.0f // in meters
|
||||
#define DUMP_DATA 0
|
||||
#define GPS_DEBUG 0
|
||||
|
||||
#if GPS_DEBUG
|
||||
@ -46,14 +48,6 @@
|
||||
# define D(...) ((void)0)
|
||||
#endif
|
||||
|
||||
#define RPC_DEBUG 0
|
||||
|
||||
#if RPC_DEBUG
|
||||
# define DD(...) LOGD(__VA_ARGS__)
|
||||
#else
|
||||
# define DD(...) ((void)0)
|
||||
#endif
|
||||
|
||||
typedef struct registered_server_struct {
|
||||
/* MUST BE AT OFFSET ZERO! The client code assumes this when it overwrites
|
||||
* the XDR for server entries which represent a callback client. Those
|
||||
@ -92,6 +86,7 @@ struct SVCXPRT {
|
||||
static uint32_t client_IDs[16];//highest known value is 0xb
|
||||
static uint32_t can_send=1; //To prevent from sending get_position when EVENT_END hasn't been received
|
||||
static uint32_t has_fix=0;
|
||||
static uint32_t use_nmea=1;
|
||||
static struct CLIENT *_clnt;
|
||||
static struct timeval timeout;
|
||||
|
||||
@ -134,7 +129,7 @@ static bool_t xdr_result_int(XDR *clnt, uint32_t *result) {
|
||||
}
|
||||
|
||||
static bool_t xdr_xtra_data_args(XDR *xdrs, struct xtra_data_params *xtra_data) {
|
||||
DD("%s() is called: 0x%x, %d, %d, %d", __FUNCTION__, (int) xtra_data->xtra_data_ptr, xtra_data->part_len, xtra_data->part, xtra_data->total_parts);
|
||||
//D("%s() is called: 0x%x, %d, %d, %d", __FUNCTION__, (int) xtra_data->xtra_data_ptr, xtra_data->part_len, xtra_data->part, xtra_data->total_parts);
|
||||
|
||||
if (!xdr_u_long(xdrs, &xtra_data->data[0]))
|
||||
return 0;
|
||||
@ -157,7 +152,7 @@ static bool_t xdr_xtra_data_args(XDR *xdrs, struct xtra_data_params *xtra_data)
|
||||
}
|
||||
|
||||
bool_t xdr_pdsm_xtra_time_info(XDR *xdrs, pdsm_xtra_time_info_type *time_info_ptr) {
|
||||
DD("%s() is called: %d, %d", __FUNCTION__, (int) time_info_ptr->time_utc, (int) time_info_ptr->uncertainty);
|
||||
//D("%s() is called: %lld, %d", __FUNCTION__, time_info_ptr->time_utc, time_info_ptr->uncertainty);
|
||||
|
||||
if (!xdr_u_quad_t(xdrs, &time_info_ptr->time_utc))
|
||||
return 0;
|
||||
@ -172,7 +167,7 @@ bool_t xdr_pdsm_xtra_time_info(XDR *xdrs, pdsm_xtra_time_info_type *time_info_pt
|
||||
}
|
||||
|
||||
static bool_t xdr_xtra_time_args(XDR *xdrs, struct xtra_time_params *xtra_time) {
|
||||
DD("%s() is called", __FUNCTION__);
|
||||
//D("%s() is called", __FUNCTION__);
|
||||
|
||||
if (!xdr_u_long(xdrs, &xtra_time->data[0]))
|
||||
return 0;
|
||||
@ -377,7 +372,7 @@ int pdsm_xtra_set_data(struct CLIENT *clnt, int val0, int client_ID, int val2, u
|
||||
(caddr_t) &xtra_data,
|
||||
(xdrproc_t) xdr_result_int,
|
||||
(caddr_t) &res, timeout);
|
||||
D("%s() is called: clnt_stat=%d", __FUNCTION__, cs);
|
||||
//D("%s() is called: clnt_stat=%d", __FUNCTION__, cs);
|
||||
if (cs != RPC_SUCCESS){
|
||||
D("pdsm_xtra_set_data(%x, %x, %d, 0x%x, %d, %d, %d, %d) failed\n", val0, client_ID, val2, (int) xtra_data_ptr, part_len, part, total_parts, val3);
|
||||
free(xtra_data.data);
|
||||
@ -402,13 +397,13 @@ int pdsm_xtra_inject_time_info(struct CLIENT *clnt, int val0, int client_ID, int
|
||||
(caddr_t) &xtra_time,
|
||||
(xdrproc_t) xdr_result_int,
|
||||
(caddr_t) &res, timeout);
|
||||
DD("%s() is called: clnt_stat=%d", __FUNCTION__, cs);
|
||||
//D("%s() is called: clnt_stat=%d", __FUNCTION__, cs);
|
||||
if (cs != RPC_SUCCESS){
|
||||
D("pdsm_xtra_inject_time_info(%x, %x, %d, %d, %d) failed\n", val0, client_ID, val2, (int) time_info_ptr->time_utc, (int) time_info_ptr->uncertainty);
|
||||
D("pdsm_xtra_inject_time_info(%x, %x, %d, %lld, %d) failed\n", val0, client_ID, val2, time_info_ptr->time_utc, time_info_ptr->uncertainty);
|
||||
free(xtra_time.data);
|
||||
exit(-1);
|
||||
}
|
||||
D("pdsm_xtra_inject_time_info(%x, %x, %d, %d, %d)=%d\n", val0, client_ID, val2, (int) time_info_ptr->time_utc, (int) time_info_ptr->uncertainty, res);
|
||||
D("pdsm_xtra_inject_time_info(%x, %x, %d, %lld, %d)=%d\n", val0, client_ID, val2, time_info_ptr->time_utc, time_info_ptr->uncertainty, res);
|
||||
free(xtra_time.data);
|
||||
return res;
|
||||
}
|
||||
@ -501,24 +496,26 @@ void dispatch_pdsm_pd(uint32_t *data) {
|
||||
fix.flags = 0;
|
||||
if(event&PDSM_PD_EVENT_POSITION) {
|
||||
D("PDSM_PD_EVENT_POSITION");
|
||||
if (use_nmea) return;
|
||||
|
||||
GpsSvStatus ret;
|
||||
int i;
|
||||
ret.num_svs=ntohl(data[82]) & 0x1F;
|
||||
|
||||
// debugged by tytung
|
||||
//DD("pd %3d: %08x ", 77, ntohl(data[77]));
|
||||
#if DUMP_DATA
|
||||
//D("pd %3d: %08x ", 77, ntohl(data[77]));
|
||||
for(i=60;i<83;++i) {
|
||||
DD("pd %3d: %08x ", i, ntohl(data[i]));
|
||||
D("pd %3d: %08x ", i, ntohl(data[i]));
|
||||
}
|
||||
for(i=83;i<83+3*(ret.num_svs-1)+3;++i) {
|
||||
DD("pd %3d: %d ", i, ntohl(data[i]));
|
||||
D("pd %3d: %d ", i, ntohl(data[i]));
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
for(i=0;i<ret.num_svs;++i) {
|
||||
ret.sv_list[i].prn=ntohl(data[83+3*i]);
|
||||
ret.sv_list[i].elevation=ntohl(data[83+3*i+1]);
|
||||
ret.sv_list[i].azimuth=ntohl(data[83+3*i+2])/100;
|
||||
ret.sv_list[i].azimuth=(float)ntohl(data[83+3*i+2])/100.0f;
|
||||
ret.sv_list[i].snr=ntohl(data[83+3*i+2])%100;
|
||||
}
|
||||
ret.used_in_fix_mask=ntohl(data[77]);
|
||||
@ -537,7 +534,7 @@ void dispatch_pdsm_pd(uint32_t *data) {
|
||||
|
||||
if (ntohl(data[75])) {
|
||||
fix.flags |= GPS_LOCATION_HAS_ACCURACY;
|
||||
fix.accuracy = (float)ntohl(data[75]) / 10.0f * 2.5; // Measurement Precision = 2.5
|
||||
fix.accuracy = (float)ntohl(data[75]) / 10.0f * MEASUREMENT_PRECISION;
|
||||
}
|
||||
|
||||
union {
|
||||
@ -558,6 +555,7 @@ void dispatch_pdsm_pd(uint32_t *data) {
|
||||
if (event&PDSM_PD_EVENT_VELOCITY)
|
||||
{
|
||||
D("PDSM_PD_EVENT_VELOCITY");
|
||||
if (use_nmea) return;
|
||||
fix.flags |= GPS_LOCATION_HAS_SPEED|GPS_LOCATION_HAS_BEARING;
|
||||
fix.speed = (float)ntohl(data[66]) / 10.0f / 3.6f; // convert kp/h to m/s
|
||||
fix.bearing = (float)ntohl(data[67]) / 10.0f;
|
||||
@ -565,6 +563,7 @@ void dispatch_pdsm_pd(uint32_t *data) {
|
||||
if (event&PDSM_PD_EVENT_HEIGHT)
|
||||
{
|
||||
D("PDSM_PD_EVENT_HEIGHT");
|
||||
if (use_nmea) return;
|
||||
fix.flags |= GPS_LOCATION_HAS_ALTITUDE;
|
||||
fix.altitude = (double)ntohl(data[64]) / 10.0f;
|
||||
}
|
||||
@ -587,18 +586,21 @@ void dispatch_pdsm_ext(uint32_t *data) {
|
||||
GpsSvStatus ret;
|
||||
int i;
|
||||
|
||||
if (use_nmea) return;
|
||||
if (has_fix) return;
|
||||
|
||||
ret.num_svs=ntohl(data[8]);
|
||||
D("%s() is called. num_svs=%d", __FUNCTION__, ret.num_svs);
|
||||
// debugged by tytung
|
||||
|
||||
#if DUMP_DATA
|
||||
for(i=0;i<12;++i) {
|
||||
DD("e %3d: %08x ", i, ntohl(data[i]));
|
||||
D("e %3d: %08x ", i, ntohl(data[i]));
|
||||
}
|
||||
for(i=101;i<101+12*(ret.num_svs-1)+6;++i) {
|
||||
DD("e %3d: %d ", i, ntohl(data[i]));
|
||||
D("e %3d: %d ", i, ntohl(data[i]));
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
for(i=0;i<ret.num_svs;++i) {
|
||||
ret.sv_list[i].prn=ntohl(data[101+12*i+1]);
|
||||
ret.sv_list[i].elevation=ntohl(data[101+12*i+5]);
|
||||
@ -682,8 +684,8 @@ int init_leo()
|
||||
xprt_register(svc);
|
||||
svc_register(svc, 0x3100005b, 0x00010001, (__dispatch_fn_t)dispatch, 0);
|
||||
svc_register(svc, 0x3100005b, 0, (__dispatch_fn_t)dispatch, 0);
|
||||
svc_register(svc, 0x3100001d, 0x00010001 /*xb93145f7*/, (__dispatch_fn_t)dispatch, 0);
|
||||
svc_register(svc, 0x3100001d, 0 /*xb93145f7*/, (__dispatch_fn_t)dispatch, 0);
|
||||
svc_register(svc, 0x3100001d, 0x00010001, (__dispatch_fn_t)dispatch, 0);
|
||||
svc_register(svc, 0x3100001d, 0, (__dispatch_fn_t)dispatch, 0);
|
||||
if(!clnt) {
|
||||
D("Failed creating client\n");
|
||||
return -1;
|
||||
@ -696,8 +698,8 @@ int init_leo()
|
||||
// PDA
|
||||
pdsm_client_init(clnt, 2);
|
||||
pdsm_client_pd_reg(clnt, 2, 0, 0, 0, 0xF3F0FFFF, 0);
|
||||
pdsm_client_pa_reg(clnt, 2, 0, 2, 0, 0x7ffefe0, 0);
|
||||
pdsm_client_ext_status_reg(clnt, 2, 0, 0, 0, 0x4, 0);
|
||||
pdsm_client_pa_reg(clnt, 2, 0, 2, 0, 0x7FFEFE0, 0);
|
||||
pdsm_client_ext_status_reg(clnt, 2, 0, 1, 0, 4, 0);
|
||||
pdsm_client_act(clnt, 2);
|
||||
|
||||
// XTRA
|
||||
@ -709,7 +711,7 @@ int init_leo()
|
||||
|
||||
// NI
|
||||
pdsm_client_init(clnt, 4);
|
||||
pdsm_client_lcs_reg(clnt, 4, 0,0,0,0x3f0, 0);
|
||||
pdsm_client_lcs_reg(clnt, 4, 0, 7, 0, 0x3F0, 0);
|
||||
pdsm_client_act(clnt, 4);
|
||||
|
||||
return 0;
|
||||
@ -728,26 +730,27 @@ int gps_xtra_set_data(unsigned char *xtra_data_ptr, uint32_t part_len, uint8_t p
|
||||
return res;
|
||||
}
|
||||
|
||||
extern int64_t elapsed_realtime();
|
||||
|
||||
int gps_xtra_inject_time_info(GpsUtcTime time, int64_t timeReference, int uncertainty)
|
||||
{
|
||||
uint32_t res = -1;
|
||||
pdsm_xtra_time_info_type time_info_ptr;
|
||||
time_info_ptr.uncertainty = uncertainty;
|
||||
time_info_ptr.time_utc = time;
|
||||
//FIXME
|
||||
//time_info_ptr.time_utc += (int64_t)(android::elapsedRealtime() - timeReference);
|
||||
time_info_ptr.time_utc += (int64_t)(elapsed_realtime() - timeReference);
|
||||
time_info_ptr.ref_to_utc_time = 1;
|
||||
time_info_ptr.force_flag = 0;
|
||||
time_info_ptr.force_flag = 1;
|
||||
res = pdsm_xtra_inject_time_info(_clnt, 0, client_IDs[0xb], 0, &time_info_ptr);
|
||||
return res;
|
||||
}
|
||||
|
||||
void gps_get_position()
|
||||
{
|
||||
D("%s() is called. can_send=%d", __FUNCTION__, can_send);
|
||||
D("%s() is called", __FUNCTION__);
|
||||
int i;
|
||||
for(i = 3; i; --i) if(!can_send) sleep(1);//Time out of 3 seconds on can_send
|
||||
can_send = 0;
|
||||
D("%s() is called. can_send=%d", __FUNCTION__, can_send);
|
||||
pdsm_get_position(_clnt,
|
||||
2, 0,
|
||||
1,
|
||||
@ -761,6 +764,7 @@ void gps_get_position()
|
||||
0, 0, 0, 0, 0,
|
||||
1, 50, 2,
|
||||
client_IDs[2]);
|
||||
can_send = 0;
|
||||
}
|
||||
|
||||
void exit_gps_rpc()
|
||||
|
||||
364
libgps/leo-gps.c
364
libgps/leo-gps.c
@ -30,14 +30,18 @@
|
||||
#include <math.h>
|
||||
#include <time.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
#define LOG_TAG "gps_leo"
|
||||
#include <cutils/log.h>
|
||||
#include <cutils/sockets.h>
|
||||
#include <gps.h>
|
||||
|
||||
#define XTRA_BLOCK_SIZE 400
|
||||
#define LOG_TAG "gps_leo"
|
||||
|
||||
#define XTRA_BLOCK_SIZE 400
|
||||
#define DISABLE_XTRA_DATA 1 // AGPS data injection not works yet
|
||||
#define DISABLE_CLEANUP 1 // fully shutting down the GPS is temporarily disabled
|
||||
|
||||
#define MEASUREMENT_PRECISION 5.0f // in meters
|
||||
#define DUMP_DATA 0
|
||||
#define GPS_DEBUG 1
|
||||
|
||||
#if GPS_DEBUG
|
||||
@ -137,6 +141,10 @@ str2int( const char* p, const char* end )
|
||||
int result = 0;
|
||||
int len = end - p;
|
||||
|
||||
if (len == 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
for ( ; len > 0; len--, p++ )
|
||||
{
|
||||
int c;
|
||||
@ -163,6 +171,10 @@ str2float( const char* p, const char* end )
|
||||
int len = end - p;
|
||||
char temp[16];
|
||||
|
||||
if (len == 0) {
|
||||
return -1.0;
|
||||
}
|
||||
|
||||
if (len >= (int)sizeof(temp))
|
||||
return 0.;
|
||||
|
||||
@ -182,15 +194,17 @@ str2float( const char* p, const char* end )
|
||||
#define NMEA_MAX_SIZE 255
|
||||
|
||||
typedef struct {
|
||||
int pos;
|
||||
int overflow;
|
||||
int utc_year;
|
||||
int utc_mon;
|
||||
int utc_day;
|
||||
int utc_diff;
|
||||
int pos;
|
||||
int overflow;
|
||||
int utc_year;
|
||||
int utc_mon;
|
||||
int utc_day;
|
||||
int utc_diff;
|
||||
GpsLocation fix;
|
||||
GpsSvStatus sv_status;
|
||||
char in[ NMEA_MAX_SIZE+1 ];
|
||||
int sv_status_changed;
|
||||
uint16_t fix_flags_cached;
|
||||
char in[ NMEA_MAX_SIZE+1 ];
|
||||
} NmeaReader;
|
||||
|
||||
typedef struct {
|
||||
@ -217,19 +231,10 @@ nmea_reader_update_utc_diff( NmeaReader* r )
|
||||
gmtime_r( &now, &tm_utc );
|
||||
localtime_r( &now, &tm_local );
|
||||
|
||||
time_local = tm_local.tm_sec +
|
||||
60*(tm_local.tm_min +
|
||||
60*(tm_local.tm_hour +
|
||||
24*(tm_local.tm_yday +
|
||||
365*tm_local.tm_year)));
|
||||
time_local = mktime(&tm_local);
|
||||
time_utc = mktime(&tm_utc);
|
||||
|
||||
time_utc = tm_utc.tm_sec +
|
||||
60*(tm_utc.tm_min +
|
||||
60*(tm_utc.tm_hour +
|
||||
24*(tm_utc.tm_yday +
|
||||
365*tm_utc.tm_year)));
|
||||
|
||||
r->utc_diff = time_utc - time_local;
|
||||
r->utc_diff = time_local - time_utc;
|
||||
D("%s() is called. utc_diff = %d", __FUNCTION__, r->utc_diff);
|
||||
}
|
||||
|
||||
@ -239,6 +244,7 @@ nmea_reader_init( NmeaReader* r )
|
||||
D("%s() is called", __FUNCTION__);
|
||||
memset( r, 0, sizeof(*r) );
|
||||
|
||||
r->fix_flags_cached = 0;
|
||||
r->pos = 0;
|
||||
r->overflow = 0;
|
||||
r->utc_year = -1;
|
||||
@ -278,11 +284,15 @@ nmea_reader_update_time( NmeaReader* r, Token tok )
|
||||
tm.tm_year = r->utc_year - 1900;
|
||||
tm.tm_mon = r->utc_mon - 1;
|
||||
tm.tm_mday = r->utc_day;
|
||||
tm.tm_isdst = -1;
|
||||
tm.tm_isdst = 0;
|
||||
|
||||
fix_time = mktime( &tm ) + r->utc_diff;
|
||||
r->fix.timestamp = (long long)fix_time * 1000+(int)(seconds*1000)%1000;
|
||||
|
||||
#if DUMP_DATA
|
||||
D("fix_time=%d", fix_time); // UTC time + utc_diff
|
||||
#endif
|
||||
|
||||
r->fix.timestamp = (long long)fix_time * 1000 + (int)(seconds*1000)%1000;;
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -359,15 +369,41 @@ nmea_reader_update_latlong( NmeaReader* r,
|
||||
static int
|
||||
nmea_reader_update_altitude( NmeaReader* r,
|
||||
Token altitude,
|
||||
Token units )
|
||||
Token units,
|
||||
Token geoid_height )
|
||||
{
|
||||
Token tok = altitude;
|
||||
/*
|
||||
* Height can be measured in two ways.
|
||||
* The altitude we get from NMEA is H.
|
||||
* The altitude in gps.h is defined as h.
|
||||
* So the required output must be h = H + N.
|
||||
*
|
||||
* h: Height (h) above the WGS84 reference ellipsoid.
|
||||
* H: Height (H) above Geoid (mean sea level).
|
||||
* N: Height of Geoid (mean sea level) above the WGS84 ellipsoid.
|
||||
*/
|
||||
if (altitude.p >= altitude.end)
|
||||
return -1;
|
||||
|
||||
if (geoid_height.p >= geoid_height.end)
|
||||
return -1;
|
||||
|
||||
r->fix.flags |= GPS_LOCATION_HAS_ALTITUDE;
|
||||
r->fix.altitude = str2float(altitude.p, altitude.end) + str2float(geoid_height.p, geoid_height.end);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int
|
||||
nmea_reader_update_accuracy( NmeaReader* r,
|
||||
Token accuracy )
|
||||
{
|
||||
Token tok = accuracy;
|
||||
|
||||
if (tok.p >= tok.end)
|
||||
return -1;
|
||||
|
||||
r->fix.flags |= GPS_LOCATION_HAS_ALTITUDE;
|
||||
r->fix.altitude = str2float(tok.p, tok.end);
|
||||
r->fix.flags |= GPS_LOCATION_HAS_ACCURACY;
|
||||
r->fix.accuracy = (float)str2float(tok.p, tok.end) * MEASUREMENT_PRECISION;
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -381,7 +417,7 @@ nmea_reader_update_bearing( NmeaReader* r,
|
||||
return -1;
|
||||
|
||||
r->fix.flags |= GPS_LOCATION_HAS_BEARING;
|
||||
r->fix.bearing = str2float(tok.p, tok.end);
|
||||
r->fix.bearing = (float)str2float(tok.p, tok.end);
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -395,7 +431,10 @@ nmea_reader_update_speed( NmeaReader* r,
|
||||
return -1;
|
||||
|
||||
r->fix.flags |= GPS_LOCATION_HAS_SPEED;
|
||||
r->fix.speed = str2float(tok.p, tok.end);
|
||||
// convert knots into m/sec (1 knot equals 1.852 km/h, 1 km/h equals 3.6 m/s)
|
||||
// since 1.852 / 3.6 is an odd value (periodic), we're calculating the quotient on the fly
|
||||
// to obtain maximum precision (we don't want 1.9999 instead of 2)
|
||||
r->fix.speed = (float)str2float(tok.p, tok.end) * 1.852 / 3.6;
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -407,18 +446,17 @@ nmea_reader_parse( NmeaReader* r )
|
||||
*/
|
||||
NmeaTokenizer tzer[1];
|
||||
Token tok;
|
||||
int i;
|
||||
int report_nmea = 0;
|
||||
|
||||
D("Received: '%.*s'", r->pos, r->in);
|
||||
#if DUMP_DATA & 0
|
||||
D("Received: %.*s", r->pos, r->in);
|
||||
#endif
|
||||
if (r->pos < 9) {
|
||||
#if DUMP_DATA & 0
|
||||
D("Too short. discarded.");
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
{
|
||||
struct timeval tv;
|
||||
gettimeofday(&tv, NULL);
|
||||
update_gps_nmea(tv.tv_sec*1000+tv.tv_usec/1000, r->in, r->pos);
|
||||
}
|
||||
|
||||
nmea_tokenizer_init(tzer, r->in, r->in + r->pos);
|
||||
/*
|
||||
@ -442,63 +480,145 @@ nmea_reader_parse( NmeaReader* r )
|
||||
|
||||
// ignore first two characters.
|
||||
tok.p += 2;
|
||||
if ( !memcmp(tok.p, "GGA", 3) ) {
|
||||
if ( !memcmp(tok.p, "GSV", 3) ) {
|
||||
// Satellites in View
|
||||
Token tok_num_svs = nmea_tokenizer_get(tzer, 3);
|
||||
int num_svs = str2int(tok_num_svs.p, tok_num_svs.end);
|
||||
report_nmea = 1;
|
||||
|
||||
if (num_svs > 0) {
|
||||
Token tok_total_sentences= nmea_tokenizer_get(tzer, 1);
|
||||
Token tok_sentence_no = nmea_tokenizer_get(tzer, 2);
|
||||
|
||||
int sentence_no = str2int(tok_sentence_no.p, tok_sentence_no.end);
|
||||
int total_sentences = str2int(tok_total_sentences.p, tok_total_sentences.end);
|
||||
int curr;
|
||||
int i;
|
||||
|
||||
if (sentence_no == 1) {
|
||||
r->sv_status_changed = 0;
|
||||
r->sv_status.num_svs = 0;
|
||||
memset( r->sv_status.sv_list, 0, sizeof(r->sv_status.sv_list) );
|
||||
}
|
||||
|
||||
curr = (sentence_no - 1) * 4;
|
||||
i = 0;
|
||||
while (i < 4 && r->sv_status.num_svs < num_svs) {
|
||||
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);
|
||||
|
||||
float snr = str2float(tok_snr.p, tok_snr.end);
|
||||
if (snr > 0) {
|
||||
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 = snr;
|
||||
r->sv_status.num_svs += 1;
|
||||
}
|
||||
#if DUMP_DATA
|
||||
D("GSV sentence %2d of %d: prn=%2d", curr+1, num_svs, r->sv_status.sv_list[curr].prn);
|
||||
#endif
|
||||
curr += 1;
|
||||
i += 1;
|
||||
}
|
||||
|
||||
if (sentence_no == total_sentences) {
|
||||
r->sv_status_changed = 1;
|
||||
}
|
||||
}
|
||||
|
||||
} else if ( !memcmp(tok.p, "GGA", 3) ) {
|
||||
// GPS fix
|
||||
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);
|
||||
Token tok_fix_status = nmea_tokenizer_get(tzer,6);
|
||||
report_nmea = 1;
|
||||
|
||||
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);
|
||||
// Fix quality: {0 = invalid}, {1 = GPS fix}, ...
|
||||
if (tok_fix_status.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_accuracy = nmea_tokenizer_get(tzer,8);
|
||||
Token tok_altitude = nmea_tokenizer_get(tzer,9);
|
||||
Token tok_altitudeUnits = nmea_tokenizer_get(tzer,10);
|
||||
Token tok_geoidHeight = nmea_tokenizer_get(tzer,11);
|
||||
|
||||
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_accuracy(r, tok_accuracy);
|
||||
nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits, tok_geoidHeight);
|
||||
}
|
||||
|
||||
} else if ( !memcmp(tok.p, "GSA", 3) ) {
|
||||
//Satellites are handled by RPC-side code.
|
||||
} else if ( !memcmp(tok.p, "GSV", 3) ) {
|
||||
//Satellites are handled by RPC-side code.
|
||||
} else if ( !memcmp(tok.p, "RMC", 3) ) {
|
||||
Token tok_time = nmea_tokenizer_get(tzer,1);
|
||||
Token tok_fixStatus = nmea_tokenizer_get(tzer,2);
|
||||
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);
|
||||
// Recommended minimum specific GPS/Transit data
|
||||
Token tok_fix_status = nmea_tokenizer_get(tzer, 2);
|
||||
report_nmea = 1;
|
||||
|
||||
// Status: {A = active} or {V = void}
|
||||
if (tok_fix_status.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);
|
||||
|
||||
D("in RMC, fixStatus=%c", tok_fixStatus.p[0]);
|
||||
if (tok_fixStatus.p[0] == 'A')
|
||||
{
|
||||
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 );
|
||||
}
|
||||
|
||||
} else if ( !memcmp(tok.p, "GSA", 3) ) {
|
||||
// GPS DOP and active satellites.
|
||||
Token tok_fix_status = nmea_tokenizer_get(tzer, 2);
|
||||
report_nmea = 1;
|
||||
r->sv_status.used_in_fix_mask = 0ul;
|
||||
|
||||
// {3 = 3D fix}, {2 = 2D fix}, {1 = no fix}
|
||||
if (tok_fix_status.p[0] == '3' || tok_fix_status.p[0] == '2') {
|
||||
// We have accuracy in GGA
|
||||
//Token tok_accuracy = nmea_tokenizer_get(tzer, 16);
|
||||
//nmea_reader_update_accuracy(r, tok_accuracy);
|
||||
|
||||
int i;
|
||||
for (i = 3; i <= 14; ++i) {
|
||||
Token tok_prn = nmea_tokenizer_get(tzer, i);
|
||||
int prn = str2int(tok_prn.p, tok_prn.end);
|
||||
if (prn > 0)
|
||||
r->sv_status.used_in_fix_mask |= (1ul << (prn-1));
|
||||
}
|
||||
}
|
||||
#if DUMP_DATA
|
||||
D("%s: used_in_fix_mask is 0x%x", __FUNCTION__, r->sv_status.used_in_fix_mask);
|
||||
#endif
|
||||
r->sv_status_changed = 1;
|
||||
|
||||
} else {
|
||||
tok.p -= 2;
|
||||
#if DUMP_DATA & 0
|
||||
D("unknown sentence '%.*s", tok.end-tok.p, tok.p);
|
||||
#endif
|
||||
}
|
||||
if (r->fix.flags != 0) {
|
||||
#if GPS_DEBUG
|
||||
#if DUMP_DATA
|
||||
if (r->fix.flags) {
|
||||
char temp[256];
|
||||
char* p = temp;
|
||||
char* end = p + sizeof(temp);
|
||||
struct tm utc;
|
||||
|
||||
p += snprintf( p, end-p, "sending fix" );
|
||||
p += snprintf( p, end-p, "fix" );
|
||||
if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
|
||||
p += snprintf(p, end-p, " lat=%g lon=%g", r->fix.latitude, r->fix.longitude);
|
||||
}
|
||||
@ -512,19 +632,37 @@ nmea_reader_parse( NmeaReader* r )
|
||||
p += snprintf(p, end-p, " bearing=%g", r->fix.bearing);
|
||||
}
|
||||
if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) {
|
||||
p += snprintf(p,end-p, " accuracy=%g", r->fix.accuracy);
|
||||
p += snprintf(p, end-p, " accuracy=%g", r->fix.accuracy);
|
||||
}
|
||||
if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
|
||||
time_t time = r->fix.timestamp / 1000;
|
||||
p += snprintf(p, end-p, " time=%s", ctime(&time) );
|
||||
}
|
||||
gmtime_r( (time_t*) &r->fix.timestamp, &utc );
|
||||
p += snprintf(p, end-p, " time=%s", asctime( &utc ) );
|
||||
D("%s", temp);
|
||||
}
|
||||
#endif
|
||||
if (_gps_state->callbacks.location_cb) {
|
||||
_gps_state->callbacks.location_cb( &r->fix );
|
||||
r->fix.flags = 0;
|
||||
}
|
||||
else {
|
||||
D("no callback, keeping data until needed !");
|
||||
}
|
||||
if (report_nmea) {
|
||||
struct timeval tv;
|
||||
gettimeofday(&tv, NULL);
|
||||
update_gps_nmea(tv.tv_sec*1000+tv.tv_usec/1000, r->in, r->pos);
|
||||
report_nmea = 0;
|
||||
}
|
||||
#if DUMP_DATA
|
||||
D("r->fix.flags = 0x%x", r->fix.flags);
|
||||
#endif
|
||||
if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
|
||||
if (r->fix_flags_cached > 0)
|
||||
r->fix.flags |= r->fix_flags_cached;
|
||||
r->fix_flags_cached = r->fix.flags;
|
||||
update_gps_location( &r->fix );
|
||||
#if DUMP_DATA
|
||||
D("r->fix.flags = 0x%x", r->fix.flags);
|
||||
#endif
|
||||
r->fix.flags = 0;
|
||||
}
|
||||
if (r->sv_status_changed) {
|
||||
update_gps_svstatus( &r->sv_status );
|
||||
r->sv_status_changed = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@ -572,8 +710,12 @@ static void gps_state_done( GpsState* s ) {
|
||||
|
||||
// tell the thread to quit, and wait for it
|
||||
char cmd = CMD_QUIT;
|
||||
int ret;
|
||||
void* dummy;
|
||||
write( s->control[0], &cmd, 1 );
|
||||
|
||||
do { ret=write( s->control[0], &cmd, 1 ); }
|
||||
while (ret < 0 && errno == EINTR);
|
||||
|
||||
pthread_join(s->thread, &dummy);
|
||||
|
||||
// close the control socket pair
|
||||
@ -586,7 +728,7 @@ static void gps_state_done( GpsState* s ) {
|
||||
}
|
||||
|
||||
static void gps_state_start( GpsState* s ) {
|
||||
//Navigation started.
|
||||
// Navigation started.
|
||||
update_gps_status(GPS_STATUS_SESSION_BEGIN);
|
||||
|
||||
char cmd = CMD_START;
|
||||
@ -601,7 +743,7 @@ static void gps_state_start( GpsState* s ) {
|
||||
}
|
||||
|
||||
static void gps_state_stop( GpsState* s ) {
|
||||
//Navigation ended (times out circa 10seconds ater last get_pos)
|
||||
// Navigation ended.
|
||||
update_gps_status(GPS_STATUS_SESSION_END);
|
||||
|
||||
char cmd = CMD_STOP;
|
||||
@ -640,6 +782,9 @@ static int epoll_deregister( int epoll_fd, int fd ) {
|
||||
}
|
||||
|
||||
void update_gps_location(GpsLocation *location) {
|
||||
#if DUMP_DATA
|
||||
D("%s(): GpsLocation=%f, %f", __FUNCTION__, location->latitude, location->longitude);
|
||||
#endif
|
||||
GpsState* state = _gps_state;
|
||||
//Should be made thread safe...
|
||||
if(state->callbacks.location_cb)
|
||||
@ -664,7 +809,9 @@ void update_gps_svstatus(GpsSvStatus *svstatus) {
|
||||
}
|
||||
|
||||
void update_gps_nmea(GpsUtcTime timestamp, const char* nmea, int length) {
|
||||
D("%s() is called", __FUNCTION__);
|
||||
#if DUMP_DATA
|
||||
D("%s(): length=%d, NMEA=%.*s", __FUNCTION__, length, length, nmea);
|
||||
#endif
|
||||
GpsState* state = _gps_state;
|
||||
//Should be made thread safe...
|
||||
if(state->callbacks.nmea_cb)
|
||||
@ -700,7 +847,7 @@ static void* gps_state_thread( void* arg ) {
|
||||
struct epoll_event events[2];
|
||||
int ne, nevents;
|
||||
|
||||
nevents = epoll_wait( epoll_fd, events, gps_fd>-1 ? 2 : 1, started ? _fix_frequency*1000 : -1);
|
||||
nevents = epoll_wait( epoll_fd, events, gps_fd>-1 ? 2 : 1, started ? _fix_frequency*900 : -1);
|
||||
if (nevents < 0) {
|
||||
if (errno != EINTR)
|
||||
LOGE("epoll_wait() unexpected error: %s", strerror(errno));
|
||||
@ -724,7 +871,7 @@ static void* gps_state_thread( void* arg ) {
|
||||
if (fd == control_fd) {
|
||||
char cmd = 255;
|
||||
int ret;
|
||||
D("gps control fd event");
|
||||
//D("gps control fd event");
|
||||
do {
|
||||
ret = read( fd, &cmd, 1 );
|
||||
} while (ret < 0 && errno == EINTR);
|
||||
@ -745,24 +892,18 @@ static void* gps_state_thread( void* arg ) {
|
||||
}
|
||||
}
|
||||
} else if (fd == gps_fd) {
|
||||
char buff[32];
|
||||
D("gps fd event");
|
||||
for (;;) {
|
||||
int nn, ret;
|
||||
|
||||
ret = read( fd, buff, sizeof(buff) );
|
||||
if (ret < 0) {
|
||||
if (errno == EINTR)
|
||||
continue;
|
||||
if (errno != EWOULDBLOCK)
|
||||
LOGE("error while reading from gps daemon socket: %s:", strerror(errno));
|
||||
break;
|
||||
}
|
||||
//D("received %d bytes: %.*s", ret, ret, buff);
|
||||
char buf[512];
|
||||
int nn, ret;
|
||||
//D("gps fd event");
|
||||
do {
|
||||
ret = read( fd, buf, sizeof(buf) );
|
||||
} while (ret < 0 && errno == EINTR);
|
||||
|
||||
if (ret > 0) {
|
||||
for (nn = 0; nn < ret; nn++)
|
||||
nmea_reader_addc( reader, buff[nn] );
|
||||
nmea_reader_addc( reader, buf[nn] );
|
||||
}
|
||||
D("gps fd event end");
|
||||
//D("gps fd event end");
|
||||
} else {
|
||||
LOGE("epoll_wait() returned unkown fd %d ?", fd);
|
||||
}
|
||||
@ -780,7 +921,7 @@ static void gps_state_init( GpsState* state ) {
|
||||
state->init = 1;
|
||||
state->control[0] = -1;
|
||||
state->control[1] = -1;
|
||||
state->fd = -1; // open("/dev/smd27", O_RDONLY );
|
||||
state->fd = open("/dev/smd27", O_RDONLY);
|
||||
|
||||
if ( socketpair( AF_LOCAL, SOCK_STREAM, 0, state->control ) < 0 ) {
|
||||
LOGE("could not create thread control socket pair: %s", strerror(errno));
|
||||
@ -824,6 +965,9 @@ static int gps_xtra_init(GpsXtraCallbacks* callbacks) {
|
||||
static int gps_xtra_inject_xtra_data(char* data, int length) {
|
||||
D("%s() is called", __FUNCTION__);
|
||||
D("gps_xtra_inject_xtra_data: xtra size = %d, data ptr = 0x%x\n", length, (int) data);
|
||||
#if DISABLE_XTRA_DATA
|
||||
return 0;
|
||||
#else
|
||||
GpsState* s = _gps_state;
|
||||
if (!s->init)
|
||||
return 0;
|
||||
@ -858,7 +1002,7 @@ static int gps_xtra_inject_xtra_data(char* data, int length) {
|
||||
if (part < total_parts)
|
||||
{
|
||||
rpc_ret_val = gps_xtra_set_data(xtra_data_ptr, part_len, part, total_parts);
|
||||
if (!rpc_ret_val)
|
||||
if (rpc_ret_val == -1)
|
||||
{
|
||||
D("gps_xtra_set_data() for xtra returned %d \n", rpc_ret_val);
|
||||
ret_val = EINVAL; // return error
|
||||
@ -875,6 +1019,7 @@ static int gps_xtra_inject_xtra_data(char* data, int length) {
|
||||
}
|
||||
|
||||
return ret_val;
|
||||
#endif
|
||||
}
|
||||
|
||||
static const GpsXtraInterface sGpsXtraInterface = {
|
||||
@ -927,9 +1072,6 @@ static const AGpsInterface sAGpsInterface = {
|
||||
|
||||
/***** GpsInterface *****/
|
||||
|
||||
// fully shutting down the GPS is temporarily disabled
|
||||
#define DISABLE_CLEANUP 1
|
||||
|
||||
static int gps_init(GpsCallbacks* callbacks) {
|
||||
D("%s() is called", __FUNCTION__);
|
||||
GpsState* s = _gps_state;
|
||||
@ -984,7 +1126,7 @@ static int gps_stop() {
|
||||
|
||||
static int gps_inject_time(GpsUtcTime time, int64_t timeReference, int uncertainty) {
|
||||
D("%s() is called", __FUNCTION__);
|
||||
D("time=%d, timeReference=%d, uncertainty=%d", (int) time, (int) timeReference, uncertainty);
|
||||
D("time=%lld, timeReference=%lld, uncertainty=%d", time, timeReference, uncertainty);
|
||||
GpsState* s = _gps_state;
|
||||
if (!s->init)
|
||||
return 0;
|
||||
@ -1008,6 +1150,8 @@ static void gps_delete_aiding_data(GpsAidingData flags) {
|
||||
}
|
||||
|
||||
static int gps_set_position_mode(GpsPositionMode mode, int fix_frequency) {
|
||||
D("%s() is called", __FUNCTION__);
|
||||
D("fix_frequency=%d", fix_frequency);
|
||||
_fix_frequency=fix_frequency;
|
||||
if(_fix_frequency==0) {
|
||||
//We don't handle single shot requests atm...
|
||||
|
||||
8
libgps/time.cpp
Normal file
8
libgps/time.cpp
Normal file
@ -0,0 +1,8 @@
|
||||
#include <utils/SystemClock.h>
|
||||
|
||||
extern "C" int64_t elapsed_realtime() {
|
||||
|
||||
int64_t realtime = android::elapsedRealtime();
|
||||
|
||||
return realtime;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user