mbtk/libgsw_lib/gsw_gnss_interface.c

#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 <ctype.h>

#include "gsw_gnss_interface.h"

#define MBTK_GNSS_IND_LOCATION (1)      // 1
#define MBTK_GNSS_IND_NMEA (1 << 1)     // 2
#define QSER_GNSS_TIMEOUT 5

#include "gsw_log_interface.h"
#define GSW_GNSS "[HAL][GSW_GNSS]"

#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);

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 (*mbtk_gnss_dl_ptr)(const char *, int);

int gnss_freq = -1;
GSW_GNSS_MODE_CONFIGURATION gnss_startmode = -1;
GSW_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 struct{
    GSW_GNSS_MODE_CONFIGURATION start_mode;
    gnss_freq_type freq;
    gsw_gnss_cb callback;
}gsw_gnss_init_configure_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;

typedef enum{
    NO_DEVICE = 0,
    HD8122,
    HD8040D,
}GSW_DEVICE;

typedef struct {
    int  fd;
    GSW_DEVICE got_hd8;
} rx_ctx;

typedef struct {
    int  fd;
    char *buf_8040_1;
    char *buf_8040_2;
    char *buf_8040;
} rx_ctx_8040;

static GSW_DEVICE gsw_device = NO_DEVICE;
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;
    }
}  

static void *rx_thread_8040(void *arg)
{
    rx_ctx_8040 *c = (rx_ctx_8040 *)arg;
    char buf[128];
    ssize_t n;
    time_t end = time(NULL) + 1;//1s
    
    while (time(NULL) < end) {
        n = read(c->fd, buf, sizeof(buf));
        if (n > 0 && memmem(buf, n, "HD8040D", 7)) 
        {
            int start_index = 6;

            int part1_start = start_index;
            int part1_end = 19;
            int part1_len = part1_end - part1_start + 1;

            int part2_start = 22;
            int part2_end = 37;
            int part2_len = part2_end - part2_start + 1;

            if (c->buf_8040_1 != NULL)
            {
                free(c->buf_8040_1);
                c->buf_8040_1 = NULL;
            }
            
            c->buf_8040_1 = (char *)malloc(part1_len + 1);
            if (c->buf_8040_1 != NULL)
            {
                memcpy(c->buf_8040_1, buf + part1_start, part1_len);
                c->buf_8040_1[part1_len] = '\0';
            }

            if (c->buf_8040_2 != NULL)
            {
                free(c->buf_8040_2);
                c->buf_8040_2 = NULL;
            }
            
            c->buf_8040_2 = (char *)malloc(part2_len + 1);
            if (c->buf_8040_2 != NULL)
            {
                memcpy(c->buf_8040_2, buf + part2_start, part2_len);
                c->buf_8040_2[part2_len] = '\0';
            }

            if (c->buf_8040_1 != NULL && c->buf_8040_2 != NULL)
            {
                if (c->buf_8040 != NULL)
                {
                    free(c->buf_8040);
                    c->buf_8040 = NULL;
                }

                int total_len = strlen(c->buf_8040_1) + strlen(c->buf_8040_2) + 1;
                c->buf_8040 = (char *)malloc(total_len);
                if (c->buf_8040 != NULL)
                {
                    strcpy(c->buf_8040, c->buf_8040_1);
                    strcat(c->buf_8040, c->buf_8040_2);
                }
            }
            else
            {
                LOGE(GSW_GNSS, "Failed to extract version parts");
            }
            break;
        }
    }
    return NULL;
}

static void *rx_thread(void *arg)
{
    rx_ctx *c = (rx_ctx *)arg;
    char buf[128];
    ssize_t n;
    time_t end = time(NULL) + 1;            /* 1 秒超时 */
    while (time(NULL) < end) {
        n = read(c->fd, buf, sizeof(buf));
        if (n > 0 && memmem(buf, n, "HD8040D", 7)) {
            c->got_hd8 = 2;
            break;
        }
		else if (n > 0 && memmem(buf, n, "HD812", 5)) {
			c->got_hd8 = 1;
			break;
		}
    }

    return NULL;
}

