内部测试第一版
This commit is contained in:
parent
c5a5c396f1
commit
f9fc38b00d
Binary file not shown.
@ -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
|
||||
@ -78,6 +78,7 @@ void writesol_mini(rtksvr_t *svr, int index)
|
||||
n = outsols(buff, &svr->rtk.sol, svr->rtk.rb, svr->solopt + i);
|
||||
}
|
||||
// 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,
|
||||
|
||||
@ -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]),
|
||||
@ -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);
|
||||
@ -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++)
|
||||
|
||||
223
RTK/nmea_zjut.c
Normal file
223
RTK/nmea_zjut.c
Normal 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
13
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;
|
||||
|
||||
@ -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,
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user