mbtk/liblynq_lib_rilv2/lynq_gnss.c

#include <time.h>
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "lynq_gnss.h"

#define QSER_RESULT_SUCCESS 0
#define QSER_RESULT_FAIL -1
#define QSER_GNSS_TIMEOUT 5
#define QSER_AGNSS_DOWNLOAD_TIMEPUT 60
#define QSER_AGNSS_INJECT_TIMEOUT 20

/**********************************VARIABLE***********************************/
static bool inited = FALSE;
static uint32_t qser_h_gnss = 0x5F6F7F8F;
gnss_handler_func_t qser_gnss_callback = NULL;
static time_t qser_gnss_time = 0;
qser_agps_info qser_agps_info_save = {0};
gnss_async_func_t qser_gnss_async_callback = NULL;

extern long timezone;
/**********************************VARIABLE***********************************/

/**********************************FUNC***********************************/
static void qser_gnss_async_set_cb(gnss_async_func_t cb)
{
    qser_gnss_async_callback = cb;
}

static gnss_async_func_t qser_gnss_async_get_cb(void)
{
    return qser_gnss_async_callback;
}


static time_t qser_get_timestamp(char *time)
{
    char tmp_char[4] = {0};
    struct tm* tmp_time = (struct tm*)malloc(sizeof(struct tm));

    memset(tmp_time, 0, sizeof(struct tm));
    memset(tmp_char, 0, sizeof(tmp_char));
    memcpy(tmp_char, &time[4], 2);
    tmp_time->tm_sec = atoi(tmp_char);
    memcpy(tmp_char, &time[2], 2);
    tmp_time->tm_min = atoi(tmp_char);
    memcpy(tmp_char, &time[0], 2);
    tmp_time->tm_hour = atoi(tmp_char);
    memcpy(tmp_char, &time[6], 2);
    tmp_time->tm_mday = atoi(tmp_char);
    memcpy(tmp_char, &time[8], 2);
    tmp_time->tm_mon = atoi(tmp_char) - 1;
    memcpy(tmp_char, &time[10], 2);
    tmp_time->tm_year = 100 + atoi(tmp_char);

    time_t _t = mktime(tmp_time);//按当地时区解析tmp_time
    //gnss_log("timestamp: %ld\n",_t);
    tzset();   // 自动设置本地时区
    _t = _t - timezone;
    //gnss_log("timestamp: %ld\n",_t);

    free(tmp_time);
    return _t;
}

static time_t qser_get_gnss_time_sec(const void *data, int data_len)
{
    int i = 0, num = 0;
    const char *nmea = (const char *)data;
    char time[15] = {0};
    char *check_state = NULL;

    //$GNRMC,024142.000,A,3039.364421,N,10403.417935,E,0.051,0.00,030124,,E,A*00
    check_state = strstr(nmea, "RMC");
    if(check_state != NULL)
    {
        for(i = 0; i < data_len; i++)
        {
            if(check_state[i] == ',')
            {
                num++;
                i++;
                if(num == 1)//get time
                {
                    if(check_state[i] >= '0' && check_state[i] <= '9')
                    {
                        memcpy(time, check_state + i, 6);
                        //LOGE("[qser_gnss] %s.", time);
                    }
                    else
                    {
                        qser_gnss_time = 0;
                        return qser_gnss_time;
                    }
                }
                else if(num == 9)//get date
                {
                    if(check_state[i] >= '0' && check_state[i] <= '9')
                    {
                        memcpy(time + 6, check_state + i, 6);
                        //LOGE("[qser_gnss] %s.", time);
                        break;
                    }
                    else
                    {
                        qser_gnss_time = 0;
                        return qser_gnss_time;
                    }
                }
                else if(num > 9)
                {
                    qser_gnss_time = 0;
                    return qser_gnss_time;
                }
            }
        }

        qser_gnss_time = qser_get_timestamp(time);
    }

    return qser_gnss_time;
}

static void* gnss_async_thread(void* arg)
{
    qser_gnss_error_e state = QSER_GNSS_ERROR_SUCCESS;
    gnss_async_func_t cb = qser_gnss_async_get_cb();
    int ret = qser_Gnss_Start(qser_h_gnss);
    if(ret != QSER_RESULT_SUCCESS)
    {
        LOGE("[qser_gnss] gnss_async_thread() fail.");
        state = QSER_GNSS_ERROR_FAIL;
    }

    if(cb != NULL)
    {
        cb(state);
    }
    return NULL;
}

