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

内部测试第一版

Browse Source
This commit is contained in:
fize 2022-07-05 13:42:48 +08:00
parent c5a5c396f1
commit f9fc38b00d
6 changed files with 282 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
.settings/.rtmenus
View File

Binary file not shown.

25
RTK/ST_rtksvr.c
View File

@ -69,7 +69,7 @@ void writesol_mini(rtksvr_t *svr, int index)
/* output solution status */ /* output solution status */
// rtksvrlock(svr); // rtksvrlock(svr);
// // n = rtkoutstat(&svr->rtk, (char*)buff); n = rtkoutstat(&svr->rtk, (char*)buff);
// rtksvrunlock(svr); // rtksvrunlock(svr);
} }
else else
@ -77,7 +77,8 @@ void writesol_mini(rtksvr_t *svr, int index)
/* output solution */ /* output solution */
n = outsols(buff, &svr->rtk.sol, svr->rtk.rb, svr->solopt + i); n = outsols(buff, &svr->rtk.sol, svr->rtk.rb, svr->solopt + i);
} }
// strwrite(svr->stream + i + 3, buff, n); // strwrite(svr->stream + i + 3, buff, n);
LOG_I("%s",buff);
// TODO: 这里应该改为从串口输出解算后的数据 // TODO: 这里应该改为从串口输出解算后的数据
/* 输出NMEA GSA GSV 相关信息 */ /* 输出NMEA GSA GSV 相关信息 */
@ -349,6 +350,7 @@ static void update_svr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav,
} }
} }
/* decode receiver raw/rtcm data ---------------------------------------------*/ /* decode receiver raw/rtcm data ---------------------------------------------*/
extern nmea_t nmea_pnt;
int decoderaw(rtksvr_t *svr, int index) int decoderaw(rtksvr_t *svr, int index)
{ {
obs_t *obs; obs_t *obs;
@ -359,24 +361,7 @@ int decoderaw(rtksvr_t *svr, int index)
for (i = 0; i < svr->nb[index]; i++) for (i = 0; i < svr->nb[index]; i++)
{ {
// input_nmea(&nmea_pnt,svr->buff[index][i]);
/* input rtcm/receiver raw data from stream */
/*
if (svr->format[index]==STRFMT_RTCM3) {
ret=input_rtcm3(svr->rtcm+index,svr->buff[index][i]);
obs=&svr->rtcm[index].obs;
nav=&svr->rtcm[index].nav;
ephsat=svr->rtcm[index].ephsat;
ephset=svr->rtcm[index].ephset;
}
else {
ret=input_raw(svr->raw+index,svr->format[index],svr->buff[index][i]);
obs=&svr->raw[index].obs;
nav=&svr->raw[index].nav;
ephsat=svr->raw[index].ephsat;
ephset=svr->raw[index].ephset;
sbsmsg=&svr->raw[index].sbsmsg;
}*/
ret = input_rtcm3(svr->rtcm + index, svr->buff[index][i]); ret = input_rtcm3(svr->rtcm + index, svr->buff[index][i]);
/* (-1: error message, 0: no message, 1: input observation data, /* (-1: error message, 0: no message, 1: input observation data,
2: input ephemeris, 5: input station pos/ant parameters, 2: input ephemeris, 5: input station pos/ant parameters,

67
RTK/lambda_par.c
View File

@ -15,14 +15,14 @@
y = tmp_; \ y = tmp_; \
} while (0) } while (0)
static int parsearch(rtk_t* rtk, int n, const double* zhat, const double* Qzhat, const double* Z, static int parsearch(rtk_t *rtk, int n, const double *zhat, const double *Qzhat, const double *Z,
const double* L, const double* D, int m, double* F, double* s); const double *L, const double *D, int m, double *F, double *s);
/* LD factorization (Q=L'*diag(D)*L) -----------------------------------------*/ /* LD factorization (Q=L'*diag(D)*L) -----------------------------------------*/
static int LD(int n, const double* Q, double* L, double* D) static int LD(int n, const double *Q, double *L, double *D)
{ {
int i, j, k, info = 0; int i, j, k, info = 0;
double a, * A = mat(n, n); double a, *A = mat(n, n);
memcpy(A, Q, sizeof(double) * n * n); memcpy(A, Q, sizeof(double) * n * n);
for (i = n - 1; i >= 0; i--) for (i = n - 1; i >= 0; i--)
@ -47,7 +47,7 @@ static int LD(int n, const double* Q, double* L, double* D)
return info; return info;
} }
/* integer gauss transformation ----------------------------------------------*/ /* integer gauss transformation ----------------------------------------------*/
static void gauss(int n, double* L, double* Z, int i, int j) static void gauss(int n, double *L, double *Z, int i, int j)
{ {
int k, mu; int k, mu;
@ -60,7 +60,7 @@ static void gauss(int n, double* L, double* Z, int i, int j)
} }
} }
/* permutations --------------------------------------------------------------*/ /* permutations --------------------------------------------------------------*/
static void perm(int n, double* L, double* D, int j, double del, double* Z) static void perm(int n, double *L, double *D, int j, double del, double *Z)
{ {
int k; int k;
double eta, lam, a0, a1; double eta, lam, a0, a1;
@ -83,7 +83,7 @@ static void perm(int n, double* L, double* D, int j, double del, double* Z)
SWAP(Z[k + j * n], Z[k + (j + 1) * n]); SWAP(Z[k + j * n], Z[k + (j + 1) * n]);
} }
/* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) (ref.[1]) ---------------*/ /* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) (ref.[1]) ---------------*/
static void reduction(int n, double* L, double* D, double* Z) static void reduction(int n, double *L, double *D, double *Z)
{ {
int i, j, k; int i, j, k;
double del; double del;
@ -116,12 +116,12 @@ static void reduction(int n, double* L, double* D, double* Z)
zs I transformed double-diff phase biases zs I transformed double-diff phase biases
zn O fixed solutions zn O fixed solutions
s O sum of residuals for fixed solutions */ s O sum of residuals for fixed solutions */
static int search(int n, int m, const double* L, const double* D, static int search(int n, int m, const double *L, const double *D,
const double* zs, double* zn, double* s) const double *zs, double *zn, double *s)
{ {
int i, j, k, c, nn = 0, imax = 0; int i, j, k, c, nn = 0, imax = 0;
double newdist, maxdist = 1E99, y; // maxdist，当前超椭圆半径 double newdist, maxdist = 1E99, y; // maxdist，当前超椭圆半径
double* S = zeros(n, n), * dist = mat(n, 1), * zb = mat(n, 1), * z = mat(n, 1), * step = mat(n, 1); double *S = zeros(n, n), *dist = mat(n, 1), *zb = mat(n, 1), *z = mat(n, 1), *step = mat(n, 1);
k = n - 1; k = n - 1;
dist[k] = 0.0; // k表示当前层，从最后一层（n-1）开始计算 dist[k] = 0.0; // k表示当前层，从最后一层（n-1）开始计算
@ -233,11 +233,11 @@ static float my_normcdf(float a) //
return 0.5f * r; return 0.5f * r;
} }
static double* nmatgetkmat_RD(const double* nmat, int n1, int n2, int m1, int m2) static void nmatgetkmat_RD(const double *nmat, int n1, int n2, int m1, int m2, double *mmat)
{ {
double* mmat; // double *mmat;
int i, j; int i, j;
mmat = mat(m1, m2); // mmat = mat(m1, m2);
//从大矩阵里取小矩阵 //从大矩阵里取小矩阵
for (i = 0; i < m1; i++) for (i = 0; i < m1; i++)
for (j = 0; j < m2; j++) for (j = 0; j < m2; j++)
@ -245,14 +245,14 @@ static double* nmatgetkmat_RD(const double* nmat, int n1, int n2, int m1, int m2
*(mmat + i * m2 + j) = *(nmat + (n1 - m1) * n2 + (n2 - m2) + i * n2 + j); *(mmat + i * m2 + j) = *(nmat + (n1 - m1) * n2 + (n2 - m2) + i * n2 + j);
} }
return mmat; // return mmat;
} }
static double* nmatgetkmat_RU(const double* nmat, int n1, int n2, int m1, int m2) static void nmatgetkmat_RU(const double *nmat, int n1, int n2, int m1, int m2, double *mmat)
{ {
double* mmat; // double *mmat;
int i, j; int i, j;
mmat = mat(m1, m2); // mmat = mat(m1, m2);
//从大矩阵里取小矩阵 //从大矩阵里取小矩阵
for (i = 0; i < m1; i++) for (i = 0; i < m1; i++)
for (j = 0; j < m2; j++) for (j = 0; j < m2; j++)
@ -260,7 +260,7 @@ static double* nmatgetkmat_RU(const double* nmat, int n1, int n2, int m1, int m2
*(mmat + i * m2 + j) = *(nmat + (n2 - m2) + i * n2 + j); *(mmat + i * m2 + j) = *(nmat + (n2 - m2) + i * n2 + j);
} }
return mmat; // return mmat;
} }
/* parlambda ------------------------------ /* parlambda ------------------------------
* integer least-square estimation. reduction is performed by lambda (ref.[1]), * integer least-square estimation. reduction is performed by lambda (ref.[1]),
@ -276,11 +276,11 @@ static double* nmatgetkmat_RU(const double* nmat, int n1, int n2, int m1, int m2
* return : status (0:ok,other:error) * return : status (0:ok,other:error)
* notes : matrix stored by column-major order (fortran convension) * notes : matrix stored by column-major order (fortran convension)
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
extern int parlambda(rtk_t* rtk, int n, int m, const double* a, const double* Q, double* F, extern int parlambda(rtk_t *rtk, int n, int m, const double *a, const double *Q, double *F,
double* s) double *s)
{ {
int info; int info;
double* L, * D, * Z, * z, * E, * Qt, * Qzhat; double *L, *D, *Z, *z, *E, *Qt, *Qzhat;
if (n <= 0 || m <= 0) if (n <= 0 || m <= 0)
return -1; return -1;
L = zeros(n, n); L = zeros(n, n);
@ -305,7 +305,10 @@ extern int parlambda(rtk_t* rtk, int n, int m, const double* a, const double* Q,
matmul("NN", n, n, n, 1.0, Qt, Z, 0.0, Qzhat); // Qz =QtZ matmul("NN", n, n, n, 1.0, Qt, Z, 0.0, Qzhat); // Qz =QtZ
if (!(info = parsearch(rtk, n, z, Qzhat, Z, L, D, 2, F, s))) if (!(info = parsearch(rtk, n, z, Qzhat, Z, L, D, 2, F, s)))
{ {
}; ;
}
free(Qt);
free(Qzhat);
} }
free(L); free(L);
free(D); free(D);
@ -329,14 +332,14 @@ args:
double *s O sum of squared residulas of fixed solutions (1 x m) double *s O sum of squared residulas of fixed solutions (1 x m)
*/ */
static int parsearch(rtk_t* rtk, int n, const double* zhat, const double* Qzhat, const double* Z, static int parsearch(rtk_t *rtk, int n, const double *zhat, const double *Qzhat, const double *Z,
const double* L, const double* D, int m, double* F, double* s) const double *L, const double *D, int m, double *F, double *s)
{ {
int info=0, p, k, i, j; int info = 0, p, k, i, j;
float Ps = 1; /* Ps ：cumulative success rate P0:Minimum required sucess rate [DEFAULT=0.8] */ float Ps = 1; /* Ps ：cumulative success rate P0:Minimum required sucess rate [DEFAULT=0.8] */
double* zk_n, * Lk_n, * Dk_n, * z1_fix, * Q11, * Q12, * Qp, * z2_fix, * z_t1, * z_t2, * z_fix; double *zk_n, *Lk_n, *Dk_n, *z1_fix, *Q11, *Q12, *Qp, *z2_fix, *z_t1, *z_t2, *z_fix;
for (k = n; k > 2; k--) for (k = n; k > 2; k--)
{ {
if (Ps > LAMBDA_P0) if (Ps > LAMBDA_P0)
@ -367,7 +370,7 @@ static int parsearch(rtk_t* rtk, int n, const double* zhat, const double* Qzhat,
zk_n[i] = zhat[j]; zk_n[i] = zhat[j];
j++; j++;
} }
Lk_n = nmatgetkmat_RD(L, n, n, p, p); nmatgetkmat_RD(L, n, n, p, p, Lk_n);
if (!(info = search(p, m, Lk_n, Dk_n, zk_n, z1_fix, s))) /* returns 0 if no error */ if (!(info = search(p, m, Lk_n, Dk_n, zk_n, z1_fix, s))) /* returns 0 if no error */
{ {
@ -381,8 +384,8 @@ static int parsearch(rtk_t* rtk, int n, const double* zhat, const double* Qzhat,
z_t2 = mat(k - 1, m); z_t2 = mat(k - 1, m);
z_fix = mat(n, m); z_fix = mat(n, m);
// first k - 1 ambiguities are adjusted based on correlation with the fixed ambiguities // first k - 1 ambiguities are adjusted based on correlation with the fixed ambiguities
Q11 = nmatgetkmat_RD(Qzhat, n, n, p, p); nmatgetkmat_RD(Qzhat, n, n, p, p, Q11);
Q12 = nmatgetkmat_RU(Qzhat, n, n, k - 1, p); nmatgetkmat_RU(Qzhat, n, n, k - 1, p, Q12);
matinv(Q11, p); matinv(Q11, p);
matmul("NN", k - 1, p, p, 1.0, Q12, Q11, 0.0, Qp); // Qp = Q12 / Q11; matmul("NN", k - 1, p, p, 1.0, Q12, Q11, 0.0, Qp); // Qp = Q12 / Q11;
for (i = 0; i < 2 * p; i++) for (i = 0; i < 2 * p; i++)
@ -402,10 +405,10 @@ static int parsearch(rtk_t* rtk, int n, const double* zhat, const double* Qzhat,
else else
z2_fix[i] = zhat[i - k + 1] - z_t2[i]; z2_fix[i] = zhat[i - k + 1] - z_t2[i];
} }
//for (i = 0; i < 2 * k - 2; i++) // for (i = 0; i < 2 * k - 2; i++)
//{ //{
// z2_fix[i] = ROUND(z2_fix[i]); // z2_fix[i] = ROUND(z2_fix[i]);
//} // }
/* z_fix = [z2_fix, z1_fix]; */ /* z_fix = [z2_fix, z1_fix]; */
for (i = 0; i < 2 * n; i++) for (i = 0; i < 2 * n; i++)
{ {

223
RTK/nmea_zjut.c Normal file
View File

@ -0,0 +1,223 @@
#include "rtklib.h"
/* 在rtklib 中定义结构体 */
// typedef struct
// {
// sol_t sol;
// int nbyte; /* number of bytes in message buffer */
// int len; /* message length (bytes) */
// uint8_t buff[256]; /* message buffer */
// } nmea_t;
#define MAXFIELD 64 /* max number of fields in a record */
nmea_t nmea_pnt= {0};
static int nmea_xor(uint8_t* buff, int len)
{
int i = 0;
int result = buff[1];
int ans = 0;
for (i = 2; buff[i] != '*' && i < len - 2; i++)
{
result ^= buff[i];
}
ans += (buff[++i] - '0') * 16;
ans += buff[++i] - '0';
if (ans == result) return 1;
else return 0;
}
/* convert time in nmea format to time ---------------------------------------*/
static void septime(double t, double* t1, double* t2, double* t3)
{
*t1 = floor(t / 10000.0);
t -= *t1 * 10000.0;
*t2 = floor(t / 100.0);
*t3 = t - *t2 * 100.0;
}
/* convert ddmm.mm in nmea format to deg -------------------------------------*/
static double dmm2deg(double dmm)
{
return floor(dmm / 100.0) + fmod(dmm, 100.0) / 60.0;
}
/* decode NMEA RMC (Recommended Minumum Specific GNSS Data) sentence ---------*/
static int decode_nmearmc(char** val, int n, sol_t* sol)
{
double tod = 0.0, lat = 0.0, lon = 0.0, vel = 0.0, dir = 0.0, date = 0.0, ang = 0.0, ep[6];
double pos[3] = { 0 };
char act = ' ', ns = 'N', ew = 'E', mew = 'E', mode = 'A';
int i;
trace(4, "decode_nmearmc: n=%d\n", n);
for (i = 0; i < n; i++)
{
switch (i)
{
case 0:
tod = atof(val[i]);
break; /* time in utc (hhmmss) */
case 1:
act = *val[i];
break; /* A=active,V=void */
case 2:
lat = atof(val[i]);
break; /* latitude (ddmm.mmm) */
case 3:
ns = *val[i];
break; /* N=north,S=south */
case 4:
lon = atof(val[i]);
break; /* longitude (dddmm.mmm) */
case 5:
ew = *val[i];
break; /* E=east,W=west */
case 6:
vel = atof(val[i]);
break; /* speed (knots) */
case 7:
dir = atof(val[i]);
break; /* track angle (deg) */
case 8:
date = atof(val[i]);
break; /* date (ddmmyy) */
case 9:
ang = atof(val[i]);
break; /* magnetic variation */
case 10:
mew = *val[i];
break; /* E=east,W=west */
case 11:
mode = *val[i];
break; /* mode indicator (>nmea 2) */
/* A=autonomous,D=differential */
/* E=estimated,N=not valid,S=simulator */
}
}
if ((act != 'A' && act != 'V') || (ns != 'N' && ns != 'S') || (ew != 'E' && ew != 'W'))
{
trace(3, "invalid nmea rmc format\n");
return 0;
}
pos[0] = (ns == 'S' ? -1.0 : 1.0) * dmm2deg(lat) * D2R;
pos[1] = (ew == 'W' ? -1.0 : 1.0) * dmm2deg(lon) * D2R;
septime(date, ep + 2, ep + 1, ep);
septime(tod, ep + 3, ep + 4, ep + 5);
ep[0] += ep[0] < 80.0 ? 2000.0 : 1900.0;
sol->time = utc2gpst(epoch2time(ep));
pos2ecef(pos, sol->rr);
sol->stat = mode == 'D' ? SOLQ_DGPS : SOLQ_SINGLE;
sol->ns = 0;
sol->type = 0; /* postion type = xyz */
trace(1, "decode_nmearmc: %s rr=%.3f %.3f %.3f stat=%d ns=%d vel=%.2f dir=%.0f ang=%.0f mew=%c mode=%c\n",
time_str(sol->time, 0), sol->rr[0], sol->rr[1], sol->rr[2], sol->stat, sol->ns,
vel, dir, ang, mew, mode);
return 2; /* update time */
}
static int decode_nmea(char* buff, sol_t* sol)
{
char* p, * q, * val[MAXFIELD];
int n = 0;
trace(4, "decode_nmea: buff=%s\n", buff);
/* parse fields */
for (p = buff; *p && n < MAXFIELD; p = q + 1)
{
if ((q = strchr(p, ',')) || (q = strchr(p, '*')))
{
val[n++] = p;
*q = '\0';
}
else
break;
}
if (n < 1)
{
return 0;
}
if (!strcmp(val[0] + 3, "RMC"))
{ /* $xxRMC */
return decode_nmearmc(val + 1, n - 1, sol);
}
//else if (!strcmp(val[0] + 3, "ZDA"))
//{ /* $xxZDA */
// return decode_nmeazda(val + 1, n - 1, sol);
//}
//else if (!strcmp(val[0] + 3, "GGA"))
//{ /* $xxGGA */
// return decode_nmeagga(val + 1, n - 1, sol);
//}
return 0;
}
extern int input_nmea(nmea_t *nmea, uint8_t data)
{
static uint8_t state = 0;
int8_t ans = 0;
trace(5, "input_nmea: data=%02x\n", data);
if (nmea->nbyte == 0 && data == '$')
{
state = 1;
}
else if (nmea->nbyte != 0 && data == '*')
{
state = 2;
}
else if (nmea->len != 0)
{
state = 3;
}
if (state == 1)
{
nmea->buff[nmea->nbyte++] = data;
if(nmea->nbyte == 2 && nmea->buff[1] != 'G')
{
ans = -1;
}
}
else if (state == 2)
{
nmea->buff[nmea->nbyte++] = data;
nmea->len = nmea->nbyte + 2;
}
else if (state == 3)
{
nmea->buff[nmea->nbyte++] = data;
if (nmea->nbyte == nmea->len) /* 数据收集完毕 进行校验 */
{
if (0 == nmea_xor(nmea->buff, nmea->len))
{
trace(1, "nmea parity error: len=%d \n", nmea->len);
ans = -1;
}
else
{
ans = 1;
}
}
}
if (ans == 1)
{
return decode_nmea((char *)nmea->buff, &(nmea->sol));
}
if(nmea->nbyte >=255) /* 越界 */
{
ans = -1;
}
if (ans != 0)
{
nmea->len = 0;
nmea->nbyte = 0;
state = 0;
}
return ans;
}

13
RTK/rtklib.h
View File

@ -31,7 +31,7 @@
#define ENAGPS #define ENAGPS
// #define ENAGLO // #define ENAGLO
// #define ENAGAL // #define ENAGAL
// #define ENACMP #define ENACMP
// #define ENAQZS // #define ENAQZS
// #define TRACE // #define TRACE
// #define L5_TO_L2 // #define L5_TO_L2
@ -537,6 +537,8 @@
//#endif //#endif
/* type definitions ----------------------------------------------------------*/ /* type definitions ----------------------------------------------------------*/
typedef struct typedef struct
{ /* time struct */ { /* time struct */
time_t time; /* time (s) expressed by standard time_t */ time_t time; /* time (s) expressed by standard time_t */
@ -1274,6 +1276,15 @@ typedef struct
double bl_reset; /* baseline length to reset (km) */ double bl_reset; /* baseline length to reset (km) */
// lock_t lock; /* lock flag */ // lock_t lock; /* lock flag */
} rtksvr_t; } rtksvr_t;
typedef struct
{
sol_t sol;
int nbyte; /* number of bytes in message buffer */
int len; /* message length (bytes) */
uint8_t buff[256]; /* message buffer */
} nmea_t;
// typedef struct { /* GIS data point type */ // typedef struct { /* GIS data point type */
// double pos[3]; /* point data {lat,lon,height} (rad,m) */ // double pos[3]; /* point data {lat,lon,height} (rad,m) */
// } gis_pnt_t; // } gis_pnt_t;

20
task/rtk_task.c
View File

@ -16,9 +16,9 @@
#define EVENT_REF_RTCM_DATA_FLAG EVENT_FLAG(0) #define EVENT_REF_RTCM_DATA_FLAG EVENT_FLAG(0)
#define EVENT_UAER_RTCM_DATA_FLAG EVENT_FLAG(1) #define EVENT_UAER_RTCM_DATA_FLAG EVENT_FLAG(1)
#define THREAD_get_user_rtcm_data_priority 10 #define THREAD_get_user_rtcm_data_priority 20
#define THREAD_get_ref_rtcm_data_priority 10 #define THREAD_get_ref_rtcm_data_priority 20
#define thread_rtk_proces_priority 9 #define thread_rtk_proces_priority 19
#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 #define get_rtcm_ref_data_stack_size DEFAULT_USER_THREAD_STACK_SIZE
@ -201,16 +201,12 @@ static void thread_rtk_proces(void *parameter)
} }
else else
{ {
// rt_thread_sleep(5);
continue; continue;
} }
uint8_t nr = 0; uint8_t nr = 0;
uint8_t nu = 0; uint8_t nu = 0;
/* 将基准站和移动站的观测数据合二为一 这里重新申请了一个MAXOBS*2 的数组, 我觉得是不是有点问题? */
obs.n = 0; obs.n = 0;
for (int i = 0; i < fobs[0]; i++) for (int i = 0; i < fobs[0]; i++)
{ {
for (int j = 0; j < svr->obs[0][i].n; j++) for (int j = 0; j < svr->obs[0][i].n; j++)
@ -227,9 +223,8 @@ static void thread_rtk_proces(void *parameter)
} }
nu = (nu > MAXOBS) ? MAXOBS : nu; nu = (nu > MAXOBS) ? MAXOBS : nu;
nr = (nr > MAXOBS * 2 - nu) ? (MAXOBS * 2) - nr : nr; nr = (nr > MAXOBS * 2 - nu) ? (MAXOBS * 2) - nr : nr;
if (nu + nr == 0) if (nu == 0||nr ==0)
{ {
// rt_thread_mdelay(5);
continue; continue;
} }
obs.data = (obsd_t *) rt_malloc(sizeof(obsd_t) * (nu + nr)); obs.data = (obsd_t *) rt_malloc(sizeof(obsd_t) * (nu + nr));
@ -264,7 +259,7 @@ static void thread_rtk_proces(void *parameter)
timeset(gpst2utc(timeadd(svr->rtk.sol.time, tt))); timeset(gpst2utc(timeadd(svr->rtk.sol.time, tt)));
/* write solution */ /* write solution */
// writesol_mini(svr, i); writesol_mini(svr, 0);
} }
/* 如果设定的运算时间到了发现还没有计算完, 则记录没有计算完的观测次数 */ /* 如果设定的运算时间到了发现还没有计算完, 则记录没有计算完的观测次数 */
// if ((int) (tickget() - tick) >= svr->cycle) // if ((int) (tickget() - tick) >= svr->cycle)
@ -274,7 +269,7 @@ static void thread_rtk_proces(void *parameter)
/* send null solution if no solution (1hz) */ /* send null solution if no solution (1hz) */
if (svr->rtk.sol.stat == SOLQ_NONE && (int) (tick - tick1hz) >= 1000) if (svr->rtk.sol.stat == SOLQ_NONE && (int) (tick - tick1hz) >= 1000)
{ {
// writesol_mini(svr, 0); writesol_mini(svr, 0);
tick1hz = tick; tick1hz = tick;
} }
/* send nmea request to base/nrtk input stream */ /* send nmea request to base/nrtk input stream */
@ -292,7 +287,6 @@ static void thread_rtk_proces(void *parameter)
} }
} }
sleepms(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,
@ -323,7 +317,7 @@ void task_init(void)
rt_completion_init(&rtcm_user_data_process_completion); rt_completion_init(&rtcm_user_data_process_completion);
rt_completion_init(&rtcm_ref_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, rt_thread_t trd = rt_thread_create("rtk_proces", thread_rtk_proces, RT_NULL, 20480, thread_rtk_proces_priority,
DEFAULT_USER_THREA_TICK); DEFAULT_USER_THREA_TICK);
rt_thread_startup(trd); rt_thread_startup(trd);
user_init_task(&get_user_rtcm_data, "thread_get_user_rtcm_data", thread_get_user_rtcm_data, user_init_task(&get_user_rtcm_data, "thread_get_user_rtcm_data", thread_get_user_rtcm_data,