gnss: add 8122 AT set command
Change-Id: Ia406f06244ec0ff7570744113e31224d99330165
diff --git a/mbtk/mbtk_gnssd/gnss_hd8122.c b/mbtk/mbtk_gnssd/gnss_hd8122.c
index 8d57fff..e61bfe6 100755
--- a/mbtk/mbtk_gnssd/gnss_hd8122.c
+++ b/mbtk/mbtk_gnssd/gnss_hd8122.c
@@ -60,6 +60,8 @@
static hd8122_msg_id_t msg_array[GNSS_MSG_NUM_MAX];
static char gnss_ctrl_path[128] = "/sys/devices/platform/mbtk-gnss/ctrl";
static gnss_agps_info_t agps_info;
+static char *gnss_set_cmd_rsp = NULL;
+static int gnss_cmd_rsp_len = 0;
int gnss_write(int fd, const void *data, int data_len);
int OpenUart(char* UART_DEV);
@@ -291,7 +293,39 @@
// 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)
+ if(pack_len == 0)
+ {
+ LOGE("pack_len(%d) error.", pack_len);
+ return -1;
+ }
+
+ //info result
+ if(gnss_set_cmd_rsp != NULL)
+ {
+ if(pack_len > 4 && pack[2] == 0x0a && pack[3] == 0x04) //VER INFO
+ {
+ char sw_ver[32] = {0};
+ char hw_ver[32] = {0};
+ memcpy(sw_ver, pack + 6, 16);
+ memcpy(hw_ver, pack + 22, 16);
+ snprintf(gnss_set_cmd_rsp, gnss_cmd_rsp_len, "%s,%s", sw_ver, hw_ver);
+ LOGE("fw ver:%s", gnss_set_cmd_rsp);
+ int index = msg_find(pack[2], pack[3]);
+ if(index >= 0)
+ {
+ msg_array[index].enable = FALSE;
+ }
+ else
+ {
+ LOGE("Unknown gid - %d, sid - %d", pack[2], pack[3]);
+ return -1;
+ }
+ return 0;
+ }
+ }
+
+ //set result
+ if(pack_len % 10)
{
LOGE("pack_len(%d) error.", pack_len);
return -1;
@@ -472,13 +506,11 @@
return GNSS_ERR_OK;
}
-static gnss_err_enum gnss_8122_minel(int fd, float *elev)
+static gnss_err_enum gnss_8122_ver(int fd)
{
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));
+ LOGD("VER");
+ int len = pack_create(HD8122_ID_TYPE_MON, HD8122_ID_MON_VER, 0, NULL, buff, sizeof(buff));
if(len <= 0)
{
LOGE("pack_create() fail.");
@@ -486,9 +518,46 @@
}
log_hex("PACK", buff, len);
gnss_write(fd, buff, len);
+ msg_insert(HD8122_ID_TYPE_MON, HD8122_ID_MON_VER);
return GNSS_ERR_OK;
}
+
+static gnss_err_enum gnss_8122_minel(int fd, uint8 *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, 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);
+ msg_insert(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_ELEV);
+ return GNSS_ERR_OK;
+}
+
+static gnss_err_enum gnss_8122_PZ90CONV(int fd, uint8 *coord)
+{
+ uint8 buff[GNSS_PACK_BUFF_SIZE];
+ LOGD("PZ90CONV");
+ int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_COORD, 44, coord, 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_COORD);
+ return GNSS_ERR_OK;
+}
+
+
static gnss_err_enum gnss_8122_nmeaver(int fd, uint8 ver)
{
uint8 buff[GNSS_PACK_BUFF_SIZE];
@@ -768,7 +837,7 @@
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)
+ if(((GNSS_SET_SYSCFG_GPS | GNSS_SET_SYSCFG_BDS | GNSS_SET_SYSCFG_GLO | GNSS_SET_SYSCFG_GAL | GNSS_SET_SYSCFG_SBAS | GNSS_SET_SYSCFG_QZSS) & mode) != mode)
{
gnss_set_result = GNSS_ERR_ARG;
goto set_fail;
@@ -780,16 +849,24 @@
}
if(mode & GNSS_SET_SYSCFG_BDS) // BDS
{
- new_mode |= 0x00000002;
+ new_mode |= 0x00000004;
}
if(mode & GNSS_SET_SYSCFG_GLO) // GLO
{
- new_mode |= 0x00000004;
+ new_mode |= 0x00000002;
}
if(mode & GNSS_SET_SYSCFG_GAL) // GAL
{
new_mode |= 0x00000010;
}
+ if(mode & GNSS_SET_SYSCFG_SBAS) // SBAS
+ {
+ new_mode |= 0x00000020;
+ }
+ if(mode & GNSS_SET_SYSCFG_QZSS) // QZSS
+ {
+ new_mode |= 0x00000040;
+ }
gnss_set_result = gnss_8122_syscfg(fd, new_mode);
if(gnss_set_result != GNSS_ERR_OK)
@@ -972,31 +1049,86 @@
should_wait_rsp = FALSE;
}
- else if(memcmp(cmd, "$MINEL", 6) == 0) // $MINEL,<elev>
+ else if(memcmp(cmd, "$VER", 4) == 0) // $VER
{
-#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);
+ gnss_set_cmd_rsp = (char *)cmd_rsp;
+ gnss_cmd_rsp_len = cmd_rsp_len;
+ gnss_set_result = gnss_8122_ver(fd);
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, "$MINEL", 6) == 0) // $MINEL,<elev>
+ {
+ uint32 elev = 0;
+ elev = (uint32)atoi(cmd + 7);
+ LOGD("set minel: %d", elev);
+ if(GNSS_SET_EVEL_5 == elev)
+ {
+ uint8 elev_buff[8] = {0x35, 0xFA, 0x8E, 0x3C, 0xC2, 0xB8, 0xB2, 0x3D};
+ gnss_set_result = gnss_8122_minel(fd, elev_buff);
+ if(gnss_set_result != GNSS_ERR_OK)
+ {
+ goto set_fail;
+ }
+ }
+ else if(GNSS_SET_EVEL_15 == elev)
+ {
+ uint8 elev_buff[8] = {0x35, 0xFA, 0x8E, 0x3C, 0x92, 0x0A, 0x86, 0x3E};
+ gnss_set_result = gnss_8122_minel(fd, elev_buff);
+ if(gnss_set_result != GNSS_ERR_OK)
+ {
+ goto set_fail;
+ }
+ }
+ else
+ {
+ gnss_set_result = GNSS_ERR_ARG;
+ goto set_fail;
+ }
+ }
+ else if(memcmp(cmd, "$PZ90CONV", 9) == 0) // $PZ90CONV,local,ref
+ {
+ int local = 0;
+ int ref = 0;
+ uint8 buff[44] = {0x00, 0x00, 0x00, 0x00, 0xA6, 0x54, 0x58, 0x41,
+ 0xCD, 0x8C, 0x8F, 0xCC, 0x56, 0x77, 0x6B, 0x3F,
+ 0x8F, 0xC2, 0xF5, 0xBC, 0x0A, 0xD7, 0x23, 0xBC,
+ 0x00, 0x00, 0x00, 0x00, 0x30, 0x62, 0x9F, 0x37,
+ 0x28, 0x29, 0x30, 0xB8, 0xBD, 0x37, 0x06, 0x36,
+ 0x00, 0x00, 0x00, 0x00};
+
+ if(2 == sscanf(cmd, "$PZ90CONV,%d,%d", &local, &ref))
+ {
+ LOGD("set pz90conv: %d, %d", local, ref);
+
+
+ if(local == 1 && ref == 0) //PZ90
+ {
+ //
+ }
+ else if(local == 0 && ref == 0)
+ {
+ memset(buff, 0x00, 44);
+ }
+ else
+ {
+ gnss_set_result = GNSS_ERR_UNSUPPORT;
+ goto set_fail;
+ }
+ }
+ else
+ {
+ gnss_set_result = GNSS_ERR_ARG;
+ goto set_fail;
+ }
+
+ gnss_set_result = gnss_8122_PZ90CONV(fd, buff);
+ if(gnss_set_result != GNSS_ERR_OK)
+ {
+ goto set_fail;
+ }
}
else if(memcmp(cmd, "$NMEACFG", 8) == 0) // $NMEACFG,<ver>
{
@@ -1049,9 +1181,19 @@
mbtk_timer_clear();
}
+ if(gnss_set_cmd_rsp != NULL)
+ {
+ gnss_set_cmd_rsp = NULL;
+ gnss_cmd_rsp_len = 0;
+ }
setting_busy = FALSE;
return gnss_set_result;
set_fail:
+ if(gnss_set_cmd_rsp != NULL)
+ {
+ gnss_set_cmd_rsp = NULL;
+ gnss_cmd_rsp_len = 0;
+ }
setting_busy = FALSE;
mbtk_timer_clear();
return gnss_set_result;