static void gnss_callback(uint32 ind_type, const void* data, uint32 data_len)
{
    if(data == NULL || data_len <= 0)
    {
        LOGE("[qser_gnss] data is NULL.");
        return;
    }

    if(qser_gnss_callback == 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("[qser_gnss] data size error");
			return;
		}
		mbtk_gnss_location_info_t *locl_info = (mbtk_gnss_location_info_t *)data;
        mopen_location_info_t qser_locl_info;
        memset(&qser_locl_info, 0x0, sizeof(mopen_location_info_t));
        qser_locl_info.latitude  = locl_info->latitude;
        qser_locl_info.longitude = locl_info->longitude;
        qser_locl_info.altitude  = locl_info->altitude;
        qser_locl_info.speed     = locl_info->speed;
        qser_locl_info.bearing   = locl_info->bearing;
        qser_locl_info.timestamp = locl_info->timestamp;
        qser_gnss_callback(NULL, E_MT_LOC_MSG_ID_LOCATION_INFO, (void *)(&qser_locl_info), NULL);
    } 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);
        qser_gnss_callback(NULL, E_MT_LOC_MSG_ID_NMEA_INFO, (void *)(&qser_nmea), NULL);
    } else {
        printf("Unknown IND : %d\n", ind_type);
    }
}


/**********************************FUNC***********************************/

/**********************************API***********************************/
int qser_Gnss_Init (uint32_t *h_gnss)
{
    //UNUSED(h_gnss);
    gnss_err_enum ret = GNSS_ERR_UNKNOWN;

    if(!inited)
    {
        ret = mbtk_gnss_init(gnss_callback);
        if(ret == GNSS_ERR_OK)
        {
            ret = mbtk_gnss_ind_set(MBTK_GNSS_IND_NMEA, QSER_GNSS_TIMEOUT);
            if(ret == GNSS_ERR_OK)
            {
                inited = TRUE;
            }
            else
            {
                LOGE("[qser_gnss] init mbtk_gnss_ind_set() fail.ret = [%d]", ret);
                return QSER_RESULT_FAIL;
            }
        }
        else
        {
            LOGE("[qser_gnss] mbtk_gnss_init() fail.ret = [%d]", ret);
            return QSER_RESULT_FAIL;
        }
    }
    *h_gnss = qser_h_gnss;

    return QSER_RESULT_SUCCESS;
}

int qser_Gnss_Deinit (uint32_t h_gnss)
{
    //UNUSED(h_gnss);
    if(h_gnss != qser_h_gnss)
    {
        LOGE("[qser_gnss] h_gnss is error.");
        return QSER_RESULT_FAIL;
    }

    gnss_err_enum ret = GNSS_ERR_UNKNOWN;

    if(inited)
    {
        ret = mbtk_gnss_deinit();
        if(ret == GNSS_ERR_OK)
        {
            inited = FALSE;
        }
        else
        {
            LOGE("[qser_gnss] mbtk_gnss_init() fail.ret = [%d]", ret);
            return QSER_RESULT_FAIL;
        }
    }

    return QSER_RESULT_SUCCESS;
}

int qser_AddRxIndMsgHandler (gnss_handler_func_t handler_ptr,uint32_t h_gnss)
{
    //UNUSED(handler_ptr);
    //UNUSED(h_gnss);
    if(h_gnss != qser_h_gnss)
    {
        LOGE("[qser_gnss] h_gnss is error.");
        return QSER_RESULT_FAIL;
    }

    if(handler_ptr == NULL)
    {
        LOGE("[qser_gnss] handler_ptr is NULL.");
        return QSER_RESULT_FAIL;
    }

    qser_gnss_callback = handler_ptr;
    return QSER_RESULT_SUCCESS;
}

