fuxiaoer/RTK_base
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

no message

Browse Source
This commit is contained in:
fize 2022-06-27 15:36:14 +08:00
parent 4ff028180d
commit 06539e0e4b
5 changed files with 26 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
RTK/ST_rtksvr.c
View File

@ -899,12 +899,10 @@ extern int rtksvrstart_mini(rtksvr_t *svr, int cycle, int buffsize, int *formats
LOG_E("rtksvrstart: buff malloc error"); LOG_E("rtksvrstart: buff malloc error");
return 0; return 0;
} }
memset(&svr->nmsg,0,sizeof(svr->nmsg)); for (j = 0; j < 10; j++)
memset(&svr->obs,0,sizeof(svr->obs)); svr->nmsg[i][j] = 0;
// for (j = 0; j < 10; j++) for (j = 0; j < MAXOBSBUF; j++)
// svr->nmsg[i][j] = 0; svr->obs[i][j].n = 0;
// for (j = 0; j < MAXOBSBUF; j++)
// svr->obs[i][j].n = 0;
init_rtcm(&svr->rtcm[i]); init_rtcm(&svr->rtcm[i]);
} }

10
RTK/pntpos.c
View File

@ -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) */ ssat[obs[i].sat - 1].resp[0] = resp[i]; /* residuals of pseudorange (m) */
} }
} }
free(rs); rt_free(rs);
free(dts); rt_free(dts);
free(var); rt_free(var);
free(azel_); rt_free(azel_);
free(resp); rt_free(resp);
return stat; return stat;
} }

6
RTK/rtkcmn.c
View File

@ -1195,7 +1195,7 @@ extern double *mat(int n, int m)
if (n <= 0 || m <= 0) if (n <= 0 || m <= 0)
return NULL; 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); 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) if (n <= 0 || m <= 0)
return NULL; 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); 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 #else
if (n <= 0 || m <= 0) if (n <= 0 || m <= 0)
return NULL; 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); fatalerr("matrix memory allocation error: n=%d,m=%d\n", n, m);
} }

2
makefile.targets
View File

@ -4,4 +4,4 @@ clean2:
-@echo ' ' -@echo ' '
*.elf: $(wildcard ../board/linker_scripts/link.lds) *.elf: $(wildcard ..D:/Dev/GitHome/rt_thread_workspace/RTK_base/board/linker_scripts/link.lds)

36
task/rtk_task.c
View File

