Fix mbtk_gnssd setting.
Change-Id: I3fc4664ff357f869466ee5aaf1648658493c64c8
diff --git a/mbtk/mbtk_gnssd/Makefile b/mbtk/mbtk_gnssd/Makefile
index 0cefa40..9f1ac01 100755
--- a/mbtk/mbtk_gnssd/Makefile
+++ b/mbtk/mbtk_gnssd/Makefile
@@ -24,7 +24,15 @@
#MY_FILES_PATH += $(LOCAL_PATH)/platform/linux
#endif
#LOCAL_SRC_FILES = $(wildcard *.c) $(wildcard *.cpp)
-LOCAL_SRC_FILES = gnss_main.c gnss_nmea.c gnss_6228.c gnss_ipc.c gnss_utils.c gnss_hd8122.c
+LOCAL_SRC_FILES = gnss_main.c gnss_nmea.c gnss_6228.c gnss_utils.c gnss_hd8122.c
+
+# Enable ubus
+#LOCAL_SRC_FILES += gnss_ubus.c
+#DEFINE += -DMBTK_GNSS_UBUS_ENABLE
+
+# Enable mbtk ipc(Disable ubus.)
+LOCAL_SRC_FILES += gnss_ipc.c
+
OBJS = $(patsubst %.c, %.o, $(patsubst %.cpp, %.o, $(LOCAL_SRC_FILES)))
$(info OBJS = $(OBJS))
diff --git a/mbtk/mbtk_gnssd/gnss_6228.c b/mbtk/mbtk_gnssd/gnss_6228.c
index 2cd86a4..066ae36 100755
--- a/mbtk/mbtk_gnssd/gnss_6228.c
+++ b/mbtk/mbtk_gnssd/gnss_6228.c
Binary files differ
diff --git a/mbtk/mbtk_gnssd/gnss_hd8122.c b/mbtk/mbtk_gnssd/gnss_hd8122.c
index 57b2e30..dff4599 100755
--- a/mbtk/mbtk_gnssd/gnss_hd8122.c
+++ b/mbtk/mbtk_gnssd/gnss_hd8122.c
@@ -29,30 +29,30 @@
#define GNSS_POWER_GPIO 43
#define GNSS_SET_TIMEOUT 3000 // 3s
#define GNSS_PACK_BUFF_SIZE 1024
+#define GNSS_MSG_NUM_MAX 30
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);
static uint16 fletcher16(const uint8_t* data, int data_len) {
- uint16_t sum1 = 0;
- uint16_t sum2 = 0;
+ uint32_t sum1 = 0;
+ uint32_t sum2 = 0;
int index;
for (index = 0; index < data_len; ++index ) {
- sum1 = (sum1 + data[index]) % 0xff;
- sum2 = (sum2 + sum1) % 0xff;
+ sum1 += data[index];
+ sum2 += sum1;
}
- // ???
- sum2--;
-
- return (sum2 << 8) | sum1;
+ return ((0xFF & sum2) << 8) | (0xFF & sum1);
}
static void gnss_set_timer_cb(int signo)
@@ -66,6 +66,77 @@
return;
}
+static void msg_init()
+{
+ int i = 0;
+ while(i < ARRAY_SIZE(msg_array)) {
+ msg_array[i].enable = FALSE;
+ i++;
+ }
+}
+
+
+static int msg_insert(uint8 gid, uint8 sid)
+{
+ 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)
+{
+ 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()
+{
+ int i = 0;
+ int count = 0;
+ while(i < ARRAY_SIZE(msg_array)) {
+ if(msg_array[i].enable)
+ count++;
+ i++;
+ }
+ return count;
+}
static int pack_create(hd8122_id_type_enum id_type, uint8 id, uint16 data_len, const uint8 *data, uint8 *pack, int pack_len)
{
@@ -86,20 +157,81 @@
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) {
+ 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);
- if(0)
- {
- mbtk_timer_clear();
- pthread_mutex_lock(&read_mutex);
- pthread_cond_signal(&read_cond);
- pthread_mutex_unlock(&read_mutex);
+ 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, int reset)
+static gnss_err_enum gnss_8122_reset(int fd, uint8 reset)
{
uint8 buff[GNSS_PACK_BUFF_SIZE];
LOGD("RESET");
@@ -113,6 +245,72 @@
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);
@@ -145,7 +343,7 @@
void gnss_8122_set_cb(const void *data, int data_len)
{
const char *buff = (const char*)data;
- if(setting_busy) {
+ if(setting_busy) { // Has setting cmd process.
gnss_cmd_rsp_process(data, data_len);
}
}
@@ -159,14 +357,187 @@
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", 5) == 0) {
- gnss_set_result = gnss_8122_reset(fd, atoi(cmd + 6));
+ 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
{
@@ -177,6 +548,7 @@
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);
diff --git a/mbtk/mbtk_gnssd/gnss_hd8122.h b/mbtk/mbtk_gnssd/gnss_hd8122.h
index fb9c1b8..a922d2d 100755
--- a/mbtk/mbtk_gnssd/gnss_hd8122.h
+++ b/mbtk/mbtk_gnssd/gnss_hd8122.h
@@ -94,6 +94,13 @@
} hd8122_id_aid_enum;
typedef struct {
+ bool enable;
+ uint8 gid; // Groud ID
+ uint8 sid; // Sub ID
+ hd8122_id_ack_enum ack_nak;
+} hd8122_msg_id_t;
+
+typedef struct {
uint16 head;
uint8 id_type;
uint8 id;
diff --git a/mbtk/mbtk_gnssd/gnss_info.h b/mbtk/mbtk_gnssd/gnss_info.h
index 78662a9..51b4421 100755
--- a/mbtk/mbtk_gnssd/gnss_info.h
+++ b/mbtk/mbtk_gnssd/gnss_info.h
@@ -19,6 +19,34 @@
#define GNSS_PRINT_PORT_USB_AT (1<<2) // 4
#define GNSS_PRINT_PORT_TTY_AT (1<<3) // 8
+#define GNSS_SET_SYSCFG_GPS (1)
+#define GNSS_SET_SYSCFG_BDS (1<<1)
+#define GNSS_SET_SYSCFG_GLO (1<<2)
+#define GNSS_SET_SYSCFG_GAL (1<<3)
+
+#define GNSS_SET_MSGCFG_RMC (1)
+#define GNSS_SET_MSGCFG_VTG (1<<1)
+#define GNSS_SET_MSGCFG_GGA (1<<2)
+#define GNSS_SET_MSGCFG_GSA (1<<3)
+#define GNSS_SET_MSGCFG_GSV (1<<4)
+#define GNSS_SET_MSGCFG_GLL (1<<5)
+#define GNSS_SET_MSGCFG_ZDA (1<<6)
+#define GNSS_SET_MSGCFG_GST (1<<7)
+#define GNSS_SET_MSGCFG_GRS (1<<8)
+#define GNSS_SET_MSGCFG_TXT (1<<9)
+#define GNSS_SET_MSGCFG_DHV (1<<10)
+#define GNSS_SET_MSGCFG_DTM (1<<11)
+#define GNSS_SET_MSGCFG_NAVPOS (1<<12)
+#define GNSS_SET_MSGCFG_NAVVEL (1<<13)
+#define GNSS_SET_MSGCFG_NAVTIME (1<<14)
+#define GNSS_SET_MSGCFG_NAVACC (1<<15)
+#define GNSS_SET_MSGCFG_LSF (1<<16)
+#define GNSS_SET_MSGCFG_ANTSTAT (1<<17)
+#define GNSS_SET_MSGCFG_ANTSTAT1 (1<<18)
+#define GNSS_SET_MSGCFG_NOTICE (1<<19)
+#define GNSS_SET_MSGCFG_ABNORMAL (1<<20)
+#define GNSS_SET_MSGCFG_EPHABNORMAL (1<<21)
+
typedef enum {
GNSS_ERR_OK,
GNSS_ERR_UNSUPPORT,
@@ -30,6 +58,18 @@
GNSS_ERR_UNKNOWN
} gnss_err_enum;
+typedef enum {
+ GNSS_RESET_TYPE_HOT = 1,
+ GNSS_RESET_TYPE_WARM,
+ GNSS_RESET_TYPE_COLD
+} gnss_reset_type_enum;
+
+typedef enum {
+ GNSS_MEMAVER_TYPE_3_0 = 0,
+ GNSS_MEMAVER_TYPE_4_0,
+ GNSS_MEMAVER_TYPE_4_1
+} gnss_memaver_type_enum;
+
typedef int (*gnss_dev_open_func)();
typedef int (*gnss_dev_close_func)();
typedef int (*gnss_open_func)(const char *dev);
diff --git a/mbtk/mbtk_gnssd/gnss_ipc.c b/mbtk/mbtk_gnssd/gnss_ipc.c
index a8dece8..7a39fbb 100755
--- a/mbtk/mbtk_gnssd/gnss_ipc.c
+++ b/mbtk/mbtk_gnssd/gnss_ipc.c
@@ -51,10 +51,10 @@
static void gnss_msg_process(int fd, const char *msg, int msg_len)
{
+ LOGD("CMD <%s>", msg);
// gnss_init:x
if(memcmp(msg, "gnss_init", 9) == 0) {
int init_mode = atoi(msg + 10);
- LOGD("init_mode=%d", init_mode);
int ret = 0;
if(init_mode == 0) { // Close gnss.
ret = gnss_deinit();
@@ -69,13 +69,18 @@
char rsp[100] = {0};
sprintf(rsp, "gnss_init:%d", ret);
gnss_write(fd, rsp, strlen(rsp));
- } else if(memcmp(msg, "gnss_deinit", 11) == 0) {
- LOGD("gnss_deinit");
+ } else if(memcmp(msg, "gnss_deinit", 11) == 0) { // gnss_deinit
int ret = gnss_deinit();
char rsp[100] = {0};
sprintf(rsp, "gnss_deinit:%d", ret);
gnss_write(fd, rsp, strlen(rsp));
+ } else if(memcmp(msg, "gnss_setting", 12) == 0) {// gnss_setting:cmd
+ int ret = gnss_set(msg + 13, strlen(msg + 13), NULL, 0);
+
+ char rsp[100] = {0};
+ sprintf(rsp, "gnss_setting:%d", ret);
+ gnss_write(fd, rsp, strlen(rsp));
} else {
LOGW("Unknown gnss msg : %s", msg);
}
diff --git a/mbtk/mbtk_gnssd/gnss_main.c b/mbtk/mbtk_gnssd/gnss_main.c
index b3ab5c6..688f8dc 100755
--- a/mbtk/mbtk_gnssd/gnss_main.c
+++ b/mbtk/mbtk_gnssd/gnss_main.c
@@ -17,7 +17,7 @@
#include "gnss_hd8122.h"
#define GNSS_DEBUG 1
-#define GNSS_UBUS_ENABLE 0
+#define GNSS_UBUS_ENABLE 1
#define GNSS_TAG "MBTK_GNSS"
#define GNSS_BUFF_SIZE 2048
@@ -37,7 +37,7 @@
gnss_info_t gnss_info;
-#if GNSS_UBUS_ENABLE
+#ifdef MBTK_GNSS_UBUS_ENABLE
struct ubus_context *gnss_ubus_init(void);
#else
int gnss_ipc_service_start();
@@ -45,8 +45,11 @@
int gnss_init_config(int fd);
-static char gnss_buff[GNSS_BUFF_SIZE*2] = {0};
-static uint32 gnss_buff_len = 0;
+static char nmea_buff[GNSS_BUFF_SIZE*2] = {0};
+static char data_buff[GNSS_BUFF_SIZE] = {0};
+static uint32 nmea_buff_len = 0;
+static uint32 data_buff_len = 0;
+
static bool nmea_found = FALSE;
#ifdef GNSS_DEBUG
static bool nmea_log_enable = FALSE;
@@ -262,7 +265,7 @@
static bool nmea_char_check(char ch)
{
if(isalnum(ch) || ch == '$' || ch == '\r' || ch == '\n' || ch == '.'
- || ch == ',' || ch == '*' || ch == '\0' || ch == '/')
+ || ch == ',' || ch == '*' || ch == '\0' || ch == '/' || ch == '_')
return TRUE;
return FALSE;
@@ -280,33 +283,40 @@
while(index < data_len) {
if(nmea_found) {
if(!nmea_char_check(data[index])) {
- gnss_buff_len = 0;
+ // Copy nmea_buff to data_buff
+ // Start with '$', but not nmea data, so copy to data_buff.
+ memcpy(data_buff + data_buff_len, nmea_buff, nmea_buff_len);
+ data_buff_len += nmea_buff_len;
+ data_buff[data_buff_len++] = data[index];
+
+ nmea_buff_len = 0;
nmea_found = FALSE;
-#if 0
- if(isascii(data[index])) {
- LOGD("\n\n###%c###%s\n\n", data[index], data);
- }
-#endif
continue;
}
if(data[index] != '\0') {
- gnss_buff[gnss_buff_len++] = data[index];
- if(gnss_buff[gnss_buff_len - 1] == '\n') {
-
- if(gnss_buff_len > 6 && gnss_buff[gnss_buff_len - 5] == '*') { // $XXX*YY\r\n
- gnss_nmea_process(gnss_buff, gnss_buff_len);
+ nmea_buff[nmea_buff_len++] = data[index];
+ if(nmea_buff[nmea_buff_len - 1] == '\n') {
+ if(data_buff_len > 0) {
+ gnss_info.gnss_set_cb(data_buff, data_buff_len);
+ data_buff_len = 0;
}
- gnss_buff_len = 0;
+ if(nmea_buff_len > 6 && nmea_buff[nmea_buff_len - 5] == '*') { // $XXX*YY\r\n
+ gnss_nmea_process(nmea_buff, nmea_buff_len);
+ }
+
+ nmea_buff_len = 0;
nmea_found = FALSE;
}
}
} else {
if(data[index] == '$') {
- gnss_buff_len = 0;
+ nmea_buff_len = 0;
nmea_found = TRUE;
- gnss_buff[gnss_buff_len++] = data[index];
+ nmea_buff[nmea_buff_len++] = data[index];
+ } else {
+ data_buff[data_buff_len++] = data[index];
}
}
index++;
@@ -331,8 +341,10 @@
fd_max = (gnss_info.fd > fd_max) ? gnss_info.fd : fd_max;
FD_SET(gnss_info.exit_fd[0], &fdr);
fd_max = (gnss_info.exit_fd[0] > fd_max) ? gnss_info.exit_fd[0] : fd_max;
- memset(gnss_buff, 0, sizeof(gnss_buff));
- gnss_buff_len = 0;
+ memset(nmea_buff, 0, sizeof(nmea_buff));
+ memset(data_buff, 0, sizeof(data_buff));
+ nmea_buff_len = 0;
+ data_buff_len = 0;
#if GNSS_DEBUG
int debug_fd = -1;
int debug_fd_len = 0;
@@ -628,6 +640,8 @@
return -1;
}
+ LOGD("gnss_ports_close() complete.");
+
gnss_info.fd = -1;
if(gnss_info.exit_fd[0] > 0) {
close(gnss_info.exit_fd[0]);
@@ -644,8 +658,10 @@
int gnss_set(const void* buf, unsigned int buf_len, void *cmd_rsp, int cmd_rsp_len)
{
- if(cmd_rsp && buf && buf_len > 0 && cmd_rsp_len > 0) {
- memset(cmd_rsp, 0, cmd_rsp_len);
+ if(buf && buf_len > 0) {
+ if(cmd_rsp && cmd_rsp_len > 0) {
+ memset(cmd_rsp, 0, cmd_rsp_len);
+ }
return gnss_info.gnss_set(gnss_info.fd, buf, cmd_rsp, cmd_rsp_len);
} else {
return -1;
@@ -764,7 +780,7 @@
}
// Init ubus and waitting IPC commands.
-#if GNSS_UBUS_ENABLE
+#ifdef MBTK_GNSS_UBUS_ENABLE
if(gnss_ubus_init()) {
LOGD("main() run...");
uloop_run();