mbtk/mbtk_gnssd/agps/8122_agnss.c

#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <stdint.h>
#include <errno.h>
#include <fcntl.h>
#include <string.h>
#include <unistd.h>


#include "8122_agnss.h"
#include "mbtk_log.h"

#define BUFFER_SIZE 1024

#define INJECT_RESULT_SUCCESS 0
#define INJECT_RESULT_FAIL    -1

static int hd_fd = -1;
static hd_eph_inject_status_type eph_inject_status = HD_EPH_INJECT_STATUS_SUCCESS;

static void hd_agnss_delay(uint32_t nms)
{
    usleep(nms * 1000);
}

static int hd_agnss_uart_write(uint8_t *buf, uint32_t len)
{
    int TxLen;
    if(hd_fd > 0)
    {
        TxLen = write(hd_fd,buf,len);
        if(TxLen != (int)len)
        {
            LOGE("hd_agnss_uart_write fail.ret = [%d], len = [%d]", TxLen, len);
            return INJECT_RESULT_FAIL;
        }
    }
    else
    {
        LOGE("8122 agps fd error.");
    }
    return TxLen;
}

static int32_t message_package(uint8_t *dst, uint8_t *src, int32_t src_len)
{
    int32_t i = 0;
    int8_t checksum1 = 0;
    int8_t checksum2 = 0;

    /* message header */
    for (i = 0; i < 4; i++)
    {
        dst[i] = *(src + i);
    }

    /* message len */
    dst[4] = src_len;
    dst[5] = 0;

    /* payload */
    for( i = 0 ; i < src_len; i++)
    {
        dst[6 + i] = *(src + 4) ;
        src++;
    }

    /* check sum */
    for(i = 2; i < (6 + dst[4]); i++)
    {
        checksum1 += dst[i];
        checksum2 += checksum1;
    }

    dst[src_len + 6] = checksum1;
    dst[src_len + 7] = checksum2;

    return src_len + 8;
}

/************************************time inject*************************************/ 
static int gnss_inject_time(HD_AGNSS_UTC_TIME_TYPE *time)
{
    uint8_t cmd[24] = {0}; /* packet head+payload */
    uint8_t message[30] = {0}; /* payload : 20, packet pad : 8 */
    int32_t len = 0;

    memset(cmd, 0, sizeof(cmd));

    cmd[0] = 0xF1;
    cmd[1] = 0xD9;
    cmd[2] = 0x0B;
    cmd[3] = 0x11;

    cmd[4] = 0x00; /* UTC */
    cmd[5] = 0x00; /* reserved */
    cmd[6] = 0x12; /* leap cnt, hd8030 not used */

    cmd[7] = time->year & 0xFF;
    cmd[8] = ((time->year >> 8) & 0xFF); /* year */
    cmd[9] = time->month; /* month */
    cmd[10] = time->day; /* day */
    cmd[11] = time->hour; /* hour */
    cmd[12] = time->min; /* minu */
    cmd[13] = time->second; /* sec */

    cmd[18] = 0x00;
    cmd[19] = 0x00;
    for (int i = 0; i < 4; i++)
    {
        cmd[20 + i] = 0x00;
    }

    len = message_package(message, cmd, sizeof(cmd) - 4);

    LOGE("%02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X",
            message[0],message[1],message[2],message[3],message[4],message[5],message[6],message[7],
            message[8],message[9],message[10],message[11],message[12],message[13],message[14],
            message[15],message[16],message[17],message[18],message[19],message[20],
            message[21],message[22],message[23],message[24],message[25],message[26],
            message[27],message[28],message[29]);

    hd_agnss_uart_write(message, len);
    hd_agnss_delay(3); // just example, use wait 3ms instead of ACK

    return 0;
}

