Fix mbtk from v1265 GSW
Change-Id: I5d8d395616f284bc74c8b9448cfa347164b5a668
diff --git a/mbtk/libgsw_lib/gsw_gnss.c b/mbtk/libgsw_lib/gsw_gnss.c
new file mode 100755
index 0000000..7beb0d1
--- /dev/null
+++ b/mbtk/libgsw_lib/gsw_gnss.c
@@ -0,0 +1,567 @@
+#include <stdio.h>
+#include <string.h>
+#include <strings.h>
+#include <stdlib.h>
+#include <errno.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 <dlfcn.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "gsw_gnss.h"
+
+#ifndef LOG_ERR_LEVEL
+#define LOG_ERR_LEVEL 3 /* error conditions */
+#endif
+#ifndef LOG_WARN_LEVEL
+#define LOG_WARN_LEVEL 4 /* warning conditions */
+#endif
+#ifndef LOG_INFO_LEVEL
+#define LOG_INFO_LEVEL 6 /* informational */
+#endif
+#ifndef LOG_DEBUG_LEVEL
+#define LOG_DEBUG_LEVEL 7 /* debug-level messages */
+#endif
+#ifndef LOG_VERBOSE_LEVEL
+#define LOG_VERBOSE_LEVEL 8
+#endif
+
+#define LOGV(fmt, args ...) \
+ do{ \
+ char *file_ptr_1001 = __FILE__; \
+ char *ptr_1001 = file_ptr_1001 + strlen(file_ptr_1001) - 1; \
+ char line_1001[10] = {0}; \
+ sprintf(line_1001, "%d", __LINE__); \
+ while(ptr_1001 >= file_ptr_1001 && *ptr_1001){ \
+ if(*ptr_1001 == '/') \
+ break; \
+ ptr_1001--; \
+ } \
+ fun_ptr_log(LOG_VERBOSE_LEVEL, "%s#%s: " fmt, ptr_1001 + 1, line_1001, ##args); \
+ } while(0)
+
+#define LOGI(fmt, args...) \
+ do{ \
+ char *file_ptr_1001 = __FILE__; \
+ char *ptr_1001 = file_ptr_1001 + strlen(file_ptr_1001) - 1; \
+ char line_1001[10] = {0}; \
+ sprintf(line_1001, "%d", __LINE__); \
+ while(ptr_1001 >= file_ptr_1001 && *ptr_1001){ \
+ if(*ptr_1001 == '/') \
+ break; \
+ ptr_1001--; \
+ } \
+ fun_ptr_log(LOG_INFO_LEVEL, "%s#%s: " fmt, ptr_1001 + 1, line_1001, ##args); \
+ } while(0)
+
+#define LOGD(fmt, args...) \
+ do{ \
+ char *file_ptr_1001 = __FILE__; \
+ char *ptr_1001 = file_ptr_1001 + strlen(file_ptr_1001) - 1; \
+ char line_1001[10] = {0}; \
+ sprintf(line_1001, "%d", __LINE__); \
+ while(ptr_1001 >= file_ptr_1001 && *ptr_1001){ \
+ if(*ptr_1001 == '/') \
+ break; \
+ ptr_1001--; \
+ } \
+ fun_ptr_log(LOG_DEBUG_LEVEL, "%s#%s: " fmt, ptr_1001 + 1, line_1001, ##args); \
+ } while(0)
+
+#define LOGW(fmt, args...) \
+ do{ \
+ char *file_ptr_1001 = __FILE__; \
+ char *ptr_1001 = file_ptr_1001 + strlen(file_ptr_1001) - 1; \
+ char line_1001[10] = {0}; \
+ sprintf(line_1001, "%d", __LINE__); \
+ while(ptr_1001 >= file_ptr_1001 && *ptr_1001){ \
+ if(*ptr_1001 == '/') \
+ break; \
+ ptr_1001--; \
+ } \
+ fun_ptr_log(LOG_WARN_LEVEL, "%s#%s: " fmt, ptr_1001 + 1, line_1001, ##args); \
+ } while(0)
+
+#define LOGE(fmt, args...) \
+ do{ \
+ char *file_ptr_1001 = __FILE__; \
+ char *ptr_1001 = file_ptr_1001 + strlen(file_ptr_1001) - 1; \
+ char line_1001[10] = {0}; \
+ sprintf(line_1001, "%d", __LINE__); \
+ while(ptr_1001 >= file_ptr_1001 && *ptr_1001){ \
+ if(*ptr_1001 == '/') \
+ break; \
+ ptr_1001--; \
+ } \
+ fun_ptr_log(LOG_ERR_LEVEL, "%s#%s: " fmt, ptr_1001 + 1, line_1001, ##args); \
+ } while(0)
+
+
+#define GSW_HAL_SUCCESS 0
+#define GSW_HAL_FAIL -1 //表示失败(通用性)
+#define GSW_HAL_MEM_INVAILD -2 //表示入参地址为NULL
+
+#define MOPEN_GNSS_NMEA_MAX_LENGTH 255 /** NMEA string maximum length. */
+static gsw_gnss_cb *gsw_cb = NULL;
+static bool inited = false;
+static bool strated = false;
+
+typedef void (*mbtk_gnss_callback_func)(uint32_t ind_type, const void* data, uint32_t data_len);
+typedef void (*mbtk_log)(int level, const char *format,...);
+static mbtk_log fun_ptr_log = NULL;
+
+int (*mbtk_gnss_init)(mbtk_gnss_callback_func cb);
+int (*mbtk_gnss_deinit)();
+
+int (*mbtk_gnss_ind_set)(uint32_t ,int);
+int (*mbtk_gnss_open)(int, int);
+int (*mbtk_gnss_close)(int);
+int (*mbtk_gnss_setting)(const char *setting_cmd, int);
+
+int (*mbtk_gnss_eph_download)(int);
+int (*mbtk_gnss_eph_inject)(int);
+
+int gnss_freq = -1;
+GSW_GNSS_MODE_CONFIGURATION gnss_startmode = -1;
+GSW_GNSS_CONF_SWITCH gnss_switch_op = -1;
+void *dlHandle_gnss;
+char *lynqLib_gnss = "/lib/libmbtk_lib.so";
+
+typedef enum
+{
+ E_MT_LOC_MSG_ID_LOCATION_INFO = 1, /**< pv_data = & mopen_location_info_t */
+ E_MT_LOC_MSG_ID_NMEA_INFO = 3, /**< pv_data = & mopen_gnss_nmea_info_t */
+} e_msg_id_t;
+
+typedef struct
+{
+ int64_t timestamp; /**< System Timestamp, marked for when got the nmea data */
+ int length; /**< NMEA string length. */
+ char nmea[MOPEN_GNSS_NMEA_MAX_LENGTH + 1]; /**< NMEA string.*/
+}mopen_gnss_nmea_info_t; /* Message */
+
+typedef struct
+{
+ uint32_t size; /**< Set to the size of mcm_gps_location_t. */
+ int flags; /**< Contains GPS location flags bits. */
+ int position_source; /**< Provider indicator for HYBRID or GPS. */ //功能暂未实现，可不用添加进结构体
+ double latitude; /**< Latitude in degrees. */
+ double longitude; /**< Longitude in degrees. */
+ double altitude; /**< Altitude in meters above the WGS 84 reference ellipsoid. */
+ float speed; /**< Speed in meters per second. */
+ float bearing; /**< Heading in degrees. */ //功能暂未实现，可不用添加进结构体
+ float accuracy; /**< Expected accuracy in meters. */ //功能暂未实现，可不用添加进结构体
+ int64_t timestamp; /**< Timestamp for the location fix in UTC million-second base. */
+ int32_t is_indoor; /**< Location is indoors. */ //功能暂未实现，可不用添加进结构体
+ float floor_number; /**< Indicates the floor number. */
+}mopen_location_info_t;//功能暂未实现，可不用添加进结构体
+
+
+typedef struct {
+ uint32_t flags;
+ double latitude; /**< Latitude in degrees. */
+ double longitude; /**< Longitude in degrees. */
+ double altitude; /**< Altitude in meters above the WGS 84 reference ellipsoid. */
+ float speed; /**< Speed in meters per second. */
+ float bearing; /**< Heading in degrees. */
+ int64_t timestamp; /**< Timestamp for the location fix in UTC million-second base. */
+} mbtk_gnss_location_info_t;
+
+typedef enum{
+ MODE_GPS = 1, /**< GPS only */
+ MODE_BEIDOU, /**< BEIDOU only*/
+ MODE_GPS_BEIDOU, /**< GPS+BEIDOU */
+ MODE_GLONASS, /**< GLONASS only */
+ MODE_GPS_GLONASS, /**< GPS+GLONASS */
+ MODE_GLONASS_BEIDOU, /**< GLONASS+BEIDOU */ /* The type does not support this type */
+ MODE_GPS_GLONASS_BEIDOU, /**< GPS+GLONASS+BEIDOU */ /* The type does not support this type */
+ MODE_GALILEO, /**< GALILEO only */
+ MODE_GPS_GALILEO, /**< GPS+GALILEO */
+ MODE_BEIDOU_GALILEO, /**< BEIDOU+GALILEO */
+ MODE_GPS_BEIDOU_GALILEO, /**< GPS+BEIDOU+GALILEO */
+ MODE_GLONASS_GALILEO, /**< GLONASS+GALILEO */
+ MODE_GPS_GLONASS_GALILEO, /**< GPS+GLONASS+GALILEO */
+ MODE_BEIDOU_GLONASS_GALILEO, /**< BEIDOU+GLONASS+GALILEO */ /* The type does not support this type */
+ MODE_END = -1, /**< init value */
+}GSW_GNSS_MODE_CONFIGURATION_HD;
+
+
+GSW_GNSS_MODE_CONFIGURATION_HD map_gnss_mode(GSW_GNSS_MODE_CONFIGURATION mode)
+{
+ switch (mode)
+ {
+ case GSW_MODE_GPS_GLONASS:
+ return MODE_GPS_GLONASS;
+ case GSW_MODE_GPS_BEIDOU:
+ return MODE_GPS_BEIDOU;
+ case GSW_MODE_GPS_GLONASS_BEIDOU:
+ return MODE_END;
+ case GSW_MODE_GPS:
+ return MODE_GPS;
+ case GSW_MODE_BEIDOU:
+ return MODE_BEIDOU;
+ case GSW_MODE_GLONASS:
+ return MODE_GLONASS;
+ case GSW_MODE_GPS_GLONASS_BEIDOU_GALILEO:
+ return MODE_END;
+ case GSW_MODE_GPS_GALILEO:
+ return MODE_GPS_GALILEO;
+ case GSW_MODE_GPS_GLONASS_GALILEO:
+ return MODE_GPS_GLONASS_GALILEO;
+ case GSW_MODE_GPS_GALILEO_ONLY:
+ return MODE_GALILEO;
+ case GSW_MODE_GPS_GLONASS_BEIDOU_GALILEO_NAVIC:
+ return MODE_END;
+ case GSW_MODE_GNSS_END:
+ return MODE_END;
+ default:
+ return MODE_END;
+ }
+}
+
+int mbtk_gnss_set_VTG()
+{
+ int ret;
+ char param_buf[32] = {0};
+ snprintf(param_buf, 32, "$MSGCFG,2,1000");
+ mbtk_gnss_setting=(int(*)(const char *setting_cmd, int))dlsym(dlHandle_gnss, "mbtk_gnss_setting");
+ ret = mbtk_gnss_setting(param_buf, QSER_GNSS_TIMEOUT);
+ if(ret != 0)
+ {
+ LOGE("[qser_gnss] mbtk_gnss_setting fail.ret = [%d]", ret);
+ return GSW_HAL_FAIL;
+ }
+ return GSW_HAL_SUCCESS;
+}
+
+/**
+ * @brief SDK interface to set gnss sampling frequency, support 1Hz、2Hz、5Hz
+ * @param [in] freq
+ * @retval 0: success
+ * @retval other: fail
+ */
+int gsw_gnss_set_freq(int freq)
+{
+ int ret;
+ if (!strated)
+ {
+ gnss_freq = freq;
+ return GSW_HAL_SUCCESS;
+ }
+ char param_buf[32] = {0};
+ snprintf(param_buf, 32, "$FREQCFG,%d", freq);
+ mbtk_gnss_setting=(int(*)(const char *setting_cmd, int))dlsym(dlHandle_gnss, "mbtk_gnss_setting");
+ ret = mbtk_gnss_setting(param_buf, QSER_GNSS_TIMEOUT);
+ if(ret != 0)
+ {
+ LOGE("[qser_gnss] mbtk_gnss_setting fail.ret = [%d]", ret);
+ return GSW_HAL_FAIL;
+ }
+ return GSW_HAL_SUCCESS;
+}
+
+/**
+ * @brief SDK interface to set gnss start mode,specific mode refreence GSW_HAL_GNSS_MODE_CONFIGURATION
+ * @param [in] start_mode
+ * @retval 0: success
+ * @retval other: fail
+ */
+int gsw_gnss_set_start_mode(GSW_GNSS_MODE_CONFIGURATION start_mode)
+{
+ int ret;
+ if (!strated)
+ {
+ gnss_startmode = start_mode;
+ return GSW_HAL_SUCCESS;
+ }
+ char param_buf[32] = {0};
+ snprintf(param_buf, 32, "$SYSCFG,%d", map_gnss_mode(start_mode));
+ if(map_gnss_mode(start_mode) == -1)
+ {
+ LOGE("[qser_gnss] mbtk_gnss_start_mode con't support");
+ return GSW_HAL_FAIL;
+ }
+ mbtk_gnss_setting=(int(*)(const char *setting_cmd, int))dlsym(dlHandle_gnss, "mbtk_gnss_setting");
+ ret = mbtk_gnss_setting(param_buf, QSER_GNSS_TIMEOUT);
+ if(ret != 0)
+ {
+ LOGE("[qser_gnss] mbtk_gnss_setting fail.ret = [%d]", ret);
+ return GSW_HAL_FAIL;
+ }
+ return GSW_HAL_SUCCESS;
+}
+
+/**
+ * @brief SDK interface to set EPO switch if open or close
+ * @param [in] switch_op
+ * @retval 0: success
+ * @retval other: fail
+ */
+int gsw_gnss_epo_switch(GSW_GNSS_CONF_SWITCH switch_op)
+{
+ int ret;
+ if (!strated)
+ {
+ gnss_switch_op = switch_op;
+ return GSW_HAL_SUCCESS;
+ }
+
+ if(switch_op)
+ {
+ mbtk_gnss_eph_download=(int(*)(int))dlsym(dlHandle_gnss, "mbtk_gnss_eph_download");
+ ret = mbtk_gnss_eph_download(QSER_GNSS_TIMEOUT);
+ if(ret != 0)
+ {
+ LOGE("[qser_gnss] mbtk_gnss_eph_download fail.ret = [%d]", ret);
+ return GSW_HAL_FAIL;
+ }
+ mbtk_gnss_eph_inject=(int(*)(int))dlsym(dlHandle_gnss, "mbtk_gnss_eph_inject");
+ ret = mbtk_gnss_eph_inject(QSER_GNSS_TIMEOUT);
+ if(ret != 0)
+ {
+ LOGE("[qser_gnss] mbtk_gnss_eph_inject fail.ret = [%d]", ret);
+ return GSW_HAL_FAIL;
+ }
+ }
+ return GSW_HAL_SUCCESS;
+}
+
+static void gnss_callback(uint32_t ind_type, const void* data, uint32_t data_len)
+{
+ if(data == NULL || data_len <= 0)
+ {
+ LOGE("[GSW_gnss] data is NULL.");
+ return;
+ }
+
+ if(gsw_cb == NULL)
+ {
+ //LOGE("[qser_gnss] qser_gnss_callback is NULL.");
+ return;
+ }
+
+ if(ind_type == MBTK_GNSS_IND_LOCATION) {
+ if(data_len != sizeof(mbtk_gnss_location_info_t))
+ {
+ LOGE("[GSW_gnss] data size error");
+ return;
+ }
+ GSW_GNSS_LOCATION_EXT_T gsw_location;
+ GSW_GNSS_LOCATION_T gsw_location_t;
+ mbtk_gnss_location_info_t *locl_info = (mbtk_gnss_location_info_t *)data;
+ gsw_location_t.altitude = locl_info->altitude;
+ gsw_location_t.latitude = locl_info->latitude;
+ gsw_location_t.longitude = locl_info->longitude;
+ gsw_location_t.speed = locl_info->speed;
+ gsw_location_t.bearing = locl_info->bearing;
+ gsw_location_t.timestamp = locl_info->timestamp;
+ gsw_location.legacyLocation = gsw_location_t;
+ gsw_cb->gsw_location_cb(&gsw_location);
+ } else if(ind_type == MBTK_GNSS_IND_NMEA) {
+ mopen_gnss_nmea_info_t qser_nmea = {0};
+ memset(&qser_nmea, 0x0, sizeof(mopen_gnss_nmea_info_t));
+ qser_nmea.length = data_len;
+ memcpy(qser_nmea.nmea, (char *)data, data_len);
+ //qser_nmea.timestamp = qser_get_gnss_time_sec(data, data_len);
+ gsw_cb->gsw_nmea_cb(data,data_len);
+ } else {
+ LOGD("Unknown IND : %d\n", ind_type);
+ }
+}
+
+int gsw_gnss_init(void)
+{
+ int ret;
+ if(!inited)
+ {
+ dlHandle_gnss = dlopen(lynqLib_gnss, RTLD_NOW);
+ fun_ptr_log = (mbtk_log)dlsym(dlHandle_gnss, "mbtk_log");
+ if(fun_ptr_log == NULL || dlHandle_gnss == NULL)
+ {
+ return GSW_HAL_FAIL;
+ }
+ mbtk_gnss_init=(int(*)(mbtk_gnss_callback_func))dlsym(dlHandle_gnss, "mbtk_gnss_init");
+ ret = mbtk_gnss_init(gnss_callback);
+ if(ret == 0)
+ {
+ mbtk_gnss_ind_set=(int(*)(uint32_t ,int))dlsym(dlHandle_gnss, "mbtk_gnss_ind_set");
+ ret = mbtk_gnss_ind_set(MBTK_GNSS_IND_NMEA, QSER_GNSS_TIMEOUT);
+ if(ret == 0)
+ {
+ inited = true;
+ }
+ else
+ {
+ LOGE("[GSW_gnss] init mbtk_gnss_ind_set() fail.ret = [%d]", ret);
+ return GSW_HAL_FAIL;
+ }
+ }
+ else
+ {
+ LOGE("[GSW_gnss] mbtk_gnss_init() fail.ret = [%d]", ret);
+ return GSW_HAL_FAIL;
+ }
+ }
+
+ return GSW_HAL_SUCCESS;
+}
+
+/**
+ * @brief SDK interface to registered callback function
+ * @param [in] callback
+ * @retval 0: success
+ * @retval other: fail
+ */
+int gsw_gnss_reg_cb_group(gsw_gnss_cb callback)
+{
+ if(callback.gsw_location_cb == NULL && callback.gsw_nmea_cb == NULL)
+ {
+ LOGE("[GSW_gnss] handler_ptr is NULL.");
+ return GSW_HAL_FAIL;
+ }
+ if (gsw_cb == NULL)
+ {
+ gsw_cb = (gsw_gnss_cb *)malloc(sizeof(gsw_gnss_cb));
+ if (gsw_cb == NULL)
+ {
+ LOGE("[GSW_gnss] Memory allocation failed.");
+ return GSW_HAL_FAIL;
+ }
+ }
+ gsw_cb->gsw_location_cb = callback.gsw_location_cb;
+ gsw_cb->gsw_nmea_cb = callback.gsw_nmea_cb;
+ return GSW_HAL_SUCCESS;
+}
+/**
+ * @brief SDK interface to start gnss
+ * @param
+ * @retval 0: success
+ * @retval other: fail
+ */
+int gsw_gnss_start(void)
+{
+ int ret;
+ mbtk_gnss_open=(int(*)(int,int))dlsym(dlHandle_gnss, "mbtk_gnss_open");
+ ret = mbtk_gnss_open(255, QSER_GNSS_TIMEOUT);
+ if(ret != 0)
+ {
+ LOGE("[GSW_gnss] mbtk_gnss_open is error.ret = [%d]", ret);
+ return GSW_HAL_FAIL;
+ }
+ strated = true;
+ if (gnss_freq > 0)
+ gsw_gnss_set_freq(gnss_freq);
+ if (gnss_startmode >= 0)
+ gsw_gnss_set_start_mode(gnss_startmode);
+ if (gnss_switch_op > 0)
+ gsw_gnss_epo_switch(gnss_switch_op);
+
+ mbtk_gnss_set_VTG();
+
+ return GSW_HAL_SUCCESS;
+}
+
+/**
+ * @brief SDK interface to stop gnss
+ * @param
+ * @retval 0: success
+ * @retval other: fail
+ */
+int gsw_gnss_stop(void)
+{
+ int ret;
+ mbtk_gnss_close=(int(*)(int))dlsym(dlHandle_gnss, "mbtk_gnss_close");
+ ret = mbtk_gnss_close(QSER_GNSS_TIMEOUT);
+ if(ret != 0)
+ {
+ LOGE("[GSW_gnss] mbtk_gnss_close is error.ret = [%d]", ret);
+ return GSW_HAL_FAIL;
+ }
+ strated = false;
+ return GSW_HAL_SUCCESS;
+}
+
+/**
+ * @brief SDK interface to de initialization gnss
+ * @param
+ * @retval 0: success
+ * @retval other: fail
+ */
+int gsw_gnss_deinit(void)
+{
+ int ret;
+ if(inited)
+ {
+ mbtk_gnss_deinit=(int(*)())dlsym(dlHandle_gnss, "mbtk_gnss_deinit");
+ ret = mbtk_gnss_deinit();
+ if(ret == 0)
+ {
+ inited = false;
+ }
+ else
+ {
+ LOGE("[GSW_gnss] mbtk_gnss_init() fail.ret = [%d]", ret);
+ dlclose(dlHandle_gnss);
+ dlHandle_gnss = NULL;
+ return GSW_HAL_FAIL;
+ }
+ }
+
+ dlclose(dlHandle_gnss);
+ dlHandle_gnss = NULL;
+ gnss_freq = -1;
+ gnss_startmode = -1;
+ gnss_switch_op = -1;
+ return GSW_HAL_SUCCESS;
+}
+
+
+/**
+ * @brief SDK interface to enable XTRA switch
+ * @param [in] state
+ * @retval 0: success
+ * @retval other: fail
+ */
+int gsw_gnss_xtra_is_enable(gsw_xtra_state_e state)
+{
+ return GSW_HAL_SUCCESS;
+}
+
+/**
+ * @brief SDK interface to delete aiding data,delete aiding data for cold start(1-H,2-W,3-C)
+ * @param [in] switch_op
+ * @retval 0: success
+ * @retval other: fail
+ */
+int gsw_gnss_delete_aiding_data(unsigned int flags) /*1-don`t delete == hot_start ; 2-delete EPH == warm start ; 3-delete all == cold start*/
+{
+ int ret;
+ char param_buf[32] = {0};
+ snprintf(param_buf, 32, "$RESET,%u", flags);
+ mbtk_gnss_setting=(int(*)(const char *setting_cmd, int))dlsym(dlHandle_gnss, "mbtk_gnss_setting");
+ ret = mbtk_gnss_setting(param_buf, QSER_GNSS_TIMEOUT);
+ if(ret != 0)
+ {
+ LOGE("[qser_gnss] mbtk_gnss_setting fail.ret = [%d]", ret);
+ return GSW_HAL_FAIL;
+ }
+ return GSW_HAL_SUCCESS;
+}
+
+/**
+ * @brief init and configure gps
+ * @param [in] init_configure
+ * @retval 0: success
+ * @retval other: fail
+ */
+int gsw_gnss_init_configure_gps(gsw_gnss_init_configure_t init_configure)
+{
+ return GSW_HAL_SUCCESS;
+}