int qser_Set_Indications (uint32_t h_gnss,e_msg_id_t type)
{
    //UNUSED(h_gnss);
    //UNUSED(type);
    if(h_gnss != qser_h_gnss)
    {
        LOGE("[qser_gnss] h_gnss is error.");
        return QSER_RESULT_FAIL;
    }

    gnss_err_enum ret = GNSS_ERR_OK;
    if(type == E_MT_LOC_MSG_ID_LOCATION_INFO)
    {
        ret = mbtk_gnss_ind_set(MBTK_GNSS_IND_LOCATION, QSER_GNSS_TIMEOUT);
    }
    else if(type == E_MT_LOC_MSG_ID_NMEA_INFO)
    {
        ret = mbtk_gnss_ind_set(MBTK_GNSS_IND_NMEA, QSER_GNSS_TIMEOUT);
    }
    else
    {
        LOGE("[qser_gnss] type is fail.");
        return QSER_RESULT_FAIL;
    }

    if(ret != GNSS_ERR_OK)
    {
        LOGE("[qser_gnss] mbtk_gnss_ind_set() fail.ret = [%d]", ret);
        return QSER_RESULT_FAIL;
    }
    return QSER_RESULT_SUCCESS;
}

int qser_Gnss_Set_Async_Callback(gnss_async_func_t cb)
{
    qser_gnss_async_set_cb(cb);
    return QSER_RESULT_SUCCESS;
}

int qser_Gnss_Start (uint32_t h_gnss)
{
    //UNUSED(h_gnss);
    if(h_gnss != qser_h_gnss)
    {
        LOGE("[qser_gnss] h_gnss is error.");
        return QSER_RESULT_FAIL;
    }

    gnss_err_enum ret = GNSS_ERR_UNKNOWN;

    ret = mbtk_gnss_open(255, QSER_GNSS_TIMEOUT);
    if(ret != GNSS_ERR_OK)
    {
        LOGE("[qser_gnss] mbtk_gnss_open is error.ret = [%d]", ret);
        return QSER_RESULT_FAIL;
    }

    return QSER_RESULT_SUCCESS;
}

int qser_Gnss_Start_Async(uint32_t h_gnss)
{
    //UNUSED(h_gnss);
    if(h_gnss != qser_h_gnss)
    {
        LOGE("[qser_gnss] h_gnss is error.");
        return QSER_RESULT_FAIL;
    }

    pthread_attr_t thread_attr;
    pthread_t gnss_thread_id;
    pthread_attr_init(&thread_attr);
    if(pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_DETACHED))
    {
        LOGE("[qser_gnss] pthread_attr_setdetachstate() fail.");
        return QSER_RESULT_FAIL;
    }

    //memcpy(&qser_data_backup, data_call, sizeof(qser_data_call_s));
    if(pthread_create(&gnss_thread_id, &thread_attr, gnss_async_thread, NULL))
    {
        LOGE("[qser_gnss] pthread_create() fail.");
        return QSER_RESULT_FAIL;
    }
    pthread_attr_destroy(&thread_attr);

    return QSER_RESULT_SUCCESS;
}

int qser_Gnss_Stop (uint32_t h_gnss)
{
    //UNUSED(h_gnss);
    if(h_gnss != qser_h_gnss)
    {
        LOGE("[qser_gnss] h_gnss is error.");
        return QSER_RESULT_FAIL;
    }

    gnss_err_enum ret = GNSS_ERR_UNKNOWN;
    ret = mbtk_gnss_close(QSER_GNSS_TIMEOUT);
    if(ret != GNSS_ERR_OK)
    {
        LOGE("[qser_gnss] mbtk_gnss_close is error.ret = [%d]", ret);
        return QSER_RESULT_FAIL;
    }

    return QSER_RESULT_SUCCESS;
}

int qser_Gnss_InjectTime (uint32_t h_gnss,LYNQ_INJECT_TIME_INTO_T *time_info)
{
    //UNUSED(h_gnss);
    UNUSED(time_info);

    if(h_gnss != qser_h_gnss)
    {
        LOGE("[qser_gnss] h_gnss is error.");
        return QSER_RESULT_FAIL;
    }
    return QSER_RESULT_SUCCESS;
}

int qser_Gnss_Delete_Aiding_Data (uint32_t h_gnss,DELETE_AIDING_DATA_TYPE_T flags)
{
    //UNUSED(h_gnss);
    //UNUSED(flags);

    if(h_gnss != qser_h_gnss)
    {
        LOGE("[qser_gnss] h_gnss is error.");
        return QSER_RESULT_FAIL;
    }
    return QSER_RESULT_SUCCESS;
}