/************************************location inject*************************************/
static int gnss_inject_location(int32_t latitude, int32_t longitude, float altitude, float accuracy)
{
    uint8_t cmd[21] = { 0 }; /* packet head+payload */
    uint32_t acc = (uint32_t)(fabs(accuracy));
    uint32_t alti = (uint32_t)(altitude * 100); // m--->cm
    uint32_t i = 0;
    uint8_t message[25] = {0}; /* payload:17 + packet pad:8 */

    memset(cmd, 0, sizeof(cmd));
    cmd[0] = 0xF1;
    cmd[1] = 0xD9;
    cmd[2] = 0x0B;
    cmd[3] = 0x10;

    /* LLA : 1 */
    cmd[4] = 0x01;

    /* lat : 4 */
    for (i = 0; i < 4; i++)
    {
        cmd[5 + i] = (latitude >> (i * 8)) & 0xff;
    }

    /* long : 4 */
    for (i = 0; i < 4; i++)
    {
        cmd[9 + i] = (longitude >> (i * 8)) & 0xff;
    }

    /* alti : 4 */
    for (i = 0; i < 4; i++)
    {
        cmd[13 + i] = (alti >> (i * 8)) & 0xff;
    }

    /* accuracy : 4 */
    for (i = 0; i < 4; i++)
    {
        cmd[17 + i] = (acc >> (i * 8)) & 0xff;
    }


    int32_t len = message_package(message, cmd, sizeof(cmd) - 4);

    LOGE("%02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X",
                message[0],message[1],message[2],message[3],message[4],message[5],message[6],message[7],
                message[8],message[9],message[10],message[11],message[12],message[13],message[14],
                message[15],message[16],message[17],message[18],message[19],message[20],
                message[21],message[22],message[23],message[24]);

    hd_agnss_uart_write(message, len);
    hd_agnss_delay(3); // just example, use wait 3ms instead of ACK

    return 0;
}

/************************************eph data inject*************************************/ 
/**
* @brief get the eph frame and send to HD80xx
* @param data: the pointer of the eph data file
* @param len : the total length of eph data file(eg:the length of HD_GPS_BD.hdb )
* @retval 0: successs -1 error
*/
static int gnss_eph_inject_data(const char *eph_file_path)
{
    int packet_length;
    int eph_file_fd = -1;
    int size = 0;
    int left_size = 0;
    int total_size = 0;
    int write_size = 0;
    int wait_time = 0;
    uint8_t *databuf = (uint8_t *)malloc(BUFFER_SIZE);
    if(databuf == NULL)
    {
        LOGE("malloc fail");
        return INJECT_RESULT_FAIL;
    }
    
    eph_file_fd = open(eph_file_path, O_RDWR);
    if (eph_file_fd <= 0)
    {
        LOGE("%s open file FAIL. errno:%d\n", __FUNCTION__, errno);
        goto error; 
    }
    while(0 < (size = read(eph_file_fd, (databuf + left_size), BUFFER_SIZE)))
    {
        total_size = size + left_size;
        left_size = 0;
        for(int i=0; i < total_size;)
        {
            if((databuf[i] == 0xF1) && (databuf[i + 1] == 0xD9))
            {
                packet_length = (databuf[i + 4] | (databuf[i + 5] << 8));
                if (i + packet_length + 8 <= total_size)
                {
                    hd_set_eph_inject_status(HD_EPH_INJECT_STATUS_WAIT_RETURN);
                    write_size = hd_agnss_uart_write(databuf + i, packet_length + 8);
                    if(write_size < 0)
                    {
                        LOGE("hd_agnss_uart_write fail");
                        goto error; 
                    }
                    LOGD("%s Write[%d]\r\n", __FUNCTION__, write_size);
                    wait_time = 0;
                    while(1)
                    {
                        wait_time += 100;
                        hd_agnss_delay(wait_time);
                        if(wait_time < 1000)
                        {
                            if(hd_get_eph_inject_status() == HD_EPH_INJECT_STATUS_SUCCESS)
                            {
                                break;
                            }
                            else if(hd_get_eph_inject_status() == HD_EPH_INJECT_STATUS_FAIL)
                            {
                                LOGE("8122 return fail");
                                goto error;
                            }
                            else
                            {
                                LOGE("eph inject wait 8122 return");
                            }
                        }
                        else
                        {
                            LOGE("8122 wait 1s return timeout");
                            goto error;
                        }
                    }
                    i = i + packet_length + 8;
                }
                else
                {
                    left_size = total_size - i;
                    uint8_t *tmp = databuf;
                    databuf = (uint8_t *)malloc(BUFFER_SIZE + left_size);
                    memcpy(databuf, tmp + i ,left_size);
                    free(tmp);
                    break;
                }
            }
            else
            {
                i++;
            }
        }
    }
    
    if(databuf)
    {
        free(databuf);
        databuf = NULL;
    }
    if(eph_file_fd > 0)
    {
        close(eph_file_fd);
        eph_file_fd = -1;
    }
    hd_set_eph_inject_status(HD_EPH_INJECT_STATUS_SUCCESS);
    return INJECT_RESULT_SUCCESS;
error:
    if(databuf)
    {
        free(databuf);
        databuf = NULL;
    }
    if(eph_file_fd > 0)
    {
        close(eph_file_fd);
        eph_file_fd = -1;
    }
    hd_set_eph_inject_status(HD_EPH_INJECT_STATUS_SUCCESS);
    return INJECT_RESULT_FAIL;
}

