| /* |
| * gnss_hd8122.c |
| * |
| * HD8122 GNSS source. |
| * |
| */ |
| /****************************************************************************** |
| |
| EDIT HISTORY FOR FILE |
| |
| WHEN WHO WHAT,WHERE,WHY |
| -------- -------- ------------------------------------------------------- |
| 2024/6/14 LiuBin Initial version |
| |
| ******************************************************************************/ |
| #include <stdio.h> |
| #include <stdlib.h> |
| #include <unistd.h> |
| #include <errno.h> |
| #include <fcntl.h> |
| #include <pthread.h> |
| |
| #include "mbtk_log.h" |
| #include "mbtk_type.h" |
| #include "mbtk_gpio.h" |
| #include "mbtk_utils.h" |
| #include "gnss_utils.h" |
| #include "gnss_hd8122.h" |
| #include "hd8122_dl/hd8040_upgrade.h" |
| |
| #define UART_BITRATE_NMEA_DEF_FW 115200 // Default bitrate. |
| #define GNSS_POWER_GPIO 43 |
| #define GNSS_SET_TIMEOUT 3000 // 3s |
| #define GNSS_PACK_BUFF_SIZE 1024 |
| #define GNSS_MSG_NUM_MAX 30 |
| #define READ_LEN_MAX 1024 //BOOT_UPGRADE_BUFF_MAX_1 |
| |
| |
| static pthread_cond_t read_cond; |
| static pthread_mutex_t read_mutex; |
| static bool setting_waitting = FALSE; |
| static bool setting_busy = FALSE; |
| static void *gnss_set_rsp_ptr = NULL; |
| static gnss_err_enum gnss_set_result = GNSS_ERR_OK; |
| static hd8122_msg_id_t msg_array[GNSS_MSG_NUM_MAX]; |
| |
| int gnss_write(int fd, const void *data, int data_len); |
| int OpenUart(char* UART_DEV); |
| int uart_close(int fd); |
| int fw_update_boot(int uart_fd, uint8_t *data, uint32_t len); |
| |
| static uint16 fletcher16(const uint8_t* data, int data_len) |
| { |
| uint32_t sum1 = 0; |
| uint32_t sum2 = 0; |
| int index; |
| |
| for (index = 0; index < data_len; ++index ) |
| { |
| sum1 += data[index]; |
| sum2 += sum1; |
| } |
| |
| return ((0xFF & sum2) << 8) | (0xFF & sum1); |
| } |
| |
| static void gnss_set_timer_cb(int signo) |
| { |
| if(setting_busy) |
| { |
| pthread_mutex_lock(&read_mutex); |
| pthread_cond_signal(&read_cond); |
| pthread_mutex_unlock(&read_mutex); |
| gnss_set_result = GNSS_ERR_TIMEOUT; |
| } |
| return; |
| } |
| |
| static void msg_init() |
| { |
| unsigned int i = 0; |
| while(i < ARRAY_SIZE(msg_array)) |
| { |
| msg_array[i].enable = FALSE; |
| i++; |
| } |
| } |
| |
| static int msg_insert(uint8 gid, uint8 sid) |
| { |
| unsigned int i = 0; |
| while(i < ARRAY_SIZE(msg_array)) |
| { |
| if(!msg_array[i].enable) |
| break; |
| i++; |
| } |
| |
| if(i == ARRAY_SIZE(msg_array)) |
| { |
| LOGE("Msg full : %d", i); |
| return -1; |
| } |
| else |
| { |
| msg_array[i].enable = TRUE; |
| msg_array[i].gid = gid; |
| msg_array[i].sid = sid; |
| return 0; |
| } |
| } |
| |
| static int msg_find(uint8 gid, uint8 sid) |
| { |
| unsigned int i = 0; |
| while(i < ARRAY_SIZE(msg_array)) |
| { |
| if(msg_array[i].enable && gid == msg_array[i].gid && sid == msg_array[i].sid) |
| break; |
| i++; |
| } |
| |
| if(i == ARRAY_SIZE(msg_array)) |
| { |
| LOGE("No found %d - %d", gid, sid); |
| return -1; |
| } |
| else |
| { |
| return i; |
| } |
| } |
| |
| static int msg_remove(uint8 gid, uint8 sid) |
| { |
| int i = msg_find(gid, sid); |
| if(i >= 0) |
| { |
| msg_array[i].enable = FALSE; |
| msg_array[i].gid = 0; |
| msg_array[i].sid = 0; |
| return 0; |
| } |
| else |
| { |
| return -1; |
| } |
| } |
| |
| static int msg_count() |
| { |
| unsigned int i = 0; |
| int count = 0; |
| while(i < ARRAY_SIZE(msg_array)) |
| { |
| if(msg_array[i].enable) |
| count++; |
| i++; |
| } |
| return count; |
| } |
| |
| static uint32_t read_bin_file(uint8_t *path, uint8_t *buff) |
| { |
| int fp = -1; |
| int ret = 0; |
| int i = 0; |
| int size = 0; |
| |
| if (NULL == path || NULL == buff) |
| { |
| LOGE("ARG error"); |
| return 0; |
| } |
| |
| fp = open((char *)path, O_RDONLY); |
| if(fp < 0) |
| { |
| LOGE( "open file failed ! errno is %d", errno); |
| return 0; |
| } |
| |
| size = lseek(fp, 0x00, SEEK_END); |
| if(size <= 0) |
| { |
| LOGE( "file is empty."); |
| return 0; |
| } |
| |
| LOGD( "file size is:%d", size); |
| lseek(fp, 0x00, SEEK_SET); |
| while(1) |
| { |
| ret = read(fp, buff, READ_LEN_MAX); |
| i += ret; |
| if(ret == READ_LEN_MAX) |
| { |
| buff += READ_LEN_MAX; |
| } |
| else |
| { |
| break; |
| } |
| } |
| |
| LOGD("file size is:%d,i:%d", size, i); |
| close(fp); |
| if(size != i) |
| { |
| return 0; |
| } |
| return size; |
| } |
| |
| |
| static int pack_create(hd8122_id_type_enum id_type, uint8 id, uint16 data_len, const uint8 *data, uint8 *pack, int pack_len) |
| { |
| if(pack == NULL || pack_len < HD8122_PACK_LEN_MIN) |
| { |
| return -1; |
| } |
| memset(pack, 0, pack_len); |
| uint8 *data_ptr = pack; |
| data_ptr += uint16_2_byte(HD8122_PACK_HEAD, data_ptr, false); |
| *data_ptr++ = (uint8)id_type; |
| *data_ptr++ = id; |
| data_ptr += uint16_2_byte(data_len, data_ptr, false); |
| if(data_len > 0) |
| { |
| memcpy(data_ptr, data, data_len); |
| data_ptr += data_len; |
| } |
| data_ptr += uint16_2_byte(fletcher16(pack + 2, 4 + data_len), data_ptr, false); |
| return (data_ptr - pack); |
| } |
| |
| // f1 d9 05 01 02 00 06 01 0f 38 |
| // or |
| // f1 d9 05 00 02 00 06 01 0f 38 |
| static int msg_array_change(const uint8 *pack, int pack_len, hd8122_id_ack_enum *ack_nak) |
| { |
| if(pack_len == 0 || pack_len % 10) |
| { |
| LOGE("pack_len(%d) error.", pack_len); |
| return -1; |
| } |
| int count = pack_len / 10; |
| int i = 0; |
| while(i < count) |
| { |
| const uint8 *ptr = pack + i * 10; |
| if(ptr[0] != 0xf1 || ptr[1] != 0xd9) |
| { |
| LOGE("Pack head error : %02x %02x", ptr[0], ptr[1]); |
| return -1; |
| } |
| |
| if(ptr[2] != 0x05) |
| { |
| LOGE("Type not 0x05 : %02x", ptr[2]); |
| return -1; |
| } |
| |
| int index = msg_find(ptr[6], ptr[7]); |
| if(index >= 0) |
| { |
| if(ptr[3] == 0x01) |
| { |
| msg_array[index].ack_nak = HD8122_ID_ACK_ACK; |
| } |
| else if(ptr[3] == 0x00) |
| { |
| msg_array[index].ack_nak = HD8122_ID_ACK_NAK; |
| |
| // There is a nak as a failure. |
| *ack_nak = HD8122_ID_ACK_NAK; |
| } |
| else |
| { |
| LOGE("ID not 0x00 or 0x01 : %02x", ptr[3]); |
| return -1; |
| } |
| |
| msg_array[index].enable = FALSE; |
| } |
| else |
| { |
| LOGE("Unknown gid - %d, sid - %d", ptr[6], ptr[7]); |
| return -1; |
| } |
| i++; |
| } |
| |
| return 0; |
| } |
| |
| static void gnss_cmd_rsp_process(const void *data, int data_len) |
| { |
| const char *ptr = (const char*)data; |
| log_hex("RSP", data, data_len); |
| |
| hd8122_id_ack_enum ack_nak = HD8122_ID_ACK_ACK; |
| if(!msg_array_change((const uint8*)data, data_len, &ack_nak)) |
| { |
| if(setting_waitting && msg_count() == 0) |
| { |
| if(ack_nak == HD8122_ID_ACK_ACK) |
| { |
| gnss_set_result = GNSS_ERR_OK; |
| } |
| else |
| { |
| gnss_set_result = GNSS_ERR_UNKNOWN; |
| } |
| |
| mbtk_timer_clear(); |
| |
| pthread_mutex_lock(&read_mutex); |
| pthread_cond_signal(&read_cond); |
| pthread_mutex_unlock(&read_mutex); |
| setting_waitting = FALSE; |
| } |
| } |
| else |
| { |
| LOGW("Unknown rsp data."); |
| } |
| } |
| |
| static gnss_err_enum gnss_8122_reset(int fd, uint8 reset) |
| { |
| uint8 buff[GNSS_PACK_BUFF_SIZE]; |
| LOGD("RESET"); |
| int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_SIMPLERST, 1, (uint8*)(&reset), buff, sizeof(buff)); |
| if(len <= 0) |
| { |
| LOGE("pack_create() fail."); |
| return GNSS_ERR_ARG; |
| } |
| log_hex("PACK", buff, len); |
| gnss_write(fd, buff, len); |
| return GNSS_ERR_OK; |
| } |
| |
| static gnss_err_enum gnss_8122_syscfg(int fd, uint32 mode) |
| { |
| uint8 buff[GNSS_PACK_BUFF_SIZE]; |
| LOGD("SYSCFG"); |
| //uint8 mode_str[4]; |
| //uint32_2_byte(mode, mode_str, TRUE); |
| int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_NAVSAT, 4, (uint8*)(&mode), buff, sizeof(buff)); |
| if(len <= 0) |
| { |
| LOGE("pack_create() fail."); |
| return GNSS_ERR_ARG; |
| } |
| log_hex("PACK", buff, len); |
| gnss_write(fd, buff, len); |
| msg_insert(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_NAVSAT); |
| return GNSS_ERR_OK; |
| } |
| |
| static gnss_err_enum gnss_8122_msgcfg(int fd, uint8 type, uint8 id, uint8 period) |
| { |
| uint8 buff[GNSS_PACK_BUFF_SIZE]; |
| LOGD("MSGCFG"); |
| uint8 data[3]; |
| data[0] = type; |
| data[1] = id; |
| data[2] = period; |
| int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_MSG, 3, data, buff, sizeof(buff)); |
| if(len <= 0) |
| { |
| LOGE("pack_create() fail."); |
| return GNSS_ERR_ARG; |
| } |
| log_hex("PACK", buff, len); |
| gnss_write(fd, buff, len); |
| msg_insert(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_MSG); |
| return GNSS_ERR_OK; |
| } |
| |
| static gnss_err_enum gnss_8122_minel(int fd, float *elev) |
| { |
| uint8 buff[GNSS_PACK_BUFF_SIZE]; |
| LOGD("ELEV"); |
| //uint8 elev_buff[4]; |
| //uint32_2_byte((uint32)elev, elev_buff, TRUE); |
| int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_ELEV, 8, (uint8*)elev, buff, sizeof(buff)); |
| if(len <= 0) |
| { |
| LOGE("pack_create() fail."); |
| return GNSS_ERR_ARG; |
| } |
| log_hex("PACK", buff, len); |
| gnss_write(fd, buff, len); |
| return GNSS_ERR_OK; |
| } |
| |
| static gnss_err_enum gnss_8122_nmeaver(int fd, uint8 ver) |
| { |
| uint8 buff[GNSS_PACK_BUFF_SIZE]; |
| LOGD("NMEA-VER"); |
| int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_NMEAVER, 1, (uint8*)(&ver), buff, sizeof(buff)); |
| if(len <= 0) |
| { |
| LOGE("pack_create() fail."); |
| return GNSS_ERR_ARG; |
| } |
| log_hex("PACK", buff, len); |
| gnss_write(fd, buff, len); |
| return GNSS_ERR_OK; |
| } |
| |
| int gnss_8122_dev_open() |
| { |
| //return mbtk_gpio_value_set(GNSS_POWER_GPIO, MBTK_GPIO_DIRECT_OUT, 1); |
| system("i2cset -y -f 2 0x31 0x15 0x86"); |
| return 0; |
| } |
| |
| int gnss_8122_dev_close(int fd) |
| { |
| //return mbtk_gpio_value_set(GNSS_POWER_GPIO, MBTK_GPIO_DIRECT_OUT, 0); |
| system("i2cset -y -f 2 0x31 0x15 0x00"); |
| return 0; |
| } |
| |
| int gnss_8122_open(const char *dev) |
| { |
| pthread_mutex_init(&read_mutex, NULL); |
| pthread_cond_init(&read_cond, NULL); |
| return gnss_port_open(dev, O_RDWR | O_NONBLOCK | O_NOCTTY, UART_BITRATE_NMEA_DEF_FW, TRUE); |
| } |
| |
| int gnss_8122_close(int fd) |
| { |
| pthread_mutex_destroy(&read_mutex); |
| pthread_cond_destroy(&read_cond); |
| return gnss_port_close(fd); |
| } |
| |
| int gnss_8122_fw_dl(int fd, const char *fw_name, const char *dev) |
| { |
| int uart_fd = OpenUart(dev);//PORT_NAME234 |
| if (uart_fd < 0) |
| { |
| LOGE("open uart failed %d[%d]", uart_fd, errno); |
| return GNSS_ERR_OPEN_DEV; |
| } |
| |
| uint8_t *g_bin_buff = (uint8_t*)malloc(500*1024); |
| if(g_bin_buff == NULL) { |
| LOGE("malloc() fail : %d", errno); |
| return GNSS_ERR_UNKNOWN; |
| } |
| memset(g_bin_buff, 0, 500*1024); |
| uint32_t len = read_bin_file(fw_name, g_bin_buff); |
| if (len <= 0) |
| { |
| LOGE("Read file failed ,len = %d", len); |
| return GNSS_ERR_DL_FW; |
| } |
| if(gnss_8122_dev_open()) { |
| LOGE("open gnss device fail:%d", errno); |
| return GNSS_ERR_DL_FW; |
| } |
| int ret = fw_update_boot(uart_fd, g_bin_buff, len); |
| free(g_bin_buff); |
| if (ret < 0) |
| { |
| LOGE("fw_update_boot() fail : %d", ret); |
| return GNSS_ERR_DL_FW; |
| } |
| uart_close(uart_fd); |
| if(ret == HDBD_UPG_SUCESS) |
| { |
| LOGD("upgrade sucess!"); |
| return GNSS_ERR_OK; |
| } |
| else |
| { |
| LOGD("upgrade FAIL, fail style:%d", ret); |
| return GNSS_ERR_DL_FW; |
| } |
| } |
| |
| void gnss_8122_set_cb(const void *data, int data_len) |
| { |
| const char *buff = (const char*)data; |
| if(setting_busy) // Has setting cmd process. |
| { |
| gnss_cmd_rsp_process(data, data_len); |
| } |
| } |
| |
| gnss_err_enum gnss_8122_set(int fd, const char *cmd, void *cmd_rsp, int cmd_rsp_len) |
| { |
| if(setting_busy) |
| { |
| return GNSS_ERR_SET_BUSY; |
| } |
| else |
| { |
| bool should_wait_rsp = TRUE; |
| setting_busy = TRUE; |
| gnss_set_rsp_ptr = cmd_rsp; |
| gnss_set_result = GNSS_ERR_OK; |
| msg_init(); |
| mbtk_timer_set(gnss_set_timer_cb, GNSS_SET_TIMEOUT); |
| |
| if(memcmp(cmd, "$RESET", 6) == 0) // $RESET,<mode> |
| { |
| gnss_reset_type_enum mode = (gnss_reset_type_enum)atoi(cmd + 7); |
| if(mode == GNSS_RESET_TYPE_HOT) |
| { |
| gnss_set_result = gnss_8122_reset(fd, 3); |
| } |
| else if(mode == GNSS_RESET_TYPE_WARM) |
| { |
| gnss_set_result = gnss_8122_reset(fd, 2); |
| } |
| else if(mode == GNSS_RESET_TYPE_COLD) |
| { |
| gnss_set_result = gnss_8122_reset(fd, 1); |
| } |
| else |
| { |
| gnss_set_result = GNSS_ERR_ARG; |
| goto set_fail; |
| } |
| if(gnss_set_result != GNSS_ERR_OK) |
| { |
| goto set_fail; |
| } |
| should_wait_rsp = FALSE; |
| } |
| else if(memcmp(cmd, "$SYSCFG", 7) == 0) // $SYSCFG,<mode> |
| { |
| uint32 mode = 0; |
| mode = (uint32)atoi(cmd + 8); |
| uint32 new_mode = 0; |
| if(((GNSS_SET_SYSCFG_GPS | GNSS_SET_SYSCFG_BDS | GNSS_SET_SYSCFG_GLO | GNSS_SET_SYSCFG_GAL) & mode) != mode) |
| { |
| gnss_set_result = GNSS_ERR_ARG; |
| goto set_fail; |
| } |
| |
| if(mode & GNSS_SET_SYSCFG_GPS) // GPS |
| { |
| new_mode |= 0x00000001; |
| } |
| if(mode & GNSS_SET_SYSCFG_BDS) // BDS |
| { |
| new_mode |= 0x00000002; |
| } |
| if(mode & GNSS_SET_SYSCFG_GLO) // GLO |
| { |
| new_mode |= 0x00000004; |
| } |
| if(mode & GNSS_SET_SYSCFG_GAL) // GAL |
| { |
| new_mode |= 0x00000010; |
| } |
| |
| gnss_set_result = gnss_8122_syscfg(fd, new_mode); |
| if(gnss_set_result != GNSS_ERR_OK) |
| { |
| goto set_fail; |
| } |
| should_wait_rsp = TRUE; |
| } |
| else if(memcmp(cmd, "$MSGCFG", 7) == 0) // $MSGCFG,<mode>,<rate> |
| { |
| uint32 mode; |
| int rate; |
| if(2 == sscanf(cmd, "$MSGCFG,%d,%d", &mode, &rate)) |
| { |
| int time = rate / 1000; // s |
| if(time < 0) |
| { |
| gnss_set_result = GNSS_ERR_ARG; |
| goto set_fail; |
| } |
| |
| if(((GNSS_SET_MSGCFG_RMC | GNSS_SET_MSGCFG_VTG | GNSS_SET_MSGCFG_GGA | GNSS_SET_MSGCFG_GSA |
| | GNSS_SET_MSGCFG_GRS | GNSS_SET_MSGCFG_GSV | GNSS_SET_MSGCFG_GLL | GNSS_SET_MSGCFG_ZDA |
| | GNSS_SET_MSGCFG_GST | GNSS_SET_MSGCFG_TXT) & mode) != mode) |
| { |
| gnss_set_result = GNSS_ERR_ARG; |
| goto set_fail; |
| } |
| |
| if(mode & GNSS_SET_MSGCFG_RMC) |
| { |
| gnss_set_result = gnss_8122_msgcfg(fd, 0xF8, 0x05, time); |
| if(gnss_set_result != GNSS_ERR_OK) |
| { |
| goto set_fail; |
| } |
| } |
| |
| if(mode & GNSS_SET_MSGCFG_VTG) |
| { |
| gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x06, time); |
| if(gnss_set_result != GNSS_ERR_OK) |
| { |
| goto set_fail; |
| } |
| } |
| |
| if(mode & GNSS_SET_MSGCFG_GGA) |
| { |
| gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x00, time); |
| if(gnss_set_result != GNSS_ERR_OK) |
| { |
| goto set_fail; |
| } |
| } |
| |
| if(mode & GNSS_SET_MSGCFG_GSA) |
| { |
| gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x02, time); |
| if(gnss_set_result != GNSS_ERR_OK) |
| { |
| goto set_fail; |
| } |
| } |
| |
| if(mode & GNSS_SET_MSGCFG_GRS) |
| { |
| gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x03, time); |
| if(gnss_set_result != GNSS_ERR_OK) |
| { |
| goto set_fail; |
| } |
| } |
| |
| if(mode & GNSS_SET_MSGCFG_GSV) |
| { |
| gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x04, time); |
| if(gnss_set_result != GNSS_ERR_OK) |
| { |
| goto set_fail; |
| } |
| } |
| |
| if(mode & GNSS_SET_MSGCFG_GLL) |
| { |
| gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x01, time); |
| if(gnss_set_result != GNSS_ERR_OK) |
| { |
| goto set_fail; |
| } |
| } |
| |
| if(mode & GNSS_SET_MSGCFG_ZDA) |
| { |
| gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x07, time); |
| if(gnss_set_result != GNSS_ERR_OK) |
| { |
| goto set_fail; |
| } |
| } |
| |
| if(mode & GNSS_SET_MSGCFG_GST) |
| { |
| gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x08, time); |
| if(gnss_set_result != GNSS_ERR_OK) |
| { |
| goto set_fail; |
| } |
| } |
| |
| if(mode & GNSS_SET_MSGCFG_TXT) |
| { |
| gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x20, time); |
| if(gnss_set_result != GNSS_ERR_OK) |
| { |
| goto set_fail; |
| } |
| } |
| } |
| else |
| { |
| gnss_set_result = GNSS_ERR_ARG; |
| goto set_fail; |
| } |
| |
| should_wait_rsp = TRUE; |
| } |
| else if(memcmp(cmd, "$MINEL", 6) == 0) // $MINEL,<elev> |
| { |
| #if 0 |
| float elev = 0.0f; |
| elev = (float)atof(cmd + 7); |
| // LOGD("ELEV : %f", elev); |
| // log_hex("ELEV", &elev, sizeof(double)); |
| if(elev < -90.0f || elev > 90.0f) |
| { |
| gnss_set_result = GNSS_ERR_ARG; |
| goto set_fail; |
| } |
| float elevs[2]; |
| elevs[0] = elev; |
| elevs[1] = elev; |
| gnss_set_result = gnss_8122_minel(fd, elevs); |
| if(gnss_set_result != GNSS_ERR_OK) |
| { |
| goto set_fail; |
| } |
| should_wait_rsp = FALSE; |
| #else |
| gnss_set_result = GNSS_ERR_UNSUPPORT; |
| goto set_fail; |
| #endif |
| } |
| else if(memcmp(cmd, "$NMEACFG", 8) == 0) // $NMEACFG,<ver> |
| { |
| #if 0 |
| gnss_memaver_type_enum version = (gnss_memaver_type_enum)atoi(cmd + 9); |
| if(version == GNSS_MEMAVER_TYPE_3_0) |
| { |
| gnss_set_result = gnss_8122_nmeaver(fd, 1); |
| } |
| else if(version == GNSS_MEMAVER_TYPE_4_0) |
| { |
| gnss_set_result = gnss_8122_nmeaver(fd, 2); |
| } |
| else if(version == GNSS_MEMAVER_TYPE_4_1) |
| { |
| gnss_set_result = gnss_8122_nmeaver(fd, 3); |
| } |
| else |
| { |
| gnss_set_result = GNSS_ERR_ARG; |
| goto set_fail; |
| } |
| if(gnss_set_result != GNSS_ERR_OK) |
| { |
| goto set_fail; |
| } |
| should_wait_rsp = FALSE; |
| #else |
| gnss_set_result = GNSS_ERR_UNSUPPORT; |
| goto set_fail; |
| #endif |
| } |
| else |
| { |
| LOGW("Unknown cmd:%s", cmd); |
| gnss_set_result = GNSS_ERR_UNSUPPORT; |
| goto set_fail; |
| } |
| |
| set_success: |
| if(should_wait_rsp) |
| { |
| setting_waitting = TRUE; |
| pthread_mutex_lock(&read_mutex); |
| pthread_cond_wait(&read_cond, &read_mutex); |
| pthread_mutex_unlock(&read_mutex); |
| } |
| else |
| { |
| mbtk_timer_clear(); |
| } |
| |
| setting_busy = FALSE; |
| return gnss_set_result; |
| set_fail: |
| setting_busy = FALSE; |
| mbtk_timer_clear(); |
| return gnss_set_result; |
| } |
| } |
| |
| |