333 lines
11 KiB
C
333 lines
11 KiB
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 20
|
||
#define THREAD_get_ref_rtcm_data_priority 20
|
||
#define thread_rtk_proces_priority 19
|
||
|
||
#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
|
||
{
|
||
continue;
|
||
}
|
||
|
||
uint8_t nr = 0;
|
||
uint8_t nu = 0;
|
||
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 == 0||nr ==0)
|
||
{
|
||
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, 0);
|
||
}
|
||
/* <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 */
|
||
|
||
}
|
||
}
|
||
}
|
||
|
||
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, 20480, 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);
|
||
|
||
}
|
||
|