static int send_and_wait(int fd)
{
    int ret;
    pthread_t tid;
    rx_ctx ctx = { .fd = fd, .got_hd8 = NO_DEVICE };

    pthread_create(&tid, NULL, rx_thread, &ctx);

    unsigned char tx[8] = {0xF1,0xD9,0x0A,0x04,0x00,0x00,0x0E,0x34};
    ret = write(fd, tx, sizeof(tx));
    if(ret < 0)
    {
        LOGE(GSW_GNSS,"[GSW_gnss] send_and_wait write fail.ret = [%d]", ret);
    }
    pthread_join(tid, NULL);
    return ctx.got_hd8;
}

static int get_gnss_device_version(void)
{
    int fd = open("/dev/ttyS3", O_RDWR | O_NOCTTY);
    if (fd < 0) { perror("open"); return 1; }
    gsw_device = send_and_wait(fd);
    close(fd);
    return gsw_device;
}


int mbtk_gnss_set_VTG()
{
    int ret;
    ret = system("/usr/sbin/gnss_gpio.sh VTG > /dev/null 2>&1");
    if(ret != 0)
    {
        LOGE(GSW_GNSS,"[GSW_gnss] mbtk_gnss_set_VTG on fail.ret = [%d]", ret);
        return GSW_HAL_NORMAL_FAIL;
    }
    return GSW_HAL_SUCCESS;
}

static int HD8040D_gsw_gnss_set_freq(int freq)
{
    int ret;
    char cmd[64] = {0};
    switch (freq)
    {
        case 1:
            snprintf(cmd, sizeof(cmd), "/usr/sbin/gnss_gpio.sh %s > /dev/null 2>&1", "1HZ");
            break;
        case 5:
            snprintf(cmd, sizeof(cmd), "/usr/sbin/gnss_gpio.sh %s > /dev/null 2>&1", "5HZ");
            break;
        case 10:
            snprintf(cmd, sizeof(cmd), "/usr/sbin/gnss_gpio.sh %s > /dev/null 2>&1", "10HZ");
            break;
        default:
            return GSW_HAL_NORMAL_FAIL;
            break;
    }
    ret = system(cmd);
    if(ret != 0)
    {
        LOGE(GSW_GNSS,"[GSW_gnss] gnss_gpio.sh GSW_HD8040D mode fail.ret = [%d]", ret);
        return GSW_HAL_NORMAL_FAIL;
    }
    return ret;
}
/**
 * @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)
{
    if(!inited)
        return GSW_HAL_NORMAL_FAIL;

    int ret;
    if (!strated)
    {
        gnss_freq = freq;
        return GSW_HAL_SUCCESS;
    }
    if(gsw_device == NO_DEVICE)
    {
        ret = get_gnss_device_version();
        LOGD(GSW_GNSS,"[GSW_gnss] get_gnss_device_version ret = [%d]", ret);
    }
    if(gsw_device == HD8040D)
    {
        ret = HD8040D_gsw_gnss_set_freq(freq);
        return ret;
    }
    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(GSW_GNSS,"[qser_gnss] mbtk_gnss_setting fail.ret = [%d]", ret);
        return GSW_HAL_NORMAL_FAIL;
    }
    return GSW_HAL_SUCCESS;
}

static int32_t HD8040D_gsw_gnss_set_start_mode(GSW_GNSS_MODE_CONFIGURATION start_mode)
{
    int ret;
    char cmd[64] = {0};
    switch (start_mode)
    {
    case GSW_MODE_GPS_GLONASS:
        return GSW_HAL_NORMAL_FAIL;
        break;
    case GSW_MODE_GPS_BEIDOU:
        snprintf(cmd, sizeof(cmd), "/usr/sbin/gnss_gpio.sh %s > /dev/null 2>&1", "GSW_GPS_BD_DUAL");
        break;
    case GSW_MODE_GPS_GLONASS_BEIDOU:
        return GSW_HAL_NORMAL_FAIL;
        break;
    case GSW_MODE_GPS:
        snprintf(cmd, sizeof(cmd), "/usr/sbin/gnss_gpio.sh %s > /dev/null 2>&1", "GSW_GPS_L1L5");
        break;
    case GSW_MODE_BEIDOU:
        snprintf(cmd, sizeof(cmd), "/usr/sbin/gnss_gpio.sh %s > /dev/null 2>&1", "GSW_BD_B1I_B1C_B2A");
        break;
    case GSW_MODE_GLONASS:
        snprintf(cmd, sizeof(cmd), "/usr/sbin/gnss_gpio.sh %s > /dev/null 2>&1", "GSW_GALILEO_E1_E5A");
        break;
    case GSW_MODE_GPS_GLONASS_BEIDOU_GALILEO:
        return GSW_HAL_NORMAL_FAIL;
        break;
    case GSW_MODE_GPS_GALILEO:
        return GSW_HAL_NORMAL_FAIL;
        break;
    case GSW_MODE_GPS_GLONASS_GALILEO:
        return GSW_HAL_NORMAL_FAIL;
        break;
    case GSW_MODE_GPS_GALILEO_ONLY:
        return GSW_HAL_NORMAL_FAIL;
        break;
    case GSW_MODE_GPS_GLONASS_BEIDOU_GALILEO_NAVIC:
        return GSW_HAL_NORMAL_FAIL;
        break;
    case GSW_MODE_GNSS_END:
        return GSW_HAL_NORMAL_FAIL;
        break;
    default:
        return GSW_HAL_NORMAL_FAIL;
        break;
    }
    ret = system(cmd);
    if(ret != 0)
    {
        LOGE(GSW_GNSS,"[GSW_gnss] gnss_gpio.sh GSW_HD8040D mode fail.ret = [%d]", ret);
        return GSW_HAL_NORMAL_FAIL;
    }
    return ret;
}

/**
 * @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
 */
