add 5311 func
Change-Id: I703449b43eebf4681e9c34f4abddd3de7283ce04
diff --git a/mbtk/mbtk_gnssd/gnss_asr5311.c b/mbtk/mbtk_gnssd/gnss_asr5311.c
index ffa43f8..c3088bb 100755
--- a/mbtk/mbtk_gnssd/gnss_asr5311.c
+++ b/mbtk/mbtk_gnssd/gnss_asr5311.c
@@ -1,343 +1,557 @@
-/*
-* gnss_asr5311.c
-*
-* ASR 5311 gnss support source.
-*
-*/
-/******************************************************************************
-
- EDIT HISTORY FOR FILE
-
- WHEN WHO WHAT,WHERE,WHY
--------- -------- -------------------------------------------------------
-2024/6/19 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_asr5311.h"
-
-#define UART_BITRATE_NMEA_DEF_FW 115200 // Default bitrate.
-#define GNSS_SET_TIMEOUT 3000 // 3s
-
-static char config_msg_pm5[] = "PMTEST,5";
-static char config_msg_pm4[] = "PMTEST,4";
-static char config_msg_boot[] = "START,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;
-
-int gnss_write(int fd, const void *data, int data_len);
-
-static int gnss_send_cmd(int fd, const char *cmd, int cmd_len)
-{
- unsigned char check_sum = 0;
- unsigned int index;
- int len = 0;
- char cmd_buff[64] = {0};
-
- for(index = 0; index < strlen( (char *)cmd); index++)
- check_sum ^= cmd[index];
-
- sprintf((char *)cmd_buff, "$%s*%02x\r\n", cmd, check_sum);
- len = strlen((char *)cmd_buff)+1;
- return gnss_write(fd, cmd_buff, len);
-}
-
-static void gnss_set_timer_cb(int signo)
-{
- if(setting_busy) {
+/*
+* gnss_asr5311.c
+*
+* ASR 5311 gnss support source.
+*
+*/
+/******************************************************************************
+
+ EDIT HISTORY FOR FILE
+
+ WHEN WHO WHAT,WHERE,WHY
+-------- -------- -------------------------------------------------------
+2024/6/19 LiuBin Initial version
+
+******************************************************************************/
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <pthread.h>
+#include <time.h>
+#include <signal.h>
+#include <sys/time.h>
+
+#include "mbtk_log.h"
+#include "mbtk_type.h"
+#include "mbtk_gpio.h"
+#include "mbtk_utils.h"
+#include "gnss_utils.h"
+#include "gnss_asr5311.h"
+
+#define UART_BITRATE_NMEA_DEF_FW 115200 // Default bitrate.
+#define GNSS_SET_TIMEOUT 3000 // 3s
+#define GNSS_FW_DOWNLOAD_TIMER_VALUE 6
+#define GNSS_PLATFORM_CTRL_PATH "/sys/devices/platform/asr-gps/ctrl"
+#define GNSS_PLATFORM_STATUS_PATH "/sys/devices/platform/asr-gps/status"
+
+static char config_msg_pm5[] = "PMTEST,5";
+static char config_msg_pm4[] = "PMTEST,4";
+static char config_msg_boot[] = "START,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 timer_t gnss_fw_download_timer_id;
+static int gnss_fw_download_timeout_state = 0;
+static int gnss_fw_download_timer_init_state = 0;
+
+int gnss_write(int fd, const void *data, int data_len);
+
+static void gnss_fw_download_state(__attribute__( (unused)) union sigval sv)
+{
+ LOGD("aboot fw_download timeout");
+ gnss_fw_download_timeout_state = 1;
+ //log2file("%s: gnss fw download timeout\n", __func__);
+
+ system("killall aboot-tiny");
+}
+
+static int gnss_fw_download_timer_init(void)
+{
+ int rc = 0;
+ struct sigevent sigev;
+
+ memset (&sigev, 0, sizeof (struct sigevent));
+ sigev.sigev_value.sival_ptr = &gnss_fw_download_timer_id;
+ sigev.sigev_notify = SIGEV_THREAD;
+ sigev.sigev_notify_attributes = NULL;
+ sigev.sigev_notify_function = gnss_fw_download_state;
+ rc = timer_create(CLOCK_REALTIME, &sigev, &gnss_fw_download_timer_id);
+ if(rc == 0)
+ {
+ gnss_fw_download_timer_init_state = 1;
+ }
+ LOGD("fw_download timer init %d", rc);
+ return rc;
+}
+
+static int gnss_fw_download_timer_start(void)
+{
+ int rc = 0;
+ if(gnss_fw_download_timer_init_state != 1)
+ {
+ rc = gnss_fw_download_timer_init();
+ if(rc != 0)
+ {
+ LOGD("gnss_fw_download_timer_init timer retry fail");
+ return -1;
+ }
+ }
+
+ struct itimerspec ts;
+
+ memset (&ts, 0x00, sizeof (struct itimerspec));
+ ts.it_value.tv_sec = GNSS_FW_DOWNLOAD_TIMER_VALUE;
+ ts.it_value.tv_nsec = 0;
+ ts.it_interval.tv_sec = 0;
+ ts.it_interval.tv_nsec = 0;
+ rc = timer_settime(gnss_fw_download_timer_id, 0, &ts, NULL);
+ gnss_fw_download_timeout_state = 0;
+ LOGD("fw_download timer start %d", rc);
+ return rc;
+}
+
+static int gnss_fw_download_timer_stop(void)
+{
+ if(gnss_fw_download_timer_init_state != 1)
+ {
+ LOGD("gps_fw_download_timer_stop timer not creat");
+ return -1;
+ }
+ int rc = 0;
+ struct itimerspec ts;
+
+ memset (&ts, 0x00, sizeof (struct itimerspec));
+ ts.it_value.tv_sec = 0;
+ ts.it_value.tv_nsec = 0;
+ ts.it_interval.tv_sec = 0;
+ ts.it_interval.tv_nsec = 0;
+ rc = timer_settime(gnss_fw_download_timer_id, 0, &ts, NULL);
+ LOGD("fw_download timer stop %d", rc);
+ return rc;
+}
+
+static int gnss_send_cmd(int fd, const char *cmd, int cmd_len)
+{
+ unsigned char check_sum = 0;
+ unsigned int index;
+ int len = 0;
+ char cmd_buff[64] = {0};
+
+ for(index = 0; index < strlen( (char *)cmd); index++)
+ check_sum ^= cmd[index];
+
+ sprintf((char *)cmd_buff, "$%s*%02x\r\n", cmd, check_sum);
+ len = strlen((char *)cmd_buff)+1;
+ return gnss_write(fd, cmd_buff, len);
+}
+
+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);
+ if(setting_waitting)
+ {
+ setting_waitting = FALSE;
+ }
+ gnss_set_result = GNSS_ERR_TIMEOUT;
+ }
+ return;
+}
+
+static void gnss_hal_gpioctrl(char* str)
+{
+ int fd;
+ fd = open(GNSS_PLATFORM_CTRL_PATH, O_WRONLY);
+
+ if (fd >= 0) {
+ write(fd, str, strlen(str));
+ close(fd);
+ }
+}
+
+static int gnss_module_wait_fw_download_done()
+{
+ int fd;
+ char buf[64];
+ int len;
+ int ret = -1;
+
+ fd = open(GNSS_PLATFORM_STATUS_PATH, O_RDONLY);
+ if (fd >= 0) {
+ sleep(1);
+ len = read(fd, buf, 64);
+ LOGD("FW DONE %s, %d", buf, len);
+ if(!memcmp(buf, "status: on", 10)) {
+ /* FW Downloading is OK. */
+ ret = 0;
+ }
+
+ close(fd);
+ }
+
+ if(!ret)
+ gnss_hal_gpioctrl("off");
+
+ return ret;
+}
+
+static void gnss_cmd_rsp_process(const void *data, int data_len)
+{
+ const char *ptr = (const char*)data;
+ bool flag = FALSE;
+ LOGD("Setting RSP : %s", ptr);
+ if(memcmp(ptr, "$ACKOK", 6) == 0)
+ {
+ flag = TRUE;
+ }
+ else if(memcmp(ptr, "$ACKFAIL", 8) == 0)
+ {
+ //$ACKFAIL,1*56
+ int code = *(ptr + 9) - '0';
+ if(code == 1)
+ {
+ gnss_set_result = GNSS_ERR_FORMAT;
+ }
+ else if(code == 2)
+ {
+ gnss_set_result = GNSS_ERR_CHECKSUM;
+ }
+ else if(code == 3)
+ {
+ gnss_set_result = GNSS_ERR_ARG;
+ }
+ else
+ {
+ gnss_set_result = GNSS_ERR_UNKNOWN;
+ }
+ flag = TRUE;
+ }
+ else
+ {
+ //gnss_set_result = GNSS_ERR_UNKNOWN;
+ }
+
+ if(setting_waitting && flag)
+ {
+ setting_waitting = FALSE;
+ mbtk_timer_clear();
pthread_mutex_lock(&read_mutex);
pthread_cond_signal(&read_cond);
- pthread_mutex_unlock(&read_mutex);
- gnss_set_result = GNSS_ERR_TIMEOUT;
- }
- return;
-}
-
-static void gnss_hal_gpioctrl(char* str)
-{
- int fd;
- fd = open("/sys/devices/platform/asr-gps/ctrl", O_WRONLY);
-
- if (fd >= 0) {
- write(fd, str, strlen(str));
- close(fd);
- }
-}
-
-static int gnss_module_wait_fw_download_done()
-{
- int fd;
- char buf[64];
- int len;
- int ret = -1;
-
- fd = open("/sys/devices/platform/asr-gps/status", O_RDONLY);
- if (fd >= 0) {
- sleep(1);
- len = read(fd, buf, 64);
- LOGD("FW DONE %s, %d", buf, len);
- if(!memcmp(buf, "status: on", 10)) {
- /* FW Downloading is OK. */
- ret = 0;
- }
-
- close(fd);
- }
-
- if(!ret)
- gnss_hal_gpioctrl("off");
-
- return ret;
-}
-
-
-int gnss_5311_dev_open()
-{
- return 0;
-}
-
-int gnss_5311_dev_close(int fd)
-{
- gnss_send_cmd(fd, config_msg_pm5, strlen(config_msg_pm5));
- return 0;
-}
-
-int gnss_5311_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_5311_close(int fd)
-{
- pthread_mutex_destroy(&read_mutex);
- pthread_cond_destroy(&read_cond);
-
- return gnss_port_close(fd);
-}
-
-int gnss_5311_fw_dl(int fd, const char *fw_name, const char *dev)
-{
- char cmd[256] = {0};
- if(memcmp(dev, "/dev/", 5) == 0) {
- snprintf(cmd, sizeof(cmd), "nice -n -10 aboot-tiny -B 2000000 -s %s -F /etc/jacana_fw.bin -P /etc/jacana_pvt.bin", dev + 5);
- } else {
- snprintf(cmd, sizeof(cmd), "nice -n -10 aboot-tiny -B 2000000 -s %s -F /etc/jacana_fw.bin -P /etc/jacana_pvt.bin", dev);
- }
- system(cmd);
-
- return gnss_module_wait_fw_download_done();
-}
-
-void gnss_5311_set_cb(const void *data, int data_len)
-{
- const char *buff = (const char*)data;
- if(setting_busy) { // Has setting cmd process.
-
- }
-}
-
-gnss_err_enum gnss_5311_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;
-
- 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) {
-
- } else if(mode == GNSS_RESET_TYPE_WARM) {
-
- } else if(mode == GNSS_RESET_TYPE_COLD) {
-
- } 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;
- }
-
-
- 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) {
-
- }
-
- if(mode & GNSS_SET_MSGCFG_VTG) {
-
- }
-
- if(mode & GNSS_SET_MSGCFG_GGA) {
-
- }
-
- if(mode & GNSS_SET_MSGCFG_GSA) {
-
- }
-
- if(mode & GNSS_SET_MSGCFG_GRS) {
-
- }
-
- if(mode & GNSS_SET_MSGCFG_GSV) {
-
- }
-
- if(mode & GNSS_SET_MSGCFG_GLL) {
-
- }
-
- if(mode & GNSS_SET_MSGCFG_ZDA) {
-
- }
-
- if(mode & GNSS_SET_MSGCFG_GST) {
-
- }
-
- if(mode & GNSS_SET_MSGCFG_TXT) {
-
- }
- } 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;
- }
-}
-
+ pthread_mutex_unlock(&read_mutex);
+ }
+}
+
+
+int gnss_5311_dev_open()
+{
+ return 0;
+}
+
+int gnss_5311_dev_close(int fd)
+{
+ gnss_send_cmd(fd, config_msg_pm5, strlen(config_msg_pm5));
+ return 0;
+}
+
+int gnss_5311_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_5311_close(int fd)
+{
+ pthread_mutex_destroy(&read_mutex);
+ pthread_cond_destroy(&read_cond);
+
+ return gnss_port_close(fd);
+}
+
+int gnss_5311_fw_dl(int fd, const char *fw_name, const char *dev)
+{
+ char cmd[256] = {0};
+ if(memcmp(dev, "/dev/", 5) == 0) {
+ snprintf(cmd, sizeof(cmd), "nice -n -10 aboot-tiny -B 2000000 -s %s -F /etc/jacana_fw.bin -P /etc/jacana_pvt.bin", dev + 5);
+ } else {
+ snprintf(cmd, sizeof(cmd), "nice -n -10 aboot-tiny -B 2000000 -s %s -F /etc/jacana_fw.bin -P /etc/jacana_pvt.bin", dev);
+ }
+
+ int num = 1;
+ LOGD("aboot-tiny start");
+retry_download:
+ gnss_fw_download_timer_start();
+ system(cmd);
+ gnss_fw_download_timer_stop();
+ if(gnss_fw_download_timeout_state == 1)
+ {
+ if(num < 3)
+ {
+ sleep(1);
+ num++;
+ goto retry_download;
+ }
+ else
+ {
+ gnss_fw_download_timeout_state = 0;
+ LOGD("aboot-tiny timeout");
+ LOGD("aboot-tiny stop");
+ return -1;
+ }
+ }
+ else
+ {
+ int ret = gnss_module_wait_fw_download_done();
+ if(ret < 0)
+ {
+ if(num < 3)
+ {
+ num++;
+ goto retry_download;
+ }
+ else
+ {
+ LOGD("aboot-tiny 3 download fail");
+ LOGD("aboot-tiny stop");
+ return -1;
+ }
+ }
+ }
+
+ LOGD("aboot-tiny stop");
+
+ return 0;
+}
+
+void gnss_5311_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_5311_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;
+ char send_cmd[128] = {0};
+ int send_cmd_len = 0;
+
+ 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 || mode == GNSS_RESET_TYPE_WARM || mode == GNSS_RESET_TYPE_COLD) {
+ memset(send_cmd, 0x0, 128);
+ snprintf(send_cmd, 128, "RESET,%d", mode);
+ send_cmd_len = strlen(send_cmd);
+ LOGD("cmd: %s", send_cmd);
+ gnss_send_cmd(fd, send_cmd, send_cmd_len);
+ } else {
+ gnss_set_result = GNSS_ERR_ARG;
+ goto set_fail;
+ }
+ should_wait_rsp = TRUE;
+ }
+ else if(memcmp(cmd, "$NAVISYSCFG", 11) == 0) //$NAVISYSCFG,0x1,2
+ {
+ uint32 navisys = 0,navirate = 0;
+ int ret_num = sscanf(cmd, "$NAVISYSCFG,%d,%d", &navisys, &navirate);
+ if(ret_num < 1 || navisys == 0)
+ {
+ gnss_set_result = GNSS_ERR_ARG;
+ goto set_fail;
+ }
+
+ uint32 new_mode = 0;
+ if(ret_num >= 1)
+ {
+ if(((GNSS_SET_SYSCFG_GPS | GNSS_SET_SYSCFG_BDS | GNSS_SET_SYSCFG_GLO) & navisys) != navisys) {
+ gnss_set_result = GNSS_ERR_ARG;
+ goto set_fail;
+ }
+
+ if(navisys & GNSS_SET_SYSCFG_GPS) { // GPS
+ new_mode |= 0x00000001;
+ }
+ if(navisys & GNSS_SET_SYSCFG_BDS) { // BDS
+ new_mode |= 0x00000002;
+ }
+ if(navisys & GNSS_SET_SYSCFG_GLO) { // GLO
+ new_mode |= 0x00000004;
+ }
+ }
+
+ if(ret_num == 2)
+ {
+ if((GNSS_SET_FREQCFG_1 != navirate) && (GNSS_SET_FREQCFG_2 != navirate) && (GNSS_SET_FREQCFG_5 != navirate) && (GNSS_SET_FREQCFG_10 != navirate))
+ {
+ gnss_set_result = GNSS_ERR_ARG;
+ goto set_fail;
+ }
+ }
+
+ memset(send_cmd, 0x0, 128);
+ if(ret_num == 1)
+ {
+ snprintf(send_cmd, 128, "NAVISYSCFG,0x%X", new_mode);
+ }
+ else
+ {
+ snprintf(send_cmd, 128, "NAVISYSCFG,0x%X,%d", new_mode, navirate);
+ }
+ send_cmd_len = strlen(send_cmd);
+ LOGD("cmd: %s", send_cmd);
+ gnss_send_cmd(fd, send_cmd, send_cmd_len);
+ should_wait_rsp = TRUE;
+ }
+ else if(memcmp(cmd, "$MSGCFG", 7) == 0) //$MSGCFG,<mode>,<rate>
+ {
+ uint32 mode;
+ uint32 msgmask = 0;
+ 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_GST | GNSS_SET_MSGCFG_GSV | GNSS_SET_MSGCFG_GLL | GNSS_SET_MSGCFG_ZDA
+ | GNSS_SET_MSGCFG_DHV | GNSS_SET_MSGCFG_TXT | GNSS_SET_MSGCFG_DTM) & mode) != mode) {
+ gnss_set_result = GNSS_ERR_ARG;
+ goto set_fail;
+ }
+
+ if(mode & GNSS_SET_MSGCFG_RMC)
+ {
+ msgmask |= 0x00000001;
+ }
+ if(mode & GNSS_SET_MSGCFG_VTG)
+ {
+ msgmask |= 0x00000002;
+ }
+ if(mode & GNSS_SET_MSGCFG_GGA)
+ {
+ msgmask |= 0x00000004;
+ }
+ if(mode & GNSS_SET_MSGCFG_GSA)
+ {
+ msgmask |= 0x00000008;
+ }
+ if(mode & GNSS_SET_MSGCFG_GSV)
+ {
+ msgmask |= 0x00000010;
+ }
+ if(mode & GNSS_SET_MSGCFG_GLL)
+ {
+ msgmask |= 0x00000020;
+ }
+ if(mode & GNSS_SET_MSGCFG_ZDA)
+ {
+ msgmask |= 0x00000040;
+ }
+ if(mode & GNSS_SET_MSGCFG_GST)
+ {
+ msgmask |= 0x00000080;
+ }
+ if(mode & GNSS_SET_MSGCFG_TXT)
+ {
+ msgmask |= 0x00000100;
+ }
+ if(mode & GNSS_SET_MSGCFG_DHV)
+ {
+ msgmask |= 0x00000200;
+ }
+ if(mode & GNSS_SET_MSGCFG_DTM)
+ {
+ msgmask |= 0x00000400;
+ }
+ memset(send_cmd, 0x0, 128);
+ snprintf(send_cmd, 128, "MSGCFG,0,0x%X,%d", msgmask, time);
+ send_cmd_len = strlen(send_cmd);
+ LOGD("cmd: %s", send_cmd);
+ gnss_send_cmd(fd, send_cmd, send_cmd_len);
+ }
+ else
+ {
+ gnss_set_result = GNSS_ERR_ARG;
+ goto set_fail;
+ }
+
+ should_wait_rsp = TRUE;
+ }
+ else if(memcmp(cmd, "$MINEL", 6) == 0) //$MINEL,<elev>
+ {
+ uint32 elev = 0;
+ elev = (uint32)atoi(cmd + 7);
+ if(elev != GNSS_SET_EVEL_5 && elev != GNSS_SET_EVEL_15)
+ {
+ gnss_set_result = GNSS_ERR_ARG;
+ goto set_fail;
+ }
+
+ memset(send_cmd, 0x0, 128);
+ snprintf(send_cmd, 128, "MINEL,%d", elev);
+ send_cmd_len = strlen(send_cmd);
+ LOGD("cmd: %s", send_cmd);
+ gnss_send_cmd(fd, send_cmd, send_cmd_len);
+ should_wait_rsp = TRUE;
+ }
+ else if(memcmp(cmd, "$NMEACFG", 8) == 0) // $NMEACFG,<ver>
+ {
+ uint32 mode,ver = 0;
+ mode = (uint32)atoi(cmd + 9);
+ if(mode == GNSS_SET_NMEAVER_4_00)
+ {
+ ver = 0x40;
+ }
+ else if(mode == GNSS_SET_NMEAVER_4_10)
+ {
+ ver = 0x41;
+ }
+ else
+ {
+ gnss_set_result = GNSS_ERR_ARG;
+ goto set_fail;
+ }
+
+ memset(send_cmd, 0x0, 128);
+ snprintf(send_cmd, 128, "NMEACFG,0x%X", ver);
+ send_cmd_len = strlen(send_cmd);
+ LOGD("cmd: %s", send_cmd);
+ gnss_send_cmd(fd, send_cmd, send_cmd_len);
+ should_wait_rsp = TRUE;
+ }
+ 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;
+ }
+}
+