RTK_base/task/rtk_task.c

/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2022-06-15     fize       the first version
 */

#include <rtk_task.h>

#define LOG_TAG "USER_TASK"
#define EVENT_FLAG(...) (1<<__VA_ARGS__)

#define EVENT_REF_RTCM_DATA_FLAG EVENT_FLAG(0)
#define EVENT_UAER_RTCM_DATA_FLAG EVENT_FLAG(1)

#define THREAD_get_user_rtcm_data_priority 10
#define THREAD_get_ref_rtcm_data_priority 10
#define thread_rtk_proces_priority 9

#define get_rtcm_user_data_stack_size DEFAULT_USER_THREAD_STACK_SIZE
#define get_rtcm_ref_data_stack_size DEFAULT_USER_THREAD_STACK_SIZE
#define rtk_proces_stack_size DEFAULT_USER_THREAD_STACK_SIZE

//rtcm_t rtcm_data;
struct rt_event uart_event;
struct rt_completion rtcm_ref_data_process_completion;
struct rt_completion rtcm_user_data_process_completion;

static struct rt_thread get_user_rtcm_data;
static char thread_user_rtcm_data_stack[get_rtcm_user_data_stack_size];
static struct rt_thread get_ref_rtcm_data;
static char thread_ref_rtcm_data_stack[get_rtcm_ref_data_stack_size];

struct rt_mutex userf_svrlock;
rtksvr_t *svr; /* rtk server struct */
//serial3  -->user   serial_rtcm_buff[0][]
//serial6  -->ref    serial_rtcm_buff[1][]
char buff[8000];
static void thread_get_ref_rtcm_data(void *parameter)
{
    while (1)
    {

        //to make sure there is only one sem exist at uart,keep data from being damaged by dma transmit.
        //rt thread could not get uart status when it receiving data.
        //it may cross frame lost if the system runs slow.
        if (rt_sem_take(&uart6_simpack.rx_sem, RT_WAITING_FOREVER) == RT_EOK) //need a completion sem connect to rtcm data processing thread.
        {
            if (rt_completion_wait(&rtcm_ref_data_process_completion, RT_WAITING_NO) == RT_EOK)
            {
                READ_SERIAL6(svr->buff[1]);

//                READ_SERIAL6(buff);
                svr->nb[1] = uart6_simpack.rx_num;
                rt_event_send(&uart_event, EVENT_REF_RTCM_DATA_FLAG);
            }
            else
            {
                if (uart6_simpack.rx_num != 0)
                {
                    char *drop = rt_malloc(uart6_simpack.rx_num);
                    if (drop == RT_NULL)
                    {
                        LOG_E("heap not enough for thread_get_ref_rtcm_data");
                    }
                    else
                    {
                        READ_SERIAL6(drop);
                        rt_free(drop);
                    }

                }
            }

        }

    }
}

static void thread_get_user_rtcm_data(void *parameter)
{

    while (1)
    {
        //to make sure there is only one sem exist at uart,keep data from being damaged by dma transmit.
        //rt thread could not get uart status when it receiving data.
        //it may cross frame lost if the system runs slow.
        if (rt_sem_take(&uart3_simpack.rx_sem, RT_WAITING_FOREVER) == RT_EOK) //need a completion sem connect to rtcm data processing thread.
        {
            if (rt_completion_wait(&rtcm_user_data_process_completion, RT_WAITING_NO) == RT_EOK)
            {
                READ_SERIAL3(svr->buff[0]);
                svr->nb[0] = uart3_simpack.rx_num;
                rt_event_send(&uart_event, EVENT_UAER_RTCM_DATA_FLAG);
            }
            else
            {
                if (uart3_simpack.rx_num != 0)
                {
                    char *drop = rt_malloc(uart3_simpack.rx_num);
                    if (drop == RT_NULL)
                    {
                        LOG_E("heap not enough for thread_get_user_rtcm_data");
                    }
                    else
                    {
                        READ_SERIAL3(drop);
                        rt_free(drop);
                    }

                }
            }

        }
    }
}

/* global variables ----------------------------------------------------------*/

