内部测试第一版

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
@@ -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_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]);
         /* (-1: error message, 0: no message, 1: input observation data,
             2: input ephemeris, 5: input station pos/ant parameters,

RTK/lambda_par.c

@@ -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);
+                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);
                 matmul("NN", k - 1, p, p, 1.0, Q12, Q11, 0.0, Qp); // Qp = Q12 / Q11;
                 for (i = 0; i < 2 * p; i++)

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;
+}

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;

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_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_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,