int32_t gsw_gnss_set_start_mode(GSW_GNSS_MODE_CONFIGURATION start_mode)
{
    if(!inited)
        return GSW_HAL_NORMAL_FAIL;
    int ret;
    if (!strated)
    {
        gnss_startmode = start_mode;
        return GSW_HAL_SUCCESS;
    }
    if(gsw_device == NO_DEVICE)
    {
        ret = get_gnss_device_version();
        LOGD(GSW_GNSS,"[GSW_gnss] get_gnss_device_version ret = [%d]", ret);
    }
    if(gsw_device == HD8040D)
    {
        ret = HD8040D_gsw_gnss_set_start_mode(start_mode);
        return ret;
    }
    char param_buf[32] = {0};
    snprintf(param_buf, 32, "$SYSCFG,%d", map_gnss_mode(start_mode));
    if(map_gnss_mode(start_mode) == -1)
    {
        LOGE(GSW_GNSS,"[qser_gnss] mbtk_gnss_start_mode con't support");
        return GSW_HAL_NORMAL_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(GSW_GNSS,"[qser_gnss] mbtk_gnss_setting fail.ret = [%d]", ret);
        return GSW_HAL_NORMAL_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
 */
int32_t gsw_gnss_epo_switch(GSW_CONF_SWITCH switch_op)
{
    if(!inited)
        return GSW_HAL_NORMAL_FAIL;
    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(GSW_GNSS,"[qser_gnss] mbtk_gnss_eph_download fail.ret = [%d]", ret);
            return GSW_HAL_NORMAL_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(GSW_GNSS,"[qser_gnss] mbtk_gnss_eph_inject fail.ret = [%d]", ret);
            return GSW_HAL_NORMAL_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,"[GSW_gnss] data is NULL.");
        return;
    }

    if(gsw_cb == NULL)
    {
        //LOGE(GSW_GNSS,"[qser_gnss] qser_gnss_callback is NULL.");
        return;
    }
    
    if(ind_type == MBTK_GNSS_IND_LOCATION && gsw_cb->gsw_location_cb != NULL) {
        if(data_len != sizeof(mbtk_gnss_location_info_t))
		{
			LOGE(GSW_GNSS,"[GSW_gnss] data size error");
			return;
		}
		GSW_GNSS_LOCATION_EXT_T gsw_location;
        GSW_GNSS_LOCATION_T gsw_location_t = {0};
        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_t.flags = locl_info->flags;
        gsw_location.legacyLocation = gsw_location_t;
        gsw_cb->gsw_location_cb(&gsw_location);  
    } else if(ind_type == MBTK_GNSS_IND_NMEA && gsw_cb->gsw_nmea_cb != NULL) {
        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(GSW_GNSS,"Unknown IND : %d\n", ind_type);
    }
}