static int svrcycle = 50; /* server cycle (ms) */
static int buffsize = 4096; /* input buffer size (bytes) */
static int strfmt[] = { STRFMT_RTCM3, STRFMT_RTCM3 }; /* stream formats */
static int nmeacycle = 5000; /* nmea request cycle (ms) */
static int nmeareq = 0; /* nmea request type (0:off,1:lat/lon,2:single) */
extern prcopt_t prcopt_;
extern solopt_t solopt_;
//static obsd_t data[MAXOBS * 2]; /* 7616 B */
static void thread_rtk_proces(void *parameter)
{
    double pos[3] = { 0.0, 0.0, 0.0 }, npos[3] = { 0.0, 0.0, 0.0 };
    static double nmeapos[] = { 0, 0, 0 };
    uint32_t recevd_event = 0;
    svr = (rtksvr_t *) rt_malloc(sizeof(rtksvr_t));
    if (svr == RT_NULL)
    {
        LOG_E("mem not enough for svr&prcopt&solopt");
    }

    /*  STEP-1: <20><><EFBFBD><EFBFBD>RTK<54><4B><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ  */
    resetsysopts(); /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ */
    getsysopts(NULL, NULL); /* ָ<><D6B8>ָ<EFBFBD><D6B8><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ */

    /*  STEP-2: <20><>ʼ<EFBFBD><CABC>RTK<54><4B><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9>  */
    rtksvrinit(svr);

    /*  STEP-3: <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>͵<EFBFBD>NMEA<45><41>Ϣ  */
    /* <20><><EFBFBD>÷<EFBFBD><C3B7><EFBFBD>GGA<47><41>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>Ϣ, Ҳ<><D2B2><EFBFBD>Դӵ<D4B4><D3B5>㶨λ<E3B6A8>л<EFBFBD>ȡ */
    pos[0] = nmeapos[0] * D2R;
    pos[1] = nmeapos[1] * D2R;
    pos[2] = nmeapos[2];
    pos2ecef(pos, npos);
    /*  STEP-5: <20><>ʼRTK<54><4B><EFBFBD>߳<EFBFBD>  */
    rtksvrstart_mini(svr, svrcycle, buffsize, strfmt, nmeacycle, nmeareq, nmeapos, &prcopt_, &solopt_);

    obs_t obs; /* 28 B */

    sol_t sol = { { 0 } }; /* 199 B */
    double tt;
    uint32_t tick, ticknmea, tick1hz, tickreset;
    uint8_t *p, *q;
    int n = 0, fobs[2] = { 0 }, cycle, cputime = 0;

    svr->state = 1;
//    obs.data = data;

    svr->tick = tickget();
    ticknmea = tick1hz = svr->tick - 1000;
    tickreset = svr->tick - 30000;

    rt_completion_done(&rtcm_user_data_process_completion);
    rt_completion_done(&rtcm_ref_data_process_completion);

    while (1)
    {
        if (!svr->state)
        {
            rt_thread_sleep(1);
        }
        else
        {

            tick = tickget();

            rt_event_recv(&uart_event, (EVENT_UAER_RTCM_DATA_FLAG | EVENT_REF_RTCM_DATA_FLAG),
                    (RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR), RT_WAITING_FOREVER, &recevd_event);
            if (recevd_event & EVENT_UAER_RTCM_DATA_FLAG || recevd_event & EVENT_REF_RTCM_DATA_FLAG)
            {
                if (recevd_event & EVENT_UAER_RTCM_DATA_FLAG)
                {
                    fobs[0] = decoderaw(svr, 0);
                    rt_completion_done(&rtcm_user_data_process_completion);
                }
                if (recevd_event & EVENT_REF_RTCM_DATA_FLAG)
                {
                    fobs[1] = decoderaw(svr, 1);
                    rt_completion_done(&rtcm_ref_data_process_completion);
                }
            }
            else
            {
//                rt_thread_sleep(5);
                continue;
            }

            uint8_t nr = 0;
            uint8_t nu = 0;

            /* <20><><EFBFBD><EFBFBD>׼վ<D7BC><D5BE><EFBFBD>ƶ<EFBFBD>վ<EFBFBD>Ĺ۲<C4B9><DBB2><EFBFBD><EFBFBD>ݺ϶<DDBA>Ϊһ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>MAXOBS*2 <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20>Ҿ<EFBFBD><D2BE><EFBFBD><EFBFBD>Dz<EFBFBD><C7B2><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>? */
            obs.n = 0;

            for (int i = 0; i < fobs[0]; i++)
            {
                for (int j = 0; j < svr->obs[0][i].n; j++)
                {
                    nu++;
                }
            }
            for (int i = 0; i < fobs[1]; i++)
            {
                for (int j = 0; j < svr->obs[1][i].n; j++)
                {
                    nr++;
                }
            }
            nu = (nu > MAXOBS) ? MAXOBS : nu;
            nr = (nr > MAXOBS * 2 - nu) ? (MAXOBS * 2) - nr : nr;
            if (nu + nr == 0)
            {
//                rt_thread_mdelay(5);
                continue;
            }
            obs.data = (obsd_t *) rt_malloc(sizeof(obsd_t) * (nu + nr));
            obs.n = 0;
            for (int i = 0; i < fobs[0]; i++)
            {
                for (int j = 0; j < svr->obs[0][i].n && obs.n < nu; j++)
                {
                    obs.data[obs.n++] = svr->obs[0][i].data[j];
                }
            }
            for (int i = 0; i < fobs[1]; i++)
            {
                for (int j = 0; j < svr->obs[1][i].n && obs.n < nu + nr; j++)
                {
                    obs.data[obs.n++] = svr->obs[1][i].data[j];
                }
            }

            /* rtk positioning */
//                rtksvrlock(svr);
            rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav);

//                rtksvrunlock(svr);
            rt_free(obs.data);
            obs.data = RT_NULL;
            /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч, <20><><EFBFBD>ý<EFBFBD><C3BD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD> */
            if (svr->rtk.sol.stat != SOLQ_NONE)
            {
                /* adjust current time */
                tt = (int) (tickget() - tick) / 1000.0 + DTTOL;
                timeset(gpst2utc(timeadd(svr->rtk.sol.time, tt)));

                /* write solution */
//                    writesol_mini(svr, i);
            }
            /* <20><><EFBFBD><EFBFBD><EFBFBD>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>䵽<EFBFBD>˷<EFBFBD><CBB7>ֻ<EFBFBD>û<EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD>¼û<C2BC>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĺ۲<C4B9><DBB2><EFBFBD><EFBFBD><EFBFBD> */
//            if ((int) (tickget() - tick) >= svr->cycle)
//            {
//                svr->prcout += fobs[0] - i - 1;
//            }
            /* send null solution if no solution (1hz) */
            if (svr->rtk.sol.stat == SOLQ_NONE && (int) (tick - tick1hz) >= 1000)
            {
//                writesol_mini(svr, 0);
                tick1hz = tick;
            }
            /* send nmea request to base/nrtk input stream */
            if (svr->nmeacycle > 0 && (int) (tick - ticknmea) >= svr->nmeacycle)
            {
                // TODO: <20><><EFBFBD><EFBFBD>send_nmea <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD>ֲ
                // send_nmea(svr, &tickreset);
                ticknmea = tick;
            }
            cputime = (int) (tickget() - tick);
            if (cputime > 0)
                svr->cputime = cputime;

            /* sleep until next cycle */

        }
    }
    sleepms(1);
}