/************************************choice func inject*************************************/
/**
  * @brief  AGNSS星历注入、位置注入、时间注入。星历、位置、时间可选择性注入。
  * @param  eph_file_path：从服务器下载的星历数据文件路径；若无则填NULL

            latitude：纬度，乘以10^7；若无则填0
            longitude：经度，乘以10^7；若无则填0
            altitude：高程，浮点，单位为m；若无则填0
            accuracy：精度；若无则填0

            UTC_time：UTC时间；若无则填NULL
  * @retval VOID
**/
int hd_agnss_inject(const char *eph_file_path, int32_t latitude, int32_t longitude, float altitude, float accuracy, /****latitude和longitude由double型乘以10^7得到*****/
                            HD_AGNSS_UTC_TIME_TYPE *UTC_time)
{
    int ret = INJECT_RESULT_SUCCESS;
    if (UTC_time != NULL) 
    {
        gnss_inject_time(UTC_time);
    }

    if ((latitude != 0) && (longitude != 0)) 
    {
        gnss_inject_location(latitude, longitude, altitude, accuracy);
    }

    if (eph_file_path != NULL) 
    {
        ret = gnss_eph_inject_data(eph_file_path);
        if(ret < 0)
        {
            LOGE("gnss_eph_inject_data fail");
            return INJECT_RESULT_FAIL;
        }
    }
    return INJECT_RESULT_SUCCESS;
}


/************************************set fd*************************************/
void hd_set_gnss_dev_fd(int fd)
{
    hd_fd = fd;
}

/************************************check return*************************************/
//gps f1 d9 05 01 02 00 0b 32 46 74
//bds f1 d9 05 01 02 00 0b 33 46 74
//glo f1 d9 05 01 02 00 0b 34 46 74

int hd_eph_inject_result_check(const uint8_t *pack, int pack_len)
{
    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)
    {
        const uint8_t *ptr = pack + i * 10;
        if(ptr[0] != 0xf1 || ptr[1] != 0xd9)
        {
            LOGE("Pack head error : %02x %02x", ptr[0], ptr[1]);
            return INJECT_RESULT_FAIL;
        }

        if(ptr[2] != 0x05)
        {
            LOGE("Type not 0x05 : %02x", ptr[2]);
            return INJECT_RESULT_FAIL;
        }

        if(ptr[6] != 0x0b || (ptr[7] < 0x32 && ptr[7] > 0x36))
        {
            LOGE("Unknown gid - %d, sid - %d", ptr[6], ptr[7]);
            return INJECT_RESULT_FAIL;
        }

        if(ptr[3] == 0x01)
        {
            return INJECT_RESULT_SUCCESS;
        }
        else if(ptr[3] == 0x00)
        {
            return INJECT_RESULT_FAIL;
        }
        else
        {
            LOGE("ID not 0x00 or 0x01 : %02x", ptr[3]);
            return INJECT_RESULT_FAIL;
        }
        i++;
    }

    return INJECT_RESULT_SUCCESS;
}
/************************************get or set status*************************************/
void hd_set_eph_inject_status(hd_eph_inject_status_type status)
{
    eph_inject_status = status;
}

hd_eph_inject_status_type hd_get_eph_inject_status(void)
{
    return eph_inject_status;
}