diff --git a/.settings/.rtmenus b/.settings/.rtmenus index dc6d203..376875d 100644 Binary files a/.settings/.rtmenus and b/.settings/.rtmenus differ diff --git a/RTK/ST_rtksvr.c b/RTK/ST_rtksvr.c index c435fcb..078dec3 100644 --- a/RTK/ST_rtksvr.c +++ b/RTK/ST_rtksvr.c @@ -69,7 +69,7 @@ void writesol_mini(rtksvr_t *svr, int index) /* output solution status */ // rtksvrlock(svr); -// // n = rtkoutstat(&svr->rtk, (char*)buff); + n = rtkoutstat(&svr->rtk, (char*)buff); // rtksvrunlock(svr); } else @@ -77,7 +77,8 @@ void writesol_mini(rtksvr_t *svr, int index) /* output solution */ 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: 这里应该改为从串口输出解算后的数据 /* 输出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 ---------------------------------------------*/ +extern nmea_t nmea_pnt; int decoderaw(rtksvr_t *svr, int index) { obs_t *obs; @@ -359,24 +361,7 @@ int decoderaw(rtksvr_t *svr, int index) for (i = 0; i < svr->nb[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; - }*/ +// input_nmea(&nmea_pnt,svr->buff[index][i]); ret = input_rtcm3(svr->rtcm + index, svr->buff[index][i]); /* (-1: error message, 0: no message, 1: input observation data, 2: input ephemeris, 5: input station pos/ant parameters, diff --git a/RTK/lambda_par.c b/RTK/lambda_par.c index e696ccf..c6491f8 100644 --- a/RTK/lambda_par.c +++ b/RTK/lambda_par.c @@ -15,14 +15,14 @@ y = tmp_; \ } while (0) -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); +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); /* 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; - double a, * A = mat(n, n); + double a, *A = mat(n, n); memcpy(A, Q, sizeof(double) * n * n); 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; } /* 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; @@ -60,7 +60,7 @@ static void gauss(int n, double* L, double* Z, int i, int j) } } /* 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; 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]); } /* 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; double del; @@ -116,12 +116,12 @@ static void reduction(int n, double* L, double* D, double* Z) zs I transformed double-diff phase biases zn O fixed solutions s O sum of residuals for fixed solutions */ -static int search(int n, int m, const double* L, const double* D, - const double* zs, double* zn, double* s) +static int search(int n, int m, const double *L, const double *D, + const double *zs, double *zn, double *s) { int i, j, k, c, nn = 0, imax = 0; 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; dist[k] = 0.0; // k表示当前层，从最后一层（n-1）开始计算 @@ -233,11 +233,11 @@ static float my_normcdf(float a) // 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; - mmat = mat(m1, m2); + // mmat = mat(m1, m2); //从大矩阵里取小矩阵 for (i = 0; i < m1; i++) 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); } - 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; - mmat = mat(m1, m2); + // mmat = mat(m1, m2); //从大矩阵里取小矩阵 for (i = 0; i < m1; i++) 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); } - return mmat; + // return mmat; } /* parlambda ------------------------------ * 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) * 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, - double* s) +extern int parlambda(rtk_t *rtk, int n, int m, const double *a, const double *Q, double *F, + double *s) { int info; - double* L, * D, * Z, * z, * E, * Qt, * Qzhat; + double *L, *D, *Z, *z, *E, *Qt, *Qzhat; if (n <= 0 || m <= 0) return -1; 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 if (!(info = parsearch(rtk, n, z, Qzhat, Z, L, D, 2, F, s))) { - }; + ; + } + free(Qt); + free(Qzhat); } free(L); free(D); @@ -329,14 +332,14 @@ args: 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, - const double* L, const double* D, int m, double* F, double* s) +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) { - 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] */ - 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--) { 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]; 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 */ { @@ -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_fix = mat(n, m); // first k - 1 ambiguities are adjusted based on correlation with the fixed ambiguities - Q11 = nmatgetkmat_RD(Qzhat, n, n, p, p); - Q12 = nmatgetkmat_RU(Qzhat, n, n, k - 1, p); + nmatgetkmat_RD(Qzhat, n, n, p, p, Q11); + nmatgetkmat_RU(Qzhat, n, n, k - 1, p, Q12); matinv(Q11, p); matmul("NN", k - 1, p, p, 1.0, Q12, Q11, 0.0, Qp); // Qp = Q12 / Q11; 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 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]; */ for (i = 0; i < 2 * n; i++) { diff --git a/RTK/nmea_zjut.c b/RTK/nmea_zjut.c new file mode 100644 index 0000000..001913f --- /dev/null +++ b/RTK/nmea_zjut.c @@ -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; +} diff --git a/RTK/rtklib.h b/RTK/rtklib.h index 38fbccf..c279b71 100644 --- a/RTK/rtklib.h +++ b/RTK/rtklib.h @@ -31,7 +31,7 @@ #define ENAGPS // #define ENAGLO // #define ENAGAL -// #define ENACMP + #define ENACMP // #define ENAQZS // #define TRACE // #define L5_TO_L2 @@ -537,6 +537,8 @@ //#endif /* type definitions ----------------------------------------------------------*/ + + typedef struct { /* time struct */ time_t time; /* time (s) expressed by standard time_t */ @@ -1274,6 +1276,15 @@ typedef struct double bl_reset; /* baseline length to reset (km) */ // lock_t lock; /* lock flag */ } 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 */ // double pos[3]; /* point data {lat,lon,height} (rad,m) */ // } gis_pnt_t; diff --git a/task/rtk_task.c b/task/rtk_task.c index 42f1816..59c1946 100644 --- a/task/rtk_task.c +++ b/task/rtk_task.c @@ -16,9 +16,9 @@ #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 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 @@ -201,16 +201,12 @@ static void thread_rtk_proces(void *parameter) } else { -// rt_thread_sleep(5); continue; } uint8_t nr = 0; uint8_t nu = 0; - - /* 将基准站和移动站的观测数据合二为一 这里重新申请了一个MAXOBS*2 的数组, 我觉得是不是有点问题? */ obs.n = 0; - for (int i = 0; i < fobs[0]; i++) { 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; nr = (nr > MAXOBS * 2 - nu) ? (MAXOBS * 2) - nr : nr; - if (nu + nr == 0) + if (nu == 0||nr ==0) { -// rt_thread_mdelay(5); continue; } 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))); /* write solution */ -// writesol_mini(svr, i); + writesol_mini(svr, 0); } /* 如果设定的运算时间到了发现还没有计算完, 则记录没有计算完的观测次数 */ // if ((int) (tickget() - tick) >= svr->cycle) @@ -274,7 +269,7 @@ static void thread_rtk_proces(void *parameter) /* send null solution if no solution (1hz) */ if (svr->rtk.sol.stat == SOLQ_NONE && (int) (tick - tick1hz) >= 1000) { -// writesol_mini(svr, 0); + writesol_mini(svr, 0); tick1hz = tick; } /* 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, @@ -323,7 +317,7 @@ void task_init(void) 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, 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); rt_thread_startup(trd); user_init_task(&get_user_rtcm_data, "thread_get_user_rtcm_data", thread_get_user_rtcm_data,