int32_t gsw_gnss_init(void)
{
    int ret;  
    if(!inited)
    {
        ret = system("serial_atcmd AT*IMLCONFIG=13");
        if(ret != 0)
        {
            LOGE(GSW_GNSS,"serial_atcmd fail\n");
            return GSW_HAL_NORMAL_FAIL;
        }
        if(dlHandle_gnss == NULL)
            dlHandle_gnss = dlopen(lynqLib_gnss, RTLD_NOW);;
        if(dlHandle_gnss == NULL)
        {
            return GSW_HAL_NORMAL_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 |MBTK_GNSS_IND_LOCATION, QSER_GNSS_TIMEOUT);
            if(ret == 0)
            {
                inited = true;
            }
            else
            {
                LOGE(GSW_GNSS,"[GSW_gnss] init mbtk_gnss_ind_set() fail.ret = [%d]", ret);
                return GSW_HAL_NORMAL_FAIL;
            }
        }
        else
        {
            LOGE(GSW_GNSS,"[GSW_gnss] mbtk_gnss_init() fail.ret = [%d]", ret);
            return GSW_HAL_NORMAL_FAIL;
        }
    }

    return GSW_HAL_SUCCESS;
}

/**
 * @brief SDK interface to registered callback function
 * @param [in] callback
 * @retval 0: success
 * @retval other: fail
 */
