rk3588-game
/
android13
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
android13/hardware/rockchip/gps/TD1030HAL/tdgnss.c

3110 lines
78 KiB
C
Executable File
Raw Blame History

#include <errno.h>
#include <pthread.h>
#include <termios.h>
#include <fcntl.h>
#include <sys/epoll.h>
#include <math.h>
#include <time.h>
#include <semaphore.h>
#include <signal.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <strings.h>
#include <cutils/sockets.h>
#include <cutils/properties.h>
#include <hardware/gps.h>
#include <dirent.h>
#include <sys/stat.h>
//#include <sys/socket.h>
//#include <arpa/inet.h>
//#include <netinet/in.h>
//#include <netdb.h>
#include "tdgnss.h"
#include "sdbp.h"
#include "update.h"
#include "agps.h"
#include "serial.h"
int agps_thread_go=0;
static struct configfile_item item_list[TD_CONFIGFILE_NUM];
unsigned char chip_standby[9]={0x23,0x3E,0x02,0x04,0x01,0x00,0x01,0x08,0x1E};
unsigned char fix_dop100[15]={0x23,0x3E,0x03,0x34,0x07,0x00,0x64,0x64,0x10,0x27,0x10,0x27,0x01,0x75,0xE7};
unsigned char gnssmode_bds[15]={0x24,0x43,0x43,0x53,0x49,0x52,0x2C,0x31,0x2C,0x30,0x2A,0x34,0x39,0x0D,0x0A};
unsigned char gnssmode_gps[15]={0x24,0x43,0x43,0x53,0x49,0x52,0x2C,0x32,0x2C,0x30,0x2A,0x34,0x41,0x0D,0x0A};
unsigned char gnssmode_gps_bds[15]={0x24,0x43,0x43,0x53,0x49,0x52,0x2C,0x33,0x2C,0x30,0x2A,0x34,0x42,0x0D,0x0A};
unsigned char gnssmode_gl[15]={0x24,0x43,0x43,0x53,0x49,0x52,0x2C,0x34,0x2C,0x30,0x2A,0x34,0x43,0x0D,0x0A};
unsigned char gnssmode_bds_gl[15]={0x24,0x43,0x43,0x53,0x49,0x52,0x2C,0x35,0x2C,0x30,0x2A,0x34,0x44,0x0D,0x0A};
unsigned char gnssmode_gps_gl[15]={0x24,0x43,0x43,0x53,0x49,0x52,0x2C,0x36,0x2C,0x30,0x2A,0x34,0x45,0x0D,0x0A};
unsigned char cold_start1[9]={0x23,0x3E,0x02,0x01,0x01,0x00,0x01,0x05,0x12};
unsigned char cold_start2[10]={0x23,0x3E,0x02,0x01,0x02,0x00,0x01,0x00,0x06,0x1B};
unsigned char get_version[8]={0x23,0x3E,0x05,0x01,0x00,0x00,0x06,0x17};
unsigned char baud9600[14]={0x23,0x3E,0x03,0x21,0x06,0x00,0x01,0x02,0x00,0x00,0x00,0x01,0x2E,0x88};
unsigned char baud19200[14]={0x23,0x3E,0x03,0x21,0x06,0x00,0x01,0x03,0x00,0x00,0x00,0x01,0x2F,0x8D};
unsigned char baud38400[14]={0x23,0x3E,0x03,0x21,0x06,0x00,0x01,0x04,0x00,0x00,0x00,0x01,0x30,0x92};
unsigned char baud57600[14]={0x23,0x3E,0x03,0x21,0x06,0x00,0x01,0x05,0x00,0x00,0x00,0x01,0x31,0x97};
unsigned char baud115200[14]={0x23,0x3E,0x03,0x21,0x06,0x00,0x01,0x06,0x00,0x00,0x00,0x01,0x32,0x9C};
unsigned int fixed[256];
enum nmea_state nmeastate;
extern TD_INFO td_agps_info;
TD1030Config tdconfig;
TDMODE td_mode = {0,0,0,0};
typedef struct
{
const char *p;
const char *end;
} Token;
#define CMD_MAX_LEN 4096
static char cmd_buff[CMD_MAX_LEN] = {0};
static int cmd_length = 0;
#define MAX_NMEA_TOKENS 32
typedef struct
{
int count;
Token tokens[ MAX_NMEA_TOKENS ];
} NmeaTokenizer;
static char *GetItemValue(char *name);
static char *remove_cr_lf(char *buf, char ch);
static int set_itemvalue(char *name, char *value);
static void intercept(char *line);
static int load_properties(const char *path);
static void LoadDriverPath(void);
static void LoadUartBaudRate(void);
static void LoadBeiDouPrnAdd(void);
static void FreeItemList(void);
static int LoadConfigFile(void);
void update_nmea(const char *p, const char *end );
void PowerOn(void);
void PowerOff(void);
int UartTxData(unsigned char *buffer, int length);
void update_gps_location(GpsLocation *location);
void update_gps_status(GpsStatusValue value);
void update_gps_svstatus(GpsSvStatus *svstatus);
static int _LibNMEA_Xor(char *p_chk_str);
static int td_set_update_mode(void);
int update_mode = 0;
struct __GSAPRN
{
int fixprnnum;
int fixprn[GPS_MAX_SVS];
// unsigned char fixprnflag;
} GsaPrn;
struct supl_agps_data td_agps_data;
GpsState _gps_state[1];
static char *GetItemValue(char *name)
{
int i;
for (i = 0; i < TD_CONFIGFILE_NUM; i++)
{
if (!item_list[i].name)
{
continue;
}
if (strcmp(item_list[i].name, name) == 0)
{
return item_list[i].value;
}
}
return NULL;
}
static char *remove_cr_lf(char *buf, char ch)
{
int len = strlen(buf);
int i;
if (len == 0)
{
return NULL;
}
for (i = len - 1; i >= 0; i--)
{
if (buf[i] == ch)
{
return &buf[i];
}
}
return NULL;
}
static int set_itemvalue(char *name, char *value)
{
int i;
for (i = 0; i < TD_CONFIGFILE_NUM; i++)
{
char *n, *v;
if (item_list[i].name && strcmp(item_list[i].name, name))
continue;
if (item_list[i].name == NULL)
{
n = malloc(strlen(name) + 1);
if (!n)
{
return -1;
}
memset(n, 0x00, strlen(name) + 1);
v = malloc(strlen(value) + 1);
if (!v)
{
free(n);
return -1;
}
memset(v, 0x00, strlen(value) + 1);
strcpy(n, name);
}
else
{
n = item_list[i].name;
v = item_list[i].value;
if (strlen(v) < strlen(value))
{
v = malloc(strlen(value) + 1);
if (!v)
{
return -1;
}
memset(v, 0x00, strlen(value) + 1);
free(item_list[i].value);
}
}
strcpy(v, value);
item_list[i].name = n;
item_list[i].value = v;
return 0;
}
return -1;
}
static void intercept(char *line)
{
char *buf = line;
char *name, *value;
if (line[0] == '#')
return;
name = strsep(&buf, "=");
value = strsep(&buf, "=");
set_itemvalue(name, value);
return ;
}
static int load_default_properties()
{
D("%s", __FUNCTION__);
strcat(tdconfig.strconfigfile.uartpath, DEFAULT_UART_DRIVER);
D("%s",tdconfig.strconfigfile.uartpath);
tdconfig.bdprnaddr = DEFAULT_BDPRNADD;
tdconfig.uartboadrate = DEFAULT_UART_BAUD_RATE;
tdconfig.gnss_mode = DEFAULT_GNSS_MODE;
tdconfig.fast_fix = 0;
tdconfig.agnss_enable = 0;
tdconfig.debugmask = 0x60;
return 0;
}
static int load_properties(const char *path)
{
char line[256];
FILE *fp;
int i = 0;
fp = fopen(path, "r");
if (!fp)
{
D("%s: can not to open %s (%d)\n", __FUNCTION__, path, errno);
return -1;
}
while (fgets(line, sizeof(line), fp))
{
i++;
char *nl;
if((line[0] == 0)||(line[0]==0xa)||(line[0]==' '))
break;
while ((nl = remove_cr_lf(line, '\n')) || (nl = remove_cr_lf(line, '\r')) )
{
*nl = '\0';
}
if (GetDebugMaskBit(D_CONFIGFILE) == 1)
{
D("%s: item open %s \n", __FUNCTION__, line);
}
intercept(line);
}
fclose(fp);
return 0;
}
static void LoadDriverPath(void)
{
char *uartpath = GetItemValue("UART_DRIVER");
if (!uartpath)
{
D("Uart Path is NULL!");
}else{
memset(tdconfig.strconfigfile.uartpath,0,sizeof(tdconfig.strconfigfile.uartpath));
strcpy(tdconfig.strconfigfile.uartpath, uartpath);
D("Uart Path : %s", tdconfig.strconfigfile.uartpath);
}
return;
}
static void LoadUartBaudRate(void)
{
char *baudrate = GetItemValue("UART_BAUD_RATE");
int selectbaudrate = 0;
if (baudrate)
{
selectbaudrate = atoi(baudrate);
}
else
{
selectbaudrate = DEFAULT_UART_BAUD_RATE;
}
if((selectbaudrate != 9600) && (selectbaudrate != 115200) && (selectbaudrate != 19200) && (selectbaudrate != 38400) && (selectbaudrate != 57600))
{
selectbaudrate = DEFAULT_UART_BAUD_RATE;
}
tdconfig.uartboadrate = selectbaudrate;
if (GetDebugMaskBit(D_CONFIGFILE) == 1)
{
D("Uart Baudrate : %d", selectbaudrate);
}
return;
}
static void LoadBeiDouPrnAdd(void)
{
char *beidouprn = GetItemValue("BDPRNADD");
int bdprnaddr = 0;
if (beidouprn)
{
bdprnaddr = atoi(beidouprn);
}
else
{
bdprnaddr = DEFAULT_BDPRNADD;
}
if(bdprnaddr <= 0)
bdprnaddr = DEFAULT_BDPRNADD;
tdconfig.bdprnaddr = bdprnaddr;
return;
}
static void LoadGnssMode(void)
{
char *p_ch = GetItemValue("GNSS_MODE");
int mode = 3;
if (p_ch)
{
mode = atoi(p_ch);
}
else
{
mode = DEFAULT_GNSS_MODE;
}
if((mode < 1) || (mode > 6))
mode = DEFAULT_GNSS_MODE;
tdconfig.gnss_mode= mode;
D("GNSS_MODE:%d",tdconfig.gnss_mode);
return;
}
static void LoadFastFix(void)
{
char *p_ch = GetItemValue("FAST_FIX");
int ff = 0;
if (p_ch)
{
ff = atoi(p_ch);
}
else
{
ff = 0;
}
if((ff < 0) || (ff > 1))
ff = 0;
tdconfig.fast_fix= ff;
D("FAST_FIX:%d",tdconfig.fast_fix);
return;
}
static void LoadAgnssEnable(void)
{
char *p_ch = GetItemValue("AGNSS_ENABLE");
int ae = 0;
if (p_ch)
{
ae = atoi(p_ch);
}
else
{
ae = 0;
}
if((ae < 0) || (ae > 1))
ae = 0;
tdconfig.agnss_enable= ae;
D("AGNSS_ENABLE:%d",tdconfig.agnss_enable);
return;
}
static void LoadDebugSetting(void)
{
char *p_ch;
unsigned int mask = 0;
// p_ch = GetItemValue("TD_DEBUG_TOKEN");
//
// if (p_ch)
// {
// mask |= atoi(p_ch)<<D_TOKEN;
// }
// else
// {
// mask |= 0<<D_TOKEN;
// }
p_ch = GetItemValue("TD_DEBUG_GSV");
if (p_ch)
{
mask |= atoi(p_ch)<<D_GSV;
}
else
{
mask |= 0<<D_GSV;
}
p_ch = GetItemValue("TD_DEBUG_GSA");
if (p_ch)
{
mask |= atoi(p_ch)<<D_GSA;
}
else
{
mask |= 0<<D_GSA;
}
p_ch = GetItemValue("TD_DEBUG_NMEA");
if (p_ch)
{
mask |= atoi(p_ch)<<D_NMEA;
}
else
{
mask |= 0<<D_NMEA;
}
p_ch = GetItemValue("TD_DEBUG_FIX");
if (p_ch)
{
mask |= atoi(p_ch)<<D_FIX;
}
else
{
mask |= 0<<D_FIX;
}
p_ch = GetItemValue("TD_DEBUG_FUCTION");
if (p_ch)
{
mask |= atoi(p_ch)<<D_FUCTION;
}
else
{
mask |= 0<<D_FUCTION;
}
p_ch = GetItemValue("TD_DEBUG_CONFIGFILE");
if (p_ch)
{
mask |= atoi(p_ch)<<D_CONFIGFILE;
}
else
{
mask |= 0<<D_CONFIGFILE;
}
tdconfig.debugmask= mask;
if (GetDebugMaskBit(D_CONFIGFILE) == 1)
{
D("[DebugMask:0x%x]",tdconfig.debugmask);
}
return;
}
unsigned int GetDebugMaskBit(enum debug_mask_bit bit)
{
return (tdconfig.debugmask>>bit)&0x01;
}
static void FreeItemList(void)
{
int i;
for (i = 0; i < TD_CONFIGFILE_NUM; i++)
{
if (!item_list[i].name)
{
continue;
}
free(item_list[i].name);
free(item_list[i].value);
item_list[i].name = NULL;
item_list[i].value = NULL;
}
return ;
}
static int LoadConfigFile(void)
{
char *path = TD_CONFIGFILE_PATH;
int n;
n = load_properties(path);
if (n < 0)
{
D("Load tdgnss.conf config file error!");
load_default_properties();
return -1;
}
LoadDebugSetting();
LoadDriverPath();
LoadUartBaudRate();
LoadBeiDouPrnAdd();
LoadGnssMode();
LoadAgnssEnable();
LoadFastFix();
FreeItemList();
D("Load config file ok!");
return 0;
}
int TdUartOpen(void)
{
int fd;
//rpdzkj
char property[PROPERTY_VALUE_MAX];
property_get("ro.factory.hasGPS", property, "true");
D("<<rpdzkj>> : gps_use_property_get*** %s ***", property);
if (strcmp(property,"false")==0) {
return -1;
}
property_get("ro.sf.gps.serial", property, "/dev/ttyS4");
D("<<rpdzkj>> : gps_uart_property_get*** %s ***", property);
// char property[PROPERTY_VALUE_MAX];
// fd = open(tdconfig.strconfigfile.uartpath, O_RDWR | O_NOCTTY | O_NONBLOCK | O_NDELAY); //O_RDONLY
fd = open(property, O_RDWR | O_NOCTTY | O_NONBLOCK | O_NDELAY); //O_RDONLY
//end
if (fd < 0)
{
perror("SERIAL ERROR : no gps_bd serial detected ");
D("ERROR : Uart Open %s", strerror(errno));
return (-1);
}
D("Succeed : open uart %s boadrate = %d\n", tdconfig.strconfigfile.uartpath, tdconfig.uartboadrate);
if ( isatty( fd ) )
{
struct termios ios;
tcgetattr( fd, &ios );
ios.c_lflag = 0;
ios.c_oflag = 0;
ios.c_iflag = 0;
ios.c_cflag |= (CLOCAL | CREAD);
ios.c_cflag &= ~CSTOPB; //stop bit 1
ios.c_cflag |= CS8; //data bit 8
ios.c_cflag &= ~PARENB; //parity non
ios.c_cflag &= ~CRTSCTS; //no flow control, need update kernel
//cfsetispeed(&ios, B115200);
//cfsetospeed(&ios, B115200);
cfsetispeed(&ios, B9600);
cfsetospeed(&ios, B9600);
tcflush(fd,TCIOFLUSH);
tcsetattr(fd, TCSANOW, &ios);
}
D("TDGPS HAL open sussed");
return fd;
}
void SetApUartbaud(int baud)
{
D("AP baud=%d\n",baud);
struct termios options;
tcgetattr(_gps_state->fd, &options );
options.c_lflag = 0;
options.c_oflag = 0;
options.c_iflag = 0;
options.c_cflag |= (CLOCAL | CREAD);
options.c_cflag &= ~CSTOPB; //stop bit 1
options.c_cflag |= CS8; //data bit 8
options.c_cflag &= ~PARENB; //parity non
options.c_cflag &= ~CRTSCTS;
if(baud==9600){
cfsetispeed(&options, B9600);
cfsetospeed(&options, B9600);
}
else if(baud==115200)
{
cfsetispeed(&options, B115200);
cfsetospeed(&options, B115200);
}
else if(baud==19200)
{
cfsetispeed(&options, B19200);
cfsetospeed(&options, B19200);
}
else if(baud==38400)
{
cfsetispeed(&options, B38400);
cfsetospeed(&options, B38400);
}
else if(baud==57600)
{
cfsetispeed(&options, B57600);
cfsetospeed(&options, B57600);
}
else
{
cfsetispeed(&options, B9600);
cfsetospeed(&options, B9600);
}
tcflush(_gps_state->fd,TCIOFLUSH);
tcsetattr(_gps_state->fd, TCSANOW, &options);
}
static int
nmea_tokenizer_init( NmeaTokenizer *t, const char *p, const char *end )
{
int count = 0;
// the initial '$' is optional
if (p < end && p[0] == '$')
p += 1;
// remove trailing newline
if (end > p && end[-1] == '\n')
{
end -= 1;
if (end > p && end[-1] == '\r')
end -= 1;
}
// get rid of checksum at the end of the sentecne
if (end >= p + 3 && end[-3] == '*')
{
end -= 3;
}
while (p < end)
{
const char *q = p;
q = memchr(p, ',', end - p);
if (q == NULL)
q = end;
if (q >= p)
{
if (count < MAX_NMEA_TOKENS)
{
t->tokens[count].p = p;
t->tokens[count].end = q;
count += 1;
}
}
if (q < end)
q += 1;
p = q;
}
t->count = count;
return count;
}
static Token
nmea_tokenizer_get( NmeaTokenizer *t, int index )
{
Token tok;
static const char *dummy = "";
static const char *changestring = "00";
if (index < 0 || index >= t->count)
{
tok.p = tok.end = dummy;
}
else if(t->tokens[index].p == t->tokens[index].end)
{
tok.p = changestring;
tok.end = changestring + 1;
}
else
tok = t->tokens[index];
return tok;
}
static int
str2int( const char *p, const char *end )
{
int result = 0;
int len = end - p;
for ( ; len > 0; len--, p++ )
{
int c;
if (p >= end)
goto Fail;
c = *p - '0';
if ((unsigned)c >= 10)
goto Fail;
result = result * 10 + c;
}
return result;
Fail:
return -1;
}
static double
str2float( const char *p, const char *end )
{
int len = end - p;
char temp[16];
if (len >= (int)sizeof(temp))
return 0.;
memcpy( temp, p, len );
temp[len] = 0;
return strtod( temp, NULL );
}
void write_agps_data(int fd)
{
if(update_mode == 1 ) return;
int i=0,cnt_w=0;
//int len;
//unsigned int temp_eph_mask;
//time_t time1,time2;
unsigned char pack_sdbpbuf[73],pack_time[24];
td_agps_data = td_agps_info.agps_data;
//temp_eph_mask = td_agps_data.ehp_new.eph_mask;
//memset(&ap_chip_agps,0,sizeof(ap_chip_agps));
//td_mode.work = 0;
UartTxData((unsigned char*)"agps",4);
//ack=0;nck=0;
if(td_agps_data.time_loc_new.mask>0){
D("\n%s",td_agps_data.time_loc_new.line);
D("\n%X\n",td_agps_data.ehp_new.eph_mask);
if((pack_sdbp_time(td_agps_data.time_loc_new.line,pack_time))==-1)
return;
write(fd,pack_time,24);
//for(i=0;i<24;i++)
// D("%X",pack_time[i]);
}else{
if(td_agps_data.ehp_new.eph_mask>0){
pack_sdbp_time_local(pack_time);
write(fd,pack_time,24);
}
}
for(i=0;i<EPH_MAX_LINES;i++){
if(((td_agps_data.ehp_new.eph_mask>>i) & 0x01) == 1)
{
//len = strlen(td_agps_data.ehp_new.line[i]);
//D("AGPS:CEP-w:%s\n",td_agps_data.ehp_new.line[i]);
memset(pack_sdbpbuf,0,sizeof(pack_sdbpbuf));
//D("\n%s\n",td_agps_data.ehp_new.line[i]);
pack_sdbp_agps(td_agps_data.ehp_new.line[i],pack_sdbpbuf);
//if(cnt_w>17)
/*{
for(j=0;j<7;j++){
D("%02X %02X %02X %02X %02X %02X %02X %02X %02X %02X",pack_sdbpbuf[j*10+0],pack_sdbpbuf[j*10+1],pack_sdbpbuf[j*10+2],pack_sdbpbuf[j*10+3],pack_sdbpbuf[j*10+4],pack_sdbpbuf[j*10+5],pack_sdbpbuf[j*10+6],pack_sdbpbuf[j*10+7],pack_sdbpbuf[j*10+8],pack_sdbpbuf[j*10+9]);
}
D("%02X %02X %02X\n",pack_sdbpbuf[70],pack_sdbpbuf[71],pack_sdbpbuf[72]);
}*/
//write(fd, td_agps_data.ehp_new.line[i], len);
write(fd,pack_sdbpbuf,73);
usleep(10000);
cnt_w++;
//if(cnt_w>17)
//break;
//D("\n cnt_w =%d \n",cnt_w);
}
}
}
char *cmd_response = 0;
static int write_agps_ok=0;
void write_agps_data_td()
{
int ret, response_size = 0;
//UartTxData(get_version,8);
D("AGPS_TD");
memset(&td_agps_info.pos_time, 0, sizeof(struct gps_time));
memset(&td_agps_info.pos, 0, sizeof(struct gps_pos));
memset(td_agps_info.gps_ephemeris, 0, sizeof(struct supl_ephemeris_s) * 32);
td_agps_info.cnt_eph = 0;
memset(&td_agps_info.eph_time, 0, sizeof(struct gps_time));
memset(&td_agps_info.agps_data.ehp_new, 0, sizeof(struct supl_ephemeris_new));
memset(&td_agps_info.agps_data.time_loc_new, 0, sizeof(struct supl_time_loc_new));
cmd_response = (char *)malloc(SUPL_RESPONSE_BUFF_SIZE);
memset(cmd_response, 0, SUPL_RESPONSE_BUFF_SIZE);
// get_agps_data();
FILE *fp;
char *p = cmd_response;
const char *path = TD_AGPSFILE_PATH;
fp = fopen(path, "r");
if(!fp)
D("no .agpsdata file\n");
else {
while(!feof(fp))
{
*p++ = fgetc(fp);
}
fclose(fp);
}
response_size = strlen(cmd_response);
// D("receive: %s",cmd_response);
D("SUPL : get response:%d bytes", response_size);
ret = handle_supl_client_response(cmd_response, response_size);
if(ret !=0){
D("agps data not ready %d",ret);
write_agps_ok=0;
}
else {
D("write agps data");
write_agps_ok=1;
int gps_fd = _gps_state->fd;
write_agps_data(gps_fd);
}
}
static time_t last_update =0;
int agps_update()
{
time_t now = time(NULL);
if(((now-last_update > 7200) ||(write_agps_ok == 0)) && tdconfig.agnss_enable) //7200
return 1;
else
return 0;
}
static void agps_thread()
{
write_agps_data_td();
last_update = time(NULL);
agps_thread_go=0;
}
#define NMEA_MAX_SIZE 255
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;
gps_location_callback callback;
char in[ NMEA_MAX_SIZE + 1 ];
} NmeaReader;
static void
nmea_reader_init( NmeaReader *r )
{
memset( r, 0, sizeof(r) );
r->pos = 0;
r->overflow = 0;
r->utc_year = -1;
r->utc_mon = -1;
r->utc_day = -1;
r->callback = NULL;
r->fix.size = sizeof(r->fix);
r->sv_status.size = sizeof(GpsSvStatus);
}
static void
nmea_reader_set_callback( NmeaReader *r, gps_location_callback cb )
{
r->callback = cb;
if (cb != NULL && r->fix.flags != 0)
{
//D("%s: sending latest fix to new callback", __FUNCTION__);
// r->callback( &r->fix );
r->fix.flags = 0;
}
}
static int
nmea_reader_update_time( NmeaReader *r, Token tok )
{
int hour, minute;
double seconds;
struct tm tm;
time_t fix_time;
time_t now;
struct tm *local_tm;
struct tm *utc_tm;
long local_time;
long utc_time;
long delta_time;
if (tok.p + 6 > tok.end)
return -1;
hour = str2int(tok.p, tok.p + 2);
minute = str2int(tok.p + 2, tok.p + 4);
seconds = str2float(tok.p + 4, tok.end);
struct utc_t GPS;
GPS.year=r->utc_year;
GPS.month=r->utc_mon;
GPS.day=r->utc_day;
GPS.hour=hour;
GPS.min=minute;
GPS.sec=seconds;
tm.tm_hour = GPS.hour;
tm.tm_min = GPS.min;
tm.tm_sec = (int) seconds;
tm.tm_year = GPS.year - 1900;
tm.tm_mon = GPS.month - 1;
tm.tm_mday = GPS.day;
tm.tm_isdst = -1;
fix_time = mktime( &tm );
now = time(NULL);
local_tm = localtime(&now);
local_time = local_tm->tm_sec + 60*(local_tm->tm_min + 60*(local_tm->tm_hour + 24*(local_tm->tm_yday + 365*local_tm->tm_year)));
utc_tm = gmtime(&now);
utc_time = utc_tm->tm_sec + 60*(utc_tm->tm_min + 60*(utc_tm->tm_hour + 24*(utc_tm->tm_yday + 365*utc_tm->tm_year)));
delta_time = local_time - utc_time;
r->fix.timestamp = (long long)(fix_time + delta_time) * 1000 + (int)(seconds * 1000) % 1000;
return 0;
}
static int
nmea_reader_update_date( NmeaReader *r, Token date, Token time )
{
Token tok = date;
int day, mon, year;
if (tok.p + 6 != tok.end)
{
D("date not properly formatted");
return -1;
}
day = str2int(tok.p, tok.p + 2);
mon = str2int(tok.p + 2, tok.p + 4);
year = str2int(tok.p + 4, tok.p + 6) + 2000;
if ((day | mon | year) < 0)
{
D("date not properly formatted");
return -1;
}
r->utc_year = year;
r->utc_mon = mon;
r->utc_day = day;
return nmea_reader_update_time( r, time );
}
static double
convert_from_hhmm( Token tok )
{
double val = str2float(tok.p, tok.end);
int degrees = (int)(floor(val) / 100);
double minutes = val - degrees * 100.;
double dcoord = degrees + minutes / 60.0;
return dcoord;
}
static int
nmea_reader_update_latlong( NmeaReader *r,
Token latitude,
char latitudeHemi,
Token longitude,
char longitudeHemi )
{
double lat, lon;
Token tok;
tok = latitude;
char temp1[30] = {0}, temp2[30] = {0};
int len1, len2;
len1 = latitude.end - latitude.p;
len2 = longitude.end - longitude.p;
memcpy(temp1, latitude.p, len1);
temp1[len1] = 0;
memcpy(temp2, longitude.p, len2);
temp2[len2] = 0;
//D("nmea latitude : %s longitude : %s ", temp1,temp2);
if (tok.p + 6 > tok.end)
{
r->fix.flags &= ~(GPS_LOCATION_HAS_LAT_LONG);
return -1;
}
lat = convert_from_hhmm(tok);
if (latitudeHemi == 'S')
lat = -lat;
tok = longitude;
if (tok.p + 6 > tok.end)
{
r->fix.flags &= ~(GPS_LOCATION_HAS_LAT_LONG);
return -1;
}
lon = convert_from_hhmm(tok);
if (longitudeHemi == 'W')
lon = -lon;
if(((lat < 0.00000001) && (lat > -0.00000001)) || ((lon < 0.00000001) && (lon > -0.00000001)))
{
r->fix.flags &= ~(GPS_LOCATION_HAS_LAT_LONG);
return -1;
}
else
{
r->fix.latitude = lat;
r->fix.longitude = lon;
r->fix.flags |= GPS_LOCATION_HAS_LAT_LONG;
}
return 0;
}
static int nmea_reader_update_altitude( NmeaReader *r,Token altitude)
{
Token tok = altitude;
char temp1[30] = {0};
int len1;
len1 = altitude.end - altitude.p;
memcpy(temp1, altitude.p, len1);
temp1[len1] = 0;
//D("nmea altitude : %s ",temp1);
if (tok.p >= tok.end){
r->fix.flags &= ~(GPS_LOCATION_HAS_ALTITUDE);
return -1;
}
r->fix.flags |= GPS_LOCATION_HAS_ALTITUDE;
if((r->fix.flags)&(GPS_LOCATION_HAS_LAT_LONG))
{
r->fix.altitude = str2float(altitude.p, altitude.end);
}
if (GetDebugMaskBit(D_FIX) == 1)
{
D("altitude : %f ", r->fix.altitude);
}
return 0;
}
static int
nmea_reader_update_accuracy( NmeaReader *r, Token accuracy )
{
Token tok = accuracy;
char temp1[30] = {0};
int len1;
len1 = accuracy.end - accuracy.p;
memcpy(temp1, accuracy.p, len1);
temp1[len1] = 0;
//D("nmea accuracy : %s ",temp1);
if (tok.p >= tok.end){
r->fix.flags &= ~(GPS_LOCATION_HAS_ACCURACY);
return -1;
}
/*
if (r->fix.accuracy == 99.99)
{
r->fix.flags &= ~(GPS_LOCATION_HAS_ACCURACY);
return 0;
}
*/
r->fix.flags |= GPS_LOCATION_HAS_ACCURACY;
if((r->fix.flags)&(GPS_LOCATION_HAS_LAT_LONG))
{
r->fix.accuracy = str2float(tok.p, tok.end);
}
else
{
r->fix.accuracy = 0.0;
}
if (GetDebugMaskBit(D_FIX) == 1)
{
D("accuracy : %f", r->fix.accuracy);
}
return 0;
}
static int
nmea_reader_update_bearing( NmeaReader *r,
Token bearing )
{
Token tok = bearing;
char temp1[30] = {0};
int len1;
len1 = bearing.end - bearing.p;
memcpy(temp1, bearing.p, len1);
temp1[len1] = 0;
//D("nmea bearing : %s ", temp1);
if (tok.p >= tok.end){
r->fix.flags &= (~GPS_LOCATION_HAS_BEARING);
return -1;
}
r->fix.flags |= GPS_LOCATION_HAS_BEARING;
if((r->fix.flags)&(GPS_LOCATION_HAS_LAT_LONG)){
r->fix.bearing = str2float(tok.p, tok.end);
}
if (GetDebugMaskBit(D_FIX) == 1)
{
D("bearing : %f ", r->fix.bearing);
}
return 0;
}
static int
nmea_reader_update_speed( NmeaReader *r,
Token speed )
{
Token tok = speed;
char temp1[30] = {0};
int len1;
len1 = speed.end - speed.p;
memcpy(temp1, speed.p, len1);
temp1[len1] = 0;
//D("nmea speed : %s ",temp1);
if (tok.p >= tok.end){
r->fix.flags &= (~GPS_LOCATION_HAS_SPEED);
return -1;
}
r->fix.flags |= GPS_LOCATION_HAS_SPEED;
/*
if((r->fix.flags)&(GPS_LOCATION_HAS_LAT_LONG)){
if((r->fix.flags)&(GPS_LOCATION_HAS_BEARING)){
r->fix.speed = (float)str2float(tok.p, tok.end) * 1.852 / 3.6;
if(r->fix.speed < 2.0) {
r->fix.speed = r->fix.speed / 2.0;
}
}
else{
r->fix.speed = 0.0;
}
}
*/
if((r->fix.flags)&(GPS_LOCATION_HAS_LAT_LONG)){
r->fix.speed = (float)str2float(tok.p, tok.end) * 1.852 / 3.6;
}
if (GetDebugMaskBit(D_FIX) == 1)
{
D("speed : %f ", r->fix.speed);
}
return 0;
}
void update_gps_svstatus(GpsSvStatus *svstatus);
static int NmeaCheck(NmeaReader *r)
{
unsigned char buff[500];
char *p, *end;
int res;
p = r->in;
end = r->in + r->pos;
memset(buff, 0, sizeof(buff));
memcpy(buff, p, (end - p));
update_nmea(r->in, r->in + r->pos);
res = _LibNMEA_Xor((char *)buff);
return res;
}
static int _LibNMEA_Xor(char *p_chk_str)
{
unsigned int check_sum = 0;
int n, i;
char crcbuf[4];
memset(crcbuf, 0, sizeof(crcbuf));
p_chk_str++;
n = strlen(p_chk_str) - 5;
if(n < 0)
return -1;
for(i = 0; i < n; i++)
{
check_sum ^= (*p_chk_str);
//D("sum:0x%x : %c",check_sum,*p_chk_str);
p_chk_str++;
if(n == 500)
return -1;
}
if(check_sum < 0x10)
sprintf(crcbuf, "0%X", check_sum);
else
sprintf(crcbuf, "%X", check_sum);
p_chk_str++;
if(memcmp(crcbuf, p_chk_str, 2) == 0)
{
return 0;
}
else
{
//D("nmea crc f : %s", p_chk_str);
return -1;
}
}
static void FixSattileCheck(int prn, NmeaReader *r)
{
int i;
if(GsaPrn.fixprnnum == 0)
return;
if((prn<=0) ||(prn>256))
return;
for(i = 0; i < GsaPrn.fixprnnum; i++)
{
if((prn == GsaPrn.fixprn[i]) || (fixed[prn] == 1))
{
fixed[prn] = 1;
r->sv_status.used_in_fix_mask |= (1 << (r->sv_status.num_svs));
return;
}
}
}
void up_svstatus(NmeaReader *r )
{
//if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s", __FUNCTION__);
if(r->callback!=NULL)
update_gps_svstatus(&r->sv_status);
memset(&GsaPrn, 0, sizeof(GsaPrn));
memset(&r->sv_status, 0, sizeof(r->sv_status));
return;
}
static void ClearSv( NmeaReader *r )
{
memset(&r->sv_status.used_in_fix_mask, 0, sizeof(r->sv_status.used_in_fix_mask));
memset(&r->sv_status, 0, sizeof(r->sv_status));
memset(&GsaPrn, 0, sizeof(GsaPrn));
r->sv_status.size = sizeof(GpsSvStatus);
return;
}
static void
nmea_reader_parse( NmeaReader *r )
{
NmeaTokenizer tzer[1];
Token tok;
int rmcmode=0;
int ret;
char gsvhead[5] = {0, 0, 0, 0, 0};
static int GNModeList[2] = {MODE_GPS,MODE_BD};//
//int i;
nmea_tokenizer_init(tzer, r->in, r->in + r->pos);
if(td_mode.work==0)
td_mode.work=1;
tdconfig.power_flag = TD_ON;
// if (GetDebugMaskBit(D_TOKEN) == 1)
// {
// int n;
// //D("Found %d tokens", tzer->count);
//
// for (n = 0; n < tzer->count; n++)
// {
// nmea_tokenizer_get(tzer, n);
//
// }
// }
tok = nmea_tokenizer_get(tzer, 0);
if (tok.p + 5 > tok.end)
{
D("nmea sentence too short, ignored.");
return;
}
memcpy(gsvhead, tok.p, 5);
tok.p += 2;
if(!memcmp(tok.p, "GSV", 3))
{
if(!memcmp(gsvhead, "GPGSV", 5))
{
Token tok_noSatellites = nmea_tokenizer_get(tzer, 3);//visibility satellites
int noSatellites = str2int(tok_noSatellites.p, tok_noSatellites.end);
if(GNModeList[1] != MODE_GPS)
{
GNModeList[0] = GNModeList[1];
GNModeList[1] = MODE_GPS;
}
if ((noSatellites > 0) && (r->sv_status.num_svs < GPS_MAX_SVS))
{
int prn;
int i = 0;
while (i < 4)
{
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);
prn = str2int(tok_prn.p, tok_prn.end);
if(prn > 0)
{
if(r->sv_status.num_svs > (GPS_MAX_SVS - 1))
{
r->sv_status_changed = 1;
return;
}
r->sv_status.sv_list[r->sv_status.num_svs].prn = prn;
FixSattileCheck(prn, r);
r->sv_status.sv_list[r->sv_status.num_svs].elevation = str2float(tok_elevation.p, tok_elevation.end);
r->sv_status.sv_list[r->sv_status.num_svs].azimuth = str2float(tok_azimuth.p, tok_azimuth.end);
r->sv_status.sv_list[r->sv_status.num_svs].snr = str2float(tok_snr.p, tok_snr.end);
r->sv_status.num_svs += 1;
r->sv_status_changed = 1;
if (GetDebugMaskBit(D_GSV) == 1)
{
D("GPGSV numsv:%d sv[%2d] prn:%d elevation:%f azimuth:%f snr:%f mask:0x%x", r->sv_status.num_svs, r->sv_status.num_svs - 1, \
r->sv_status.sv_list[r->sv_status.num_svs - 1].prn, r->sv_status.sv_list[r->sv_status.num_svs - 1].elevation, r->sv_status.sv_list[r->sv_status.num_svs - 1].azimuth, \
r->sv_status.sv_list[r->sv_status.num_svs - 1].snr, r->sv_status.used_in_fix_mask);
}
i++;
}
else
{
break;
}
}
}
}
else if(!memcmp(gsvhead, "BDGSV", 5))
{
Token tok_noSatellites = nmea_tokenizer_get(tzer, 3);//visibility satellites
int noSatellites = str2int(tok_noSatellites.p, tok_noSatellites.end);
if(GNModeList[1] != MODE_BD)
{
GNModeList[0] = GNModeList[1];
GNModeList[1] = MODE_BD;
}
if ((noSatellites > 0) && (r->sv_status.num_svs < (GPS_MAX_SVS)))
{
int prn;
int i = 0;
while (i < 4)
{
if(r->sv_status.num_svs > (GPS_MAX_SVS - 1))
{
r->sv_status_changed=1;
return;
}
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);
prn = str2int(tok_prn.p, tok_prn.end);
if(prn > 0)
{
prn += tdconfig.bdprnaddr;
r->sv_status.sv_list[r->sv_status.num_svs].prn = prn;
FixSattileCheck(prn, r);
r->sv_status.sv_list[r->sv_status.num_svs].elevation = str2float(tok_elevation.p, tok_elevation.end);
r->sv_status.sv_list[r->sv_status.num_svs].azimuth = str2float(tok_azimuth.p, tok_azimuth.end);
r->sv_status.sv_list[r->sv_status.num_svs].snr = str2float(tok_snr.p, tok_snr.end);
r->sv_status.num_svs += 1;
r->sv_status_changed = 1;
if (GetDebugMaskBit(D_GSV) == 1)
{
D("BDGSV numsv:%d sv[%2d] prn:%d elevation:%f azimuth:%f snr:%f mask:0x%x", r->sv_status.num_svs, r->sv_status.num_svs - 1, \
r->sv_status.sv_list[r->sv_status.num_svs - 1].prn, r->sv_status.sv_list[r->sv_status.num_svs - 1].elevation, r->sv_status.sv_list[r->sv_status.num_svs - 1].azimuth, \
r->sv_status.sv_list[r->sv_status.num_svs - 1].snr, r->sv_status.used_in_fix_mask);
}
i++;
}
else
{
break;
}
}
}
}
else if(!memcmp(gsvhead, "GLGSV", 5))
{
Token tok_noSatellites = nmea_tokenizer_get(tzer, 3);//visibility satellites
int noSatellites = str2int(tok_noSatellites.p, tok_noSatellites.end);
if(GNModeList[1] != MODE_GL)
{
GNModeList[0] = GNModeList[1];
GNModeList[1] = MODE_GL;
}
if ((noSatellites > 0) && (r->sv_status.num_svs < (GPS_MAX_SVS)))
{
int prn;
int i = 0;
while (i < 4)
{
if(r->sv_status.num_svs > (GPS_MAX_SVS - 1))
{
r->sv_status_changed=1;
return;
}
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);
prn = str2int(tok_prn.p, tok_prn.end);
if(prn > 0)
{
r->sv_status.sv_list[r->sv_status.num_svs].prn = prn;
FixSattileCheck(prn, r);
r->sv_status.sv_list[r->sv_status.num_svs].elevation = str2float(tok_elevation.p, tok_elevation.end);
r->sv_status.sv_list[r->sv_status.num_svs].azimuth = str2float(tok_azimuth.p, tok_azimuth.end);
r->sv_status.sv_list[r->sv_status.num_svs].snr = str2float(tok_snr.p, tok_snr.end);
r->sv_status.num_svs += 1;
r->sv_status_changed = 1;
if (GetDebugMaskBit(D_GSV) == 1)
{
D("GLGSV numsv:%d sv[%2d] prn:%d elevation:%f azimuth:%f snr:%f mask:0x%x", r->sv_status.num_svs, r->sv_status.num_svs - 1, \
r->sv_status.sv_list[r->sv_status.num_svs - 1].prn, r->sv_status.sv_list[r->sv_status.num_svs - 1].elevation, r->sv_status.sv_list[r->sv_status.num_svs - 1].azimuth, \
r->sv_status.sv_list[r->sv_status.num_svs - 1].snr, r->sv_status.used_in_fix_mask);
}
i++;
}
else
{
break;
}
}
}
}
}
///////////////////////////////////////////////////////GGA
else if ( !memcmp(tok.p, "GGA", 3) )
{
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_fixStatus = nmea_tokenizer_get(tzer, 6);
Token tok_accuracy = nmea_tokenizer_get(tzer, 8);
Token tok_altitude = nmea_tokenizer_get(tzer, 9);
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);
nmea_reader_update_time(r, tok_time);
if ((r->callback) && (tok_fixStatus.p[0] != '0'))
{
if (GetDebugMaskBit(D_FIX) == 1)
{
D("fixstatus : %c", tok_fixStatus.p[0]);
D("lat : %f lon : %f ", r->fix.latitude, r->fix.longitude);
}
r->callback( &r->fix );//callback up fix
r->fix.flags = 0;
}
///////////////////////////////////////////////////////RMC
}
else if ( !memcmp(tok.p, "RMC", 3))
{
if(!memcmp(gsvhead, "GNRMC", 5))
{
nmeastate = NMEA_GNRMC;
if(GNModeList[0] + GNModeList[1] == MODE_GPS + MODE_BD)
{
rmcmode=0x30;
}
else if(GNModeList[0] + GNModeList[1] == MODE_GPS + MODE_GL)
{
rmcmode=0x60;
}
else if(GNModeList[0] + GNModeList[1] == MODE_BD + MODE_GL)
{
rmcmode=0x50;
}
}
else if(!memcmp(gsvhead, "BDRMC", 5)){
nmeastate = NMEA_BDRMC;
rmcmode=0x10;
}
else if(!memcmp(gsvhead, "GPRMC", 5)){
nmeastate = NMEA_GPRMC;
rmcmode=0x20;
}
else if(!memcmp(gsvhead, "GLRMC", 5)){
nmeastate = NMEA_GLRMC;
rmcmode=0x40;
}
td_mode.nv_mode=rmcmode;
if((r->sv_status_changed) == 1)
{
up_svstatus(r);
r->sv_status_changed = 0;
}
else
{
ClearSv(r);
up_svstatus(r);
}
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);
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{
r->fix.flags &= (~GPS_LOCATION_HAS_BEARING);
r->fix.flags &= (~GPS_LOCATION_HAS_SPEED);
if (agps_thread_go == 0 && agps_update())
{
agps_thread_go = 1;
ret = _gps_state->callbacks.create_thread_cb("agps_thread", agps_thread, _gps_state);
if (!ret)
{
agps_thread_go = 0;
D("agps_thread: could not create thread, ret=%d, errno=%d\n", ret, errno);
}
else
{
D("agps_thread create\n");
}
}
}
///////////////////////////////////////////////////////GSA
}
else if ( !memcmp(tok.p, "GSA", 3) )
{
Token tok_fixStatus = nmea_tokenizer_get(tzer, 2);
if((!memcmp(gsvhead, "GNGSA", 5))&&(nmeastate == NMEA_GNRMC))
{
nmeastate = NMEA_1ST_GNGSA;
if(GNModeList[0] + GNModeList[1] == MODE_GPS + MODE_BD)
{
gsvhead[1]='P';
}
else if(GNModeList[0] + GNModeList[1] == MODE_GPS + MODE_GL)
{
gsvhead[1]='P';
}
else if(GNModeList[0] + GNModeList[1] == MODE_BD + MODE_GL)
{
gsvhead[1]='L';
}
}
if((!memcmp(gsvhead, "GNGSA", 5))&&(nmeastate == NMEA_1ST_GNGSA))
{
nmeastate = NMEA_2ND_GNGSA;
if(GNModeList[0] + GNModeList[1] == MODE_GPS + MODE_BD)
{
gsvhead[0]='B';
gsvhead[1]='D';
}
else if(GNModeList[0] + GNModeList[1] == MODE_GPS + MODE_GL)
{
gsvhead[0]='G';
gsvhead[1]='L';
}
else if(GNModeList[0] + GNModeList[1] == MODE_BD + MODE_GL)
{
gsvhead[0]='B';
gsvhead[1]='D';
}
}
if (tok_fixStatus.p[0] != '\0' && tok_fixStatus.p[0] != '1')
{
int i;
int prn;
if((!memcmp(gsvhead, "GPGSA", 5)) && (GsaPrn.fixprnnum < GPS_MAX_SVS))
{
for(i = 3; i < 14; i++)
{
Token tok_prn = nmea_tokenizer_get(tzer, i);
prn = str2int(tok_prn.p, tok_prn.end);
if(prn > 0)
{
if(GsaPrn.fixprnnum >= GPS_MAX_SVS)
break;
GsaPrn.fixprn[GsaPrn.fixprnnum] = prn;
if (GetDebugMaskBit(D_GSA) == 1)
{
D("GPGSA useprn=%d", GsaPrn.fixprn[GsaPrn.fixprnnum]);
}
GsaPrn.fixprnnum++;
}
else
break;
}
}
else if((!memcmp(gsvhead, "BDGSA", 5)) && (GsaPrn.fixprnnum < GPS_MAX_SVS))
{
for(i = 3; i < 14; i++)
{
Token tok_prn = nmea_tokenizer_get(tzer, i);
prn = str2int(tok_prn.p, tok_prn.end);
if(prn > 0)
{
if(GsaPrn.fixprnnum >= GPS_MAX_SVS)
break;
GsaPrn.fixprn[GsaPrn.fixprnnum] = prn + tdconfig.bdprnaddr;
if (GetDebugMaskBit(D_GSA) == 1)
{
D("BDGSA useprn=%d", GsaPrn.fixprn[GsaPrn.fixprnnum]);
}
GsaPrn.fixprnnum++;
}
else
break;
}
}
else if((!memcmp(gsvhead, "GLGSA", 5)) && (GsaPrn.fixprnnum < GPS_MAX_SVS))
{
for(i = 3; i < 14; i++)
{
Token tok_prn = nmea_tokenizer_get(tzer, i);
prn = str2int(tok_prn.p, tok_prn.end);
if(prn > 0)
{
if(GsaPrn.fixprnnum >= GPS_MAX_SVS)
break;
GsaPrn.fixprn[GsaPrn.fixprnnum] = prn;
if (GetDebugMaskBit(D_GSA) == 1)
{
D("GLGSA useprn=%d", GsaPrn.fixprn[GsaPrn.fixprnnum]);
}
GsaPrn.fixprnnum++;
}
else
break;
}
}
}
}
else
{
tok.p -= 2;
}
}
static void SDBP_Prase(int data)
{
#define Maxlen 256
static unsigned char ACKData[Maxlen] = {0};
static unsigned int DataIndex = 0;
char tmp_buff[Maxlen] = {0};
char tmp[4] = {0};
unsigned int i;
unsigned int datalen;
unsigned short crc16 = 0;
//int ret = 0;
for(i = 0; i < Maxlen - 1;i ++)
{
ACKData[i] = ACKData[i + 1];
}
ACKData[Maxlen - 1] = data;
if(DataIndex > 0)DataIndex --;
if(ACKData[Maxlen - 2] == 0x23 && ACKData[Maxlen - 1] == 0x3e)
{
DataIndex = Maxlen - 2;
return;
}
if((DataIndex + 7)< Maxlen)
{
if(ACKData[DataIndex] == 0x23 && ACKData[DataIndex + 1] == 0x3e)
{
datalen = ACKData[DataIndex + 4] + (ACKData[DataIndex + 5]<<8) + 8;
if(DataIndex + datalen - 1 < Maxlen)
{
crc16 = ACKData[DataIndex + datalen - 2] + (ACKData[DataIndex + datalen - 1]<<8);
if(crc16 != FletCher16_sdbp(&ACKData[DataIndex + 2],0,datalen - 4))
{
return;
}
}
else return;
}
else return;
}
else return;
if( ACKData[DataIndex + 2] == 0x05 && ACKData[DataIndex + 3] == 0x01)
{
snprintf(tmp_buff, datalen - 8 + 1,"%s",ACKData + DataIndex + 6);//
D("SDBP : Ver %s",tmp_buff);
}
else
{
for(i = 0;i < datalen;i ++)
{
sprintf(tmp, "%02x ",ACKData[DataIndex + i]);
strcat(tmp_buff, tmp);
}
D("SDBP : %s",tmp_buff);
}
if(update_mode == 1)//update mode rx
{
update_td_handler_return(&ACKData[DataIndex]);
}
memset(&ACKData[DataIndex],0,datalen);
DataIndex = 0;
}
static void
nmea_reader_addc( NmeaReader *r, int c )
{
if (r->overflow)
{
r->overflow = (c != '\n');//restart from a new parase
return;
}
//r->overflow
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' && r->in[r->pos - 2] == '\r')
{
if(NmeaCheck(r) == 0)
nmea_reader_parse( r );
r->pos = 0;
}
}
void update_gps_status(GpsStatusValue value)
{
// D("%s(): GpsStatusValue=%d", __FUNCTION__, value);
GpsState *state = _gps_state;
state->status.status = value;
if(state->callbacks.status_cb)
state->callbacks.status_cb(&state->status);
}
static void
td_gps_state_done( GpsState *s )
{
update_gps_status(GPS_STATUS_ENGINE_OFF);
char cmd = CMD_QUIT;
void *dummy;
write( s->control[0], &cmd, 1 );
pthread_join(s->thread, &dummy);
close( s->control[0] );
s->control[0] = -1;
close( s->control[1] );
s->control[1] = -1;
close( s->fd );
s->fd = -1;
s->init = 0;
}
static void
td_gps_state_start( GpsState *s )
{
// Navigation started.
update_gps_status(GPS_STATUS_SESSION_BEGIN);//up gps navigation status
char cmd = CMD_START;
int ret;
do
{
ret = write( s->control[0], &cmd, 1 );
}
while (ret < 0 && errno == EINTR);
if (ret != 1)
D("%s: could not send CMD_START command: ret=%d: %s",
__FUNCTION__, ret, strerror(errno));
}
static void
td_gps_state_stop( GpsState *s )
{
// Navigation ended.
update_gps_status(GPS_STATUS_SESSION_END);
char cmd = CMD_STOP;
int ret;
do
{
ret = write( s->control[0], &cmd, 1 );
}
while (ret < 0 && errno == EINTR);
if (ret != 1)
D("%s: could not send CMD_STOP command: ret=%d: %s",
__FUNCTION__, ret, strerror(errno));
}
static int
epoll_register( int epoll_fd, int fd )
{
struct epoll_event ev;
int ret, flags;
flags = fcntl(fd, F_GETFL);
fcntl(fd, F_SETFL, flags | O_NONBLOCK);
ev.events = EPOLLIN;
ev.data.fd = fd;
do
{
ret = epoll_ctl( epoll_fd, EPOLL_CTL_ADD, fd, &ev );
}
while (ret < 0 && errno == EINTR);
return ret;
}
void update_gps_location(GpsLocation *location)
{
// D("%s(): GpsLocation=%f, %f", __FUNCTION__, location->latitude, location->longitude);
GpsState *state = _gps_state;
//Should be made thread safe...
if(state->callbacks.location_cb)
state->callbacks.location_cb(location);
}
void ChipUpdateResult(unsigned char flag)
{
GpsState *state = _gps_state;
char buff[256] = {0};
int percent;
memset(buff, 0, sizeof(buff));
GpsUtcTime system_time;
struct timeval tv;
gettimeofday(&tv, NULL);
system_time = (GpsUtcTime) tv.tv_sec * 1000 + tv.tv_usec / 1000;
if(flag == 1)
{
strcpy(buff, "TD Chip Update Succeed");
D("CallBack : %s",buff);
}
else if(flag == 2 )
{
int sync_now, sync_toall;
update_td_get_sync(&sync_now, &sync_toall);
if(sync_toall > 0)
percent = 100 * sync_now / sync_toall;
else percent = 0;
sprintf(buff, "Percent:%d", percent);
D("CallBack : %s",buff);
}
else
{
strcpy(buff, "TD Chip Update Failed");
D("CallBack : %s",buff);
}
if(state->callbacks.nmea_cb != 0)
{
state->callbacks.nmea_cb(system_time, buff, strlen(buff));
}
}
void SendResultToApp(int res)
{
if(res == 2)
{
ChipUpdateResult(2);
}
else if(res == 1)
{
D("update_td success");
ChipUpdateResult(1);
update_mode = 0;
if(td_mode.on_need==0)
PowerOff();
}
else if(res == 3)
{
ChipUpdateResult(3);
D("update_td failed");
update_mode = 0;
if(td_mode.on_need==0)
PowerOff();
}
}
void update_gps_svstatus(GpsSvStatus *svstatus) // callback of svstatus in _gps_state
{
//if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s", __FUNCTION__);
GpsState *state = _gps_state;
if(state->callbacks.sv_status_cb)
state->callbacks.sv_status_cb(svstatus);
}
void update_nmea(const char *p, const char *end )
{
GpsState *state = _gps_state;
char buff[500];
memset(buff, 0, sizeof(buff));
memcpy(buff, p, (end - p));
GpsUtcTime system_time;
struct timeval tv;
gettimeofday(&tv, NULL);
system_time = (GpsUtcTime) tv.tv_sec * 1000 + tv.tv_usec / 1000;
if (GetDebugMaskBit(D_NMEA) == 1)
{
D("nmea:%s", buff);
}
if(state->callbacks.nmea_cb && update_mode == 0)
{
state->callbacks.nmea_cb(system_time, buff, end - p);
}
}
int UartTxData(unsigned char *buffer, int length)
{
int tx_len = 0;
int com_fd = _gps_state->fd;
if(com_fd != -1)
{
do
{
tx_len = write( com_fd, buffer, length );
}
while (tx_len < 0 && (errno == EINTR || errno == EAGAIN));
return tx_len;
}
else
{
return 0;
}
}
static void
gps_state_thread( void *arg )
{
GpsState *state = (GpsState *) arg;
NmeaReader reader[1];
int epoll_fd = epoll_create(2);
int started = 0;
int gps_fd = state->fd;
int control_fd = state->control[1];
int nn, ret;
char buff[32];
static int cnt=0;
int wret = 0;
//static int ack=0;
nmea_reader_init( reader );
memset(&GsaPrn, 0, sizeof(GsaPrn));
memset(fixed,0,sizeof(fixed));
// register control file descriptors for polling
epoll_register( epoll_fd, control_fd );
epoll_register( epoll_fd, gps_fd );
D("Enter : %s",__FUNCTION__);
// now loop
for (;;)
{
struct epoll_event events[2];
int ne, nevents;
nevents = epoll_wait( epoll_fd, events, 2, -1 );
if (nevents < 0)
{
if (errno != EINTR)
D("epoll_wait() unexpected error: %s", strerror(errno));
continue;
}
//D("gps thread received %d events", nevents);
for (ne = 0; ne < nevents; ne++)
{
reader[0].sv_status.size = sizeof(GpsSvStatus);
if ((events[ne].events & (EPOLLERR | EPOLLHUP)) != 0)
{
D("EPOLLERR or EPOLLHUP after epoll_wait() !?");
return;
}
if ((events[ne].events & EPOLLIN) != 0)
{
int fd = events[ne].data.fd;
if (fd == control_fd)
{
char cmd = 255;
int ret;
D("Event : gps control fd event %d",fd);
do
{
ret = read( fd, &cmd, 1 );
}
while (ret < 0 && errno == EINTR);
D("Event : gps control cmd %d",cmd);
if (cmd == CMD_QUIT)
{
D("CMD : TD gps quitting on demand");
return;
}
else if (cmd == CMD_START)
{
if (!started)
{
D("CMD : TD gps starting navigation");
memset(fixed,0,sizeof(fixed));
started = 1;
nmea_reader_set_callback( reader, state->callbacks.location_cb);
//reader->fix.flags = 0x1f;
//update_gps_location(&reader->fix);
}
}
else if (cmd == CMD_STOP)
{
if (started)
{
if(tdconfig.power_flag != TD_OFF)
{
PowerOff();
D("CMD : TD gps stopping Power off");
}
tdconfig.power_flag = TD_OFF;
started = 0;
nmea_reader_set_callback( reader, NULL );
}
}
else if(cmd == CMD_ENABLE_GPS && update_mode == 0)//
{
D("CMD : CMD_ENABLE_GPS");
memset(fixed,0,sizeof(fixed));
write( gps_fd, CMD_GPS_HOT, SET_CMD_TDMODULE_LEN);
memset(reader->sv_status.sv_list, 0, sizeof(reader->sv_status.sv_list));
}
else if(cmd == CMD_ENABLE_BD && update_mode == 0)//
{
D("CMD : CMD_ENABLE_BDS");
memset(fixed,0,sizeof(fixed));
write( gps_fd, CMD_BD_HOT, SET_CMD_TDMODULE_LEN);
memset(reader->sv_status.sv_list, 0, sizeof(reader->sv_status.sv_list));
}
else if(cmd == CMD_ENABLE_GL && update_mode == 0)//
{
D("CMD : CMD_ENABLE_GLO");
memset(fixed,0,sizeof(fixed));
write( gps_fd, CMD_GL_HOT, SET_CMD_TDMODULE_LEN);
memset(reader->sv_status.sv_list, 0, sizeof(reader->sv_status.sv_list));
}
else if(cmd == CMD_ENABLE_BD_GP && update_mode == 0)//
{
D("CMD : CMD_ENABLE_BDS_GPS");
memset(fixed,0,sizeof(fixed));
write( gps_fd, CMD_BD_GP_HOT, SET_CMD_TDMODULE_LEN);
memset(reader->sv_status.sv_list, 0, sizeof(reader->sv_status.sv_list));
}
else if(cmd == CMD_ENABLE_GP_GL && update_mode == 0)//
{
D("CMD : CMD_ENABLE_GPS_GLO");
memset(fixed,0,sizeof(fixed));
write( gps_fd, CMD_GP_GL_HOT, SET_CMD_TDMODULE_LEN);
memset(reader->sv_status.sv_list, 0, sizeof(reader->sv_status.sv_list));
}
else if(cmd == CMD_ENABLE_BD_GL && update_mode == 0)//
{
D("CMD : CMD_ENABLE_BDS_GLO");
memset(fixed,0,sizeof(fixed));
write( gps_fd, CMD_BD_GL_HOT, SET_CMD_TDMODULE_LEN);
memset(reader->sv_status.sv_list, 0, sizeof(reader->sv_status.sv_list));
}
else if(cmd == GPS_COLD && update_mode == 0)
{
D("CMD : GPS_COLD");
memset(fixed,0,sizeof(fixed));
write( gps_fd, CMD_GPS_COLD, SET_CMD_TDMODULE_LEN);
memset(reader->sv_status.sv_list, 0, sizeof(reader->sv_status.sv_list));
}
else if(cmd == GL_COLD && update_mode == 0)
{
D("CMD : GLO_COLD");
memset(fixed,0,sizeof(fixed));
write( gps_fd, CMD_GL_COLD, SET_CMD_TDMODULE_LEN);
memset(reader->sv_status.sv_list, 0, sizeof(reader->sv_status.sv_list));
}
else if(cmd == BD_COLD && update_mode == 0)
{
D("CMD : BD_COLD");
memset(fixed,0,sizeof(fixed));
write( gps_fd, CMD_BD_COLD, SET_CMD_TDMODULE_LEN);
memset(reader->sv_status.sv_list, 0, sizeof(reader->sv_status.sv_list));
}
else if(cmd == BD_GL_COLD && update_mode == 0)
{
D("CMD : BDS_GLO_COLD");
memset(fixed,0,sizeof(fixed));
write( gps_fd, CMD_BD_GL_COLD, SET_CMD_TDMODULE_LEN);
memset(reader->sv_status.sv_list, 0, sizeof(reader->sv_status.sv_list));
}
else if(cmd == BD_GP_COLD && update_mode == 0)
{
D("CMD : BDS_GPS_COLD");
memset(fixed,0,sizeof(fixed));
write( gps_fd, CMD_BD_GP_COLD, SET_CMD_TDMODULE_LEN);
memset(reader->sv_status.sv_list, 0, sizeof(reader->sv_status.sv_list));
}
else if(cmd == GP_GL_COLD && update_mode == 0)
{
D("CMD : GPS+GLO_COLD");
memset(fixed,0,sizeof(fixed));
write( gps_fd, CMD_GP_GL_COLD, SET_CMD_TDMODULE_LEN);
memset(reader->sv_status.sv_list, 0, sizeof(reader->sv_status.sv_list));
}
else if(cmd == CMD_AGPS && update_mode == 0)
{
if (started)
{
D("CMD : CMD_AGPS");
write_agps_data(gps_fd);
}
}
else if(cmd == CMD_UPDATE_TD && update_mode == 0)
{
D("CMD : CMD_UPDATE_TD update_td_handler");
Creat_update_thread();
update_mode = 1;
memset(fixed,0,sizeof(fixed));
memset(reader->sv_status.sv_list, 0, sizeof(reader->sv_status.sv_list));
}
else if(cmd == CMD_SENT_TO_CHIP && update_mode == 0)
{
D("CMD : SENT_CMD_TO_CHIP");
wret = write( gps_fd, cmd_buff, cmd_length);
}
}
else if (fd == gps_fd)
{
//for (;;)
{
ret = read( fd, buff, sizeof(buff) );
if (ret < 0)
{ cnt++;
if(cnt<6)
D("ret%d",ret);
if (errno == EINTR)
continue;
if (errno != EWOULDBLOCK)
D("error while reading from gps daemon socket: %s:", strerror(errno));
break;
}
for(nn = 0; nn < ret; nn++)
{
SDBP_Prase(buff[nn]);
}
for (nn = 0; nn < ret; nn++)
{
nmea_reader_addc( reader, buff[nn] );
}
}
}
else
{
D("epoll_wait() returned unkown fd %d ?", fd);
}
}
}
}
}
static void
gps_state_init( GpsState *state, GpsCallbacks *callbacks )
{
static int isInitialized = 0;
state->init = 1;
state->control[0] = -1;
state->control[1] = -1;
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s", __FUNCTION__);
if( (callbacks != NULL) && (callbacks->size == sizeof(GpsCallbacks)) )
{
state->callbacks = *callbacks;
}
else
{
D("Error : gps_state_init Callback Function Error");
return;
}
if(isInitialized == 1)
{
D(" GPS/BD has been initialized!!!");
return;
}
isInitialized=1;
if ( socketpair( AF_LOCAL, SOCK_STREAM, 0, state->control ) < 0 )
{
D("could not create thread control socket pair: %s", strerror(errno));
goto Fail;
}
callbacks->set_capabilities_cb(GPS_CAPABILITY_NAV_MESSAGES);
state->thread = callbacks->create_thread_cb( "gps_state_thread", gps_state_thread, state );
if ( !state->thread )
{
D("Error : Fail to create gps thread: %s", strerror(errno));
goto Fail;
}
return;
Fail:
td_gps_state_done( state );
}
void PowerOn(void)
{
D("Enter Normal WorkMode");
UartTxData((unsigned char*)"on",2);
usleep(150000);
if(tdconfig.fast_fix)
UartTxData(fix_dop100,15);
tdconfig.power_flag = TD_ON;
}
void PowerOff(void)
{
if(update_mode==0){
if(tdconfig.power_flag == TD_ON)
{
D("Enter Standby WorkMode");
UartTxData((unsigned char*)"off",3);
usleep(150000);
UartTxData(chip_standby,9);
tdconfig.power_flag = TD_OFF;
}
}else
D(" TD chip is updateing not poweroff");
}
static int
td_gps_init(GpsCallbacks *callbacks)
{
GpsState *s = _gps_state;
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s", __FUNCTION__);
if ((!s->init) || (s->fd < 0))
gps_state_init(s, callbacks);
// if (s->fd < 0)
// return -1;
return 0;
}
static void
td_gps_cleanup(void)
{
GpsState *s = _gps_state;
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s", __FUNCTION__);
char cmd = CMD_STOP;
int ret;
if(update_mode==1)
return;
do
{
ret = write( s->control[0], &cmd, 1 );
}
while (ret < 0 && errno == EINTR);
// if (s->init)
// td_gps_state_done(s);
}
static int
td_gps_start()
{
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s", __FUNCTION__);
GpsState *s = _gps_state;
PowerOn();
if (!s->init)
{
D("%s: called with uninitialized state !!", __FUNCTION__);
return -1;
}
td_gps_state_start(s);
return 0;
}
static int
td_gps_stop()
{
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s", __FUNCTION__);
GpsState *s = _gps_state;
td_mode.on_need=0;
if (!s->init)
{
if (GetDebugMaskBit(D_FUCTION) == 1)
{
D("%s: called with uninitialized state !!", __FUNCTION__);
}
return -1;
}
td_gps_state_stop(s);
PowerOff();
return 0;
}
static int
td_gps_inject_time(GpsUtcTime time, int64_t timeReference, int uncertainty)
{
if(tdconfig.power_flag == TD_ON)
{
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s(%d,%lld,%d)", __FUNCTION__,(int)time,(long long)timeReference,uncertainty);
int gps_fd = _gps_state->fd;
write_agps_data(gps_fd);
}
return 0;
}
static int
td_gps_inject_location(double latitude, double longitude, float accuracy)
{
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s(%f,%f,%f)", __FUNCTION__,latitude,longitude,accuracy);
int gps_fd = _gps_state->fd;
unsigned char pack_loc[24];
pack_sdbp_location(latitude,longitude,accuracy,pack_loc);
write(gps_fd,pack_loc,24);
write_agps_data(gps_fd);
return 0;
}
static void
td_gps_delete_aiding_data(GpsAidingData flags)
{
(void)flags;
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s", __FUNCTION__);
UartTxData(cold_start1,9);
UartTxData(cold_start2,10);
usleep(150000);
return;
}
static int td_gps_set_position_mode(GpsPositionMode mode, GpsPositionRecurrence recurrence,
unsigned int min_interval, unsigned int preferred_accuracy, unsigned int preferred_time)
{
char cmd = 0x00;
int ret ;
GpsState *s = _gps_state;
int t_mode=0;
time_t time1,time2;
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s", __FUNCTION__);
if(update_mode == 1)
return 0;
D("Set : Fix Mode:0x%X %d %d %d %d", mode,recurrence,min_interval,preferred_accuracy,preferred_time);
td_mode.on_need=1;
td_mode.work=0;
UartTxData((unsigned char*)"mode",4);
if(tdconfig.fast_fix)
UartTxData(fix_dop100,15);
time1 = time(NULL);
while(td_mode.work==0)
{
usleep(200000);
time2 = time(NULL);
if((time2-time1)>5){
D("Error : TD gps not work break!!!");
return 0;
}
}
t_mode = td_mode.nv_mode;
if(t_mode==(int)mode)
{
D("Warning : POSITION MODE SET ALREADY");
return 0;
}
switch(mode&0xff)
{
case 0x02:cmd = CMD_START_AGPS;break;
case 0x10:cmd = CMD_ENABLE_BD;break;
case 0x20:cmd = CMD_ENABLE_GPS;break;
case 0x30:cmd = CMD_ENABLE_BD_GP;break;
case 0x40:cmd = CMD_ENABLE_GL;break;
case 0x50:cmd = CMD_ENABLE_BD_GL;break;
case 0x60:cmd = CMD_ENABLE_GP_GL;break;
case 0x11:cmd = BD_COLD;break;
case 0x21:cmd = GPS_COLD;break;
case 0x31:cmd = BD_GP_COLD;break;
case 0x41:cmd = GL_COLD;break;
case 0x51:cmd = BD_GL_COLD;break;
case 0x61:cmd = GP_GL_COLD;break;
//default:cmd = 0x00;D("Warning : Undefined work mode!!!");break;
default:cmd = 0x00;break;
}
if(cmd != 0x00)
{
do
{
ret = write( s->control[0], &cmd, sizeof(cmd) );
}
while (ret < 0 && errno == EINTR);
}
return 0;
}
static int td_set_update_mode(void)
{
char cmd;
int ret ;
GpsState *s = _gps_state;
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s", __FUNCTION__);
cmd = CMD_UPDATE_TD;
do
{
ret = write( s->control[0], &cmd, sizeof(cmd) );
}
while (ret < 0 && errno == EINTR);
return 0;
}
int CheckSDBPData(const unsigned char *pdata,const int len)
{
int i;
//least sdbp data lenth is 8
if(len < 8)return 0;
for(i = 0;i < len - 8 + 5;i ++)
{
if(pdata[i] == 0x23 && pdata[i + 1] == 0x3e)
{
if(pdata[i + 2] == 0x02 && pdata[i + 3] == 0x04)
{
tdconfig.power_flag = TD_OFF;
D("Warning : Send Standby Mode CMD");
}
return 1;
}
}
return 0;
}
int CheckStringCmd(const unsigned char *pdata,const int len)
{
if(pdata[0] == '$' && pdata[len - 1] == '\n')
{
return 1;
}
else
return 0;
}
static void send_cmd_to_TDchip(char cmd)
{
int ret ;
GpsState *s = _gps_state;
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s", __FUNCTION__);
do
{
ret = write( s->control[0], &cmd, sizeof(cmd) );
}
while (ret < 0 && errno == EINTR);
return ;
}
static void command_handler(const char *data, const int length)
{
//int i = 0;
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s", __FUNCTION__);
memset(cmd_buff,0,CMD_MAX_LEN);
if(length < CMD_MAX_LEN)
{
memcpy(cmd_buff, data, length);
cmd_length = length;
}
else
{
D("length > CMD_MAX_LEN %d",length);
memcpy(cmd_buff, data, CMD_MAX_LEN);
cmd_length = CMD_MAX_LEN;
}
send_cmd_to_TDchip(CMD_SENT_TO_CHIP);
}
int td_xtra_init( GpsXtraCallbacks *callbacks )
{
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s", __FUNCTION__);
if(callbacks == 0)return 0;
return 0;
}
unsigned char img_data[1200*1024];
int img_length = 0;
int td_inject_xtra_data( char *data, int length )
{
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s", __FUNCTION__);
if(data == NULL || length <= 0)
{
return -1;
}
int ret;
int is_img_data1 = 0;
int is_img_data2 = 0;
if(update_mode == 1)
return 0;
is_img_data1 = CheckImgData1((unsigned char *)data, 0, length);
if(is_img_data1)
{
img_length = length - 4;
//img_data = (unsigned char *)data + 4;
memcpy(img_data, data+4,length-4);
D("Succeed : Img data1 size is %d",img_length);
return 1;
}
is_img_data2 = CheckImgData2((unsigned char *)data, 0, length);
if(is_img_data2)
{
memcpy(img_data+img_length, data+4,length-4);
img_length += length - 4;
//img_data = (unsigned char *)data + 4;
if(img_length > 0)
{
D("Succeed : Img total data size is %d",img_length);
if((ret = update_td_init((unsigned char *)img_data, img_length, _gps_state[0].fd)) < 0)
{
D("Error : update_td_init failed %d .",ret);
return ret;
}
img_length =0;
}
else return 0;
}
if(is_img_data2)
{
td_set_update_mode();
return 1;
}
if(CheckSDBPData((unsigned char *)data,length))
{
command_handler(data, length);
//D("Succeed : Send SDBP Data To Chip TD1030");
}
else if(CheckStringCmd((unsigned char *)data,length))
{
command_handler(data, length);
//D("Succeed : Send String CMD To Chip TD1030");
}
else
{
//td_update_rtcm(data, length);
}
return 1;
}
static const GpsXtraInterface xtraInterface =
{
sizeof(GpsXtraInterface),
td_xtra_init,
td_inject_xtra_data,
};
static const void *
td_gps_get_extension(const char *name)
{
if (GetDebugMaskBit(D_FUCTION) == 1)D("Enter : %s(%s)", __FUNCTION__,name);
if(0 == strcmp(name, GPS_XTRA_INTERFACE))
{
return &xtraInterface;
}
// if(0 == strcmp(name, AGPS_INTERFACE))
// {
// return &tdAGpsInterface;
// }
// if(0 == strcmp(name, GPS_NI_INTERFACE))
// {
// return &tdGpsNiInterface;
// }
// if (0 == strcmp(name, GPS_DEBUG_INTERFACE))
// {
// return &tdGpsDebugInterface;
// }
// if (0 == strcmp(name, AGPS_RIL_INTERFACE))
// {
// return &tdAGpsRilInterface;
// }
return NULL;
}
static GpsInterface hardwareGpsInterface =
{
sizeof(GpsInterface),
td_gps_init,
td_gps_start,
td_gps_stop,
td_gps_cleanup,
td_gps_inject_time,
td_gps_inject_location,
td_gps_delete_aiding_data,
td_gps_set_position_mode,
td_gps_get_extension,
};
void InitTdconfig(void)
{
memset(&tdconfig, 0, sizeof(tdconfig));
}
void InitTdModule(void)
{
GpsState *state = _gps_state;
D("Ver:2020-11");
LoadConfigFile();
//VCC LDO enable
//system("echo -wmode 138 0 > /sys/devices/virtual/misc/mtgpio/pin");
//system("echo -wdir 138 1 > /sys/devices/virtual/misc/mtgpio/pin");
//system("echo -wdout 138 1 > /sys/devices/virtual/misc/mtgpio/pin");
usleep(150000);
state->fd = TdUartOpen();
if(state->fd == -1)
{
state->init = 0;
return;
}
usleep(500000);
switch(tdconfig.uartboadrate)
{
case 9600:
UartTxData(baud9600,14);
usleep(50000);
UartTxData(baud9600,14);
usleep(50000);
UartTxData(baud9600,14);
break;
case 115200:
UartTxData(baud115200,14);
usleep(50000);
UartTxData(baud115200,14);
usleep(50000);
UartTxData(baud115200,14);
break;
case 19200:
UartTxData(baud19200,14);
usleep(50000);
UartTxData(baud19200,14);
usleep(50000);
UartTxData(baud19200,14);
break;
case 38400:
UartTxData(baud38400,14);
usleep(50000);
UartTxData(baud38400,14);
usleep(50000);
UartTxData(baud38400,14);
break;
case 57600:
UartTxData(baud57600,14);
usleep(50000);
UartTxData(baud57600,14);
usleep(50000);
UartTxData(baud57600,14);
break;
default:
break;
}
SetApUartbaud(tdconfig.uartboadrate);
UartTxData((unsigned char*)"init",4);
usleep(150000);
switch(tdconfig.gnss_mode)
{
case 1:UartTxData(gnssmode_bds,15);break;
case 2:UartTxData(gnssmode_gps,15);break;
case 3:UartTxData(gnssmode_gps_bds,15);break;
case 4:UartTxData(gnssmode_gl,15);break;
case 5:UartTxData(gnssmode_bds_gl,15);break;
case 6:UartTxData(gnssmode_gps_gl,15);break;
default:UartTxData(gnssmode_gps_bds,15);break;
}
usleep(150000);
if(tdconfig.fast_fix)
UartTxData(fix_dop100,15);
// usleep(150000);
// UartTxData(chip_standby,9);
// tdconfig.power_flag = TD_OFF;
D("Init Module END");
}
const GpsInterface *get_gps_interface()
{
static int check = 0;
if(check == 0)
{
check = 1;
InitTdModule();
}
return &hardwareGpsInterface;
}
static int open_gps(const struct hw_module_t *module, char const *name,
struct hw_device_t **device)
{
D("Enter : %s name = %s", __FUNCTION__,name);
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 = 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 = 2,
.version_minor = 0,
.id = GPS_HARDWARE_MODULE_ID,
.name = "TD1030",
.author = "Techtotop",
.methods = &gps_module_methods,
};
static pthread_t update_thread_id = 0;
void Creat_update_thread(void)
{
int ret;
pthread_attr_t attr;
pthread_attr_init (&attr);
ret = pthread_create(&update_thread_id, &attr, update_td_handler_thread, NULL);
if ( ret != 0 )
{
D("Error : Could not create thread:update_td_handler_thread");
return;
}
else
D("Succeed : create thread:update_td_handler_thread");
}
Reference in New Issue View Git Blame Copy Permalink