| /** |
| * \file mbtk_gnss.c |
| * \brief gnss module. |
| * |
| * Detailed description |
| * \Author: Sniper <js.wang@mobiletek.cn> |
| * \Version: 1.0.0 |
| * \Date: 2022-03-16 |
| */ |
| #include <stdio.h> |
| #include <string.h> |
| #include <strings.h> |
| #include <stdlib.h> |
| #include <errno.h> |
| #include <termios.h> |
| #include <unistd.h> |
| #include <fcntl.h> |
| #include <signal.h> |
| #include <sys/types.h> |
| #include <unistd.h> |
| #include <pthread.h> |
| #include <termios.h> |
| #include <time.h> |
| #include <sys/ioctl.h> |
| #include "mbtk_type.h" |
| #include "mbtk_gnss.h" |
| #include "mbtk_http.h" |
| #include "mbtk_log.h" |
| #include "ringbuffer.h" |
| |
| // #define DEBUG 1 |
| |
| #ifdef DEBUG |
| #define gnss_log(...) printf(__VA_ARGS__) |
| #else |
| #define gnss_log(...) |
| #endif |
| |
| // 默认为 9600，打开为 115200，但是 AT+MGPSCMD 会重启。 |
| #define BAUDRATE_115200 1 |
| |
| #define TTFF_TEST 0 |
| |
| #define MBTK_GNSS_DEV "/dev/ttyS2" |
| |
| #define MBTK_UART_RECV_BUFFER_SIZE 1024 |
| #define MBTK_UART_SEND_BUFFER_MAX 128 |
| |
| /****************************************************************************** |
| * 时间处理相关的宏 |
| *****************************************************************************/ |
| // 获取当前时间 |
| #define GET_TIME() { gettimeofday(&time_m, NULL); \ |
| time_m.tv_sec += TIMEOUT_SEC;\ |
| } |
| // 设置从循环中退出的时间 |
| #define SET_TIME_OUT(x) { gettimeofday(&time_m, NULL); \ |
| time_m.tv_sec += x;\ |
| } |
| // 检测时间是否超时，超时则退出当前函数 |
| #define CHK_TIME() { gettimeofday(&time_n, NULL); \ |
| if(time_n.tv_sec > time_m.tv_sec) { \ |
| printf("\ntimeout!!!\n\n");\ |
| close(fd); \ |
| return ret; \ |
| } \ |
| } |
| // 检测时间是否超时，超时则退出当前循环 |
| #define CHK_TIME_BREAK() { gettimeofday(&time_n, NULL); \ |
| if(time_n.tv_sec > time_m.tv_sec) { \ |
| printf("\ntimeout!!!\n\n");\ |
| break; \ |
| } \ |
| } |
| // 检测延时是否到达，到达则退出当前循环 |
| #define DELAY_TIME_BREAK() { gettimeofday(&time_n, NULL); \ |
| if(time_n.tv_sec > time_m.tv_sec) { \ |
| break; \ |
| } \ |
| } |
| |
| typedef void (*gnss_msg_func_t) |
| ( |
| int index, |
| char *in_data, |
| void *out_ptr |
| ); |
| |
| struct mbtk_gnss_cmd_msg_t |
| { |
| int index; // 序号 |
| char *cmd_str; // 匹配字符 |
| gnss_msg_func_t gnss_msg_func; // 回调函数 |
| int is_continue; // 是否随NEMA数据一起输出 |
| }; |
| |
| typedef enum |
| { |
| E_MT_LOC_MSG_ID_STATUS_INFO = 0, /**< pv_data = &E_QL_LOC_STATUS_VALUE_T */ |
| E_MT_LOC_MSG_ID_LOCATION_INFO, /**< pv_data = &QL_LOC_LOCATION_INFO_T */ |
| E_MT_LOC_MSG_ID_SV_INFO, /**< pv_data = &QL_LOC_SV_STATUS_T */ |
| E_MT_LOC_MSG_ID_NMEA_INFO, /**< pv_data = &QL_LOC_NMEA_INFO_T */ |
| E_MT_LOC_MSG_ID_CAPABILITIES_INFO, /**< pv_data = &E_QL_LOC_CAPABILITIES_T */ |
| E_MT_LOC_MSG_ID_AGPS_STATUS, /**< pv_data = &QL_LOC_AGPS_STATUS_T */ |
| E_MT_LOC_MSG_ID_NI_NOTIFICATION, /**< pv_data = &QL_LOC_NI_NOTIFICATION_INTO_T */ |
| E_MT_LOC_MSG_ID_XTRA_REPORT_SERVER, /**< pv_data = &QL_LOC_XTRA_REPORT_SERVER_INTO_T */ |
| }e_mt_loc_msg_id_t; |
| |
| typedef struct |
| { |
| int64_t timestamp; /**< System Timestamp, marked for when got the nmea data */ |
| int length; /**< NMEA string length. */ |
| char nmea[255 + 1]; /**< NMEA string.*/ |
| }mopen_gnss_nmea_info_t; /* Message */ |
| |
| struct mbtk_gnss_handle_t |
| { |
| int dev_fd; |
| pthread_t uart_pthread; |
| pthread_t gnss_pthread; |
| mbtk_gnss_handler_func_t gnss_handler_func; |
| int mode; // 0 - stop, 1 - single, 2 - periodic, 3 - start |
| pthread_mutex_t _cond_mutex; |
| int reset_state; |
| int inited; |
| ring_buffer_t ring_buffer; |
| int getap_status; |
| char *rb; |
| |
| #if TTFF_TEST |
| pthread_t ttff_pid; |
| int location_state; |
| #endif |
| /******************** |
| 存储handle的地址指针 |
| phandle = &handle |
| handle = mbtk_gnss_handle |
| *********************/ |
| uint32 *phandle; // handle的地址指针 |
| }; |
| |
| #if TTFF_TEST |
| struct mbtk_gnss_ttff_t |
| { |
| int type; |
| int timeout_sec; |
| int test_count; |
| int average_loc_time; |
| }; |
| |
| // ttff 测试 flag |
| static int location_test = 0; |
| static pthread_mutex_t loc_cond_mutex_r = PTHREAD_MUTEX_INITIALIZER; |
| static pthread_cond_t loc_sync_cond = PTHREAD_COND_INITIALIZER; |
| #endif |
| |
| struct mopen_gnss_device_info_t |
| { |
| int device_info_valid; |
| int usrt_bandrate; |
| char product_name[10]; |
| char dev_config[6]; |
| char hw_ver[6]; |
| char fw_ver[32]; |
| char pn[16]; |
| char sn[16]; |
| char nmea_ver[4]; |
| }; |
| |
| static struct mopen_gnss_device_info_t mopen_gnss_device_info; |
| static struct mbtk_gnss_handle_t *mbtk_gnss_handle = NULL; |
| static int firmware_extren_state = 0; |
| |
| static char g_no_sv = 0;// 参与定位的卫星数量 |
| |
| int mopen_gnss_get_nmea_config(uint32 h_gnss); |
| int mopen_gnss_get_ant_state_info(uint32 h_gnss); |
| |
| static void get_gnss_time_info(int cmd, char *str, void *data); |
| static void get_gnss_agnss_state(int cmd, char *str, void *data); |
| static void get_gnss_device_info(int type, char *str, void *usr_ptr); |
| static void gnss_uart_info(int cmd, char *str, void *data); |
| static void gnss_gsa_info(int cmd, char *str, void *data); |
| static int mopen_uart_change(int fd, int check); |
| |
| static int select_read( int fd, int timeout ); |
| ssize_t deal_read(int fd, void *buf, size_t count); |
| |
| static struct mbtk_gnss_cmd_msg_t mbtk_gnss_cmd_msg_map[] = { |
| { 1, "$OK", NULL, 0}, |
| { 2, "$Fail", NULL, 0}, |
| { 3, "RMC", get_gnss_time_info, 1}, |
| { 4, "GGA", get_gnss_time_info, 1}, |
| { 5, "$PDTINFO", get_gnss_device_info, 0}, |
| { 6, "$CFGNMEA", get_gnss_device_info, 0}, |
| { 7, "$CFGPRT", gnss_uart_info, 0}, |
| { 8, "$CFGAID", get_gnss_agnss_state, 0}, |
| { 9, "$ANTSTAT", NULL, 0}, |
| #if TTFF_TEST |
| {10, "GSA", gnss_gsa_info, 1}, |
| #endif |
| }; |
| |
| /** |
| * \brief strstr_n |
| * |
| * find string return number |
| * |
| * \param param |
| * \return return type |
| */ |
| int strstr_n(const char *s1, const char *s2) |
| { |
| int n; |
| int strlen = 0; |
| |
| if(*s2) |
| { |
| while(*s1) |
| { |
| for(n = 0; *(s1+n) == *(s2 + n); n++) |
| { |
| if(!*(s2 + n + 1)) |
| { |
| strlen++; |
| return strlen; |
| } |
| } |
| s1++; |
| strlen++; |
| } |
| return 0; |
| } |
| else |
| return 0; |
| } |
| |
| /** |
| * @brief gnss_get_para_from_nmea |
| * |
| * @details 从buf里面得到第num个逗号所在的位置 |
| * |
| * @param param |
| * |
| * @return 0~0xfe,代表逗号所在位置的偏移. |
| * 0xff,代表不存在第cx个逗号 |
| */ |
| static int gnss_get_para_from_nmea(const char *data, char *out_data, int num) |
| { |
| int i = 0; |
| int n[2] = {0}; |
| int tmp; |
| |
| // 找到第num个","，结果放到 n[0] |
| for (i = 0; i < num; ++i) { |
| tmp = strstr_n(&data[n[0]], ","); |
| if(0 == tmp) { |
| gnss_log("%s %d : error\n", __func__, __LINE__); |
| gnss_log("error: [%d] %s\n", num, data); |
| return -1; |
| } |
| n[0] += tmp; |
| } |
| if ((n[1] = strstr_n(&data[n[0]], ",")) || |
| (n[1] = strstr_n(&data[n[0]], "*")) && |
| (n[1] > 1)) { |
| memcpy(out_data, &data[n[0]], n[1] - 1); |
| } else { |
| gnss_log("%s %d : error [%d]\n" , __func__, __LINE__, n[1]); |
| gnss_log("error: [%d] %s\n", num, data); |
| return -1; |
| } |
| |
| return 0; |
| } |
| /** |
| * @brief get_timestamp |
| * |
| * @details |
| * |
| * @param param |
| * |
| * @return return type |
| */ |
| static time_t get_timestamp(char *time) |
| { |
| char tmp_char[4] = {0}; |
| struct tm* tmp_time = (struct tm*)malloc(sizeof(struct tm)); |
| |
| memset(tmp_time, 0, sizeof(struct tm)); |
| memset(tmp_char, 0, sizeof(tmp_char)); |
| memcpy(tmp_char, &time[4], 2); |
| tmp_time->tm_sec = atoi(tmp_char); |
| memcpy(tmp_char, &time[2], 2); |
| tmp_time->tm_min = atoi(tmp_char); |
| memcpy(tmp_char, &time[0], 2); |
| tmp_time->tm_hour = atoi(tmp_char); |
| memcpy(tmp_char, &time[6], 2); |
| tmp_time->tm_mday = atoi(tmp_char); |
| memcpy(tmp_char, &time[8], 2); |
| tmp_time->tm_mon = atoi(tmp_char); |
| memcpy(tmp_char, &time[10], 2); |
| tmp_time->tm_year = 100 + atoi(tmp_char); |
| |
| time_t _t = mktime(tmp_time);//按当地时区解析tmp_time |
| // gnss_log("timestamp: %ld\n",_t); |
| free(tmp_time); |
| |
| return _t; |
| } |
| /** |
| * @brief get_gnss_device_info |
| * |
| * @details 获取设备信息 |
| * |
| * @param type: 5-从$PDTINFO获取 |
| * 6-从$CFGNMEA获取 |
| * |
| * @return return type |
| */ |
| static void get_gnss_device_info(int type, char *str, void *usr_ptr) |
| { |
| char tmp_str[32] = {0}; |
| int i, ret; |
| |
| if(5 == type) // define mbtk_gnss_cmd_msg_map |
| { |
| // $PDTINFO get product info |
| char *tmp_ptr[6] = {mopen_gnss_device_info.product_name, |
| mopen_gnss_device_info.dev_config, |
| mopen_gnss_device_info.hw_ver, |
| mopen_gnss_device_info.fw_ver, |
| mopen_gnss_device_info.pn, |
| mopen_gnss_device_info.sn |
| }; |
| for (i = 0; i < 6; ++i) { |
| memset(tmp_str, 0, sizeof(tmp_str)); |
| // get product name |
| ret = gnss_get_para_from_nmea(str, tmp_str, i + 1); |
| if(ret) |
| continue; |
| memcpy(tmp_ptr[i], tmp_str, strlen(tmp_str)); |
| } |
| gnss_log("*************************\n"); |
| gnss_log("-Pn: %s\n dc: %s\n hv: %s\n fw: %s\n pn: %s\n sn: %s\n ", |
| mopen_gnss_device_info.product_name, |
| mopen_gnss_device_info.dev_config, |
| mopen_gnss_device_info.hw_ver, |
| mopen_gnss_device_info.fw_ver, |
| mopen_gnss_device_info.pn, |
| mopen_gnss_device_info.sn); |
| } |
| if(6 == type) // define mbtk_gnss_cmd_msg_map |
| { |
| // $CFGNMEA get nmea version |
| memset(tmp_str, 0, sizeof(tmp_str)); |
| ret = gnss_get_para_from_nmea(str, tmp_str, 1); |
| if(ret) |
| return; |
| memcpy(mopen_gnss_device_info.nmea_ver, tmp_str, strlen(tmp_str)); |
| mopen_gnss_device_info.device_info_valid = TRUE; |
| } |
| } |
| |
| static short int from_hex(char a) |
| { |
| if (a >= 'A' && a <= 'F') |
| return a - 'A' + 10; |
| else if (a >= 'a' && a <= 'f') |
| return a - 'a' + 10; |
| else |
| return a - '0'; |
| } |
| |
| static int str_to_hex(char *str) |
| { |
| unsigned char str_len = strlen(str); |
| int i; |
| int ret = 0; |
| |
| for (i = 0; i < str_len; ++i) { |
| ret = ret * 16 + from_hex(str[i]); |
| } |
| return ret; |
| } |
| |
| static void get_gnss_agnss_state(int cmd, char *str, void *data) |
| { |
| int ret; |
| char tmp_str[10] = {0}; |
| int agps; |
| static int count = 0; |
| |
| ret = gnss_get_para_from_nmea(str, tmp_str, 2); |
| if(ret) { |
| printf("\n%s[%d] error!\n" , __FUNCTION__, __LINE__); |
| } |
| agps = str_to_hex(tmp_str); |
| gnss_log("\n%s[%d] agnss: %s[%x]\n" , __FUNCTION__, __LINE__, tmp_str, agps); |
| if(0 == agps && count < 5) { |
| mopen_gnss_get_aidinfo(mbtk_gnss_handle); |
| count++; |
| } else { |
| printf("\nagnss: %s\n", str); |
| count = 0; |
| } |
| // $CFGAID,0,00000000,00000000,00*3E |
| } |
| // 1节=1海里/小时=1.852公里/小时 |
| /** |
| * @brief function description |
| * |
| * @details 获取位置信息 |
| * |
| * @param type: 1-从$RMC获取 |
| * 2-从$GGA获取 |
| * @return return type |
| */ |
| static void get_gnss_loc_info(int type, char *str, |
| struct mbtk_gnss_location_info_t *_mopen_location_info) |
| |
| { |
| char tmp_str[32] = {0}; |
| int ret; |
| |
| if(1 == type) |
| { |
| // $PDTINFO get product info |
| memset(tmp_str, 0, sizeof(tmp_str)); |
| // get product name |
| ret = gnss_get_para_from_nmea(str, tmp_str, 7); |
| if(ret) |
| return; |
| _mopen_location_info->speed = atof(tmp_str); |
| memset(tmp_str, 0, sizeof(tmp_str)); |
| // get product name |
| ret = gnss_get_para_from_nmea(str, tmp_str, 8); |
| if(ret) |
| return; |
| _mopen_location_info->bearing = atof(tmp_str); |
| } |
| else if(2 == type) |
| { |
| // $XXGGA get product info |
| memset(tmp_str, 0, sizeof(tmp_str)); |
| // get product name |
| ret = gnss_get_para_from_nmea(str, tmp_str, 2); |
| if(ret) |
| return; |
| _mopen_location_info->latitude = atof(tmp_str); |
| memset(tmp_str, 0, sizeof(tmp_str)); |
| // get product name |
| ret = gnss_get_para_from_nmea(str, tmp_str, 4); |
| if(ret) |
| return; |
| _mopen_location_info->longitude = atof(tmp_str); |
| memset(tmp_str, 0, sizeof(tmp_str)); |
| // get product name |
| ret = gnss_get_para_from_nmea(str, tmp_str, 9); |
| if(ret) |
| return; |
| _mopen_location_info->altitude = atof(tmp_str); |
| } |
| } |
| |
| static void get_gnss_time_info(int cmd, char *str, void *data) |
| { |
| int ret; |
| char param[36] = {0}; |
| struct mbtk_gnss_location_info_t *mopen_location_info_ptr = (struct mbtk_gnss_location_info_t *)data; |
| |
| if (3 == cmd) { |
| memset(param, 0, sizeof(param)); |
| // get time |
| ret = gnss_get_para_from_nmea(str, param, 1); |
| if(ret) |
| return; |
| // get date |
| ret = gnss_get_para_from_nmea(str, ¶m[6], 9); |
| if(ret) |
| return; |
| |
| mopen_location_info_ptr->timestamp = get_timestamp(param); |
| get_gnss_loc_info(1, str, mopen_location_info_ptr); |
| } else if(4 == cmd) /* GGA */{ |
| get_gnss_loc_info(2, str, mopen_location_info_ptr); |
| ret = gnss_get_para_from_nmea(str, param, 7); |
| if(ret) |
| return; |
| char no_sv = (char)atoi(param); |
| gnss_log("SV number: %d, %d\n", g_no_sv, no_sv); |
| /* |
| 只能在临时固件下，才能获取APdata星历数据 |
| 在6颗卫星保存文件，每增加2颗保存一次。 |
| */ |
| if (1 == firmware_extren_state && |
| g_no_sv < (no_sv - 1) && no_sv > 5) { |
| |
| g_no_sv = no_sv; |
| mbtk_gnss_get_ap_data(); |
| } |
| } |
| } |
| |
| static void gnss_uart_info(int cmd, char *str, void *data) |
| { |
| int ret; |
| char tmp_str[12] = {0}; |
| |
| // $CFGPRT,1,h0,9600,129,3*57 |
| ret = gnss_get_para_from_nmea(str, tmp_str, 3); |
| if(ret) |
| return; |
| mopen_gnss_device_info.usrt_bandrate = atoi(tmp_str); |
| gnss_log("CFGPRT: %s\n", str); |
| gnss_log("Uart bandrate: %d\n" , mopen_gnss_device_info.usrt_bandrate); |
| gnss_log("*************************\n"); |
| } |
| /** |
| * \brief function description |
| * |
| * Detailed 处理gnss数据 |
| * |
| * \param param |
| * \return return type |
| */ |
| static void process_gnss_callback(struct mbtk_gnss_handle_t *handle, |
| const char *data, int data_len) |
| { |
| int ret = 0; |
| int i = 0; |
| static struct mbtk_gnss_location_info_t mopen_location_info; |
| static int64_t tmp_time = 0; |
| mopen_gnss_nmea_info_t nmea_info; |
| |
| memset(&nmea_info, 0, sizeof(nmea_info)); |
| if(0 == tmp_time) |
| memset(&mopen_location_info, 0, sizeof(mopen_location_info)); |
| |
| for (i = 0; |
| i < (sizeof(mbtk_gnss_cmd_msg_map) / sizeof(struct mbtk_gnss_cmd_msg_t)); |
| ++i) { |
| if(strstr_n(data, mbtk_gnss_cmd_msg_map[i].cmd_str)) { |
| if(mbtk_gnss_cmd_msg_map[i].gnss_msg_func) |
| mbtk_gnss_cmd_msg_map[i].gnss_msg_func(mbtk_gnss_cmd_msg_map[i].index, |
| data, &mopen_location_info); |
| break; |
| } |
| } |
| if(0 == mbtk_gnss_cmd_msg_map[i].is_continue) |
| return; |
| |
| tmp_time = mopen_location_info.timestamp; |
| nmea_info.timestamp = mopen_location_info.timestamp; |
| nmea_info.length = data_len; |
| memcpy(nmea_info.nmea, data, data_len); |
| gnss_log("nmea:[%d] %s", data_len, data); |
| if(handle->gnss_handler_func && handle->mode == 3 && |
| nmea_info.timestamp) |
| handle->gnss_handler_func(handle, E_MT_LOC_MSG_ID_NMEA_INFO, &nmea_info, NULL); |
| if(handle->gnss_handler_func && handle->mode == 1 && |
| mopen_location_info.latitude && |
| mopen_location_info.longitude && |
| mopen_location_info.altitude && |
| mopen_location_info.timestamp && |
| mopen_location_info.speed) |
| { |
| handle->gnss_handler_func(handle, E_MT_LOC_MSG_ID_LOCATION_INFO, &mopen_location_info, NULL); |
| memset(&mopen_location_info, 0, sizeof(mopen_location_info)); |
| } |
| |
| return; |
| } |
| |
| /** |
| * \brief get_gnss_from_str |
| * |
| * Detailed 从串口数据解析出每条消息 |
| * |
| * \param param |
| * \return return type |
| */ |
| static int get_gnss_from_str(struct mbtk_gnss_handle_t *handle, |
| const char *data, int data_len) |
| { |
| char *tail = NULL; |
| static int seek = 0; |
| // 等待 OK, 如果20条结果没有等到，就异常 |
| static int reset_count = 0; |
| int i = 0, ret = -1; |
| |
| if (handle->reset_state) |
| { |
| // 等待 reset 回复的 OK |
| if(0 != memcmp(data, "$OK", 3) && reset_count < 20) { |
| printf("gnss reset invalid: [%s]\n", data); |
| reset_count++; |
| return -1; |
| } |
| if (reset_count > 19) { |
| printf("%s: device reset timeout!!!\n", __FUNCTION__); |
| LOGI("%s: device reset timeout!!!\n", __FUNCTION__); |
| } |
| reset_count = 0; |
| gnss_log("reset ok: %s\n", data); |
| #if BAUDRATE_115200 |
| ret = mopen_uart_change(handle->dev_fd, 0); |
| if(ret) { |
| printf("reset Uart set 115200 error\n"); |
| } |
| #endif |
| pthread_mutex_lock(&handle->_cond_mutex); |
| handle->reset_state = 0; |
| pthread_mutex_unlock(&handle->_cond_mutex); |
| } |
| |
| if((data[0] == '$' || data[0] == '#') && |
| data[data_len - 1] == '\n' && |
| data_len < 128) { |
| process_gnss_callback(handle, data, data_len); |
| } else { |
| gnss_log("nmea error: %s\n", data); |
| } |
| |
| return 1; |
| } |
| |
| void mopen_gnss_NonBlock(int fd, int cmd) |
| { |
| int flags; |
| |
| flags = fcntl(fd, F_GETFL, 0); |
| if(cmd) |
| flags |= O_NONBLOCK; |
| else |
| flags &= ~O_NONBLOCK; |
| |
| fcntl(fd, F_SETFL, flags); |
| } |
| |
| int set_baudrate(int fd, int baudrate) |
| { |
| struct termios options, oldtio; |
| |
| if(fcntl(fd, F_SETFL, 0) < 0) { |
| printf("fcntl failed!\n"); |
| return -1; |
| } |
| |
| if(tcgetattr(fd, &oldtio) != 0) { |
| printf("setup serial error!\n"); |
| return -1; |
| } |
| |
| /* Get the current options for the port... */ |
| tcgetattr(fd, &options); |
| |
| /* Set the baud rates to baudrate... */ |
| cfsetispeed(&options,baudrate); |
| cfsetospeed(&options,baudrate); |
| tcsetattr(fd, TCSANOW, &options); |
| |
| if (0 != tcgetattr(fd, &options)) |
| { |
| printf("get options error!\n"); |
| return -1; |
| } |
| |
| /* |
| * 8bit Data,no partity,1 stop bit... |
| */ |
| options.c_cflag &= ~PARENB;//无奇偶校验 |
| options.c_cflag &= ~CSTOPB;//停止位，1位 |
| options.c_cflag &= ~CSIZE; //数据位的位掩码 |
| options.c_cflag |= CS8; //数据位，8位 |
| |
| cfmakeraw(&options); |
| |
| /* |
| * Set the new options for the port... |
| */ |
| if (tcsetattr(fd, TCSANOW, &options) != 0) |
| { |
| printf("setup serial error!\n"); |
| return -1 ; |
| } |
| |
| return 0 ; |
| } |
| /* |
| 自适应波特率设置 |
| */ |
| static int auto_set_uart_baudrate(int fd) |
| { |
| char rbuf[512]; |
| int rByte = 0; |
| int b[3] = {B115200, B9600, 0}; |
| int ret = B9600; |
| struct timeval time_m, time_n; |
| // 时间超时标志 |
| int timeout_sign = 0; |
| // 先测试默认的9600波特率 |
| SET_TIME_OUT(3); |
| do { |
| gettimeofday(&time_n, NULL); |
| if(time_n.tv_sec > time_m.tv_sec) { |
| printf("Baudrate--test-9600--- timeout!\n"); |
| if(timeout_sign) |
| break; |
| set_baudrate(fd, B115200); |
| ret = B115200; |
| timeout_sign = 1; |
| SET_TIME_OUT(3); |
| continue; |
| } |
| |
| if(select_read(fd, 1) > 0) |
| usleep(50000); |
| else |
| continue; |
| |
| rByte = deal_read(fd,&rbuf,sizeof(rbuf)); |
| if(rByte > 0) { |
| gnss_log("Auto Baudrate[%d]%s\n", rByte, rbuf); |
| if(strstr(rbuf, "$")) |
| return ret; |
| memset(rbuf, 0, sizeof(rbuf)); |
| } else { |
| printf("*** read error\n"); |
| } |
| }while(1); |
| |
| return -1; |
| } |
| |
| int mopen_gnss_open(char *dev, int baudrate) |
| { |
| int ret; |
| int fd = 0; |
| |
| fd = open(dev, O_RDWR | O_NOCTTY | O_NDELAY); |
| if(fd < 0) { |
| return -1; |
| } |
| gnss_log("curent dev: %s, fd: %d \n", dev, fd); |
| |
| if(baudrate) { |
| gnss_log("set baudrate: %d \n", baudrate); |
| ret = set_baudrate(fd, baudrate); |
| if(-1 == ret) { |
| close(fd); |
| return -1; |
| } |
| } else { |
| set_baudrate(fd, B9600); |
| } |
| |
| return fd; |
| } |
| |
| static int mopen_gnss_read(int fd, char* buf, unsigned int buf_len) |
| { |
| buf_len=(buf_len > MBTK_UART_RECV_BUFFER_SIZE ? MBTK_UART_RECV_BUFFER_SIZE : buf_len); |
| return read(fd, buf, buf_len); |
| } |
| |
| int mopen_gnss_write(int fd, const char* buf, unsigned int buf_len) |
| { |
| size_t size; |
| size_t size_to_wr; |
| ssize_t size_written; |
| if(MBTK_UART_SEND_BUFFER_MAX < buf_len) |
| { |
| return -1; |
| } |
| for(size = 0; size < buf_len;) |
| { |
| size_to_wr = buf_len - size; |
| if( size_to_wr > MBTK_UART_SEND_BUFFER_MAX) |
| size_to_wr = MBTK_UART_SEND_BUFFER_MAX; |
| |
| size_written = write(fd, &buf[size], size_to_wr); |
| if (size_written==-1) |
| { |
| return -1; |
| } |
| gnss_log("send cmd: %s", &buf[size]); |
| size += size_written; |
| if(size_written != size_to_wr) |
| { |
| return size; |
| } |
| } |
| return size; |
| } |
| |
| int mopen_gnss_close(int fd) |
| { |
| return close(fd); |
| } |
| |
| static void gnss_info_pthread(void* hdl) |
| { |
| struct mbtk_gnss_handle_t *gnss_handle = (struct mbtk_gnss_handle_t *)hdl; |
| int ret = 0, i; |
| char tmp; |
| char tmp_arr[128] = {0}; |
| |
| pthread_detach(pthread_self()); |
| |
| memset(tmp_arr, 0, sizeof(tmp_arr)); |
| while(mbtk_gnss_handle->inited) |
| { |
| for (i = 0; i < 256; ++i) { |
| if (0 == mbtk_gnss_handle->inited) |
| goto exit; |
| ret = ring_buffer_peek(&gnss_handle->ring_buffer, &tmp, i); |
| if (0 == ret) { |
| usleep(300000); |
| gnss_log("ring_buffer_peek ringbuffer read error\n"); |
| i--; |
| continue; |
| } |
| if (tmp == '\n') { |
| break; |
| } |
| } |
| |
| if (i > (256 - 2)) |
| continue; |
| |
| ret = ring_buffer_dequeue_arr(&gnss_handle->ring_buffer, tmp_arr, i + 1); |
| |
| if(ret > 0 && 0 == mbtk_gnss_handle->getap_status) { |
| // gnss_log("NEMA:[%d] %s", ret, tmp_arr); |
| get_gnss_from_str(gnss_handle, tmp_arr, ret); |
| memset(tmp_arr, 0, sizeof(tmp_arr)); |
| } else { |
| gnss_log("ringbuffer read error\n"); |
| } |
| usleep(5000); |
| } |
| exit: |
| pthread_exit(NULL); |
| } |
| |
| static void gnss_uart_pthread(void* hdl) |
| { |
| struct mbtk_gnss_handle_t *gnss_handle = (struct mbtk_gnss_handle_t *)hdl; |
| int ret = 0; |
| char buf[MBTK_UART_RECV_BUFFER_SIZE] = {0}; |
| |
| pthread_detach(pthread_self()); |
| |
| memset(buf, 0, sizeof(buf)); |
| while(mbtk_gnss_handle->inited) |
| { |
| while(mbtk_gnss_handle->getap_status){ |
| // 在读AP_DATA星历时，不能输出NMEA |
| printf("g"); |
| usleep(100000); |
| } |
| ret = mopen_gnss_read(gnss_handle->dev_fd, buf, MBTK_UART_RECV_BUFFER_SIZE); |
| if(ret > 0) { |
| // gnss_log("read: [%d] %s\n", ret, buf); |
| ring_buffer_queue_arr(&gnss_handle->ring_buffer, buf, ret); |
| memset(buf, 0, sizeof(buf)); |
| } else { |
| gnss_log("read error\n"); |
| } |
| usleep(100000); |
| } |
| |
| pthread_exit(NULL); |
| } |
| |
| ssize_t deal_read(int fd, void *buf, size_t count) |
| { |
| int ret = 0; |
| |
| #if 0 |
| ret = read(fd, buf, count); |
| return ret; |
| #else |
| while (1) |
| { |
| ret = read(fd, buf, count); |
| if (ret == 0) { |
| printf("read serial return 0, please check serial device.\n"); |
| exit(-5); |
| } |
| if(ret < 0) { |
| if ((errno == EAGAIN) || (errno == EINTR)) { |
| printf("read serial return -1, errno = %d, retry.\n", errno); |
| continue; |
| } else { |
| printf("read serial return -1, errno = %d, please check serial device.\n", errno); |
| exit(-5); |
| } |
| } |
| return ret; |
| } |
| #endif |
| } |
| |
| static int select_read( int fd, int timeout ) //1ms |
| { |
| fd_set set; |
| struct timeval t; |
| int ret; |
| int i = timeout; |
| |
| do { |
| FD_ZERO(&set); |
| FD_SET(fd, &set); |
| t.tv_sec = 0; |
| t.tv_usec = 100; |
| |
| ret = select(FD_SETSIZE, &set, NULL, NULL, &t ); |
| if(ret == 0) continue; |
| if(ret < 0 && errno == EINTR)continue; |
| else return ret; |
| } while(i--); |
| |
| return ret; |
| } |
| |
| // check: 是否需要校验 |
| static int mopen_uart_change(int fd, int check) |
| { |
| int rByte = 0, i = 20; |
| char name[32]; |
| char rbuf[1024]; |
| |
| sprintf(name,"$CFGPRT,1,h0,115200,129,3\r\n"); |
| rByte = write( fd, name, strlen(name)); |
| tcdrain(fd); |
| |
| set_baudrate(fd, B115200); |
| usleep(200000); |
| tcflush(fd, TCIFLUSH); |
| |
| if (0 == check) |
| return 0; |
| do{ |
| rByte = 0; |
| memset(rbuf, 0, sizeof(rbuf)); |
| if(select_read(fd, 1) > 0) { |
| rByte = deal_read(fd, &rbuf, sizeof(rbuf) - 1); |
| rbuf[rByte] = 0; |
| gnss_log("%s: %s", __FUNCTION__, rbuf); |
| } |
| if(strstr(rbuf, "$")) { |
| return 0; |
| } else { |
| gnss_log("%d rByte = %d, [%s]\n", 20 - i, rByte, rbuf); |
| } |
| usleep(5000 * 100); |
| }while(i--); |
| |
| return -1; |
| } |
| |
| #define GPS_DEV "/sys/devices/soc.0/d4000000.apb/mbtk-dev-op/gps_power" |
| |
| static int mopen_open_gps(int state) |
| { |
| int fd, ret; |
| char s[4] = "on"; |
| fd = open(GPS_DEV, O_RDWR | O_TRUNC, 0644); |
| if(fd < 0) { |
| LOGE("[%s] file [%s] open error\n", __FUNCTION__, GPS_DEV); |
| return -1; |
| } |
| if(0 == state) |
| { |
| memcpy(s, "off", 3); |
| } |
| ret = write(fd, s, 4); |
| if (ret < 0) { |
| LOGE("%s: error writing to file!\n", __FUNCTION__); |
| close(fd); |
| return -2; |
| } |
| |
| close(fd); |
| return 0; |
| } |
| |
| int mbtk_gnss_client_init(uint32 *ph_gnss) |
| { |
| int ret; |
| |
| if(ph_gnss == NULL) { |
| printf("ARG error or has inited."); |
| return -1; |
| } |
| |
| if (mbtk_gnss_handle) { |
| printf("GNSS has inited."); |
| *ph_gnss = (uint32)mbtk_gnss_handle; |
| return 0; |
| } |
| mbtk_gnss_handle = malloc(sizeof(struct mbtk_gnss_handle_t)); |
| if(NULL == mbtk_gnss_handle) |
| { |
| printf("malloc memory error\n"); |
| return -3; |
| } |
| memset(mbtk_gnss_handle, 0, sizeof(struct mbtk_gnss_handle_t)); |
| memset(&mopen_gnss_device_info, 0, sizeof(mopen_gnss_device_info)); |
| |
| ret = mopen_open_gps(1); |
| if(ret) { |
| printf("GNSS open init error\n"); |
| return -4; |
| } |
| sleep(1); |
| |
| mbtk_gnss_handle->dev_fd = mopen_gnss_open(MBTK_GNSS_DEV, 0); |
| |
| printf("Gnss Config Uart Baudrate Start -> \n"); |
| ret = auto_set_uart_baudrate(mbtk_gnss_handle->dev_fd); |
| if(-1 == ret) { |
| ret = -2; |
| goto err; |
| } else if (B9600 == ret) { |
| ret = mopen_uart_change(mbtk_gnss_handle->dev_fd, 1); |
| if(ret) |
| { |
| printf("GNSS Uart set B115200 error\n"); |
| mopen_gnss_close(mbtk_gnss_handle->dev_fd); |
| mopen_open_gps(0); |
| return -1; |
| } |
| } |
| printf("Gnss Config Uart Baudrate Successful.\n"); |
| |
| mbtk_gnss_handle->rb = malloc(MBTK_UART_RECV_BUFFER_SIZE); |
| if(NULL == mbtk_gnss_handle->rb) |
| { |
| printf("malloc memory error\n"); |
| return -1; |
| } |
| |
| ring_buffer_init(&mbtk_gnss_handle->ring_buffer, |
| mbtk_gnss_handle->rb, |
| MBTK_UART_RECV_BUFFER_SIZE); |
| |
| mbtk_gnss_handle->inited = 1; |
| |
| pthread_mutex_init(&mbtk_gnss_handle->_cond_mutex, NULL); |
| pthread_create(&mbtk_gnss_handle->uart_pthread, NULL, (void *)gnss_uart_pthread, (void *)mbtk_gnss_handle); |
| pthread_create(&mbtk_gnss_handle->gnss_pthread, NULL, (void *)gnss_info_pthread, (void *)mbtk_gnss_handle); |
| |
| mopen_gnss_get_ant_state_info((uint32)mbtk_gnss_handle); |
| mopen_gnss_get_device_info((uint32)mbtk_gnss_handle); |
| mopen_gnss_get_nmea_config((uint32)mbtk_gnss_handle); |
| mopen_gnss_get_uart((uint32)mbtk_gnss_handle); |
| mopen_gnss_get_aidinfo((uint32)mbtk_gnss_handle); |
| |
| *ph_gnss = (uint32)mbtk_gnss_handle; |
| mbtk_gnss_handle->phandle = ph_gnss; |
| |
| return 0; |
| err: |
| mopen_gnss_close(mbtk_gnss_handle->dev_fd); |
| mopen_open_gps(0); |
| firmware_extren_state = 0; |
| if (mbtk_gnss_handle) free(mbtk_gnss_handle); |
| mbtk_gnss_handle = NULL; |
| |
| return ret; |
| } |
| static int _kill_pthread(pthread_t pid, int kill) |
| { |
| int ret; |
| |
| if (kill) { |
| ret = pthread_cancel(pid); |
| pthread_join(pid, NULL); |
| } |
| do{ |
| ret = pthread_kill(pid, 0); |
| if(ret == ESRCH) |
| gnss_log("The specified thread does not exist or has terminated\n"); |
| else if(ret == EINVAL) |
| gnss_log("Useless signal\n"); |
| else |
| gnss_log("The thread exists\n"); |
| usleep(100000); |
| }while(0 == ret); |
| |
| return 0; |
| } |
| int mbtk_gnss_client_deinit(uint32 h_gnss) |
| { |
| int ret; |
| struct mbtk_gnss_handle_t *gnss_handle = (struct mbtk_gnss_handle_t *)h_gnss; |
| |
| if(h_gnss == NULL) |
| { |
| gnss_log("ARG error or not inited."); |
| return -1; |
| } |
| gnss_handle->inited = 0; |
| #if TTFF_TEST |
| // ttff测试线程在运行，而且不是临时固件模式 |
| if (gnss_handle->ttff_pid && |
| 0 == firmware_extren_state && |
| 0 == location_test) { |
| gnss_log("kill thread ttff.\n"); |
| _kill_pthread(gnss_handle->ttff_pid, 1); |
| } |
| #endif |
| gnss_log("kill thread info 0.\n"); |
| _kill_pthread(gnss_handle->gnss_pthread, 0); |
| |
| gnss_log("kill thread uart.\n"); |
| _kill_pthread(gnss_handle->uart_pthread, 0); |
| |
| mopen_gnss_close(gnss_handle->dev_fd); |
| |
| ret = mopen_open_gps(0); |
| if(ret) |
| { |
| printf("GNSS close init error\n"); |
| return -1; |
| } |
| |
| firmware_extren_state = 0; |
| if (gnss_handle->rb) free(gnss_handle->rb); |
| free(h_gnss); |
| mbtk_gnss_handle = NULL; |
| |
| return 0; |
| } |
| |
| int mopen_gnss_send_cmd(uint32 h_gnss, const char *cmd, int cmd_len) |
| { |
| struct mbtk_gnss_handle_t *gnss_handle = (struct mbtk_gnss_handle_t *)h_gnss; |
| return mopen_gnss_write(gnss_handle->dev_fd, cmd, cmd_len); |
| } |
| |
| /** |
| * \brief mbtk_gnss_dev_reset |
| * |
| * Detailed description |
| * |
| * \param |
| * type: 0 软件复位 |
| * 1 芯片级复位(看门狗) |
| * 2 板级复位 |
| * 3 接收机停止 |
| * mode : |
| * 0 : 热启动 |
| * 1 : 温启动 |
| * 2 : 冷启动 |
| * \return return type |
| */ |
| int mbtk_gnss_dev_reset(uint32 h_gnss, int type, int mode) |
| { |
| int ret; |
| struct mbtk_gnss_handle_t *handle = (struct mbtk_gnss_handle_t *)h_gnss; |
| // h00 热启动 |
| // h01 温启动 |
| // h85 冷启动 |
| char send_buf[24] = {0}; |
| |
| if(0 == h_gnss){ |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| |
| if (1 == handle->reset_state) { |
| printf("%s already reset_state.\n", __func__); |
| return -2; |
| } |
| |
| if (0 == mode || 1 == mode) { |
| snprintf(send_buf, sizeof(send_buf), "$RESET,%d,h0%d\r\n", type, mode); |
| } else if (2 == mode) { |
| snprintf(send_buf, sizeof(send_buf), "$RESET,%d,h85\r\n", type); |
| } else if (3 == mode) { |
| snprintf(send_buf, sizeof(send_buf), "$RESET,%d,hFF\r\n", type); |
| } else { |
| printf("%s reset mode invalid.\n", __func__); |
| return -2; |
| } |
| |
| if ( 1 == firmware_extren_state ) { |
| if (mode > 1) { |
| mbtk_gnss_firmware_update(); |
| } else { |
| memset(send_buf, 0, sizeof(send_buf)); |
| // 在有GLONASS固件的情况下，冷启动指令为: $CFGSYS,H101 |
| // 只发$RESET,0,hFF, 会重置波特率，待验证 |
| snprintf(send_buf, sizeof(send_buf), "$CFGSYS,H101\r\n"); |
| } |
| } |
| |
| gnss_log("%s : %s\n", __FUNCTION__, send_buf); |
| LOGI("%s : %s", __FUNCTION__, send_buf); |
| pthread_mutex_lock(&handle->_cond_mutex); |
| handle->reset_state = 1; |
| pthread_mutex_unlock(&handle->_cond_mutex); |
| ret = mopen_gnss_send_cmd(h_gnss, send_buf, strlen(send_buf)); |
| if(ret < 0) |
| { |
| printf("%s %d FAIL. ret:%d\n", __FUNCTION__, __LINE__, ret); |
| return -1; |
| } |
| |
| // 加载GLONASS固件后，波特率为115200 |
| if ( 0 == firmware_extren_state ) { |
| set_baudrate(handle->dev_fd, B9600); |
| gnss_log("%s : set B9600\n", __FUNCTION__); |
| } |
| |
| return 0; |
| } |
| |
| int mopen_gnss_get_device_info(uint32 h_gnss) |
| { |
| int ret; |
| char *send_buf = "$PDTINFO\r\n"; |
| if(0 == h_gnss) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| |
| ret = mopen_gnss_send_cmd(h_gnss, send_buf, strlen(send_buf)); |
| if(ret < 0) |
| { |
| printf("mopen_gnss_client_init FAIL. ret:%d\n",ret); |
| return -1; |
| } |
| |
| return 0; |
| } |
| /** |
| * @brief mopen_gnss_set_system_config |
| * |
| * @details 设置卫星系统配置 |
| * |
| * @param mode |
| * 0 -> H01(1) –GPS L1+SBAS+QZSS |
| * 1 -> H10 – BDS B1 |
| * 2 -> H101 2 – GPS+GLONASS+GALILEO+SBAS+QZSS |
| * 3 -> H11 3 – GPS+BDS+GALILEO+SBAS+QZSS |
| * @return return type |
| */ |
| int mbtk_gnss_set_system_config(uint32 h_gnss, int mode) |
| { |
| int ret; |
| char send_buf[20] = "$CFGSYS,H10\r\n"; |
| char *str_mode[4] = {"H01", "H10", "H101", "H11"}; |
| if(0 == h_gnss) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| if( mode > 3 ) |
| { |
| printf("%s param invalid.\n", __func__); |
| return -2; |
| } |
| snprintf(send_buf, sizeof(send_buf), "$CFGSYS,%s\r\n", str_mode[mode]); |
| ret = mopen_gnss_send_cmd(h_gnss, send_buf, strlen(send_buf)); |
| if(ret < 0) |
| { |
| printf("%s FAIL. ret:%d\n", __FUNCTION__, ret); |
| return -3; |
| } |
| |
| return 0; |
| } |
| /** |
| * @brief mopen_gnss_set_nema_config |
| * |
| * @details 设定NMEA 配置 |
| * 输出的NMEA 协议版本 |
| * h30 - 在 NMEA 标准 version 3.0 基础上扩展北斗相关的语句(NMEA 3.0) |
| * h51 - 在标准NMEA4.1 基础上扩展北斗相关语(NMEA 4.1) |
| * 默认配置: h51 |
| * |
| * @param mode |
| * 0 -> h30 |
| * 1 -> h51 |
| * |
| * @return return type |
| */ |
| int mbtk_gnss_set_nema_config(uint32 h_gnss, int mode) |
| { |
| int ret; |
| char send_buf[16] = "$CFGNMEA,h30\r\n"; |
| if(0 == h_gnss) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| |
| if (mode) { |
| send_buf[10] = '5'; |
| send_buf[11] = '1'; |
| } |
| ret = mopen_gnss_send_cmd(h_gnss, send_buf, strlen(send_buf)); |
| if(ret < 0) |
| { |
| printf("%s FAIL. ret:%d\n", __FUNCTION__, ret); |
| return -3; |
| } |
| |
| return 0; |
| } |
| int mopen_gnss_aidpos(uint32 h_gnss) |
| { |
| int ret; |
| char *send_buf = "$AIDPOS,4002.229934,N,11618.096855,E,37.254\r\n"; |
| if(0 == h_gnss) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| |
| ret = mopen_gnss_send_cmd(h_gnss, send_buf, strlen(send_buf)); |
| if(ret < 0) |
| { |
| printf("mopen_gnss_client_init FAIL. ret:%d\n",ret); |
| return -1; |
| } |
| return 0; |
| } |
| /** |
| * @brief mopen_gnss_get_aidinfo |
| * |
| * @details 查询辅助数据状态 |
| * $CFGAID,0,D7FBFBDF,00000000,08*47 |
| * @param param |
| * |
| * @return return type |
| */ |
| int mopen_gnss_get_aidinfo(uint32 h_gnss) |
| { |
| int ret; |
| // char *send_buf = "$AIDINFO\r\n"; |
| char *send_buf = "$CFGAID,0\r\n"; |
| if(0 == h_gnss) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| |
| ret = mopen_gnss_send_cmd(h_gnss, send_buf, strlen(send_buf)); |
| if(ret < 0) |
| { |
| printf("%s FAIL. ret:%d\n", __FUNCTION__, ret); |
| return -1; |
| } |
| return 0; |
| } |
| /** |
| * @brief mopen_gnss_get_uart |
| * |
| * @details get uart config info. |
| * $CFGPRT,1,h0,9600,129,3*57 |
| * |
| * @param param |
| * |
| * @return return type |
| */ |
| int mopen_gnss_get_uart(uint32 h_gnss) |
| { |
| int ret; |
| char *send_buf = "$CFGPRT,1\r\n"; |
| // char *send_buf = "$CFGPRT,2\r\n"; |
| if(0 == h_gnss) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| |
| ret = mopen_gnss_send_cmd(h_gnss, send_buf, strlen(send_buf)); |
| if(ret < 0) |
| { |
| printf("mopen_gnss_client_init FAIL. ret:%d\n",ret); |
| return -1; |
| } |
| return 0; |
| } |
| |
| int mopen_gnss_set_uart(uint32 h_gnss, int baudrate) |
| { |
| int ret; |
| char send_buf[28] = {0}; |
| // char *send_buf = "$CFGPRT,1,h0,9600,1,3\r\n"; |
| |
| if(0 == h_gnss) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| sprintf(send_buf, "$CFGPRT,1,h0,%d,1,3\r\n", baudrate); |
| gnss_log("%s %s", __FUNCTION__, send_buf); |
| ret = mopen_gnss_send_cmd(h_gnss, send_buf, strlen(send_buf)); |
| if(ret < 0) |
| { |
| printf("mopen_gnss_client_init FAIL. ret:%d\n",ret); |
| return -1; |
| } |
| return 0; |
| } |
| |
| int mopen_gnss_get_msg_output(uint32 h_gnss) |
| { |
| int ret; |
| char *send_buf = "$CFGMSG,0,1\r\n"; // msg class, msg id |
| |
| if(0 == h_gnss) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| ret = mopen_gnss_send_cmd(h_gnss, send_buf, strlen(send_buf)); |
| if(ret < 0) |
| { |
| printf("%s FAIL. ret:%d\n", __func__, ret); |
| return -1; |
| } |
| return 0; |
| } |
| |
| int mopen_gnss_set_msg_output(uint32 h_gnss) |
| { |
| int ret; |
| char *send_buf = "$CFGMSG,0,1,1\r\n";// msg class, msg id, msg switch |
| |
| if(0 == h_gnss) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| ret = mopen_gnss_send_cmd(h_gnss, send_buf, strlen(send_buf)); |
| if(ret < 0) |
| { |
| gnss_log("%s FAIL. ret:%d\n", __func__, ret); |
| return -1; |
| } |
| return 0; |
| } |
| |
| int mopen_gnss_set_lowpower(uint32 h_gnss) |
| { |
| int ret; |
| char *send_buf = "$CFGLOWPOWER,0\r\n";// 0 - nomale, 1 - lowpower |
| |
| if(0 == h_gnss) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| ret = mopen_gnss_send_cmd(h_gnss, send_buf, strlen(send_buf)); |
| if(ret < 0) |
| { |
| gnss_log("%s FAIL. ret:%d\n", __func__, ret); |
| return -1; |
| } |
| return 0; |
| } |
| |
| int mopen_gnss_get_ant_state_info(uint32 h_gnss) |
| { |
| int ret; |
| char *cmd1_buf = "$ANTSTAT,1\r\n"; |
| char *cmd2_buf = "$ANTSTAT1\r\n"; |
| |
| if(0 == h_gnss) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| ret = mopen_gnss_send_cmd(h_gnss, cmd1_buf, strlen(cmd1_buf)); |
| if(ret < 0) |
| { |
| gnss_log("%s FAIL. ret:%d\n", __func__, ret); |
| return -1; |
| } |
| ret = mopen_gnss_send_cmd(h_gnss, cmd2_buf, strlen(cmd2_buf)); |
| if(ret < 0) |
| { |
| gnss_log("%s FAIL. ret:%d\n", __func__, ret); |
| return -1; |
| } |
| return 0; |
| } |
| |
| int mopen_gnss_get_nmea_config(uint32 h_gnss) |
| { |
| int ret; |
| char *send_buf = "$CFGNMEA\r\n"; |
| |
| if(0 == h_gnss) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| ret = mopen_gnss_send_cmd(h_gnss, send_buf, strlen(send_buf)); |
| if(ret < 0) |
| { |
| gnss_log("%s FAIL. ret:%d\n", __func__, ret); |
| return -1; |
| } |
| return 0; |
| } |
| |
| int mbtk_gnss_add_rx_msg_handler(uint32 h_gnss, mbtk_gnss_handler_func_t handler_ptr) |
| { |
| struct mbtk_gnss_handle_t *gnss_handle = (struct mbtk_gnss_handle_t *)h_gnss; |
| |
| if(0 == h_gnss && NULL == handler_ptr) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| gnss_handle->gnss_handler_func = handler_ptr; |
| |
| return 0; |
| } |
| |
| #define AGNSS_TLE_FILE "/tmp/agnss_tle" |
| |
| static void http_data_cb_func( |
| int session_id, mbtk_http_data_type_enum type, |
| void *data,int data_len) |
| { |
| static int agnss_fd = 0; |
| int ret = 0; |
| |
| if(type == MBTK_HTTP_DATA_HEADER) { |
| gnss_log("Header(%d):%s\n", data_len, (char*)data); |
| if(agnss_fd > 0) |
| return; |
| unlink(AGNSS_TLE_FILE); |
| agnss_fd = open(AGNSS_TLE_FILE, O_RDWR|O_CREAT|O_TRUNC, 0644); |
| if (agnss_fd <= 0) |
| gnss_log("file open error\n"); |
| gnss_log("agnss file open: %d\n", agnss_fd); |
| } else if(type == MBTK_HTTP_DATA_CONTENT) { |
| gnss_log("http Data(%d)\n", data_len); |
| |
| ret = write(agnss_fd, (char*)data, data_len); |
| if (ret < 0) { |
| gnss_log("%s: error writing to file!\n", __FUNCTION__); |
| } else if (ret < data_len) { |
| gnss_log("%s: wrote less the buffer size!\n", __FUNCTION__); |
| } |
| } else { |
| gnss_log(">>>>>Complete<<<<<\n"); |
| if(agnss_fd <= 0) |
| return; |
| close(agnss_fd); |
| agnss_fd = 0; |
| } |
| } |
| |
| static int gnss_http_requst(char *id, char *pw) |
| { |
| char tmp[128] = {0}; |
| |
| int http_handle = mbtk_http_handle_get(TRUE, http_data_cb_func); |
| if(http_handle < 0) |
| { |
| printf("mbtk_http_handle_get() fail."); |
| return -1; |
| } |
| |
| int http_session = mbtk_http_session_create(http_handle, HTTP_OPTION_POST, HTTP_VERSION_1_1); |
| if(http_handle < 0) |
| { |
| printf("mbtk_http_session_create() fail."); |
| return -2; |
| } |
| |
| if(mbtk_http_session_url_set(http_handle, http_session, "http://unicore-api.rx-networks.cn/rxn-api/locationApi/rtcm")) { |
| printf("mbtk_http_session_url_set() fail.\n"); |
| return -3; |
| } |
| |
| char* post_data = "[{\"rtAssistance\":{\"format\":\"rtcm\",\"msgs\":[\"GPS:2NAF\",\"BDS:2NAF\",\"QZS:2NAF\"]}}]\r\n"; |
| |
| mbtk_http_session_head_add(http_handle, http_session, \ |
| "Host", "unicore-api.rx-networks.cn"); |
| |
| sprintf(tmp, "RXN-BASIC cId=%s,mId=Unicore,dId=12-23-34-45-58,pw=%s", id, pw); |
| mbtk_http_session_head_add(http_handle, http_session, \ |
| "Authorization", tmp); |
| |
| mbtk_http_session_head_add(http_handle, http_session, \ |
| "Content-Type", "application/json"); |
| |
| mbtk_http_session_head_add(http_handle, http_session, \ |
| "Accept", "application/octet-stream"); |
| mbtk_http_session_content_set(http_handle, http_session, |
| post_data, strlen(post_data)); |
| |
| if(mbtk_http_session_start(http_handle, http_session)) { |
| printf("mbtk_http_session_start() fail.\n"); |
| return -4; |
| } |
| |
| if(mbtk_http_handle_free(http_handle)) |
| { |
| printf("mbtk_http_handle_free() fail."); |
| return -5; |
| } |
| |
| return 0; |
| } |
| |
| /********************************** |
| |
| ID1: TempID1Expire20221031 |
| Base 64 PW1: RlJYdkFTNE9DWXJhN2ZWTA== |
| **************************************/ |
| #define AGNSS_CONFIG_FILE "/etc/mbtk/gps.conf" |
| |
| /** |
| * @brief mopen_gnss_download_tle |
| * |
| * @details 下载星历数据 |
| * (卫星星历，又称为两行轨道数据（TLE，Two-Line Orbital Element）) |
| * 保存到文件:AGNSS_TLE_FILE |
| * @param param |
| * |
| * @return return type |
| */ |
| int mbtk_gnss_download_tle(void) |
| { |
| FILE *fp; |
| char StrLine[64]; |
| char _id[24] = {0}; |
| char _passwd[28] = {0}; |
| int i; |
| if((fp = fopen(AGNSS_CONFIG_FILE, "r")) == NULL) |
| { |
| printf("open %s error!\n", AGNSS_CONFIG_FILE); |
| return -1; |
| } |
| |
| while (!feof(fp)) |
| { |
| memset(StrLine, 0, 64); |
| fgets(StrLine, 64, fp); |
| gnss_log("%s\n", StrLine); |
| i = strstr_n(StrLine, ": "); |
| if(i && strstr_n(StrLine, "ID")) |
| { |
| memcpy(_id, &StrLine[i + 1], strlen(StrLine) - i - 2); |
| } |
| else if( i && strstr_n(StrLine, "Base 64")) |
| { |
| memcpy(_passwd, &StrLine[i + 1], strlen(StrLine) - i - 2); |
| } |
| } |
| fclose(fp); |
| gnss_log("%s : %s[%d], %s[%d]\n", __FUNCTION__, _id, strlen(_id), _passwd, strlen(_passwd)); |
| |
| return gnss_http_requst(_id, _passwd); |
| } |
| |
| /** |
| * @brief mopen_gnss_injects_aidpos |
| * |
| * @details 注入星历, 128 bytes |
| * |
| * @param param |
| * |
| * @return return type |
| */ |
| int mbtk_gnss_injects_aidpos(uint32 h_gnss) |
| { |
| int ret; |
| int agnss_fd = 0; |
| int size = 0; |
| |
| if(0 == h_gnss) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| |
| agnss_fd = open(AGNSS_TLE_FILE, O_RDWR); |
| if (agnss_fd <= 0) |
| { |
| printf("%s open file FAIL. errno:%d\n", __FUNCTION__, errno); |
| return -1; |
| } |
| char* databuf = (char*)malloc(128); |
| if(databuf == NULL) |
| { |
| gnss_log("%s malloc() fail.", __FUNCTION__); |
| return -1; |
| } |
| memset(databuf, 0, 128); |
| while(0 < (size = read(agnss_fd, databuf, 128))) |
| { |
| gnss_log("%s Write[%d]\r\n", __FUNCTION__, size); |
| ret = mopen_gnss_send_cmd(h_gnss, databuf, size); |
| if(ret < 0) |
| { |
| printf("%s send cmd FAIL. ret:%d\n", __FUNCTION__, ret); |
| break; |
| } |
| memset(databuf, 0, 128); |
| } |
| close(agnss_fd); |
| free(databuf); |
| mopen_gnss_get_aidinfo(h_gnss); |
| |
| return 0; |
| } |
| /** |
| * @brief mopen_gnss_set_mode |
| * |
| * @details detailed description |
| * |
| * @param mode |
| * 0 : stop |
| * 1 : 输出一次坐标 |
| * 2 : stop |
| * 3 : 输出nmea数据到回调函数 |
| * 0 - stop, 1 - single, 2 - periodic, 3 - start |
| * |
| * @return return type |
| */ |
| int mbtk_gnss_set_mode(uint32 h_gnss, int mode) |
| { |
| struct mbtk_gnss_handle_t *gnss_handle = (struct mbtk_gnss_handle_t *)h_gnss; |
| if(0 == h_gnss) |
| { |
| printf("%s handler invalid.\n", __func__); |
| return -1; |
| } |
| gnss_handle->mode = mode; |
| |
| return 0; |
| } |
| |
| int mbtk_gnss_print_version(uint32 h_gnss) |
| { |
| printf("*************************\n"); |
| printf("-Pn: %s\n dc: %s\n hv: %s\n fw: %s\n pn: %s\n sn: %s\n ", |
| mopen_gnss_device_info.product_name, |
| mopen_gnss_device_info.dev_config, |
| mopen_gnss_device_info.hw_ver, |
| mopen_gnss_device_info.fw_ver, |
| mopen_gnss_device_info.pn, |
| mopen_gnss_device_info.sn); |
| printf("Uart bandrate: %d\n" , mopen_gnss_device_info.usrt_bandrate); |
| printf("*************************\n"); |
| |
| return 0; |
| } |
| |
| /** |
| * @brief 使用popen调用终端并获取执行结果 |
| * |
| * @param[in] cmd 命令内容 |
| * @param[out] result 保存结果的地址 |
| * @return 0或1 执行状态，成功或失败 |
| */ |
| int exec_cmd(const char *cmd, char *result) |
| { |
| FILE *pipe = popen(cmd, "r"); |
| if(!pipe) |
| return -1; |
| |
| char buffer[256] = {0}; |
| while(!feof(pipe)) |
| { |
| if(fgets(buffer, 256, pipe)) |
| { |
| printf("%s", buffer); |
| memset(buffer, 0, sizeof(buffer)); |
| } |
| } |
| pclose(pipe); |
| return 0; |
| } |
| |
| #define GNSS_AP_DATA_FILE "/etc/mbtk/rtm.bin" |
| |
| int mbtk_gnss_get_ap_data(void) |
| { |
| int state = 0; |
| uint32 *ph_gnss = NULL; |
| mbtk_gnss_handler_func_t cb; |
| int current_mode; |
| const char* cmd = "mbtk_gnss_update getap -d /dev/ttyS2 -b 115200 -a /etc/mbtk/rtm.bin"; |
| |
| if(access(GNSS_AP_DATA_FILE, F_OK) != -1) { |
| unlink(GNSS_AP_DATA_FILE); |
| } |
| mbtk_gnss_handle->getap_status = 1; |
| sleep(1); |
| printf("Mopen Gnss Get Ap Data -> \n"); |
| int ret = exec_cmd(cmd, NULL); |
| |
| usleep(100000); |
| mbtk_gnss_handle->getap_status = 0; |
| if(0 != ret) { |
| printf("Gnss getap result: %x\n", ret); |
| return -1; |
| } |
| LOGI("%s %d: %d.\n", __FUNCTION__, __LINE__, ret); |
| return ret; |
| } |
| /* |
| sync : 1 |
| */ |
| int mbtk_gnss_firmware_update(void) |
| { |
| int state = 0; |
| uint32 *ph_gnss = NULL; |
| mbtk_gnss_handler_func_t cb; |
| int current_mode; |
| int fd = -1; |
| int ret = 0; |
| |
| const char* cmd_1 = "mbtk_gnss_update downbl -d /dev/ttyS2 \ |
| -l /etc/mbtk/bootloader_r3.0.0_build6773_uartboot_921600.pkg"; |
| |
| const char* cmd_2 = "mbtk_gnss_update sendap -d /dev/ttyS2 -b 921600 -a /etc/mbtk/rtm.bin"; |
| |
| const char* cmd_3 = "mbtk_gnss_update downfw -d /dev/ttyS2 -b 921600\ |
| -f /etc/mbtk/UC6228CI-R3.4.21.0Build16211_G1L1E1_mfg.pkg"; |
| // /etc/mbtk/UC6228CI-R3.4.0.0Build7258_mfg.pkg |
| |
| if (mbtk_gnss_handle) { |
| printf("%s gnss thread runing!!!\n", __func__); |
| if (mbtk_gnss_handle->gnss_handler_func) |
| cb = mbtk_gnss_handle->gnss_handler_func; |
| |
| ph_gnss = mbtk_gnss_handle->phandle; |
| current_mode = mbtk_gnss_handle->mode; |
| // 主线程是否在运行 |
| if (mbtk_gnss_handle->gnss_pthread) |
| state = 1; |
| mbtk_gnss_client_deinit(mbtk_gnss_handle); |
| } |
| printf("Mopen Gnss Bootloader Update -> \n"); |
| //int ret = exec_cmd(cmd_1, NULL); |
| |
| fd=initial_serialPort("/dev/ttyS2"); |
| ret = downloadBL(fd, "/etc/mbtk/bootloader_r3.0.0_build6773_uartboot_921600.pkg"); |
| if(0 != ret) { |
| printf("Gnss update result: %x\n", ret); |
| close(fd); |
| return -1; |
| } |
| |
| if(access(GNSS_AP_DATA_FILE, F_OK) != -1) |
| { |
| printf("Mopen Gnss Send AP Data -> \n"); |
| //ret = exec_cmd(cmd_2, NULL); |
| |
| set_baudrate(fd, B921600); |
| ret = sendAPData(fd, "/etc/mbtk/rtm.bin"); |
| |
| if(0 != ret) { |
| close(fd); |
| printf("Gnss update result: %x\n", ret); |
| } |
| } |
| |
| printf("Mopen Gnss Firmware Update -> \n"); |
| //ret = exec_cmd(cmd_3, NULL); |
| set_baudrate(fd, B921600); |
| ret = downloadFW(fd, "/etc/mbtk/UC6228CI-R3.4.21.0Build16211_G1L1E1_mfg.pkg"); |
| close(fd); |
| |
| if(0 != ret) { |
| printf("Gnss update result: %x\n", ret); |
| return -2; |
| } |
| |
| if (0 == firmware_extren_state) |
| firmware_extren_state = 1; |
| if (state && ph_gnss) { |
| ret = mbtk_gnss_client_init(ph_gnss); |
| if (cb) { |
| mbtk_gnss_handle->gnss_handler_func = cb; |
| mbtk_gnss_handle->mode = current_mode; |
| } |
| } |
| |
| LOGI("%s %d: %d.\n", __FUNCTION__, __LINE__, ret); |
| return ret; |
| } |
| |
| int mbtk_at_gnss(int start_stop, void *cb) |
| { |
| static uint32 loc = 0; |
| int ret = -1; |
| |
| LOGI("%s %d", __FUNCTION__, __LINE__); |
| switch (start_stop) { |
| case 0: { |
| ret = mbtk_gnss_set_mode(loc, 0); |
| if(ret) |
| return -3; |
| ret = mbtk_gnss_client_deinit(loc); |
| if(ret) |
| return -3; |
| loc = 0; |
| return 0; |
| } |
| case 1: { |
| if (0 != loc) |
| return -1; |
| |
| ret = mbtk_gnss_client_init(&loc); |
| if(ret || 0 == loc) |
| return -1; |
| |
| if(NULL == cb) |
| return -2; |
| |
| ret = mbtk_gnss_add_rx_msg_handler(loc, (mbtk_gnss_handler_func_t)cb); |
| if(ret) |
| return -2; |
| ret = mbtk_gnss_set_mode(loc, 3); |
| break; |
| } |
| case 2: { |
| ret = mbtk_gnss_set_mode(loc, 0); |
| break; |
| } |
| case 3: { |
| ret = mbtk_gnss_set_mode(loc, 3); |
| break; |
| } |
| case 4: { |
| ret = mopen_uart_change(((struct mbtk_gnss_handle_t *)loc)->dev_fd, 1); |
| if(ret) { |
| printf("reset Uart set 115200 error\n"); |
| } |
| break; |
| } |
| case 6: { |
| ret = mbtk_gnss_firmware_update(); |
| if(ret) { |
| printf("gnss firmware update error!!\n"); |
| } |
| break; |
| } |
| case 11:{ |
| ret = mbtk_gnss_set_system_config((uint32)mbtk_gnss_handle,3);//GPS+BD |
| break; |
| } |
| case 10:{ |
| ret = mbtk_gnss_set_system_config((uint32)mbtk_gnss_handle,0);// only GPS |
| break; |
| } |
| case 9:{ |
| ret = mbtk_gnss_set_system_config((uint32)mbtk_gnss_handle,2);//GPS+GLONASS |
| break; |
| } |
| default: |
| break; |
| } |
| |
| return ret; |
| } |
| /* |
| 0 热启动 |
| 1 冷启动 |
| 2 温启动 |
| */ |
| int mbtk_at_gnss_reset(int type) |
| { |
| switch (type) |
| { |
| case 0: { |
| // $RESET,0,h0 |
| return mbtk_gnss_dev_reset((uint32)mbtk_gnss_handle, 0, 0); |
| } |
| case 1: { |
| // $RESET,0,h85 |
| return mbtk_gnss_dev_reset((uint32)mbtk_gnss_handle, 0, 2); |
| } |
| case 2: { |
| // $RESET,0,h01 |
| return mbtk_gnss_dev_reset((uint32)mbtk_gnss_handle, 0, 1); |
| } |
| case 3: { |
| return mbtk_gnss_dev_reset((uint32)mbtk_gnss_handle, 0, 3); |
| } |
| case 4: { |
| return mbtk_gnss_dev_reset((uint32)mbtk_gnss_handle, 3, 0); |
| } |
| case 5: { |
| return mbtk_gnss_dev_reset((uint32)mbtk_gnss_handle, 1, 0); |
| } |
| default: |
| break; |
| } |
| return -9; |
| } |
| |
| #if TTFF_TEST |
| static void gnss_gsa_info(int cmd, char *str, void *data) |
| { |
| char tmp_str[32] = {0}; |
| int ret; |
| |
| gnss_log("[ GSA ]: "); |
| // $xxGSA |
| ret = gnss_get_para_from_nmea(str, tmp_str, 1); |
| if(0 == ret && ('A' == tmp_str[0])){ |
| gnss_log("Smode: %s, ", tmp_str); |
| memset(tmp_str, 0, 32); |
| } else { |
| printf("%s [exit]: %s\n", __FUNCTION__, str); |
| return; |
| } |
| |
| /* 定位模式: |
| 1-未定位 |
| 2-2D 定位 |
| 3-3D 定位 |
| */ |
| ret = gnss_get_para_from_nmea(str, tmp_str, 2); |
| if(ret){ |
| gnss_log("L%d %s error! \n"); |
| return; |
| } |
| |
| gnss_log("fs: %s - [%d]", tmp_str, atoi(tmp_str)); |
| if ( atoi(tmp_str) != mbtk_gnss_handle->location_state ) { |
| mbtk_gnss_handle->location_state = atoi(tmp_str); |
| if (mbtk_gnss_handle->location_state > 1 && location_test) { |
| pthread_mutex_lock(&loc_cond_mutex_r); |
| location_test = 0; |
| pthread_cond_signal(&loc_sync_cond); |
| pthread_mutex_unlock(&loc_cond_mutex_r); |
| } |
| } |
| gnss_log(" -- \n"); |
| } |
| |
| int mbtk_gnss_test_ttff(int type, int timeout_sec) |
| { |
| struct timeval tnow; |
| struct timespec tout; |
| long t_start, t_end; |
| int ret; |
| |
| pthread_mutex_lock(&loc_cond_mutex_r); |
| location_test = 1; |
| pthread_mutex_unlock(&loc_cond_mutex_r); |
| |
| switch (type) |
| { |
| case 0: { |
| // $RESET,0,h0 |
| mbtk_gnss_dev_reset((uint32)mbtk_gnss_handle, 0, 0); |
| break; |
| } |
| case 1: { |
| // $RESET,0,hFF |
| mbtk_gnss_dev_reset((uint32)mbtk_gnss_handle, 0, 3); |
| break; |
| } |
| case 2: { |
| // $RESET,0,h01 |
| mbtk_gnss_dev_reset((uint32)mbtk_gnss_handle, 0, 1); |
| break; |
| } |
| case 3: { |
| // 在有GLONASS固件的情况下，冷启动指令为: $CFGSYS,H101 |
| // 只发$RESET,0,hFF, 会重置波特率，待验证 |
| mopen_gnss_set_system_config((uint32)mbtk_gnss_handle, 2); |
| break; |
| } |
| case 4: { |
| if ( 0 == firmware_extren_state ) |
| return -2; |
| |
| ret = mopen_gnss_firmware_update(); |
| if(ret) { |
| printf("gnss firmware update error!!\n"); |
| } |
| break; |
| } |
| default: |
| break; |
| } |
| if (0 == timeout_sec ) |
| timeout_sec = 60 * 3; |
| gettimeofday(&tnow, NULL); |
| t_start = tnow.tv_sec * 1000 * 1000 + tnow.tv_usec; |
| tout.tv_sec = tnow.tv_sec + timeout_sec; |
| tout.tv_nsec = tnow.tv_usec; |
| if (tout.tv_nsec > 1000000000) |
| { |
| tout.tv_sec += 1; |
| tout.tv_nsec -= 1000000000; |
| } |
| pthread_mutex_lock(&loc_cond_mutex_r); |
| ret = pthread_cond_timedwait(&loc_sync_cond, &loc_cond_mutex_r, &tout); |
| pthread_mutex_unlock(&loc_cond_mutex_r); |
| gettimeofday(&tnow, NULL); |
| t_end = tnow.tv_sec * 1000 * 1000 + tnow.tv_usec; |
| gnss_log("gnss ttff time:%ld\n", t_end - t_start); |
| if(ret == ETIMEDOUT) { |
| location_test = 0; |
| return -1; |
| } |
| |
| return (t_end - t_start)/1000; |
| } |
| |
| #define GNSS_TEST_FILE "/tmp/gnss_test" |
| #define INSERT_CUT_OFF_RULE() { memset(buffer, 0, sizeof(buffer)); \ |
| sprintf(buffer, "----------------------------------\r\n"); \ |
| ret = write(fd, buffer, strlen(buffer)); \ |
| if (ret < 0) { \ |
| printf("%s write error !!\n", __func__); \ |
| } \ |
| } |
| |
| void *gnss_ttff_thread(void *data) |
| { |
| struct mbtk_gnss_ttff_t *handle = (struct mbtk_gnss_ttff_t *)data; |
| int fd, ret, i; |
| int index, time_ms; |
| float loc_time; |
| char buffer[128] = {0}; |
| |
| pthread_detach(pthread_self()); |
| |
| if ( !access(GNSS_TEST_FILE, F_OK) ){ |
| unlink(GNSS_TEST_FILE); |
| } |
| |
| pthread_cond_init(&loc_sync_cond, NULL); |
| pthread_mutex_init(&loc_cond_mutex_r, NULL); |
| fd = open(GNSS_TEST_FILE, O_RDWR|O_CREAT|O_TRUNC, 0644); |
| if (fd <= 0) { |
| gnss_log("file open error\n"); |
| } |
| INSERT_CUT_OFF_RULE() |
| sprintf(buffer, "type: %d, timeout: %d, count: %d\r\n", handle->type, |
| handle->timeout_sec, |
| handle->test_count); |
| ret = write(fd, buffer, strlen(buffer)); |
| if (ret < 0) { |
| printf("%s write error !!\n", __func__); |
| } |
| INSERT_CUT_OFF_RULE() |
| for (i = 0; i < handle->test_count; ++i) { |
| memset(buffer, 0, sizeof(buffer)); |
| time_ms = mbtk_gnss_test_ttff(handle->type, handle->timeout_sec); |
| if (-1 == time_ms) |
| loc_time = time_ms; |
| else |
| loc_time = ((float)time_ms) / 1000; |
| sprintf(buffer, "\t %d - [ %f s ]\r\n", i + 1, loc_time); |
| printf("\t %d - %f\n", i + 1, loc_time); |
| ret = write(fd, buffer, strlen(buffer)); |
| if (ret < 0) { |
| printf("%s write error !!\n", __func__); |
| } |
| } |
| |
| INSERT_CUT_OFF_RULE() |
| close(fd); |
| mbtk_gnss_handle->ttff_pid = 0; |
| pthread_cond_destroy(&loc_sync_cond); |
| pthread_mutex_destroy(&loc_cond_mutex_r); |
| pthread_exit(NULL); |
| } |
| #endif |
| |
| int mbtk_at_gnss_start_ttff(int type, int timeout_sec, int count) |
| { |
| #if TTFF_TEST |
| int ret; |
| static struct mbtk_gnss_ttff_t mbtk_gnss_ttff; |
| |
| LOGI("%s %d, %d, %d", __FUNCTION__, type, timeout_sec, count); |
| if (NULL == mbtk_gnss_handle) { |
| printf("%s not init!!!\n", __func__); |
| return -1; |
| } |
| if (mbtk_gnss_handle->ttff_pid) { |
| printf("%s busy!!!\n", __func__); |
| return -2; |
| } |
| mbtk_gnss_ttff.type = type; |
| mbtk_gnss_ttff.timeout_sec = timeout_sec; |
| mbtk_gnss_ttff.test_count = count; |
| ret = pthread_create(&mbtk_gnss_handle->ttff_pid, NULL, (void *)gnss_ttff_thread, &mbtk_gnss_ttff); |
| if (ret != 0) |
| { |
| fprintf(stderr, "\n%s: Failed create pthread\n", __FUNCTION__); |
| return ret; |
| } |
| |
| #endif |
| return 0; |
| } |