/* * 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 #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 * 4 #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]; static struct rt_thread rtk_proces; static char thread_rtk_proces_stack[rtk_proces_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][] static void thread_get_ref_rtcm_data(void *parameter) { while (1) { rt_completion_wait(&rtcm_ref_data_process_completion, RT_WAITING_FOREVER); //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. { READ_SERIAL6(svr->buff[1]); svr->nb[1] = uart6_simpack.rx_num; rt_event_send(&uart_event, EVENT_REF_RTCM_DATA_FLAG); } } } static void thread_get_user_rtcm_data(void *parameter) { while (1) { rt_completion_wait(&rtcm_user_data_process_completion, RT_WAITING_FOREVER); //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. { READ_SERIAL3(svr->buff[0]); // char test[2048]; // READ_SERIAL3(test); svr->nb[0] = uart3_simpack.rx_num; rt_event_send(&uart_event, EVENT_UAER_RTCM_DATA_FLAG); } } } /* 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 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 }; svr = (rtksvr_t *) rt_malloc(sizeof(rtksvr_t)); if (svr == RT_NULL) { LOG_E("mem not enough for svr&prcopt&solopt"); } /* STEP-1: 加载RTK配置信息 */ resetsysopts(); /* 清空配置信息 */ getsysopts(NULL, NULL); /* 指针指向新的配置信息 */ /* STEP-2: 初始化RTK服务结构体 */ rtksvrinit(svr); /* STEP-3: 初始化发送的NMEA信息 */ /* 设置发送GGA的位置信息, 也可以从单点定位中获取 */ pos[0] = nmeapos[0] * D2R; pos[1] = nmeapos[1] * D2R; pos[2] = nmeapos[2]; pos2ecef(pos, npos); /* STEP-5: 开始RTK子线程 */ rtksvrstart_mini(svr, svrcycle, buffsize, strfmt, nmeacycle, nmeareq, nmeapos, &prcopt_, &solopt_); obs_t obs; /* 28 B */ // obsd_t data[MAXOBS * 2]; /* 7616 B */ sol_t sol = { { 0 } }; /* 199 B */ double tt; uint32_t tick, ticknmea, tick1hz, tickreset; uint8_t *p, *q; int i, j, n = 0, fobs[2] = { 0 }, cycle, cputime=0; uint8_t nr, nu; svr->state = 1; obs.data = (obsd_t *)rt_malloc(sizeof(obsd_t)*MAXOBS*2); 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_yield(); } else { tick = tickget(); uint32_t recevd_event = 0; 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) { rtcm_user_data_process_completion.flag = RT_UNCOMPLETED; fobs[0] = decoderaw(svr, 0); } else if (recevd_event & EVENT_REF_RTCM_DATA_FLAG) { rtcm_ref_data_process_completion.flag = RT_UNCOMPLETED; fobs[1] = decoderaw(svr, 1); } else { continue; } /* 将基准站和移动站的观测数据合二为一 这里重新申请了一个MAXOBS*2 的数组, 我觉得是不是有点问题? */ for (i = 0; i < fobs[0]; i++) { obs.n = 0; nr = nu = 0; for (j = 0; j < svr->obs[0][i].n && obs.n < MAXOBS * 2; j++) { obs.data[obs.n++] = svr->obs[0][i].data[j]; nu++; } for (j = 0; j < svr->obs[1][0].n && obs.n < MAXOBS * 2; j++) { obs.data[obs.n++] = svr->obs[1][0].data[j]; nr++; } /* rtk positioning */ // rtksvrlock(svr); //// rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav); // rtksvrunlock(svr); /* 如果解算结果有效, 设置解算时间点 */ 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); } /* 如果设定的运算时间到了发现还没有计算完, 则记录没有计算完的观测次数 */ 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: 完成send_nmea 到到串口的移植 // send_nmea(svr, &tickreset); ticknmea = tick; } cputime = (int) (tickget() - tick); if (cputime > 0) svr->cputime = cputime; /* sleep until next cycle */ sleepms(15); } } while (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, 3000, 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); }