#include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include //#include //#include //#include //#include #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)<>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("<> : gps_use_property_get*** %s ***", property); if (strcmp(property,"false")==0) { return -1; } property_get("ro.sf.gps.serial", property, "/dev/ttyS4"); D("<> : 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>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"); }