@ -21,7 +21,7 @@
#define thread_rtk_proces_priority 9 #define thread_rtk_proces_priority 9
#define get_rtcm_user_data_stack_size DEFAULT_USER_THREAD_STACK_SIZE #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 #define rtk_proces_stack_size DEFAULT_USER_THREAD_STACK_SIZE
//rtcm_t rtcm_data; //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 char thread_user_rtcm_data_stack[get_rtcm_user_data_stack_size];
static struct rt_thread get_ref_rtcm_data; static struct rt_thread get_ref_rtcm_data;
static char thread_ref_rtcm_data_stack[get_rtcm_ref_data_stack_size]; 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; 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. //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. //rt thread could not get uart status when it receiving data.
//it may cross frame lost if the system runs slow. //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]); READ_SERIAL6(svr->buff[1]);
svr->nb[1] = uart6_simpack.rx_num; svr->nb[1] = uart6_simpack.rx_num;
@ -65,15 +63,12 @@ static void thread_get_user_rtcm_data(void *parameter)
while (1) while (1)
{ {
rt_completion_wait(&rtcm_user_data_process_completion, RT_WAITING_FOREVER); 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. //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. //rt thread could not get uart status when it receiving data.
//it may cross frame lost if the system runs slow. //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(&uart3_simpack.rx_sem, RT_WAITING_FOREVER) == RT_EOK) //need a completion sem connect to rtcm data processing thread.
{ {
READ_SERIAL3(svr->buff[0]); READ_SERIAL3(svr->buff[0]);
// char test[2048];
// READ_SERIAL3(test);
svr->nb[0] = uart3_simpack.rx_num; svr->nb[0] = uart3_simpack.rx_num;
rt_event_send(&uart_event, EVENT_UAER_RTCM_DATA_FLAG); 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) */ static int nmeareq = 0; /* nmea request type (0:off,1:lat/lon,2:single) */
extern prcopt_t prcopt_; extern prcopt_t prcopt_;
extern solopt_t solopt_; extern solopt_t solopt_;
static obsd_t data[MAXOBS * 2]; /* 7616 B */
static void thread_rtk_proces(void *parameter) static void thread_rtk_proces(void *parameter)
{ {
double pos[3] = { 0.0, 0.0, 0.0 }, npos[3] = { 0.0, 0.0, 0.0 }; 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_); rtksvrstart_mini(svr, svrcycle, buffsize, strfmt, nmeacycle, nmeareq, nmeapos, &prcopt_, &solopt_);
obs_t obs; /* 28 B */ obs_t obs; /* 28 B */
// obsd_t data[MAXOBS * 2]; /* 7616 B */
sol_t sol = { { 0 } }; /* 199 B */ sol_t sol = { { 0 } }; /* 199 B */
double tt; double tt;
uint32_t tick, ticknmea, tick1hz, tickreset; uint32_t tick, ticknmea, tick1hz, tickreset;
@ -126,7 +122,8 @@ static void thread_rtk_proces(void *parameter)
uint8_t nr, nu; uint8_t nr, nu;
svr->state = 1; 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(); svr->tick = tickget();
ticknmea = tick1hz = svr->tick - 1000; ticknmea = tick1hz = svr->tick - 1000;
tickreset = svr->tick - 30000; tickreset = svr->tick - 30000;
@ -138,7 +135,7 @@ static void thread_rtk_proces(void *parameter)
{ {
if (!svr->state) if (!svr->state)
{ {
rt_thread_yield(); rt_thread_sleep(1);
} }
else 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); (RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR), RT_WAITING_FOREVER, &recevd_event);
if (recevd_event & EVENT_UAER_RTCM_DATA_FLAG) if (recevd_event & EVENT_UAER_RTCM_DATA_FLAG)
{ {
rtcm_user_data_process_completion.flag = RT_UNCOMPLETED;
fobs[0] = decoderaw(svr, 0); fobs[0] = decoderaw(svr, 0);
rt_completion_done(&rtcm_user_data_process_completion);
} }
else if (recevd_event & EVENT_REF_RTCM_DATA_FLAG) else if (recevd_event & EVENT_REF_RTCM_DATA_FLAG)
{ {
rtcm_ref_data_process_completion.flag = RT_UNCOMPLETED;
fobs[1] = decoderaw(svr, 1); fobs[1] = decoderaw(svr, 1);
rt_completion_done(&rtcm_ref_data_process_completion);
} }
else { else {
continue; continue;
@ -174,15 +169,15 @@ static void thread_rtk_proces(void *parameter)
nu++; 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]; obs.data[obs.n++] = svr->obs[1][0].data[j];
nr++; nr++;
} }
/* rtk positioning */ /* rtk positioning */
// rtksvrlock(svr); rtksvrlock(svr);
//// rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav); rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav);
// rtksvrunlock(svr); rtksvrunlock(svr);
/* 如果解算结果有效, 设置解算时间点 */ /* 如果解算结果有效, 设置解算时间点 */
if (svr->rtk.sol.stat != SOLQ_NONE) if (svr->rtk.sol.stat != SOLQ_NONE)
@ -221,11 +216,6 @@ static void thread_rtk_proces(void *parameter)
sleepms(15); sleepms(15);
} }
} }
while (1)
{
}
} }
static void user_init_task(rt_thread_t thread, const char *name, void (*entry)(void *parameter), void *stack_start, static void user_init_task(rt_thread_t thread, const char *name, void (*entry)(void *parameter), void *stack_start,