diff --git a/RTK/ST_rtksvr.c b/RTK/ST_rtksvr.c index 88d960f..aee3f71 100644 --- a/RTK/ST_rtksvr.c +++ b/RTK/ST_rtksvr.c @@ -899,12 +899,10 @@ extern int rtksvrstart_mini(rtksvr_t *svr, int cycle, int buffsize, int *formats LOG_E("rtksvrstart: buff malloc error"); return 0; } - memset(&svr->nmsg,0,sizeof(svr->nmsg)); - memset(&svr->obs,0,sizeof(svr->obs)); -// for (j = 0; j < 10; j++) -// svr->nmsg[i][j] = 0; -// for (j = 0; j < MAXOBSBUF; j++) -// svr->obs[i][j].n = 0; + for (j = 0; j < 10; j++) + svr->nmsg[i][j] = 0; + for (j = 0; j < MAXOBSBUF; j++) + svr->obs[i][j].n = 0; init_rtcm(&svr->rtcm[i]); } diff --git a/RTK/pntpos.c b/RTK/pntpos.c index a2b3ad2..0e02d91 100644 --- a/RTK/pntpos.c +++ b/RTK/pntpos.c @@ -1211,10 +1211,10 @@ extern int pntpos(const obsd_t *obs, int n, const nav_t *nav, ssat[obs[i].sat - 1].resp[0] = resp[i]; /* residuals of pseudorange (m) */ } } - free(rs); - free(dts); - free(var); - free(azel_); - free(resp); + rt_free(rs); + rt_free(dts); + rt_free(var); + rt_free(azel_); + rt_free(resp); return stat; } diff --git a/RTK/rtkcmn.c b/RTK/rtkcmn.c index 7fa539c..7f52a2e 100644 --- a/RTK/rtkcmn.c +++ b/RTK/rtkcmn.c @@ -1195,7 +1195,7 @@ extern double *mat(int n, int m) if (n <= 0 || m <= 0) return NULL; - if (!(p = (double *)malloc(sizeof(double) * n * m))) + if (!(p = (double *)rt_malloc(sizeof(double) * n * m))) { fatalerr("matrix memory allocation error: n=%d,m=%d\n", n, m); } @@ -1212,7 +1212,7 @@ extern int *imat(int n, int m) if (n <= 0 || m <= 0) return NULL; - if (!(p = (int *)malloc(sizeof(int) * n * m))) + if (!(p = (int *)rt_malloc(sizeof(int) * n * m))) { fatalerr("integer matrix memory allocation error: n=%d,m=%d\n", n, m); } @@ -1234,7 +1234,7 @@ extern double *zeros(int n, int m) #else if (n <= 0 || m <= 0) return NULL; - if (!(p = (double *)calloc(sizeof(double), n * m))) + if (!(p = (double *)rt_calloc( n * m,sizeof(double)))) { fatalerr("matrix memory allocation error: n=%d,m=%d\n", n, m); } diff --git a/makefile.targets b/makefile.targets index 05666e7..1b31929 100644 --- a/makefile.targets +++ b/makefile.targets @@ -4,4 +4,4 @@ clean2: -@echo ' ' -*.elf: $(wildcard ../board/linker_scripts/link.lds) +*.elf: $(wildcard ..D:/Dev/GitHome/rt_thread_workspace/RTK_base/board/linker_scripts/link.lds) diff --git a/task/rtk_task.c b/task/rtk_task.c index 25d37bf..906f473 100644 --- a/task/rtk_task.c +++ b/task/rtk_task.c @@ -21,7 +21,7 @@ #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 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; @@ -33,8 +33,6 @@ 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; @@ -50,7 +48,7 @@ static void thread_get_ref_rtcm_data(void *parameter) //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_sem_take(&uart6_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; @@ -65,15 +63,12 @@ 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); } @@ -89,6 +84,7 @@ 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 }; @@ -117,7 +113,7 @@ static void thread_rtk_proces(void *parameter) 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; @@ -126,7 +122,8 @@ static void thread_rtk_proces(void *parameter) uint8_t nr, nu; svr->state = 1; - obs.data = (obsd_t *)rt_malloc(sizeof(obsd_t)*MAXOBS*2); + obs.data = data; +// obs.data = (obsd_t *)rt_malloc(sizeof(obsd_t)*MAXOBS*2); svr->tick = tickget(); ticknmea = tick1hz = svr->tick - 1000; tickreset = svr->tick - 30000; @@ -138,7 +135,7 @@ static void thread_rtk_proces(void *parameter) { if (!svr->state) { - rt_thread_yield(); + rt_thread_sleep(1); } else { @@ -149,15 +146,13 @@ static void thread_rtk_proces(void *parameter) (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); - + rt_completion_done(&rtcm_user_data_process_completion); } else if (recevd_event & EVENT_REF_RTCM_DATA_FLAG) { - rtcm_ref_data_process_completion.flag = RT_UNCOMPLETED; fobs[1] = decoderaw(svr, 1); - + rt_completion_done(&rtcm_ref_data_process_completion); } else { continue; @@ -174,15 +169,15 @@ static void thread_rtk_proces(void *parameter) nu++; } - for (j = 0; j < svr->obs[1][0].n && obs.n < MAXOBS * 2; j++) + for (j = 0; j < svr->obs[1][i].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); + rtksvrlock(svr); + rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav); + rtksvrunlock(svr); /* 如果解算结果有效, 设置解算时间点 */ if (svr->rtk.sol.stat != SOLQ_NONE) @@ -221,11 +216,6 @@ static void thread_rtk_proces(void *parameter) sleepms(15); } } - - while (1) - { - - } } static void user_init_task(rt_thread_t thread, const char *name, void (*entry)(void *parameter), void *stack_start,