static void user_init_task(rt_thread_t thread, const char *name, void (*entry)(void *parameter), void *stack_start,
        rt_uint32_t stack_size, rt_uint8_t priority, rt_uint32_t tick, rt_uint32_t start)
{
    rt_err_t result = RT_EOK;
    result = rt_thread_init(thread, name, entry, RT_NULL, stack_start, stack_size, priority, tick);
    if (start)
    {
        if (result == RT_EOK)
        {
            rt_thread_startup(thread);
        }
        else
        {
            LOG_E("task init failed :%s", name);
        }
    }
}

/*===========================================================================================================*/

void task_init(void)
{

    rt_event_init(&uart_event, "uart_event", RT_IPC_FLAG_PRIO);
    rt_mutex_init(&userf_svrlock, "svrlock", RT_IPC_FLAG_PRIO);
    rt_completion_init(&rtcm_user_data_process_completion);
    rt_completion_init(&rtcm_ref_data_process_completion);

    rt_thread_t trd = rt_thread_create("rtk_proces", thread_rtk_proces, RT_NULL, 8000, thread_rtk_proces_priority,
    DEFAULT_USER_THREA_TICK);
    rt_thread_startup(trd);
    user_init_task(&get_user_rtcm_data, "thread_get_user_rtcm_data", thread_get_user_rtcm_data,
            thread_user_rtcm_data_stack,
            get_rtcm_user_data_stack_size, THREAD_get_user_rtcm_data_priority, DEFAULT_USER_THREA_TICK, RT_TRUE);
    user_init_task(&get_ref_rtcm_data, "thread_get_ref_rtcm_data", thread_get_ref_rtcm_data, thread_ref_rtcm_data_stack,
    get_rtcm_ref_data_stack_size, THREAD_get_ref_rtcm_data_priority, DEFAULT_USER_THREA_TICK, RT_TRUE);
//    user_init_task(&rtk_proces, "rtk_proces", thread_rtk_proces, thread_rtk_proces_stack,
//    rtk_proces_stack_size, thread_rtk_proces_priority, DEFAULT_USER_THREA_TICK, RT_TRUE);

}