int qser_Gnss_Server_Configuration(char *host, char *id, char *password)
{
    //UNUSED(host);
    //UNUSED(id);
    //UNUSED(password);

    if(!inited)
    {
        LOGE("[qser_gnss] api not init.");
        return QSER_RESULT_FAIL;
    }

    memset(qser_agps_info_save.host, 0x0, QSER_LEN_MAX);
    if(host != NULL && strlen(host) > 0 && strlen(host) < QSER_LEN_MAX)
    {
        memcpy(qser_agps_info_save.host, host, strlen(host));
    }

    memset(qser_agps_info_save.id, 0x0, QSER_LEN_MAX);
    if(id != NULL && strlen(id) > 0 && strlen(id) < QSER_LEN_MAX)
    {
        memcpy(qser_agps_info_save.id, id, strlen(id));
    }

    memset(qser_agps_info_save.passwd, 0x0, QSER_LEN_MAX);
    if(password != NULL && strlen(password) > 0 && strlen(password) < QSER_LEN_MAX)
    {
        memcpy(qser_agps_info_save.passwd, password, strlen(password));
    }

    gnss_err_enum ret = GNSS_ERR_UNKNOWN;
    char write_buff[512] = {0};
//    int write_length = 0;
    snprintf(write_buff, 512, "$AGPSCFG,%s,%s,%s", strlen(qser_agps_info_save.host) > 0 ? qser_agps_info_save.host : "NULL",
                                                        strlen(qser_agps_info_save.id) > 0 ? qser_agps_info_save.id : "NULL",
                                                        strlen(qser_agps_info_save.passwd) > 0 ? qser_agps_info_save.passwd : "NULL");
    ret = mbtk_gnss_setting(write_buff, QSER_GNSS_TIMEOUT);
    if(ret != GNSS_ERR_OK)
    {
        LOGE("[qser_gnss] mbtk_gnss_setting fail.ret = [%d]", ret);
        return QSER_RESULT_FAIL;
    }
    return QSER_RESULT_SUCCESS;
}

int qser_Gnss_download_tle()
{
    if(!inited)
    {
        LOGE("[qser_gnss] api not init.");
        return QSER_RESULT_FAIL;
    }

    gnss_err_enum ret = GNSS_ERR_UNKNOWN;
    ret = mbtk_gnss_eph_download(QSER_AGNSS_DOWNLOAD_TIMEPUT);
    if(ret != GNSS_ERR_OK)
    {
        LOGE("[qser_gnss] mbtk_gnss_eph_download fail.ret = [%d]", ret);
        return QSER_RESULT_FAIL;
    }
    return QSER_RESULT_SUCCESS;
}

int qser_Gnss_injectEphemeris(uint32_t h_gnss)
{
    //UNUSED(h_gnss);

    if(h_gnss != qser_h_gnss)
    {
        LOGE("[qser_gnss] h_gnss is error.");
        return QSER_RESULT_FAIL;
    }

    gnss_err_enum ret = GNSS_ERR_UNKNOWN;
    ret = mbtk_gnss_eph_inject(QSER_AGNSS_INJECT_TIMEOUT);
    if(ret != GNSS_ERR_OK)
    {
        LOGE("[qser_gnss] mbtk_gnss_eph_inject fail.ret = [%d]", ret);
        return QSER_RESULT_FAIL;
    }
    return QSER_RESULT_SUCCESS;
}

int qser_Gnss_Set_Frequency(uint32_t h_gnss, int frequency)
{
    //UNUSED(h_gnss);
    //UNUSED(frequency);

    if(h_gnss != qser_h_gnss)
    {
        LOGE("[qser_gnss] h_gnss is error.");
        return QSER_RESULT_FAIL;
    }

    if(frequency != 1 && frequency != 2 && frequency != 5)
    {
        LOGE("[qser_gnss] frequency out of range.");
        return QSER_RESULT_FAIL;
    }

    gnss_err_enum ret = GNSS_ERR_UNKNOWN;
    char param_buf[32] = {0};
//    int  length = 0;
    snprintf(param_buf, 32, "$FREQCFG,%d", frequency);
    ret = mbtk_gnss_setting(param_buf, QSER_GNSS_TIMEOUT);
    if(ret != GNSS_ERR_OK)
    {
        LOGE("[qser_gnss] mbtk_gnss_setting fail.ret = [%d]", ret);
        return QSER_RESULT_FAIL;
    }
    return QSER_RESULT_SUCCESS;
}
/**********************************API***********************************/