int32_t gsw_gnss_reg_cb_group(gsw_gnss_cb callback)
{
    if(!inited)
        return GSW_HAL_NORMAL_FAIL;
    if(callback.gsw_location_cb == NULL && callback.gsw_nmea_cb == NULL)
    {
        LOGE(GSW_GNSS,"[GSW_gnss] handler_ptr is NULL.");
        return GSW_HAL_NORMAL_FAIL;
    }
    if (gsw_cb == NULL)
    {
        gsw_cb = (gsw_gnss_cb *)malloc(sizeof(gsw_gnss_cb));
        if (gsw_cb == NULL)
        {
            LOGE(GSW_GNSS,"[GSW_gnss] Memory allocation failed.");
            return GSW_HAL_NORMAL_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
 */
int32_t gsw_gnss_start(void)
{
    int ret;
    if(!inited)
        return GSW_HAL_NORMAL_FAIL;
    ret = system("/usr/sbin/gnss_gpio.sh on > /dev/null 2>&1");
    if(ret != 0)
    {
        LOGE(GSW_GNSS,"[GSW_gnss] gnss_gpio.sh on fail.ret = [%d]", ret);
        return GSW_HAL_NORMAL_FAIL;
    }
    // 记录mbtk_gnss_open前的时间戳
    struct timespec start_time;
    if (clock_gettime(CLOCK_MONOTONIC, &start_time) != 0) {
        LOGE(GSW_GNSS,"[GSW_gnss] Failed to get start time");
        return GSW_HAL_NORMAL_FAIL;
    }

    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,"[GSW_gnss] mbtk_gnss_open is error.ret = [%d]", ret);
        return GSW_HAL_NORMAL_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前的时间戳并计算等待时间
    struct timespec end_time;
    if (clock_gettime(CLOCK_MONOTONIC, &end_time) != 0) {
        LOGE(GSW_GNSS,"[GSW_gnss] Failed to get end time");
        return GSW_HAL_NORMAL_FAIL;
    }
    long start_ms = start_time.tv_sec * 1000 + start_time.tv_nsec / 1000000;
    long end_ms = end_time.tv_sec * 1000 + end_time.tv_nsec / 1000000;
    long diff_ms = end_ms - start_ms;
    if (diff_ms < 700) {
        long wait_ms = 700 - diff_ms;
        usleep(wait_ms * 1000);  // 转换为微秒等待
    }
    
    mbtk_gnss_set_VTG();

    return GSW_HAL_SUCCESS;
}


/**
 * @brief SDK interface to stop gnss
 * @param 
 * @retval 0: success
 * @retval other: fail
 */
int32_t gsw_gnss_stop(void)
{
    int ret;
    if(!inited)
        return GSW_HAL_NORMAL_FAIL;
    mbtk_gnss_close=(int(*)(int))dlsym(dlHandle_gnss, "mbtk_gnss_close");
    ret = mbtk_gnss_close(QSER_GNSS_TIMEOUT);
    if(ret != 0)
    {
        LOGE(GSW_GNSS,"[GSW_gnss] mbtk_gnss_close is error.ret = [%d]", ret);
        return GSW_HAL_NORMAL_FAIL;
    }
    ret = system("/usr/sbin/gnss_gpio.sh off > /dev/null 2>&1");
    if(ret != 0)
    {
        LOGE(GSW_GNSS,"[GSW_gnss] gnss_gpio.sh off fail.ret = [%d]", ret);
        return GSW_HAL_NORMAL_FAIL;
    }
    strated = false;
    return GSW_HAL_SUCCESS;
}

/**
 * @brief SDK interface to de initialization gnss
 * @param 
 * @retval 0: success
 * @retval other: fail
 */
int32_t gsw_gnss_deinit(void)
{
    int ret;
    if(!inited)
        return GSW_HAL_NORMAL_FAIL;
    if(inited)
    {
        ret = system("serial_atcmd AT*IMLCONFIG=12");
        if(ret != 0)
        {
            LOGE(GSW_GNSS,"serial_atcmd fail\n");
            return GSW_HAL_NORMAL_FAIL;
        }
        mbtk_gnss_deinit=(int(*)())dlsym(dlHandle_gnss, "mbtk_gnss_deinit");
        ret = mbtk_gnss_deinit();
        if(ret == 0)
        {
            inited = false;
        }
        else
        {
            LOGE(GSW_GNSS,"[GSW_gnss] mbtk_gnss_deinit() fail.ret = [%d]", ret);
            /*
            dlclose(dlHandle_gnss);
            dlHandle_gnss = NULL;
            */
            return GSW_HAL_NORMAL_FAIL;
        }
    }

    /*
    dlclose(dlHandle_gnss);
    dlHandle_gnss = NULL;
    */
    gnss_freq = -1;
    gnss_startmode = -1;
    gnss_switch_op = -1;
    return GSW_HAL_SUCCESS;
}

int gsw_gnss_add_lib(void)
{
    return GSW_HAL_SUCCESS;
}

/**
 * @brief SDK interface to enable XTRA switch
 * @param [in] state
 * @retval 0: success
 * @retval other: fail
 */
int32_t 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
 */
int32_t gsw_gnss_delete_aiding_data(unsigned int flags) /*1-don`t delete == hot_start ; 2-delete EPH == warm start ; 3-delete all == cold start*/
{
    if(!inited)
        return GSW_HAL_NORMAL_FAIL;
    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(GSW_GNSS,"[qser_gnss] mbtk_gnss_setting fail.ret = [%d]", ret);
        return GSW_HAL_NORMAL_FAIL;
    }
    return GSW_HAL_SUCCESS;
}

/**
 * @brief init and configure gps
 * @param [in] init_configure
 * @retval 0: success
 * @retval other: fail
 */
int32_t gsw_gnss_init_configure_gps(gsw_gnss_init_configure_t init_configure)
{
    return GSW_HAL_SUCCESS;
}

int gsw_gnss_get_tail_nmea_type(char *tail_type, int len)
{
    if(NULL == tail_type){
        LOGE(GSW_GNSS,"get_tail_nmea_type fail, tail_type is NULL");
        return GSW_HAL_ERROR_GNSS_FAIL;
    }

    if(len >= 3){
        strcpy(tail_type, "RMC");
        return GSW_HAL_GNSS_SUCCESS;
    }else{
        LOGE(GSW_GNSS,"get_tail_nmea_type fail, len[%d] is too short", len);
        return GSW_HAL_ERROR_GNSS_FAIL;
    }
}

/**
* @brief Start install gnss software
* @param  [in] char* file_path
* @param  [out] NULL
* @retval GSW_HAL_SUCCESS\GSW_HAL_FAIL
*/
int gsw_update_gnss_start(const char *fw_path)
{
    if (fw_path == NULL)
    {
        LOGE(GSW_GNSS, "[qser_gnss] Input fw_path is null");
        return GSW_HAL_NORMAL_FAIL;
    }

    int ret;
    mbtk_gnss_dl_ptr = (int(*)(const char *, int))dlsym(dlHandle_gnss, "mbtk_gnss_dl");

    ret = mbtk_gnss_dl_ptr(fw_path, 60);
    if(ret != 0)
    {
        if(ret != 4)
        {
            LOGE(GSW_GNSS,"[qser_gnss] Firmware download failed. ret = [%d]", ret);
            return GSW_HAL_NORMAL_FAIL;
        }
        else
        {
            LOGW(GSW_GNSS,"[qser_gnss] Firmware download warning (ignored). ret = [%d]", ret);
        }
    }
    return GSW_HAL_SUCCESS;
}

/**
* @brief get gnss version info
* @param  [in] NULL
* @param  [out] char* version_info
* @retval GSW_HAL_SUCCESS\GSW_HAL_FAIL
*/
int32_t gsw_get_gnss_version_info(char* version_info)
{
    if (version_info == NULL)
    {
        LOGE(GSW_GNSS, "version_info is NULL");
        return GSW_HAL_ERROR_GNSS_FAIL;
    }

    int fd = open("/dev/ttyS3", O_RDWR | O_NOCTTY);
    if (fd < 0)
    {
        LOGE(GSW_GNSS, "Failed to open /dev/ttyS3: %s", strerror(errno));
        return GSW_HAL_ERROR_GNSS_FAIL;
    }

    rx_ctx_8040 thread_args = {
        .fd = fd,
        .buf_8040 = NULL
    };

    pthread_t read_thread_8040;
    if (pthread_create(&read_thread_8040, NULL, rx_thread_8040, &thread_args) != 0)
    {
        LOGE(GSW_GNSS, "Failed to create read thread");
        close(fd);
        return GSW_HAL_ERROR_GNSS_FAIL;
    }

    unsigned char tx_cmd[] = {0xF1, 0xD9, 0x0A, 0x04, 0x00, 0x00, 0x0E, 0x34};
    int ret = write(fd, tx_cmd, sizeof(tx_cmd));
    if (ret != sizeof(tx_cmd))
    {
        LOGE(GSW_GNSS, "[GSW_gnss] send_and_wait write fail.ret = [%d]", ret);
        pthread_cancel(read_thread_8040);
        close(fd);
        return GSW_HAL_ERROR_GNSS_FAIL;
    }

    pthread_join(read_thread_8040, NULL);
    close(fd);
    if (thread_args.buf_8040 != NULL)
    {
        strncpy(version_info, thread_args.buf_8040, 128);
        version_info[127] = '\0';
        free(thread_args.buf_8040);
        LOGE(GSW_GNSS, "Final version info: %s", version_info);
        return GSW_HAL_SUCCESS;
    }
    else
    {
        LOGE(GSW_GNSS, "No data received or HD8040 not found");
        return GSW_HAL_ERROR_GNSS_FAIL;
    }
}